四元数、李群SO(3)/李代数so(3)的作用及应用场景

四元数、李群 SO (3)/ 李代数 so (3) 的作用：不只是做坐标系变换

很多初学者的误区："四元数 / 李群就是用来把点从 A 坐标系转到 B 坐标系"。

✅坐标变换只是它们其中一个小能力，不是全部目的。

结合之前的motion.cpp仿真、IMU、SLAM / 自动驾驶真实工程场景来讲，不用纯抽象数学。

先理清：

• 旋转矩阵、四元数：用来描述三维旋转（李群 SO (3)），描述姿态；

• so (3) 李代数：旋转矢量，对应微小旋转、角速度，是 SO (3) 流形原点的切空间。

位置（平移）本身就是普通三维向量，位置不需要四元数、不需要李群 ；只有旋转 / 姿态 才需要这套工具。

完整位姿 SE (3) = 旋转 SO (3) + 平移向量（普通向量）。

场景 1：简单坐标变换

应用：同一个三维点，在车体、相机、世界坐标系互相转换。

例：激光雷达探测到一个点，在雷达坐标系下坐标plp_lpl，要转到世界坐标系。

公式：

pw=Rwl,pl+twlp_w = R_{wl},p_l + t_{wl}pw=Rwl,pl+twl

这里RwlR_{wl}Rwl可以用旋转矩阵（SO3） ，也可以用四元数转成矩阵之后做乘法。

👉这件事，只用旋转矩阵就可以干完，四元数只是旋转的另一种存储形式。李代数 so (3) 在这里甚至都用不到。

也就是说：单纯的静态坐标变换，不需要李代数。
现实例子：标定，把相机、雷达点云转到车体坐标系，只需要存好标定的旋转（矩阵 / 四元数），直接相乘，不涉及李代数。

---

场景 2：随时间变化的姿态积分（motion.cpp 仿真、IMU 解算）

这是引入四元数、李代数非常核心的第一个理由：处理连续时间下旋转的更新。

硬件 IMU 输出：车体坐标系下的三维角速度 ω\boldsymbol\omegaω**（3 维向量，正好就是 so (3) 元素）** 。

IMU不会直接输出旋转矩阵 / 四元数，它输出的是角速度。我们需要根据角速度，迭代更新姿态。

两条路线实现姿态更新

路线 A：李群李群方式（SO3）

Rk+1=Rk⋅Exp(ω⋅Δt)R_{k+1}=R_k \cdot Exp(\boldsymbol\omega \cdot \Delta t)Rk+1=Rk⋅Exp(ω⋅Δt)

• ωΔt\boldsymbol\omega\Delta tωΔt：李代数 so (3)（旋转矢量）

• Exp()Exp()Exp()指数映射：so (3) → SO (3)，罗德里格斯公式，算出增量旋转矩阵。

对应 motion.cpp 中pose.so3() * SO3::exp(omega*dt)

路线 B：四元数方式

qk+1=qk⊗Expq(12ωΔt)q_{k+1}=q_k \otimes Exp_{q}(\frac12\boldsymbol\omega\Delta t)qk+1=qk⊗Expq(21ωΔt)

工程常用一阶近似：qk+1≈qk⊗1,12ωΔtq_{k+1}\approx q_k\otimes1,\\frac12\\boldsymbol\\omega\\Delta tqk+1≈qk⊗1,21ωΔt

对应 motion.cpp 的 use_quaternion 分支。
📌这里重点：

1. 角速度本身就是李代数 so (3) 元素；

2. 我们拿到角速度（李代数），通过指数映射得到旋转增量，更新李群（旋转矩阵 / 四元数）；

3. 这里干的事情不是坐标变换 ；而是随时间更新自身姿态。

反例思考：如果不用这套工具会怎么样？

如果用欧拉角（roll‑pitch‑yaw）表达姿态：

1. 欧拉角有万向锁；

2. 角速度和欧拉角导数之间是非常复杂非线性矩阵，很难做离散积分。

   👉IMU 解算几乎没人用欧拉角做积分，优先用四元数 / SO3 李群。

工程背景：消费级 IMU、车载 IMU，每毫秒输出角速度，解算器不停做姿态积分，输出载体姿态。

---

场景 3：SLAM / 状态估计中，对旋转做优化（这是李代数 so (3) 不可替代的主战场）

这个场景，单纯四元数、旋转矩阵不好直接做优化，必须引入李代数 so (3) 。

背景：BA 光束平差、图优化、EKF/ESKF 误差状态卡尔曼滤波。

为什么旋转矩阵 / 四元数不能直接拿给优化器？

• SO (3) 是弯曲流形。旋转矩阵有强约束：RTR=I,det⁡(R)=1R^TR=I,\det(R)=1RTR=I,det(R)=1；

• 四元数约束：必须是单位四元数。

普通优化算法（高斯‑牛顿、LM）工作在平直向量空间 ：变量直接加减 x←x+Δx\boldsymbol x \leftarrow \boldsymbol x+\Delta xx←x+Δx。

❌你不能直接写：

Rnew=Rold+ΔRR_{new}=R_{old}+\Delta RRnew=Rold+ΔR

两个旋转矩阵直接相加，结果不再是旋转矩阵，直接跑出 SO (3) 流形，约束被破坏。

四元数也不能直接简单做向量加法。

✅李代数 so (3) 是3 维平直切空间，可以自由加减 。

工程采用扰动模型（右扰动为主） ：

Rnew=Rold⋅Exp(Δϕ)R_{new}=R_{old}\cdot Exp(\Delta \phi)Rnew=Rold⋅Exp(Δϕ)

• Δϕ\Delta\phiΔϕ：so (3) 微小扰动（3 维向量！可以交给优化器求解增量）；

• 优化器求解出微小增量Δϕ\Delta\phiΔϕ（向量加减，普通 LM/GN 算法）；

• 指数映射把增量映射到李群上，右乘更新旋转。

举个 SLAM BA 实际例子：

我们要最小化重投影误差，误差是关于相机旋转RRR的函数。

我们不对RRR本身求导，而是对李代数扰动Δϕ\Delta\phiΔϕ求雅可比；求解得到扰动增量，用 R←R,Exp(Δϕ)R\leftarrow R,Exp(\Delta\phi)R←R,Exp(Δϕ) 更新姿态。
四元数在这里能干什么？

四元数可以存储旋转，但四元数是 4 维变量，真实自由度只有 3，存在等式约束，不适合直接作为优化变量。所以优化的增量依然用 3 维李代数，更新完再转成四元数保存。
现实工程：g2o、ceres‑solver 做 SLAM 优化，旋转部分，增量全部用 so (3) 的 3 维扰动，而不是直接修改旋转矩阵或者四元数四个元素。
拓展到 ESKF 误差状态卡尔曼滤波（高翔书第 3 章重点）：

误差状态δx\delta\boldsymbol xδx里面，姿态误差项就是so (3) 的三维微小旋转，不是旋转矩阵、不是四元数。

👉这里李代数的作用完全不是坐标系转换，而是为了解决「旋转姿态的优化、求导、增量更新」这个难题。

---

场景 4：旋转插值、姿态平滑

应用：机器人运动规划、三维动画、仿真。

需求：从姿态R1R_1R1平滑过渡到姿态R2R_2R2，做插值。

• 如果拿欧拉角插值，会有万向锁，运动不光滑；

• 四元数可以做slerp球面线性插值；

• 李群上也可以利用对数映射Log(R)Log(R)Log(R)转到 so (3) 切空间插值，再 Exp 映射回 SO3。

例子：机器人机械臂，从 A 姿态平滑转到 B 姿态；自动驾驶仿真生成平滑车辆姿态序列。

这里做的是姿态插值，不是坐标变换。

场景 5：旋转的误差、扰动分析

比如两个旋转之间求 "旋转差"：

给定R1,R2R_1,R_2R1,R2，求两者之间微小旋转偏差。

ΔR=R1⊤R2\Delta R = R_1^\top R_2ΔR=R1⊤R2

然后用对数映射 Δϕ=Log(ΔR)\Delta\phi = Log(\Delta R)Δϕ=Log(ΔR)，得到 so (3) 三维向量，这个向量的模就是两个姿态之间旋转角度误差。

应用：标定评估，评估标定出来的姿态和真值之间差了多少角度；IMU 标定评估姿态漂移。

这里得到的Δϕ\Delta\phiΔϕ是三维旋转误差，方便计算角度误差指标。

---

汇总表格，区分三件事（结合工程背景）

应用场景	主要做什么	用到什么	是否属于坐标变换？
点云 / 相机静态坐标转换	把三维点从 A 系转到 B 系	旋转矩阵 / 四元数	✅是；so (3) 李代数几乎不用
IMU 姿态积分（motion.cpp）	利用角速度不断更新载体姿态	SO3+Exp；或者四元数微分更新	❌不是坐标变换；是姿态随时间迭代更新
SLAM‑BA / 图优化 / ESKF 滤波	对旋转姿态做优化、求雅可比、增量更新	so (3) 李代数，BCH 公式，扰动模型；存储可以用四元数 / 旋转矩阵	❌不是坐标变换；解决流形上优化问题
姿态插值平滑	两个姿态之间做平滑过渡	四元数 slerp / SO3 Log‑Exp 插值	❌不是坐标变换；姿态插值
姿态误差评估	计算两个姿态之间角度偏差	Log 对数映射得到 so (3) 旋转误差向量	❌不是坐标变换；误差度量

核心总结（通俗版，脱离晦涩数学）

1. 四元数 / 旋转矩阵（李群 SO (3)）：用来存储、表达一个三维旋转姿态。

   • 能干坐标变换，但这只是它的一个用途；

   • 还能干：IMU 姿态积分、姿态插值。

2. 李代数 so (3)：代表角速度、微小旋转增量。

   • IMU 输出角速度天然就是 so (3) 向量；

   • SLAM 优化的时候，旋转不能直接加减，用 so (3) 切空间做扰动增量，BCH 处理求导；

   • 通过 Exp/Log，完成李代数↔李群互相转换。

3. 平移位置本身就是普通三维向量，不需要四元数，不需要李群李代数。只有旋转姿态才需要这套工具。

一句话纠正你的最初疑问：

"四元数、李群李代数不仅仅用来坐标系转换 ；

坐标系转换只是其中一个小功能；更大的价值在于姿态时间积分、流形上的优化求导、微小扰动处理、姿态插值，这也是 SLAM、IMU 解算必须引入这套数学工具的真正原因。"

举个现实工程通俗类比

• 旋转矩阵 / 四元数（李群 SO3）= 已经摆好的机器人姿态（现实世界朝向）；

• 李代数 so3 = 转动指令：往哪个轴转多大角度 / 转动的角速度；

• Exp：接收转动指令，输出执行完转动之后新的姿态；

• Log：看两个姿态，反推中间转了多大的转动指令。

坐标变换：拿着 "已经摆好的姿态"，变换点的坐标。

IMU 积分：不停接收 "转动指令（角速度‑so3）"，不断执行 Exp 更新姿态。

SLAM 优化：求解最优的微小 "转动指令增量（so3 扰动）"，修正机器人姿态。