李群与李代数：让旋转和位姿拥有正确的加减法

1. 为什么机器人课程突然需要“群”

若两个位置是 $p_1,p_2\in\mathbb R^3$，可以计算误差 $p_2-p_1$，也可以用 $p_1+\delta p$ 更新位置。但两个旋转矩阵直接平均通常不是旋转矩阵。例如单位阵 $I$ 与绕正 $z$ 轴转 $\pi$ 的 $\operatorname{diag}(-1,-1,1)$，平均后是 $\operatorname{diag}(0,0,1)$，会把二维向量压扁，显然不是刚体旋转。

解决方法不是给优化器再加一条随意的修正，而是认识到：旋转的“合并动作”本来就是乘法。它有单位旋转 $I$，有逆旋转 $R^T$，复合仍是旋转，而且乘法满足结合律。这些性质组成“群”的定义；它不要求交换律。

“李”强调这个群还能连续变化，并可在当前位置讨论微小增量和导数。对工程应用，最重要的是：用受约束的旋转/位姿表示大状态，用无约束的小向量表达局部增量，再用正确映射把两者连接。

2. SO(3) 的约束与局部自由度

三维旋转群记为

$O$ 对应正交，$S$ 对应行列式正一，括号中的三表示作用的空间维数。矩阵虽然存九个元素，但正交单位轴只具有三个独立旋转自由度。它不是整个九维平面，而是嵌在矩阵空间中的三维曲面。

考虑单位旋转附近的一条曲线 $R(\epsilon)=I+\epsilon A+O(\epsilon^2)$。代入正交约束：

因此一阶项必须满足 $A^T=-A$。所有这样的反对称矩阵构成 $\mathfrak{so}(3)$，也就是单位元处的切空间：

$\phi$ 是三个独立数，$[\phi]_\times$ 是对应的 $3\times3$ 矩阵。“帽算子”把向量变成矩阵，“vee 算子”把反对称矩阵还原成向量。在本教程中用 $[\cdot]_\times$ 比通用帽号更明确，因为帽号还常表示估计值。

切空间是局部方向空间，不是说一整段有限旋转都能不加区分地线性相加。两次小转角相加只是一阶近似；高阶会出现由不交换产生的叉乘项。

3. 指数映射从哪里来：解一个旋转微分方程

假设机体系角速度在一段时间内恒定，上一节有 $\dot R=R[\omega^b]_\times$。先从 $R(0)=I$ 开始，对常系数线性微分方程的解是矩阵指数：

令旋转向量 $\phi=\omega^b t$，它的方向是轴，模长 $\theta=\|\phi\|$ 是旋转角。为了区分普通矩阵指数与输入为向量的工程接口，定义

这不是随意的符号转换：Exp 接收三维转角，输出合法旋转矩阵；任意一个直接加法得到的九维矩阵则没有这个保证。

把无穷级数化为三个矩阵

写 $\phi=\theta u$，$\|u\|=1$，令 $U=[u]_\times$。向量三重积给出

因此所有高次幂都能化成 $I,U,U^2$。把指数级数的奇数次项合起来得到 $\sin\theta\,U$，偶数次非零项合起来得到 $(1-\cos\theta)U^2$：

这就是 Rodrigues 公式。它的几何版本是“沿轴分量不变，垂轴分量在一个二维平面内转动”。公式对 $\theta\rightarrow0$ 有连续极限：两个系数分别趋近一与二分之一。小角程序用

比先算两个几乎相等的数再相减更稳定。

如果起点不是单位阵，而是 $R_k$，机体系增量应更新成 $R_{k+1}=R_k\operatorname{Exp}(\phi)$。这保留了前一时刻的姿态，又把局部增量放在正确方向。

4. 对数映射：怎样从旋转矩阵取回误差角

从 Rodrigues 公式取迹，因为 $\operatorname{tr}U=0$、$\operatorname{tr}U^2=-2$，得到

再用矩阵减去其转置，只有反对称部分保留：$R-R^T=2\sin\theta\,U$。因此在 $0<\theta<\pi$ 且不靠近端点时，

$\operatorname{Log}$ 输出三个数；它并不是对矩阵元素逐个取自然对数。

两种边界必须单独处理：

1. 接近零：用 $\phi\simeq\tfrac12(R-R^T)^\vee$ 及更高阶修正。
2. 接近 $\pi$：$\sin\theta$ 很小，不能继续除。可从 $R+I\simeq2uu^T$ 的最大对角元素恢复轴，或转换到经过符号选择的单位四元数再用 atan2；需核验数值分支。

任何主值 Log 都有分支边界。绕同一轴转 $\theta+2\pi$ 与转 $\theta$ 得到相同旋转；在 $\pi$ 处轴的正负还具有特殊等价性。故“Exp/Log 在全局完全一一对应”是错误的。对 ESKF 与迭代优化，局部小误差假设恰好让它们大多在良好分支内工作。

5. 左增量与右增量：同一次小旋转写在哪个系里

本教程采用右局部增量

符号 $\boxplus$ 读成“按合法局部方式加上”，$\boxminus$ 读成“取局部误差”。右误差向量在 $R_1$ 对应的机体系表达。

同一物理增量也可左乘写为 $\operatorname{Exp}(\delta\theta^n)R$。利用叉乘坐标变换 $R[a]_\times R^T=[Ra]_\times$，可以逐项变换指数级数，从而得到

所以左/右不是谁更高级，而是扰动坐标选择。若右局部误差的协方差是 $P_b$，相同线性化点上的左误差协方差为 $P_n=RP_bR^T$。变换雅可比时也必须同步改变坐标；不能把右误差的 $H$ 与左误差的 $P$ 混用。

6. SE(3)：把平移和旋转放进同一个刚体变换

空间点的坐标转换包含旋转与原点位移：

$p$ 是机体系原点在导航系中的位置，单位 m；$R$ 无量纲。最后一个齐次坐标一让平移也可以写成矩阵乘法。方向向量不应该加原点位移，若用齐次形式，其最后分量应为零。

乘法与逆直接由代入坐标式得到：

逆平移不是单纯的 $-p$，因为坐标系也反过来了。机械臂基座到末端、机器人机体到相机以及声学基站到机器人，都可以统一使用这个结构。

7. 六维局部位姿为什么不是“位置加欧拉角”

本节采用平移在前的六维向量 $\xi=[\rho^T,\phi^T]^T$，其中 $\rho$ 的单位 m、$\phi$ 的单位 rad。定义

对矩阵指数逐幂展开，其右上角是

因此

为什么会有 $V$？在一段同时旋转和前进的运动中，局部前进方向也持续转动，不能把局部位移直接当成最终导航系位移。$V=\int_0^1\operatorname{Exp}(s\phi)\,ds$ 正是把不同瞬间的方向累加起来。这也解释差速车恒定转速走圆弧的几何。

数例：$\phi=[0,0,\pi/2]^T$，$\rho=[1,0,0]^T\ \mathrm m$，有

不是 $[1,0,0]^T$。这可理解为单位时长内前向速度一、角速度 $\pi/2$ 的圆弧终点。这里 $\rho$ 是归一化时间的切向平移参数，不是直接读取的终点位置。

对数反求时先得到 $\phi=\operatorname{Log}(R)$，再解线性方程 $V(\phi)\rho=p$。实现时求解比随意显式求逆稳健；主旋转分支的边界仍需处理。若旋转为零，$V=I$，自然退化成纯平移。

**顺序警告：**Modern Robotics 常把 twist 写成 $[\omega^T,v^T]^T$，即角在前。本节写 $[v^T,\omega^T]^T$；两种都正确，但所有 $6\times6$ 矩阵必须按同一置换交换行列，不能直接复制数值块。

8. 刚体速度与伴随：为什么一个三维旋转不够变换六维量

沿时间轨迹的位姿为 $T(t)$。定义机体系 twist 矩阵 $\widehat{\mathcal V}_b=T^{-1}\dot T$，空间 twist 矩阵 $\widehat{\mathcal V}_n=\dot T T^{-1}$。展开有

下半部分是角速度；上半部分是 twist 的平移分量。特别注意，空间 twist 的平移分量一般不等于机体原点的速度 $\dot p$，它还与空间坐标原点的选择有关。混淆二者会导致机械臂空间雅可比的线速度项写错。

由 $\widehat{\mathcal V}_n=T\widehat{\mathcal V}_bT^{-1}$ 做矩阵乘法，得到平移在前的伴随：

右上角来自 $p\times\omega^n$。对速度而言，它的量纲是 m 乘 rad/s，可与 m/s 相加；对于位姿小增量，也表达旋转中心改变所造成的平移增量。它不是把姿态矩阵“扩大成六维”那么简单。

同理，右位姿扰动 $T\operatorname{Exp}(\delta\xi_b)$ 与左扰动 $\operatorname{Exp}(\delta\xi_n)T$ 表示同一局部变化时，$\delta\xi_n=\operatorname{Ad}_T\delta\xi_b$。协方差转换为 $P_n=\operatorname{Ad}_TP_b\operatorname{Ad}_T^T$；其中平移块、角度块和交叉块单位不同，应保留单位含义，不能把所有对角线当同一种“精度”。

9. “李代数”里的代数体现在哪里

对两个小旋转矩阵 $A=[a]_\times$、$B=[b]_\times$，交换子的定义是 $[A,B]=AB-BA$。作用到任意向量并使用向量三重积可证明

这说明不同顺序的小旋转差异仍对应一个旋转方向。有限增量复合的局部展开是

因此小角一阶算法经常能把角度相加，但角度变大或高动态累计很多次时，忽略叉乘项就会积累偏差。理解这一点比背诵“旋转不交换”更接近实际 IMU 积分问题。

10. 手写与验收

建议先实现 SO(3) Exp，再实现 SE(3) Exp；不要第一步就写完整李群库。

1. 输入零向量，输出单位旋转。
2. 输入绕 $z$ 轴九十度，变换 $x$ 轴到 $y$ 轴。
3. 对多个轴角检查 $R^TR\approx I$、$\det R\approx1$。
4. 实现圆弧数例，检查终点为 $[2/\pi,2/\pi,0]^T$。
5. 以两个非平行小旋转检查直接向量和只是近似，不能当精确复合。

#include <array>
#include <cmath>
#include <cassert>
using V=std::array<double,3>;
using M=std::array<std::array<double,3>,3>;
M identity(){M m{};for(int i=0;i<3;i++)m[i][i]=1;return m;}
M skew(V x){return {{{0,-x[2],x[1]},
 {x[2],0,-x[0]},{-x[1],x[0],0}}};}
M mm(M a,M b){
 M c{};
 for(int i=0;i<3;i++){for(int j=0;j<3;j++){
  for(int k=0;k<3;k++){c[i][j]+=a[i][k]*b[k][j];}
 }}
 return c;
}
V mv(M a,V b){
 V c{};
 for(int i=0;i<3;i++){for(int j=0;j<3;j++){c[i]+=a[i][j]*b[j];}}
 return c;
}
M polynomial(M a,double c1,double c2){
 M r=identity(),a2=mm(a,a);
 for(int i=0;i<3;i++){for(int j=0;j<3;j++){
  r[i][j]+=c1*a[i][j]+c2*a2[i][j];
 }}
 return r;
}
double norm(V x){return std::sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);}
M expSO3(V x){double t=norm(x),t2=t*t;
 double a=t<1e-4?1-t2/6+t2*t2/120:std::sin(t)/t;
 double b=t<1e-4?0.5-t2/24+t2*t2/720:(1-std::cos(t))/t2;
 return polynomial(skew(x),a,b);}
M leftJ(V x){double t=norm(x),t2=t*t;
 double b=t<1e-4?0.5-t2/24+t2*t2/720:(1-std::cos(t))/t2;
 double c=t<1e-4?1.0/6-t2/120+t2*t2/5040:(t-std::sin(t))/(t2*t);
 return polynomial(skew(x),b,c);}
struct Pose{M R;V p;};
Pose expSE3(V rho,V phi){return {expSO3(phi),mv(leftJ(phi),rho)};}
int main(){double pi=std::acos(-1.0);
 auto t=expSE3({1,0,0},{0,0,pi/2});
 assert(std::abs(t.p[0]-2/pi)<1e-12);
 assert(std::abs(t.p[1]-2/pi)<1e-12);
 for(V phi: {V{0,0,0},V{1e-12,0,0},V{.3,-.2,.7},V{pi,0,0}}){
  M r=expSO3(phi);
  for(int i=0;i<3;i++)for(int j=0;j<3;j++){
   double s=0;for(int k=0;k<3;k++)s+=r[k][i]*r[k][j];
   assert(std::abs(s-(i==j?1:0))<1e-12);
  }
 }
}

原始资料与阅读顺序

• Modern Robotics 第三章：先看旋转、齐次变换，再看 twist；注意其角在前顺序。
• Modern Robotics：Twists Part 2：核对刚体速度与伴随变换。
• Solà 等：A micro Lie theory for state estimation in robotics：查阅 Exp/Log、局部加减、伴随及附录中的 SO(3)/SE(3) 工具。

本节从坐标、级数和微分方程独立组织推导；目标是能够自己解释每个矩阵块，而不是复制不同资料的公式后混用约定。