1. 四元数

在Pinocchio中
二维位姿由 $\mathbf{P_4}$ 表示：

三维位姿由 $\mathbf{P_7}$ 表示：

复数 $\cos{\theta} + \sin{\theta}i$
四元数 $\cos{\frac{\theta}{2}} + \sin{\frac{\theta}{2}}\mathbf{u}$ ($w, \mathbf{v})$
单位四元数（$|q| = 1$）可以表示三维空间中的任意绕单位轴 $\mathbf{u}$ 旋转角度 $\theta$ 的旋转。
单位四元数 $q$ 和 $-q$ 表示同一个旋转，这一性质称为 SO(3) 的双覆盖（double cover）。在数值计算中，我们通常约定取 $w \ge 0$ 的那一个来保证唯一性。

2. 三维exp和log

2.1 $[\omega]_\times$的exp

2.1.1 $[\omega]_\times$的多项式

$[\omega]\times$的特征根根方程为三次多项式，根据凯莱哈密顿定理，$[\omega]^3\times$ 一定可以用低次表示。经过计算：

旋量也有相似的结论：

2.1.2 $e^{[\omega]_\times}$

2.1.2 $t$——平移部分

根据 $\mathbf{V}(\omega){\mathbf{V}(\omega)}^{-1}=\mathbf{I}$ ，整理多项式系数得到：

即

3. Pinocchio库中的log实现

3.1 log3

  1. 函数签名
    1
    2
    3
    4
    5
    6
    7
    8
    template<typename QuaternionLike>
    Eigen::Matrix<
    typename QuaternionLike::Scalar,
    3, 1,
    PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
    log3(
    const Eigen::QuaternionBase<QuaternionLike> & quat,
    typename QuaternionLike::Scalar & theta)
  • 输入：quat —— 单位四元数（通过 Eigen::QuaternionBase 接受任意兼容类型）
  • 输出：
    • 返回值：3×1 旋转向量 $\boldsymbol{\omega} \in \mathbb{R}^3$
    • 引用参数 theta：旋转角度 $\theta$（范围 $[0, \pi]$）
  1. 类型与常量准备
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    typedef typename QuaternionLike::Scalar Scalar;
    static constexpr int Options = ...;
    typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;

    Vector3 res;
    const Scalar norm_squared = quat.vec().squaredNorm();

    static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
    static const Scalar ts_prec = TaylorSeriesExpansion<Scalar>::template precision<2>();
    const Scalar norm = math::sqrt(norm_squared + eps * eps);
  • norm_squared = $|\mathbf{v}|^2$，即四元数虚部的平方范数
  • eps：机器精度，用于正则化（避免除以零）
  • ts_prec：泰勒展开的阈值，由 TaylorSeriesExpansion<Scalar>::precision<2>() 计算
  • norm = $\sqrt{|\mathbf{v}|^2 + \epsilon^2}$：保证即使在零旋转时也有正数值，供 atan2 使用
  1. 符号归一化（关键步骤）
    1
    2
    3
    4
    5
    const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0), Scalar(+1), Scalar(-1));

    Eigen::Quaternion<Scalar, Options> quat_pos;
    quat_pos.w() = pos_neg * quat.w();
    quat_pos.vec() = pos_neg * quat.vec();

由于 $q$ 和 $-q$ 表示同一旋转，为保证对数映射的唯一性，我们强制 $w \ge 0$：

  • 若原 w < 0，则取负四元数（pos_neg = -1），虚部也相应变号
  • 归一化后，角度 $\theta \in [0, \pi]$，保证了唯一性
  1. 计算角度 $\theta$
    1
    2
    3
    4
    5
    6
    7
    8
    const Scalar theta_2 = math::atan2(norm, quat_pos.w()); // in [0,pi]
    const Scalar y_x = norm / quat_pos.w(); // nonnegative
    const Scalar y_x_sq = norm_squared / (quat_pos.w() * quat_pos.w());

    theta = if_then_else(
    LT, norm_squared, ts_prec,
    Scalar(2.) * (Scalar(1) - y_x_sq / Scalar(3)) * y_x,
    Scalar(2.) * theta_2);

这里分两种情况：

精确分支（角度较大）：$\theta = 2\theta2 = 2\arctan2(|\mathbf{v}|, w{\text{pos}})$

小角度分支（norm_squared < ts_prec）：使用泰勒展开。令 $x = |\mathbf{v}| / w_{\text{pos}}$，则：

这个展开避免了在接近零时 atan2 的精度损失。

  1. 计算缩放因子 inv_sinc
    1
    2
    3
    4
    5
    const Scalar th2_2 = theta * theta / Scalar(4); // (theta/2)^2
    const Scalar inv_sinc = if_then_else(
    LT, norm_squared, ts_prec,
    Scalar(2) * (Scalar(1) + th2_2 / Scalar(6) + Scalar(7) / Scalar(360) * th2_2 * th2_2),
    theta / math::sin(theta_2));

inv_sinc 对应公式中的 $\theta / \sin(\theta/2)$：

精确分支：$\theta / \sin(\theta_2)$，其中 $\theta_2 = \theta/2$

小角度分支：利用 $\sin x = x - x^3/6 + x^5/120 - \cdots$：

代码中的 th2_2 = x^2，展开式与公式完全一致。

  1. 计算旋转向量
    1
    2
    3
    4
    for (Eigen::Index k = 0; k < 3; ++k)
    res[k] = inv_sinc * quat_pos.vec()[k];

    return res;

最终得到：

这正是 SO(3) 对数映射的公式。

  1. 数值稳定性策略总结
问题 解决方案
$q$ 和 $-q$ 二义性 符号归一化：强制 $w \ge 0$
小角度时 $\sin(\theta/2) \to 0$ 泰勒展开代替直接计算
除零风险 加 $\epsilon$ 正则化

3.2 log6

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
template<typename Vector3Like, typename QuaternionLike, typename MotionDerived>
    static void run(
      const Eigen::QuaternionBase<QuaternionLike> & quat,
      const Eigen::MatrixBase<Vector3Like> & vec,
      MotionDense<MotionDerived> & mout)
    {
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, vec, 3, 1);

      typedef typename Vector3Like::Scalar Scalar;
      static constexpr int Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options;
      typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
      const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();

      using namespace internal;

      const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0), Scalar(+1), Scalar(-1));

      Scalar theta;
      Vector3 w(quaternion::log3(quat, theta)); // theta nonsingular by construction
      const Scalar t2 = w.squaredNorm();

      // Scalar st,ct; SINCOS(theta,&st,&ct);
      Scalar st_2, ct_2;
      ct_2 = pos_neg * quat.w();
      st_2 = math::sqrt(quat.vec().squaredNorm() + eps * eps);
      const Scalar cot_th_2 = ct_2 / st_2;
      // const Scalar cot_th_2 = ( st / (Scalar(1) - ct) ); // cotan of half angle

      // we use formula (9.26) from
      // https://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf for the linear
      // part of the Log map. A Taylor series expansion of cotan can be used up to order 4
      const Scalar th_2_squared = t2 / Scalar(4); // (theta / 2) squared

      //      const Scalar alpha = if_then_else(LE,theta,TaylorSeriesExpansion<Scalar>::template
      //      precision<3>(),
      //                                        static_cast<Scalar>(Scalar(1) - t2/Scalar(12) -
      //                                        t2*t2/Scalar(720)), // then
      //                                        static_cast<Scalar>(theta * cot_th_2 /(Scalar(2)))
      //                                        // else
      //                                        );

      const Scalar beta_alt = (Scalar(1) / Scalar(3) - th_2_squared / Scalar(45)) / Scalar(4);
      const Scalar beta = if_then_else(
        LE, theta, TaylorSeriesExpansion<Scalar>::template precision<3>(),
        static_cast<Scalar>(beta_alt),                                       // then
        static_cast<Scalar>(Scalar(1) / t2 - cot_th_2 * Scalar(0.5) / theta) // else
        // static_cast<Scalar>(Scalar(1) / t2 - st/(Scalar(2)*theta*(Scalar(1)-ct))) // else
      );

      // mout.linear().noalias() = alpha * vec - Scalar(0.5) * w.cross(vec) + (beta * w.dot(vec)) *
      // w;
      mout.linear().noalias() = vec - Scalar(0.5) * w.cross(vec) + beta * w.cross(w.cross(vec));
      mout.angular() = w;
    }
  };

在更完整的 run 函数中，log3 被用于合成 SE(3) 上的速度：

1
2
mout.linear() = vec - 0.5 * w.cross(vec) + beta * w.cross(w.cross(vec))
mout.angular() = w

其中 $w$ 是 log3 返回的旋转向量，vec 是位移向量，beta 是依赖于 $\theta$ 的系数。这个公式来源于 SE(3) 对数映射的线性部分，用于从位姿变化合成空间速度。

1
2
3
4
5
6
const Scalar th_2_squared = t2 / Scalar(4);
const Scalar beta_alt = (Scalar(1) / Scalar(3) - th_2_squared / Scalar(45)) / Scalar(4);
const Scalar beta = if_then_else(
LE, theta, TaylorSeriesExpansion<Scalar>::template precision<3>(),
static_cast<Scalar>(beta_alt),                                       
static_cast<Scalar>(Scalar(1) / t2 - cot_th_2 * Scalar(0.5) / theta));

这里泰勒展开符号似乎是Pinocchio库写错了，已经提了issue。

3. Pinocchio库中的exp实现

3.1 轴角转四元数

1
2
3
4
5
6
7
8
9
10
11
12
/** Set \c *this from an angle-axis \a aa and returns a reference to \c *this
  */
template<class Derived>
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
{
  EIGEN_USING_STD(cos)
  EIGEN_USING_STD(sin)
  Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
  this->w() = cos(ha);
  this->vec() = sin(ha) * aa.axis();
  return derived();
}

3.2 旋量exp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
///
    /// \brief Exp: so3 -> SO3 (quaternion)
    ///
    /// \returns the integral of the velocity vector as a quaternion.
    ///
    /// \param[in] v The angular velocity vector.
    /// \param[out] qout The quaternion where the result is stored.
    ///
    template<typename Vector3Like, typename QuaternionLike>
    void
    exp3(const Eigen::MatrixBase<Vector3Like> & v, Eigen::QuaternionBase<QuaternionLike> & quat_out)
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
      assert(v.size() == 3);

      typedef typename Vector3Like::Scalar Scalar;
      static constexpr int Options =
        PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options;
      typedef Eigen::Quaternion<typename QuaternionLike::Scalar, Options> QuaternionPlain;
      const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();

      const Scalar t2 = v.squaredNorm();
      const Scalar t = math::sqrt(t2 + eps * eps);

      static const Scalar ts_prec =
        TaylorSeriesExpansion<Scalar>::template precision<3>(); // Precision for the Taylor series
                                                                // expansion.

      Eigen::AngleAxis<Scalar> aa(t, v / t);
      QuaternionPlain quat_then(aa);

      // order 4 Taylor expansion in theta / (order 2 in t2)
      QuaternionPlain quat_else;
      const Scalar t2_2 = t2 / 4; // theta/2 squared
      quat_else.vec() =
        Scalar(0.5) * (Scalar(1) - t2_2 / Scalar(6) + t2_2 * t2_2 / Scalar(120)) * v;
      quat_else.w() = Scalar(1) - t2_2 / 2 + t2_2 * t2_2 / 24;

      using ::pinocchio::internal::if_then_else;
      for (Eigen::Index k = 0; k < 4; ++k)
      {
        quat_out.coeffs().coeffRef(k) = if_then_else(
          ::pinocchio::internal::GT, t2, ts_prec, quat_then.coeffs().coeffRef(k),
          quat_else.coeffs().coeffRef(k));
      }
    }

依然是对小角度进行了泰勒展开处理。

3.2 平移exp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
/// \brief The se3 -> SE3 exponential map, using quaternions to represent the output rotation.
///
/// \returns the integral of the twist motion over unit time.
///
/// \param[in] motion the spatial motion.
/// \param[out] q the output transform in \f$\mathbb{R}^3 x S^3\f$.
template<typename MotionDerived, typename Config_t>
void exp6(const MotionDense<MotionDerived> & motion, Eigen::MatrixBase<Config_t> & qout)
{
  static constexpr int Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t)::Options;
  typedef typename Config_t::Scalar Scalar;
  typedef typename MotionDerived::Vector3 Vector3;
  typedef Eigen::Quaternion<Scalar, Options> Quaternion_t;
  const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();

  const typename MotionDerived::ConstAngularType & w = motion.angular();
  const typename MotionDerived::ConstLinearType & v = motion.linear();

  const Scalar t2 = w.squaredNorm() + eps * eps;
  const Scalar t = math::sqrt(t2);

  Scalar ct, st;
  SINCOS(t, &st, &ct);

  const Scalar inv_t2 = Scalar(1) / t2;
  const Scalar ts_prec =
    TaylorSeriesExpansion<Scalar>::template precision<3>(); // Taylor expansion precision

  using ::pinocchio::internal::if_then_else;
  using ::pinocchio::internal::LT;

  const Scalar alpha_wxv = if_then_else(
    LT, t, ts_prec,
    Scalar(0.5) - t2 / Scalar(24), // then: use Taylor expansion
    (Scalar(1) - ct) * inv_t2      // else
  );

  const Scalar alpha_w2 = if_then_else(
    LT, t, ts_prec, Scalar(1) / Scalar(6) - t2 / Scalar(120), (t - st) * inv_t2 / t);

  // linear part
  Eigen::Map<Vector3> trans_(qout.derived().template head<3>().data());
  trans_.noalias() = v + alpha_wxv * w.cross(v) + alpha_w2 * w.cross(w.cross(v));

  // quaternion part
  typedef Eigen::Map<Quaternion_t> QuaternionMap_t;
  QuaternionMap_t quat_(qout.derived().template tail<4>().data());
  exp3(w, quat_);
}

3.2.1 函数签名与作用

exp6 实现了从李代数 se(3)（运动旋量/twist）到李群 SE(3)（刚体变换）的指数映射，输出采用 平移 + 单位四元数 的表示（$\mathbb{R}^3 \times S^3$）。

函数签名如下：

1
2
template<typename MotionDerived, typename Config_t>
void exp6(const MotionDense<MotionDerived> & motion, Eigen::MatrixBase<Config_t> & qout)

  • 输入：motion 是一个 MotionDense 类型，表示空间运动旋量，包含角速度 angular()（记为 $\mathbf{w}$）和线速度 linear()（记为 $\mathbf{v}$）。
  • 输出：qout 是一个 7 维向量（或兼容的 Eigen 表达式），前 3 个元素为平移向量 $\mathbf{t}$，后 4 个元素为单位四元数 $\mathbf{q}$（顺序为 $x, y, z, w$，与 Eigen 的 Quaternion 内存布局一致）。
  • 功能：计算单位时间内的指数映射：其中 $\hat{\xi}$ 是 se(3) 的 4×4 矩阵表示。

3.2.2 类型与常量准备

1
2
3
4
5
static constexpr int Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t)::Options;
typedef typename Config_t::Scalar Scalar;
typedef typename MotionDerived::Vector3 Vector3;
typedef Eigen::Quaternion<Scalar, Options> Quaternion_t;
const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
  • 提取标量类型、向量类型，并定义四元数类型。
  • eps 为机器精度，用于后续防止除零。

3.2.3 提取角速度与线速度

1
2
const typename MotionDerived::ConstAngularType & w = motion.angular();
const typename MotionDerived::ConstLinearType & v = motion.linear();
  • w 为角速度向量，v 为线速度向量。

3.2.4 计算角度与三角函数

1
2
3
4
5
const Scalar t2 = w.squaredNorm() + eps * eps;
const Scalar t = math::sqrt(t2);

Scalar ct, st;
SINCOS(t, &st, &ct);
  • t2 为角速度模长平方（加 eps² 避免零模长导致除零）。
  • t 为角度 $\theta$。
  • SINCOS 是 Pinocchio 提供的宏/函数，同时计算 sin(t) 和 cos(t)，分别存入 st 和 ct。

3.2.5 计算系数 $\alpha$ 和 $\beta$（带泰勒展开）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
const Scalar inv_t2 = Scalar(1) / t2;
const Scalar ts_prec = TaylorSeriesExpansion<Scalar>::template precision<3>();

using ::pinocchio::internal::if_then_else;
using ::pinocchio::internal::LT;

const Scalar alpha_wxv = if_then_else(
    LT, t, ts_prec,
    Scalar(0.5) - t2 / Scalar(24),   // 泰勒展开（小角度）
    (Scalar(1) - ct) * inv_t2        // 精确公式
);

const Scalar alpha_w2 = if_then_else(
    LT, t, ts_prec,
    Scalar(1) / Scalar(6) - t2 / Scalar(120),  // 泰勒展开
    (t - st) * inv_t2 / t                      // 精确公式
);
  • 当 t < ts_prec 时使用泰勒展开，否则使用精确公式。
    • $\alpha = \frac{1-\cos\theta}{\theta^2}$：
      • 精确：(1 - ct) * inv_t2
      • 泰勒：$\frac{1}{2} - \frac{\theta^2}{24} + O(\theta^4)$，代码中 0.5 - t2/24（因为 t2 ≈ θ²）。
    • $\beta = \frac{\theta - \sin\theta}{\theta^3}$：
      • 精确：(t - st) * inv_t2 / t，即 (θ - sinθ)/θ³。
      • 泰勒：$\frac{1}{6} - \frac{\theta^2}{120} + O(\theta^4)$，代码中 1/6 - t2/120。
  • 使用 if_then_else 和 LT 实现无分支选择，利于向量化。

3.2.6 计算平移部分

1
2
Eigen::Map<Vector3> trans_(qout.derived().template head<3>().data());
trans_.noalias() = v + alpha_wxv * w.cross(v) + alpha_w2 * w.cross(w.cross(v));
  • 将 qout 的前 3 个元素映射为 Vector3（平移向量）。
  • 直接计算：
  • noalias() 避免临时变量，提高效率。

3.2.7 计算旋转部分（四元数）

1
2
3
typedef Eigen::Map<Quaternion_t> QuaternionMap_t;
QuaternionMap_t quat_(qout.derived().template tail<4>().data());
exp3(w, quat_);
  • 将 qout 的后 4 个元素映射为四元数。
  • 调用之前解析过的 exp3 函数，将角速度 w 转换为单位四元数，直接写入 quat_。
  • 注意 exp3 要求输出四元数的 coeffs() 顺序为 (x, y, z, w)，与 Eigen 内存布局一致