四元数 (Quaternion)与李群SE（3）知识点（1）

四元数基础

一、什么是四元数 (Quaternion)

四元数最早由 Hamilton (1843) 提出，是复数的扩展。

形式：

q=w+xi+yj+zk q = w + xi + yj + zk q=w+xi+yj+zk

其中：

• w∈Rw \in \mathbb{R}w∈R 为实部

• x,y,z∈Rx,y,z \in \mathbb{R}x,y,z∈R 为虚部

• i,j,ki,j,ki,j,k 为虚数单位，满足：

  i2=j2=k2=ijk=−1 i^2 = j^2 = k^2 = ijk = -1 i2=j2=k2=ijk=−1

所以四元数是 4 维实数向量：

q=(w,x,y,z)∈R4 q = (w, x, y, z) \in \mathbb{R}^4 q=(w,x,y,z)∈R4

---

二、四元数的运算

1. 加法

   逐分量相加。

2. 乘法 （非交换！）

   若

q1=(w1,v1),q2=(w2,v2),v=(x,y,z) q_1 = (w_1, \mathbf{v}_1), \quad q_2 = (w_2, \mathbf{v}_2), \quad \mathbf{v}=(x,y,z) q1=(w1,v1),q2=(w2,v2),v=(x,y,z)

则

q1⋅q2=(w1w2−v1⋅v2,    w1v2+w2v1+v1×v2) q_1 \cdot q_2 = (w_1w_2 - \mathbf{v}_1 \cdot \mathbf{v}_2,\;\; w_1\mathbf{v}_2 + w_2\mathbf{v}_1 + \mathbf{v}_1 \times \mathbf{v}_2) q1⋅q2=(w1w2−v1⋅v2,w1v2+w2v1+v1×v2)

3. 共轭

q∗=(w,−x,−y,−z) q^* = (w, -x, -y, -z) q∗=(w,−x,−y,−z)

4. 模长

∣q∣=w2+x2+y2+z2 |q| = \sqrt{w^2 + x^2 + y^2 + z^2} ∣q∣=w2+x2+y2+z2

5. 单位四元数
   若 ∣q∣=1|q|=1∣q∣=1，称为单位四元数。
   在三维空间旋转中，必须使用 单位四元数。

---

三、四元数与旋转的关系

在 3D 空间，一个旋转由 旋转轴 u\mathbf{u}u 和旋转角度 θ\thetaθ 唯一确定。

对应的单位四元数为：

q=cos⁡θ2+(uxi+uyj+uzk)sin⁡θ2 q = \cos\frac{\theta}{2} + (u_x i + u_y j + u_z k)\sin\frac{\theta}{2} q=cos2θ+(uxi+uyj+uzk)sin2θ

写成向量形式：

q=(cos⁡θ2,  uxsin⁡θ2,  uysin⁡θ2,  uzsin⁡θ2) q = \big( \cos\frac{\theta}{2},\; u_x\sin\frac{\theta}{2},\; u_y\sin\frac{\theta}{2},\; u_z\sin\frac{\theta}{2} \big) q=(cos2θ,uxsin2θ,uysin2θ,uzsin2θ)

---

四、用四元数旋转向量

给定一个三维向量 p\mathbf{p}p，可写为"纯四元数"：

p=(0,x,y,z) p = (0, x, y, z) p=(0,x,y,z)

旋转后向量：

p′=q⋅p⋅q∗ p' = q \cdot p \cdot q^* p′=q⋅p⋅q∗

其中 qqq 是单位四元数。

这避免了使用旋转矩阵时的数值漂移。

---

五、四元数与旋转矩阵互换

1. 四元数 → 旋转矩阵
   若 q=(w,x,y,z)q = (w, x, y, z)q=(w,x,y,z)：

R(q)=[1−2(y2+z2)2(xy−wz)2(xz+wy)2(xy+wz)1−2(x2+z2)2(yz−wx)2(xz−wy)2(yz+wx)1−2(x2+y2)] R(q) = \begin{bmatrix} 1 - 2(y^2+z^2) & 2(xy - wz) & 2(xz + wy) \\ 2(xy + wz) & 1 - 2(x^2+z^2) & 2(yz - wx) \\ 2(xz - wy) & 2(yz + wx) & 1 - 2(x^2+y^2) \end{bmatrix} R(q)= 1−2(y2+z2)2(xy+wz)2(xz−wy)2(xy−wz)1−2(x2+z2)2(yz+wx)2(xz+wy)2(yz−wx)1−2(x2+y2)

2. 旋转矩阵 → 四元数
   设 R∈SO(3)R \in SO(3)R∈SO(3)，

w=121+R00+R11+R22 w = \frac{1}{2}\sqrt{1 + R_{00} + R_{11} + R_{22}} w=211+R00+R11+R22

x=R21−R124w,y=R02−R204w,z=R10−R014w x = \frac{R_{21} - R_{12}}{4w}, \quad y = \frac{R_{02} - R_{20}}{4w}, \quad z = \frac{R_{10} - R_{01}}{4w} x=4wR21−R12,y=4wR02−R20,z=4wR10−R01

---

六、四元数插值（SLERP）

1. 原理简介

四元数球面线性插值用于在两个四元数之间做平滑旋转插值。给定两个单位四元数 q0q_0q0 和 q1q_1q1，插值公式为：

slerp(q0,q1,t)=sin⁡((1−t)θ)sin⁡θq0+sin⁡(tθ)sin⁡θq1 \text{slerp}(q_0, q_1, t) = \frac{\sin((1-t)\theta)}{\sin\theta} q_0 + \frac{\sin(t\theta)}{\sin\theta} q_1 slerp(q0,q1,t)=sinθsin((1−t)θ)q0+sinθsin(tθ)q1

其中：

• θ=arccos⁡(q0⋅q1)\theta = \arccos(q_0 \cdot q_1)θ=arccos(q0⋅q1) 是两四元数之间的夹角
• t∈[0,1]t \in [0,1]t∈[0,1] 为插值参数

注意：

1. 如果 q0⋅q1<0q_0 \cdot q_1 < 0q0⋅q1<0，需要将 q1q_1q1 取负号，保证旋转沿最短路径。
2. 当 θ≈0\theta \approx 0θ≈0 时，退化为线性插值（Lerp）。

---

2. 基于 Eigen 的 C++ 实现

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry> // 包含 Eigen::Quaternion

// SLERP 实现
Eigen::Quaterniond slerp(const Eigen::Quaterniond& q0, const Eigen::Quaterniond& q1, double t) {
    Eigen::Quaterniond q1_copy = q1;

    // 保证沿最短路径
    double dot = q0.dot(q1);
    if (dot < 0.0) {
        q1_copy.coeffs() *= -1.0;
        dot = -dot;
    }

    const double DOT_THRESHOLD = 0.9995;
    if (dot > DOT_THRESHOLD) {
        // 当角度很小时，使用线性插值避免数值不稳定
        Eigen::Quaterniond result = q0.coeffs() + t * (q1_copy.coeffs() - q0.coeffs());
        result.normalize();
        return result;
    }

    // 计算角度 theta
    double theta_0 = acos(dot);        // 两四元数间夹角
    double theta = theta_0 * t;        // 插值角度
    Eigen::Quaterniond q2 = q1_copy - q0 * dot;
    q2.normalize();

    return q0 * cos(theta) + q2 * sin(theta);
}

int main() {
    Eigen::Quaterniond q0(1, 0, 0, 0); // w, x, y, z
    Eigen::Quaterniond q1(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY())); // 绕Y轴90度

    double t = 0.5; // 插值参数
    Eigen::Quaterniond q_interp = slerp(q0, q1, t);

    std::cout << "Interpolated Quaternion: \n" << q_interp.coeffs().transpose() << std::endl;
    return 0;
}

---

3. 说明

1. Eigen::Quaterniond 的存储顺序是 (x, y, z, w)，但是构造函数可以用 (w, x, y, z)。
2. 当 t = 0 返回 q0，t = 1 返回 q1。
3. 适合平滑旋转动画、机器人运动规划和 SLAM 中的姿态插值。

---

七、四元数在 SLAM / 机器人学中的应用

1. 姿态表示

   避免欧拉角的万向节锁 (gimbal lock)。

   比旋转矩阵存储更紧凑（4 元 vs 9 元）。

2. 后端优化

   在 GTSAM、Ceres 等优化框架中，旋转变量常用四元数表示。

3. IMU 预积分

   利用四元数积分陀螺仪角速度，得到姿态更新。

4. 点云配准 (ICP)

   用四元数优化旋转参数，数值稳定性更好。

---

八、优缺点总结

优点

• 无奇异性（不像欧拉角有万向节锁）。
• 存储紧凑（4 个数 vs 9 个矩阵元素）。
• 插值方便（SLERP）。
• 数值稳定性好。

缺点

• 多一个归一化约束 (∣q∣=1|q|=1∣q∣=1)。
• 存在符号冗余 (qqq 与 −q-q−q 表示同一旋转)。

---

总结：

四元数是 3D 旋转的最优雅表示方式，核心思想是 用一个 4D 单位向量表示 3D 旋转 ，其本质是 旋转的双覆盖群 SU(2) 对应 SO(3)。

---

四元数与李群 SE(3)

一、李群 SE(3) 与四元数的关系

1. SO(3) 与四元数

• 三维旋转群：

  SO(3)={R∈R3×3  ∣  RTR=I,det⁡(R)=1} SO(3) = \{ R \in \mathbb{R}^{3\times 3} \;|\; R^TR = I, \det(R)=1 \} SO(3)={R∈R3×3∣RTR=I,det(R)=1}

• 四元数：

  单位四元数集合

  H1={q∈R4  ∣  ∥q∥=1} \mathbb{H}_1 = \{ q \in \mathbb{R}^4 \;|\; \|q\|=1 \} H1={q∈R4∣∥q∥=1}

• 关系：
  H1\mathbb{H}_1H1 与 SO(3)SO(3)SO(3) 是 双覆盖（double cover）

  q∼−q q \sim -q q∼−q

  表示同一个旋转。

---

2. SE(3) 与四元数

特殊欧式群：

SE(3)={T=[Rt01]  ∣  R∈SO(3),t∈R3} SE(3) = \left\{ T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \;|\; R \in SO(3), t \in \mathbb{R}^3 \right\} SE(3)={T=[R0t1]∣R∈SO(3),t∈R3}

所以一个位姿可以用：

• 旋转部分 R↔q∈H1R \leftrightarrow q \in \mathbb{H}_1R↔q∈H1
• 平移部分 t∈R3t \in \mathbb{R}^3t∈R3

即 位姿 = (四元数 q, 平移向量 t) 。

常见表示：

T=(q,t)∈SE(3) T = (q, t) \in SE(3) T=(q,t)∈SE(3)

---

3. 李代数 se(3)

SE(3) 的李代数为 se(3)se(3)se(3)：

se(3)={ξ∧=[ω∧ρ00],    ω,ρ∈R3} \mathfrak{se}(3) = \left\{ \xi^\wedge = \begin{bmatrix} \omega^\wedge & \rho \\ 0 & 0 \end{bmatrix}, \;\; \omega,\rho \in \mathbb{R}^3 \right\} se(3)={ξ∧=[ω∧0ρ0],ω,ρ∈R3}

其中：

• ω∈R3\omega \in \mathbb{R}^3ω∈R3：旋转向量
• ρ∈R3\rho \in \mathbb{R}^3ρ∈R3：平移向量

指数映射：

exp⁡:se(3)→SE(3) \exp: \mathfrak{se}(3) \to SE(3) exp:se(3)→SE(3)

对旋转部分用 四元数指数映射：

exp⁡(ω)=cos⁡θ2+ωθsin⁡θ2,θ=∥ω∥ \exp(\omega) = \cos\frac{\theta}{2} + \frac{\omega}{\theta}\sin\frac{\theta}{2}, \quad \theta=\|\omega\| exp(ω)=cos2θ+θωsin2θ,θ=∥ω∥

---

二、后端优化中的四元数约束

在 SLAM / BA (Bundle Adjustment) / PG优化中，我们优化位姿变量 T∈SE(3)T \in SE(3)T∈SE(3)。

旋转部分用四元数时有两个核心问题：

---

1. 单位约束

• 四元数必须满足 ∣q∣=1|q|=1∣q∣=1
• 这意味着优化空间不是 R4\mathbb{R}^4R4，而是单位球面 S3S^3S3

在优化时需要 约束优化，否则梯度下降可能跑出单位球。

---

2. 常见解决方案

(a) 参数化更新

在 Ceres / GTSAM 中，通常定义：

qnew=δq⊗qold q_{new} = \delta q \otimes q_{old} qnew=δq⊗qold

其中 δq\delta qδq 是一个"小旋转"，通常从李代数 so(3)\mathfrak{so}(3)so(3) 的三维向量指数映射得到：

δq=exp⁡(12[δθ]) \delta q = \exp\left(\frac{1}{2}[ \delta\theta ] \right) δq=exp(21[δθ])

这样优化变量是 δθ∈R3\delta\theta \in \mathbb{R}^3δθ∈R3，而不是直接更新四元数。

这样就避免了手动保持单位长度。

---

(b) 局部参数化 (Ceres)

Ceres 提供 LocalParameterization 接口：

class QuaternionParameterization : public ceres::LocalParameterization {
 public:
  bool Plus(const double* x, const double* delta, double* x_plus_delta) const override {
    Eigen::Quaterniond q(x[0], x[1], x[2], x[3]);
    Eigen::Vector3d delta_theta(delta[0], delta[1], delta[2]);
    Eigen::Quaterniond dq = Eigen::AngleAxisd(delta_theta.norm(), delta_theta.normalized());
    Eigen::Quaterniond q_new = dq * q;
    q_new.normalize();
    x_plus_delta[0] = q_new.w();
    x_plus_delta[1] = q_new.x();
    x_plus_delta[2] = q_new.y();
    x_plus_delta[3] = q_new.z();
    return true;
  }

  bool ComputeJacobian(const double* x, double* jacobian) const override {
    Eigen::Map<Eigen::Matrix<double,4,3,Eigen::RowMajor>> J(jacobian);
    J.setIdentity(); // 实际上是 4x3 的映射 Jacobian
    return true;
  }

  int GlobalSize() const override { return 4; }
  int LocalSize() const override { return 3; }
};

这样在 Ceres 中四元数的优化变量是 4 维，但实际更新量是 3 维。

---

© GTSAM 的 manifold 抽象

GTSAM 里所有旋转变量都封装成 manifold ，比如 Rot3。

优化时内部使用李代数扰动：

Rnew=exp⁡(δθ)Rold R_{new} = \exp(\delta\theta) R_{old} Rnew=exp(δθ)Rold

---

三、总结与联系

1. 数学层面

   • 四元数是 SO(3) 的参数化（双覆盖）
   • SE(3) = (q, t) 表示位姿
   • 李代数 se(3) 提供最小参数化（6 维）

2. 工程实现

   • 不能直接优化 4 维四元数，要用局部更新（3 维旋转向量）。
   • Ceres 用 LocalParameterization
   • GTSAM 用 manifold + Expmap/Logmap

3. 优势

   • 避免奇异性
   • 插值和数值优化稳定
   • 方便多机器人/SLAM 后端 Pose-Graph 优化

---