当前位置：首页 > news >正文

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

news 2025/9/10 8:11:16

四元数基础

一、什么是四元数 (Quaternion)

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

$q = w + x i + y j + z k$

其中：

$\in \mathbb{R}$ 为实部
$\in \mathbb{R}$ 为虚部
$i, j, k$ 为虚数单位，满足：

$i^2 = j^2 = k^2 = ijk = -1$

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

$\in \mathbb{R}^4$

二、四元数的运算

加法
逐分量相加。
乘法（非交换！）
若

$q_1 = (w_1, \mathbf{v}_1), \quad q_2 = (w_2, \mathbf{v}_2), \quad \mathbf{v}=(x,y,z)$

则

$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)$

共轭

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

模长

$\sqrt{w^2 + x^2 + y^2 + z^2}$

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

三、四元数与旋转的关系

在 3D 空间，一个旋转由 旋转轴 $u\mathbf{u}$ 和旋转角度 $θ\theta$ 唯一确定。
对应的单位四元数为：

$\cos\frac{\theta}{2} + (u_x i + u_y j + u_z k)\sin\frac{\theta}{2}$

写成向量形式：

$\big( \cos\frac{\theta}{2},\; u_x\sin\frac{\theta}{2},\; u_y\sin\frac{\theta}{2},\; u_z\sin\frac{\theta}{2} \big)$

四、用四元数旋转向量

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

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

旋转后向量：

$\cdot p \cdot q^*$

其中 $q$ 是单位四元数。
这避免了使用旋转矩阵时的数值漂移。

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

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

$\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}$

旋转矩阵 → 四元数
设 $\in SO(3)$ ，

$\frac{1}{2}\sqrt{1 + R_{00} + R_{11} + R_{22}}$

$\frac{R_{21} - R_{12}}{4w}, \quad y = \frac{R_{02} - R_{20}}{4w}, \quad z = \frac{R_{10} - R_{01}}{4w}$

六、四元数插值（SLERP）

1. 原理简介

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

$\text{slerp}(q_0, q_1, t) = \frac{\sin((1-t)\theta)}{\sin\theta} q_0 + \frac{\sin(t\theta)}{\sin\theta} q_1$

其中：

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

注意：

如果 $q0⋅q1<0q_0 \cdot q_1 < 0$ ，需要将 $q_1$ 取负号，保证旋转沿最短路径。
当 $θ≈0\theta \approx 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;}// 计算角度 thetadouble 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, zEigen::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. 说明

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

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

姿态表示
避免欧拉角的万向节锁 (gimbal lock)。
比旋转矩阵存储更紧凑（4 元 vs 9 元）。
后端优化
在 GTSAM、Ceres 等优化框架中，旋转变量常用四元数表示。
IMU 预积分
利用四元数积分陀螺仪角速度，得到姿态更新。
点云配准 (ICP)
用四元数优化旋转参数，数值稳定性更好。

八、优缺点总结

优点

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

缺点

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

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

四元数与李群 SE(3)

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

1. SO(3) 与四元数

三维旋转群：

$\{ R \in \mathbb{R}^{3\times 3} \;|\; R^TR = I, \det(R)=1 \}$
四元数：
单位四元数集合

$\mathbb{H}_1 = \{ q \in \mathbb{R}^4 \;|\; \|q\|=1 \}$
关系：
$H1\mathbb{H}_1$ 与 $SO (3)$ 是 双覆盖（double cover）

$\sim -q$

表示同一个旋转。

2. SE(3) 与四元数

特殊欧式群：

$\left\{ T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \;|\; R \in SO(3), t \in \mathbb{R}^3 \right\}$

所以一个位姿可以用：

旋转部分 $\leftrightarrow q \in \mathbb{H}_1$
平移部分 $\in \mathbb{R}^3$

即 位姿 = (四元数 q, 平移向量 t)。
常见表示：

$\in SE(3)$

3. 李代数 se(3)

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

$\mathfrak{se}(3) = \left\{ \xi^\wedge = \begin{bmatrix} \omega^\wedge & \rho \\ 0 & 0 \end{bmatrix}, \;\; \omega,\rho \in \mathbb{R}^3 \right\}$

其中：

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

指数映射：

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

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

$\exp(\omega) = \cos\frac{\theta}{2} + \frac{\omega}{\theta}\sin\frac{\theta}{2}, \quad \theta=\|\omega\|$

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

在 SLAM / BA (Bundle Adjustment) / PG优化中，我们优化位姿变量 $\in SE(3)$ 。
旋转部分用四元数时有两个核心问题：

1. 单位约束

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

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

2. 常见解决方案

(a) 参数化更新

在 Ceres / GTSAM 中，通常定义：

$q_{new} = \delta q \otimes q_{old}$

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

$\delta q = \exp\left(\frac{1}{2}[ \delta\theta ] \right)$

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

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

(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 的映射 Jacobianreturn true;}int GlobalSize() const override { return 4; }int LocalSize() const override { return 3; }
};

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

GTSAM 里所有旋转变量都封装成 manifold，比如 Rot3。
优化时内部使用李代数扰动：

$R_{new} = \exp(\delta\theta) R_{old}$

三、总结与联系

数学层面
- 四元数是 SO(3) 的参数化（双覆盖）
- SE(3) = (q, t) 表示位姿
- 李代数 se(3) 提供最小参数化（6 维）
工程实现
- 不能直接优化 4 维四元数，要用局部更新（3 维旋转向量）。
- Ceres 用 LocalParameterization
- GTSAM 用 manifold + Expmap/Logmap
优势
- 避免奇异性
- 插值和数值优化稳定
- 方便多机器人/SLAM 后端 Pose-Graph 优化