刚体运动学研究的是：不考虑物体受力和形变时，如何统一描述它的位置、姿态和瞬时速度。在现代机器人学中，这三个问题分别由 $SE(3)$、李代数 $se(3)$ 和指数映射联系起来：

$$T\in SE(3),\qquad [V]\in se(3),\qquad T(\theta)=e^{[S]\theta}T(0)$$

本文从齐次变换矩阵出发，逐步解释运动旋量（Twist）、螺旋轴、矩阵指数，以及空间坐标和物体坐标下的速度表示。

• 从 SO(3) 到 SE(3)
  • 齐次变换矩阵
  • 齐次坐标与点的变换
  • 变换的复合与求逆
• 运动旋量 Twist
  • 从位姿变化率得到 Twist
  • 反对称矩阵与括号算子
  • Twist 的几何意义
• 螺旋轴 Screw Axis
  • 纯转动与纯平移
  • 一般螺旋运动
• 指数映射：从速度得到位姿
  • 旋转的指数映射
  • 刚体运动的指数映射
• 空间 Twist 与物体 Twist
  • 伴随表示与坐标系变换
• 计算示例
• 常见易错点
• 小结
• 参考

从 SO(3) 到 SE(3)

齐次变换矩阵

旋转矩阵 $R\in SO(3)$ 只能描述姿态，其中

$$SO(3)=\{R\in\mathbb{R}^{3\times3}\mid R^TR=I,\ \det(R)=1\}.$$

如果还要表示坐标系原点的位置，就需要增加平移向量 $p\in\mathbb{R}^3$。将二者放入同一个矩阵，得到特殊欧氏群 $SE(3)$：

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

记 $T_{sb}$ 为坐标系 $\{b\}$ 相对于空间坐标系 $\{s\}$ 的位姿。$R_{sb}$ 的三列分别是 $\{b\}$ 的三个单位轴在 $\{s\}$ 中的坐标，$p_{sb}$ 则是 $\{b\}$ 原点在 $\{s\}$ 中的位置。

齐次坐标与点的变换

空间点的普通坐标为 $x\in\mathbb{R}^3$，补上最后一维 $1$ 后得到齐次坐标：

$$\bar{x}=\begin{bmatrix}x\\1\end{bmatrix}.$$

同一个点 $P$ 在两个坐标系中的坐标满足

$$\bar{x}_s=T_{sb}\bar{x}_b, \qquad x_s=R_{sb}x_b+p_{sb}.$$

方向向量不受平移影响，因此它的齐次形式最后一维取 $0$：

$$\begin{bmatrix}d_s\\0\end{bmatrix} =T_{sb}\begin{bmatrix}d_b\\0\end{bmatrix}, \qquad d_s=R_{sb}d_b.$$

这一区别很实用：点表示“在哪里”，方向向量只表示“朝哪里”。

变换的复合与求逆

若已知 $T_{sa}$ 和 $T_{ab}$，则从 $\{b\}$ 到 $\{s\}$ 的变换为

$$T_{sb}=T_{sa}T_{ab}.$$

矩阵乘法的顺序不能交换。阅读下标时，可以把中间重复的坐标系 $a$ 看作被“消去”。齐次变换的逆矩阵具有简单形式：

$$T_{bs}=T_{sb}^{-1} =\begin{bmatrix} R_{sb}^T&-R_{sb}^Tp_{sb}\\ 0&1 \end{bmatrix}.$$

注意逆变换的平移部分并非简单的 $-p$；它还需要先旋转到新的参考坐标系。

运动旋量 Twist

位姿 $T$ 描述某一时刻刚体“在哪里”，Twist 则描述刚体此刻“怎样运动”。它是同时包含角速度与线速度的六维向量：

$$V=\begin{bmatrix}\omega\\v\end{bmatrix} =\begin{bmatrix} \omega_x&\omega_y&\omega_z&v_x&v_y&v_z \end{bmatrix}^T.$$

从位姿变化率得到 Twist

设 $T_{sb}(t)$ 是随时间变化的刚体位姿。固定在刚体上的点满足

$$\bar{x}_s=T_{sb}\bar{x}_b, \qquad \dot{\bar{x}}_b=0.$$

求导并代入 $\bar{x}_b=T_{sb}^{-1}\bar{x}_s$：

$$\dot{\bar{x}}_s =\dot{T}_{sb}\bar{x}_b =\dot{T}_{sb}T_{sb}^{-1}\bar{x}_s.$$

因此定义空间坐标下的 Twist 矩阵

$$[V_s]=\dot{T}_{sb}T_{sb}^{-1} =\begin{bmatrix} \dot{R}R^T&\dot{p}-\dot{R}R^Tp\\ 0&0 \end{bmatrix} =\begin{bmatrix}[\omega_s]&v_s\\0&0\end{bmatrix}.$$

相应地，从右侧消去 $T$ 可得到物体坐标下的 Twist：

$$[V_b]=T_{sb}^{-1}\dot{T}_{sb} =\begin{bmatrix} R^T\dot{R}&R^T\dot{p}\\ 0&0 \end{bmatrix}.$$

反对称矩阵与括号算子

三维向量 $\omega=(\omega_x,\omega_y,\omega_z)^T$ 对应的反对称矩阵为

$$[\omega]= \begin{bmatrix} 0&-\omega_z&\omega_y\\ \omega_z&0&-\omega_x\\ -\omega_y&\omega_x&0 \end{bmatrix},$$

它满足 $[\omega]x=\omega\times x$。六维 Twist 的矩阵形式（也常写作 $[V]^\wedge$）为

$$[V]= \begin{bmatrix} [\omega]&v\\ 0&0 \end{bmatrix}\in se(3).$$

反向操作称为 vee 算子：$V=[V]^\vee$。这里的 $se(3)$ 是 $SE(3)$ 在单位元附近的切空间，也就是刚体的瞬时速度空间。

Twist 的几何意义

对空间中位置为 $x$ 的刚体点，其瞬时线速度为

$$\dot{x}=\omega\times x+v.$$

所以 $v$ 并不总是刚体某个指定点的平移速度；更准确地说，它是由当前参考坐标系原点选取所决定的速度项。当 $x=0$ 时才有 $\dot{x}=v$。改变参考坐标系后，$\omega$ 和 $v$ 的数值一般都会改变，但它们描述的是同一个物理运动。

螺旋轴 Screw Axis

Chasles 定理指出，任意刚体位移都可以等价为：绕空间中的某条轴旋转，同时沿这条轴平移。这条轴及其运动参数构成螺旋轴。

设单位轴方向为 $\omega$，轴上任一点为 $q$，螺距为 $h$。单位螺旋轴写为

$$S=\begin{bmatrix}\omega\\v\end{bmatrix}, \qquad v=-\omega\times q+h\omega, \qquad \|\omega\|=1.$$

其中 $h$ 表示每旋转 $1\ \mathrm{rad}$ 沿轴移动的距离。当运动速率为 $\dot{\theta}$ 时，实际 Twist 为

$$V=S\dot{\theta}.$$

因此应区分三个概念：$S$ 描述单位螺旋轴，$\dot\theta$ 描述运动快慢，$S\theta$ 描述累计运动量。

纯转动与纯平移

纯转动时 $h=0$，有

$$S=\begin{bmatrix}\omega\\-\omega\times q\end{bmatrix}.$$

若转轴恰好通过坐标原点，则 $q=0$，从而 $v=0$。这并不表示刚体上的所有点线速度都为零，因为它们仍满足 $\dot{x}=\omega\times x$。

纯平移可以看作螺距趋于无穷大的极限。此时

$$S=\begin{bmatrix}0\\v\end{bmatrix}, \qquad \|v\|=1,$$

参数 $\theta$ 不再是角度，而是平移距离。

一般螺旋运动

若已知一般 Twist $V=(\omega,v)$ 且 $\omega\neq0$，可先令

$$\dot{\theta}=\|\omega\|, \qquad \hat{\omega}=\frac{\omega}{\|\omega\|},$$

再将 $V$ 归一化为螺旋轴 $S=V/\dot\theta$。对于单位角速度形式，可由

$$h=\omega^Tv, \qquad q=\omega\times v+\lambda\omega$$

恢复螺距与轴上一点；$\lambda$ 任意，因为沿轴方向移动仍在同一条轴上。常取 $\lambda=0$，得到离原点最近的轴上点。

指数映射：从速度得到位姿

Twist 是瞬时量。若螺旋轴 $S$ 在运动过程中保持不变，将它对参数 $\theta$ 积分，就得到有限位移：

$$T(\theta)=e^{[S]\theta}T(0).$$

这就是从李代数 $se(3)$ 到李群 $SE(3)$ 的指数映射。

旋转的指数映射

对于单位旋转轴 $\omega$，Rodrigues 公式为

$$e^{[\omega]\theta} =I+\sin\theta[\omega]+(1-\cos\theta)[\omega]^2.$$

它把“轴 $\omega$ + 转角 $\theta$”转换成旋转矩阵 $R$。例如绕 $z$ 轴旋转 $\theta$：

$$R_z(\theta)= \begin{bmatrix} \cos\theta&-\sin\theta&0\\ \sin\theta&\cos\theta&0\\ 0&0&1 \end{bmatrix}.$$

刚体运动的指数映射

当 $\|\omega\|=1$ 时，$S=(\omega,v)$ 的矩阵指数可写成闭式：

$$e^{[S]\theta} =\begin{bmatrix} e^{[\omega]\theta}&G(\theta)v\\ 0&1 \end{bmatrix},$$

其中

$$G(\theta)=I\theta+(1-\cos\theta)[\omega] +(\theta-\sin\theta)[\omega]^2.$$

纯平移时 $\omega=0$，公式退化为

$$e^{[S]\theta} =\begin{bmatrix}I&v\theta\\0&1\end{bmatrix}.$$

反过来，矩阵对数 $[S]\theta=\log(T)$ 可以从相对位姿恢复等价的螺旋运动。这在轨迹插值、位姿误差计算和逆运动学中非常常见。

空间 Twist 与物体 Twist

同一个刚体速度可以在固定的空间坐标系 $\{s\}$ 中表示，也可以在随刚体运动的物体坐标系 $\{b\}$ 中表示：

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

空间表示可理解为“站在世界坐标系观察”，物体表示则是“把测量坐标轴固定在刚体上”。它们不是两个不同的运动，只是同一速度的不同坐标表达。

伴随表示与坐标系变换

由上述定义可得

$$[V_s]=T_{sb}[V_b]T_{sb}^{-1}.$$

将矩阵形式展开为六维向量形式：

$$V_s=\operatorname{Ad}_{T_{sb}}V_b,$$

其中，对于本文采用的 $V=(\omega,v)$ 排列约定，

$$\operatorname{Ad}_{T} =\begin{bmatrix} R&0\\ [p]R&R \end{bmatrix}.$$

于是

$$V_b=\operatorname{Ad}_{T_{sb}}^{-1}V_s =\operatorname{Ad}_{T_{bs}}V_s.$$

从分量看，$\omega_s=R\omega_b$，而

$$v_s=p\times(R\omega_b)+Rv_b.$$

线速度项不仅要旋转，还会受到坐标系原点偏移 $p$ 的影响，这正是仅用块对角矩阵 $\operatorname{diag}(R,R)$ 转换 Twist 会出错的原因。相同的伴随变换也适用于螺旋轴。

计算示例

考虑一条平行于 $z$ 轴、通过点 $q=(1,0,0)^T$ 的旋转轴，刚体绕它以单位角速度转动，无沿轴平移。此时

$$\omega=\begin{bmatrix}0\\0\\1\end{bmatrix}, \qquad v=-\omega\times q =\begin{bmatrix}0\\-1\\0\end{bmatrix}.$$

所以螺旋轴为

$$S=\begin{bmatrix}0&0&1&0&-1&0\end{bmatrix}^T.$$

取刚体上一点 $x=(2,0,0)^T$，其瞬时速度为

$$\dot{x}=\omega\times x+v =\begin{bmatrix}0\\2\\0\end{bmatrix} +\begin{bmatrix}0\\-1\\0\end{bmatrix} =\begin{bmatrix}0\\1\\0\end{bmatrix}.$$

这个结果符合直觉：点 $x$ 到转轴的距离是 $1$，角速度也是 $1\ \mathrm{rad/s}$，所以线速度大小为 $1\ \mathrm{m/s}$。

转过 $\theta=\pi/2$ 后，旋转部分为

$$R=R_z(\pi/2)= \begin{bmatrix}0&-1&0\\1&0&0\\0&0&1\end{bmatrix}.$$

利用指数映射或“绕不经过原点的轴旋转”的几何关系 $p=(I-R)q$，得到

$$p=\begin{bmatrix}1\\-1\\0\end{bmatrix}, \qquad T= \begin{bmatrix} 0&-1&0&1\\ 1&0&0&-1\\ 0&0&1&0\\ 0&0&0&1 \end{bmatrix}.$$

检验轴上的点 $q$：$Rq+p=q$，说明它在变换前后保持不动。

常见易错点

1. 把 $v$ 直接叫作“刚体的线速度”：刚体上不同点的速度不同，应使用 $\dot{x}=\omega\times x+v$；$v$ 与参考原点有关。
2. 混淆空间表示与物体表示：$[V_s]=\dot TT^{-1}$，$[V_b]=T^{-1}\dot T$，矩阵相乘顺序不同。
3. 忽略 Twist 的排列约定：本文使用 $(\omega,v)$；部分软件库使用 $(v,\omega)$，其伴随矩阵块结构会随之改变。
4. 认为平移向量求逆只需取负号：正确结果是 $-R^Tp$。
5. 混淆螺旋轴、Twist 和旋量：$S$ 是单位运动方向，$V=S\dot\theta$ 是速度，$S\theta$ 是有限运动的指数坐标。
6. 忽略单位：$\omega$ 通常为 $\mathrm{rad/s}$，$v$ 为 $\mathrm{m/s}$，螺距 $h$ 为 $\mathrm{m/rad}$。

小结

可以用下面这条链路串起全文：

$$\boxed{ \text{螺旋轴 }S \xrightarrow{\times\dot\theta} \text{瞬时速度 }V \xrightarrow{[\cdot]} se(3) \xrightarrow{\exp([S]\theta)} SE(3) }$$

• $T\in SE(3)$ 统一描述刚体的位置与姿态；
• $V\in\mathbb{R}^6$ 统一描述角速度与线速度；
• 螺旋轴为刚体运动提供直观的几何解释；
• 指数映射把瞬时运动积分为有限位移；
• 伴随矩阵 $\operatorname{Ad}_T$ 在不同坐标系之间转换 Twist 和螺旋轴。

这些工具也是后续学习指数积公式（PoE）、机械臂雅可比矩阵、正逆运动学和轨迹规划的基础。

参考

[1] Kevin M. Lynch, Frank C. Park, Modern Robotics: Mechanics, Planning, and Control, Cambridge University Press, 2017.

[2] 许中兴，RISE 团队，《机器人学笔记》。

[3] Richard M. Murray, Zexiang Li, S. Shankar Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994.