李群与李代数：让机器人在正确的空间里计算位姿

前面的文章里，我们已经遇到过许多看似不同的公式：PoE 把关节运动连乘成末端位姿，雅可比把关节速度映射为末端 twist，逆运动学又把目标位姿变成一个六维误差。它们其实都在回答同一个问题：

机器人当前位姿为 $T$，目标位姿为 $T_d$。怎样描述二者的差，算出一小步修正，并保证更新后的结果仍是合法位姿？

若状态只是普通向量，这件事很简单：目标减当前，再把修正量加回去。但刚体位姿同时含有平移和旋转，旋转不能像位置一样直接相减，位姿也不能逐元素相加。于是，一个完整的位姿算法至少要分清四件事：

• 真实位姿保存在哪里；
• 速度、误差与优化步长在哪里计算；
• 怎样从局部增量得到有限运动；
• 怎样从两个有限位姿中取出一段局部差异。

李群、李代数、指数映射和对数映射，恰好分别承担这四个角色。本文会沿着“从当前位姿走向目标位姿”这条主线，把它们连成一套可以直接用于运动学、控制和状态估计的工具。

1. 位姿为什么不能直接做加减

先只看旋转。一个合法旋转矩阵必须满足：

$$R^{\mathsf T}R=I, \qquad \det R=1$$

因此，旋转矩阵并不是九个可以任意变化的数。若写成：

$$R_{next}=R+\Delta R$$

结果通常会离开旋转矩阵的集合：三个轴不再正交，长度也不再为一。即便事后把矩阵重新正交化，也很难说清这一步原本代表绕哪根轴旋转了多少。

所有三维旋转组成特殊正交群：

$$SO(3)= \left\{ R\in\mathbb R^{3\times3} \mid R^{\mathsf T}R=I,\ \det R=1 \right\}$$

空间刚体位姿则组成特殊欧氏群：

$$T= \begin{bmatrix} R&p\\ 0&1 \end{bmatrix} \in SE(3)$$

这里的“群”首先是一套运算规则。两个位姿通过矩阵乘法复合，单位矩阵表示不运动，每个位姿都有逆变换。对于：

$$T_1= \begin{bmatrix} R_1&p_1\\ 0&1 \end{bmatrix}, \qquad T_2= \begin{bmatrix} R_2&p_2\\ 0&1 \end{bmatrix}$$

复合结果是：

$$T_1T_2= \begin{bmatrix} R_1R_2&R_1p_2+p_1\\ 0&1 \end{bmatrix}$$

平移 $p_2$ 会先被 $R_1$ 改变方向，因此一般有 $T_1T_2\ne T_2T_1$。这正是三维运动的真实顺序性：先转再移与先移再转，本来就不是同一件事。

1.1 群负责保存真实状态

从工程角度看，$SO(3)$ 和 $SE(3)$ 最重要的价值是：它们明确规定了什么才是合法姿态和合法位姿。正运动学的输出、里程计的状态、相机外参和机械臂末端目标，都应该落在这些集合上。

但群上的元素不能随意相加，这会让求导、优化和误差计算变得不方便。好在这些集合虽然整体是弯曲的，在任意一个位姿附近却都可以用一个线性空间近似，就像地球整体是球面，脚下的一小片区域却可以用平面地图描述。

机器人算法的基本分工由此出现：有限位姿留在群上，局部计算转移到切空间里。

2. 李代数：把局部运动变成可以计算的向量

每个群元素附近都有一个切空间。借助群的乘法，我们可以把这些局部方向统一搬到单位元附近；单位元处的切空间，就是这个群的李代数。

对 $SO(3)$，李代数记作 $\mathfrak{so}(3)$，由所有 $3\times3$ 反对称矩阵组成。给定三维向量 $\boldsymbol\phi=(\phi_x,\phi_y,\phi_z)^{\scriptscriptstyle\mathsf T}$，帽算子把它写成：

$$[\boldsymbol\phi]^\wedge= \begin{bmatrix} 0&-\phi_z&\phi_y\\ \phi_z&0&-\phi_x\\ -\phi_y&\phi_x&0 \end{bmatrix}$$

并满足：

$$[\boldsymbol\phi]^\wedge\boldsymbol x =\boldsymbol\phi\times\boldsymbol x$$

反对称矩阵并不是额外发明的一套符号，它只是把“与某个向量做叉乘”改写成矩阵乘法。反过来，vee 算子再把反对称矩阵还原成三维向量：

$$\left([\boldsymbol\phi]^\wedge\right)^\vee =\boldsymbol\phi$$

2.1 从角速度走到六维 twist

若姿态随时间变化，角速度在世界坐标系表达时有：

$$\dot R=[\boldsymbol\omega_s]^\wedge R$$

若同一角速度改在刚体自身坐标系表达，则有：

$$\dot R=R[\boldsymbol\omega_b]^\wedge$$

对完整刚体运动，把角速度和线速度合成 twist：

$$\mathcal V= \begin{bmatrix} \boldsymbol\omega\\ \boldsymbol v \end{bmatrix} \in\mathbb R^6, \qquad [\mathcal V]^\wedge= \begin{bmatrix} [\boldsymbol\omega]^\wedge&\boldsymbol v\\ 0&0 \end{bmatrix} \in\mathfrak{se}(3)$$

本文始终采用“角速度在前、线速度在后”的顺序。很多库采用相反排列；公式可以等价，接口却不能混用。

同一段刚体速度也有空间形式和体形式：

$$[\mathcal V_s]^\wedge=\dot T T^{-1}, \qquad [\mathcal V_b]^\wedge=T^{-1}\dot T$$

$\mathcal V_s$ 站在世界坐标系观察运动，$\mathcal V_b$ 则站在随刚体运动的坐标系观察。它们不是两种不同运动，而是同一运动的两套坐标。

2.2 李括号记录了“运动顺序”

李代数不只是一个六维向量空间。两个矩阵方向 $A$ 与 $B$ 的李括号定义为：

$$[A,B]=AB-BA$$

在 $\mathfrak{so}(3)$ 中，它对应三维向量的叉乘：

$$\left[ [\boldsymbol a]^\wedge, [\boldsymbol b]^\wedge \right] = [\boldsymbol a\times\boldsymbol b]^\wedge$$

它记录的正是“先做 $A$ 再做 $B$”与“先做 $B$ 再做 $A$”之间的差。非常小的旋转在一阶近似下可以相加，但到二阶就会暴露顺序：

$$\operatorname{Exp}(A)\operatorname{Exp}(B) = \operatorname{Exp} \left( A+B+\frac12[A,B]+\cdots \right)$$

这条 Baker–Campbell–Hausdorff 关系把上一节群上的“不交换”，准确地投影到了李代数中。我们不需要在后文展开它，却应该知道：局部空间是线性的，但它没有遗忘刚体运动的几何结构。

2.3 与旋量、雅可比的关系

PoE 中的螺旋轴 $\mathcal S_i$ 是 $\mathfrak{se}(3)$ 中一个允许运动的方向。关节以速度 $\dot q_i$ 运动时，各列按雅可比组合成末端 twist：

$$\mathcal V=J(q)\dot q$$

于是，前几篇文章中的工具终于连了起来：PoE 把李代数方向积累成有限位姿；雅可比组合瞬时运动；逆运动学再把位姿误差变回一个可求解的局部向量。接下来缺少的，只是群与代数之间往返的两座桥。

3. 指数映射：让局部速度长成有限运动

李代数适合描述瞬时速度和局部增量，但机器人最终需要的是一个有限姿态或有限位姿。指数映射完成的正是这一步“积分”：

$$\operatorname{Exp}:\mathfrak{so}(3)\rightarrow SO(3), \qquad \operatorname{Exp}:\mathfrak{se}(3)\rightarrow SE(3)$$

3.1 从恒定角速度自然长出来

设刚体以在空间坐标系表达的恒定角速度 $\boldsymbol\omega$ 旋转，初始姿态为 $R(0)=I$。速度方程是：

$$\dot R(t)=[\boldsymbol\omega]^\wedge R(t)$$

这是一条线性常微分方程。与一维系统 $\dot x=ax$ 的解 $x(t)=e^{at}x(0)$ 完全对应，它的解为：

$$R(t)=e^{[\boldsymbol\omega]^\wedge t}$$

这里的 $e^A$ 是矩阵指数，定义为幂级数：

$$e^A=I+A+\frac{A^2}{2!}+\frac{A^3}{3!}+\cdots$$

因此，指数映射并不是凭空规定“向量怎样变成旋转”。它只是把恒定的瞬时角速度精确积分一段时间。

若运动持续时间为 $t$，令：

$$\boldsymbol\phi=\boldsymbol\omega t$$

则 $\boldsymbol\phi$ 的方向是旋转轴，模长是累计转角。于是：

$$R=\operatorname{Exp}([\boldsymbol\phi]^\wedge)$$

这里的 $\boldsymbol\phi$ 就是常说的旋转向量或旋转的指数坐标。它不是欧拉角：没有规定绕 $x,y,z$ 的先后顺序，而是直接说“绕这条轴旋转这么多”。

3.2 为什么最后会变成罗德里格斯公式

令：

$$\theta=\|\boldsymbol\phi\|, \qquad \boldsymbol u=\frac{\boldsymbol\phi}{\theta}$$

其中 $\boldsymbol u$ 是单位旋转轴。单位轴的叉乘矩阵有一个非常特别的循环关系：

$$([\boldsymbol u]^\wedge)^2 =\boldsymbol u\boldsymbol u^{\mathsf T}-I, \qquad ([\boldsymbol u]^\wedge)^3=-[\boldsymbol u]^\wedge$$

所以矩阵指数级数中，三次、四次以及更高次的项不会无休止地产生新结构，而只会在 $I$、$[\boldsymbol u]^\wedge$ 和 $([\boldsymbol u]^\wedge)^2$ 三者之间循环。把奇数次项与偶数次项分别合并，恰好得到正弦和余弦：

$$\operatorname{Exp}([\boldsymbol\phi]^\wedge) =I+ \frac{\sin\theta}{\theta}[\boldsymbol\phi]^\wedge+ \frac{1-\cos\theta}{\theta^2} \left([\boldsymbol\phi]^\wedge\right)^2$$

这就是罗德里格斯公式。它也把几何意义直接写在了结果里：沿 $\boldsymbol u$ 的分量保持不变，垂直于 $\boldsymbol u$ 的平面内发生角度为 $\theta$ 的二维旋转。因此，指数映射的输出天然满足 $R^{\mathsf T}R=I$ 与 $\det R=1$。

从这里还可以看出一个重要边界：旋转向量不是全局唯一坐标。绕 $\boldsymbol u$ 旋转 $\theta$，与绕同一轴旋转 $\theta+2k\pi$ 表示同一个旋转；在转角接近 $\pi$ 时，旋转轴的符号选择也会敏感。指数映射始终能给出合法旋转，但它的逆映射需要在这些等价表示中选取一个分支。

3.3 小角度下，公式仍然是连续的

当 $\boldsymbol\phi$ 很小时，罗德里格斯公式约化为：

$$\operatorname{Exp}([\boldsymbol\phi]^\wedge) \approx I+[\boldsymbol\phi]^\wedge$$

这正是“切平面近似”的数学版本。但实践中更新姿态时，应使用完整的 $\operatorname{Exp}$，而不是把近似式长期累积；否则正交性会一点点流失。

实现完整公式时反而要格外注意小角度，因为系数中出现了 $\sin\theta/\theta$ 与 $(1-\cos\theta)/\theta^2$。在 $\theta$ 接近零时，它们的数学极限完全良好：

$$\frac{\sin\theta}{\theta} =1-\frac{\theta^2}{6}+O(\theta^4)$$ $$\frac{1-\cos\theta}{\theta^2} =\frac12-\frac{\theta^2}{24}+O(\theta^4)$$

可靠的库实现会在小角度区域使用这些 Taylor 展开，避免直接相除造成数值精度损失。这里的近似只用于稳定地计算指数映射本身；最终输出仍然是合法旋转，而不是把 $I+[\boldsymbol\phi]^\wedge$ 当作长期姿态更新。

3.4 从三维旋转走到六维位姿

设：

$$\boldsymbol\xi= \begin{bmatrix} \boldsymbol\phi\\ \boldsymbol\rho \end{bmatrix}$$

其中 $\boldsymbol\phi$ 是旋转指数坐标，$\boldsymbol\rho$ 是与之配对的平移代数坐标。则：

$$\operatorname{Exp}([\boldsymbol\xi]^\wedge)= \begin{bmatrix} R&J(\boldsymbol\phi)\boldsymbol\rho\\ 0&1 \end{bmatrix}$$

其中：

$$R=\operatorname{Exp}([\boldsymbol\phi]^\wedge)$$

而：

$$J(\boldsymbol\phi)= I+ \frac{1-\cos\theta}{\theta^2}[\boldsymbol\phi]^\wedge+ \frac{\theta-\sin\theta}{\theta^3} \left([\boldsymbol\phi]^\wedge\right)^2$$

称为 $SO(3)$ 的左雅可比。它解释了一件常被忽略的事：在 $SE(3)$ 中，平移部分并不总是简单等于 $\boldsymbol\rho$。旋转与平移会通过 $J(\boldsymbol\phi)$ 耦合，这正是螺旋运动的代数痕迹。

这个 $J(\boldsymbol\phi)$ 也不是一块神秘的修正项。让 $s\in[0,1]$ 表示沿同一段 twist 前进的比例，位移是旋转过程中线速度不断改变方向后的积分：

$$p= \int_0^1 \operatorname{Exp}([\boldsymbol\phi]^\wedge s) \boldsymbol\rho\,ds =J(\boldsymbol\phi)\boldsymbol\rho$$

所以，$SE(3)$ 的指数映射并不是“先转完再平移”，也不是“把位置与姿态分开各算一次”。它把两者作为同一段刚体运动同时积累；这正是它与前文螺旋运动、PoE 公式自然相接的地方。

不过不必因为这条矩阵感到负担。它有两个非常直观的退化情形：

• 纯平移：$\boldsymbol\phi=0$ 时，$J=I$，因此 $p=\boldsymbol\rho$；
• 很小的旋转：$J\approx I$，平移与代数坐标几乎相同。

真正需要它的时刻，是我们希望一次用一个六维增量同时表达有限旋转和平移，而不是把两者拆开处理时。

4. 对数映射：从有限位姿中取出局部差异

指数映射从局部增量生成有限运动；对数映射则反过来，把一个群元素带回李代数：

$$\operatorname{Log}:SO(3)\rightarrow\mathfrak{so}(3), \qquad \operatorname{Log}:SE(3)\rightarrow\mathfrak{se}(3)$$

“反过来”需要加一点限定。指数映射不是全局一一对应：绕同一轴旋转 $\theta$ 与 $\theta+2k\pi$ 会得到相同姿态。对数映射通常选择主值，也就是转角位于 $[0,\pi]$ 的代表。

4.1 从旋转矩阵恢复旋转向量

对于 $R\in SO(3)$，希望找到 $\boldsymbol\phi$ 使得：

$$R=\operatorname{Exp}([\boldsymbol\phi]^\wedge)$$

令 $\theta=\lVert\boldsymbol\phi\rVert$。由罗德里格斯公式的迹可得：

$$\theta= \arccos \left( \frac{\operatorname{tr}(R)-1}{2} \right)$$

再从反对称部分恢复旋转轴与转角：

$$\boldsymbol\phi= \frac{\theta}{2\sin\theta} \left(R-R^{\mathsf T}\right)^\vee$$

矩阵的迹给出 $\cos\theta$，反对称部分给出 $\sin\theta\,\boldsymbol u$；二者合起来，便回答了“绕哪根轴、转了多少”。

4.2 从刚体位姿恢复六维指数坐标

设一个相对变换为：

$$\Delta T= \begin{bmatrix} \Delta R&\Delta p\\ 0&1 \end{bmatrix}$$

先求旋转坐标：

$$\boldsymbol\phi= \operatorname{Log}(\Delta R)^\vee$$

再回忆指数映射中的 $\Delta p=J(\boldsymbol\phi)\boldsymbol\rho$。由于平移是在旋转过程中不断积累的，必须用逆左雅可比将它解回代数坐标：

$$\boldsymbol\rho= J(\boldsymbol\phi)^{-1}\Delta p$$

最终得到：

$$\operatorname{Log}(\Delta T)^\vee= \begin{bmatrix} \boldsymbol\phi\\ \boldsymbol\rho \end{bmatrix}$$

当 $\theta\ne0$ 时：

$$J(\boldsymbol\phi)^{-1}= I-\frac12[\boldsymbol\phi]^\wedge+ \left( \frac{1}{\theta^2} -\frac{1+\cos\theta}{2\theta\sin\theta} \right) \left([\boldsymbol\phi]^\wedge\right)^2$$

这个 $J^{-1}$ 不是额外添加的“修正”。它只是在追溯：若最终位移为 $\Delta p$，而刚体在移动的同时还在旋转，那么生成这段螺旋运动的线速度坐标 $\boldsymbol\rho$ 应该是多少。

4.3 当前位姿与目标位姿怎样相减

现在回到开头的问题。已知当前位姿 $T$ 和目标位姿 $T_d$，正确的第一步不是 $T_d-T$，而是构造相对变换。

站在当前工具坐标系中看这段差异，得到体误差：

$$T_{err}^b=T^{-1}T_d, \qquad \boldsymbol\xi_{err}^b= \operatorname{Log}(T_{err}^b)^\vee$$

它回答的是：“从当前工具坐标系看，还要怎样运动才能到达目标？”

站在世界坐标系中看同一段差异，则得到空间误差：

$$T_{err}^s=T_dT^{-1}, \qquad \boldsymbol\xi_{err}^s= \operatorname{Log}(T_{err}^s)^\vee$$

两个误差描述同一段有限运动，只是坐标不同。它们通过当前位姿的伴随变换联系：

$$\boldsymbol\xi_{err}^s= \operatorname{Ad}_T \boldsymbol\xi_{err}^b$$

4.4 两个必须认真处理的数值边界

当 $\theta\to0$ 时，$\theta/(2\sin\theta)$ 的极限为 $1/2$，但直接计算会损失精度。此时可以使用：

$$\boldsymbol\phi\approx \frac12\left(R-R^{\mathsf T}\right)^\vee$$

并对 $J^{-1}$ 使用 Taylor 展开。实现 $\arccos$ 前，还应把 $(\operatorname{tr}(R)-1)/2$ 限制到 $[-1,1]$，避免浮点误差产生非法输入。

当 $\theta\approx\pi$ 时，$\sin\theta$ 很小，而绕 $\boldsymbol u$ 转 $\pi$ 与绕 $-\boldsymbol u$ 转 $\pi$ 表示同一姿态。稳健实现通常从 $R+I$ 中选取数值最可靠的轴分量，并在连续轨迹中维持与上一时刻一致的符号。

这些边界提醒我们：对数映射提供的是局部坐标，不是覆盖整个 $SO(3)$ 的唯一全局坐标。算法应使用经过测试的实现，并避免让一次优化步长跨越过大的旋转。

5. 同一段运动的两种坐标：空间与体

空间误差和体误差的区别，不只是公式中乘法顺序不同。它决定了增量从哪一侧作用到当前位姿。

若增量在世界坐标系表达，应从左侧更新：

$$T_{new}= \operatorname{Exp}([\delta\boldsymbol\xi_s]^\wedge)T$$

若增量在工具坐标系表达，应从右侧更新：

$$T_{new}= T\operatorname{Exp}([\delta\boldsymbol\xi_b]^\wedge)$$

两种表达通过伴随矩阵转换：

$$\delta\boldsymbol\xi_s= \operatorname{Ad}_T \delta\boldsymbol\xi_b$$

若 $T=[R,p]$，并采用“角速度在前”的排列：

$$\operatorname{Ad}_T= \begin{bmatrix} R&0\\ [p]^\wedge R&R \end{bmatrix}$$

下方的非零块说明：改变参考原点不仅会旋转速度分量，角速度还会通过力臂诱导出线速度。伴随变换因此不是简单地把两个三维向量各自乘一个 $R$。

这套记号没有唯一的“最佳选择”。真正重要的是从误差定义到雅可比、增量和更新方式始终一致：

空间误差配空间雅可比并从左侧更新；体误差配体雅可比并从右侧更新。

6. 把整条链闭合：误差、求解与更新

现在，开头的问题终于可以从头到尾写完。群保存当前位姿和目标位姿；对数映射生成局部误差；雅可比或优化器计算修正量；指数映射再把修正放回群上。

6.1 已知速度时，怎样积分位姿

若空间 twist $\mathcal V_s$ 在短时间 $dt$ 内近似恒定：

$$T(t+dt)= \operatorname{Exp}([\mathcal V_s]^\wedge dt)T(t)$$

若使用体 twist $\mathcal V_b$：

$$T(t+dt)= T(t)\operatorname{Exp}([\mathcal V_b]^\wedge dt)$$

无论积分多少次，结果都会留在 $SE(3)$ 上。IMU 姿态传播、轮式里程计、视觉里程计和末端速度控制都在反复使用这套结构。

这里的“短时间内恒定”仍然是一阶离散假设。指数映射保证单步更新满足群结构，却不自动提高时间积分阶数；若速度在一步内变化很快，仍需减小步长或采用更高阶的群积分方法。

6.2 数值逆运动学的一次完整迭代

设当前关节角为 $q_k$。先由正运动学得到：

$$T_k=f(q_k)$$

若选择体误差与体雅可比，则计算：

$$\boldsymbol\xi_{err}^b= \operatorname{Log}(T_k^{-1}T_d)^\vee$$

再在线性化模型中求关节修正：

$$\Delta q= \alpha J_b(q_k)^\dagger \boldsymbol\xi_{err}^b$$

最后更新关节角并重新计算正运动学：

$$q_{k+1}=q_k+\Delta q, \qquad T_{k+1}=f(q_{k+1})$$

这里 $\alpha$ 是步长，奇异附近通常还要把伪逆换成阻尼最小二乘。李群没有代替 IK 求解器；它做的是把位姿误差写在正确的空间里，让后续线性化真正对应一段刚体运动。

6.3 位姿优化与状态估计也是同一结构

在视觉 SLAM、手眼标定、外参优化和多传感器融合中，常见测量关系为：

$$T_{ij}\approx T_i^{-1}T_j$$

可以构造六维残差：

$$r_{ij}= \operatorname{Log} \left( T_{ij}^{-1}T_i^{-1}T_j \right)^\vee$$

高斯牛顿或 Levenberg–Marquardt 在线性空间中求出 $\delta\boldsymbol\xi$ 后，再通过群乘法更新状态。例如采用右扰动时：

$$T\leftarrow T\operatorname{Exp}([\delta\boldsymbol\xi_b]^\wedge)$$

IK、位姿图优化和误差状态滤波的业务目标不同，数学骨架却非常相似：在群上保存状态，在代数中构造残差与增量。

7. 插值：沿相对刚体运动走过去

给定起点 $T_0$ 和终点 $T_1$，先计算从起点看到的相对位姿：

$$\Delta T=T_0^{-1}T_1$$

再将它带回李代数：

$$\boldsymbol\xi= \operatorname{Log}(\Delta T)^\vee$$

当 $\alpha\in[0,1]$ 时，定义：

$$T(\alpha)= T_0\operatorname{Exp}([\alpha\boldsymbol\xi]^\wedge)$$

$\alpha=0$ 时位于 $T_0$，$\alpha=1$ 时准确到达 $T_1$；中间过程沿同一根相对螺旋轴推进。它既不是对矩阵元素做线性混合，也不是先把平移与旋转拆成两段互不相关的运动。

这条曲线只是两个离散位姿之间一种结构自然的连接方式。真实机器人轨迹还要处理速度和加速度连续性、避障、关节限位与动力学约束；李群插值给出几何路径，时间参数化和可执行性仍需另外设计。

8. 工程实现中最容易出错的地方

8.1 先固定符号、顺序与坐标系

同一个六维量，有的库使用 $[\boldsymbol\omega;\boldsymbol v]$，有的使用 $[\boldsymbol v;\boldsymbol\omega]$；有的接口返回左雅可比，有的返回右雅可比。开始编码前，应把 twist 顺序、世界系与工具系、左扰动与右扰动写进接口约定和单元测试。

8.2 不要手写一个只在普通姿态下工作的 Log

小角度需要级数展开，接近 $\pi$ 时需要稳定恢复旋转轴，输入旋转矩阵还可能因浮点误差略微偏离 $SO(3)$。实际项目更适合使用自身已经验证过的 Sophus、manif、Pinocchio 或同类几何库实现，并为零角、微小角和接近半周的姿态准备测试。

8.3 一米与一弧度不会自动等权

李代数把旋转和平移放进同一个六维向量，只代表它们属于同一段刚体运动，并不表示数值尺度天然一致。IK 和优化中应根据任务使用特征长度、协方差或显式权重矩阵，否则求解器可能为了减少一个数值更大的分量而牺牲真正重要的目标。

8.4 群上的正确更新不等于完整算法

指数映射能保证位姿更新合法，却不会自动解决碰撞、关节限位、奇异性、可达性、噪声模型或动力学约束。几何结构正确，是可靠算法的地基；雅可比、优化器、控制律和规划器仍要在其上各司其职。

9. 回到这个系列：这些工具原本就在一起

现在再看这个系列中的文章，关系会清楚很多：

• 旋量与 PoE 给出 $\mathfrak{se}(3)$ 中的关节方向，并通过指数映射生成有限运动；
• 雅可比矩阵 把关节速度映射成空间或体 twist，转置又连接到 wrench 与关节力矩；
• 逆运动学 用对数映射构造位姿误差，再在局部线性模型中求关节修正；
• 对偶四元数 用另一种紧凑代数表示同一个刚体运动群，尤其适合复合与螺旋插值；
• 拉格朗日法 处理能量与动力学，而任务空间速度、约束和控制仍会通过雅可比与这套几何语言相接。

它们共同构成了一条完整道路：

$$\text{群上的有限位姿} \xleftrightarrow{\ \operatorname{Log}/\operatorname{Exp}\ } \text{代数中的速度与误差} \xleftrightarrow{\ J\ } \text{关节空间中的运动与控制}$$

李群与李代数的价值，不是把简单问题换成更复杂的符号，而是让每种量待在它真正属于的空间里。位姿负责复合，代数负责线性化，指数与对数负责往返，伴随负责更换观察坐标系。当前位姿如何走向目标位姿，也就不再是一堆彼此孤立的技巧，而是一条从几何到计算都保持一致的路线。

参考资料

1. Kevin M. Lynch, Frank C. Park, Modern Robotics: Mechanics, Planning, and Control, Chapters 3–5, Cambridge University Press, 2017. 官方课程资源
2. Richard M. Murray, Zexiang Li, S. Shankar Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994.
3. Timothy D. Barfoot, State Estimation for Robotics, Cambridge University Press, 2017.
4. Joan Solà, Jérémie Deray, Dinesh Atchuthan, “A micro Lie theory for state estimation in robotics,” 2018. arXiv:1812.01537