为什么机器人学要用运动旋量描述刚体速度？

刚体位形 $T\in SE(3)$ 描述“刚体在哪里、朝哪里”。

如果刚体正在运动，怎样描述它的瞬时速度？

直觉上会想看 $\dot{T}$，但 $\dot{T}$ 本身并不是一个好用的刚体速度表示。这和 3.2 里的角速度完全类似：旋转矩阵 $R(t)$ 描述姿态，$\dot{R}$ 描述姿态变化，但真正能读出角速度的是：

$$\dot{R}{R}^{-1}=[{\omega }_s],R^{-1}\dot{R}=[{\omega }_b]$$

现在把 $R$ 换成完整的刚体位形 $T$，自然会问：

$$\dot{T}{T}^{-1}\text{和}{T}^{-1}\dot{T}$$

会不会也有类似的速度含义？答案是：会。它们给出的就是刚体的运动旋量，也就是 twist。

先记住本文的主线：

twist 是刚体瞬时速度的 6 维表达，前三维是角速度，后三维是线速度项；同一个运动可以有不同的 twist 表达，区别在于用哪个参考系表达，以及线速度项对应哪个点。

一、从运动中的齐次变换矩阵开始

设 { s } \{s\} {s} 是固定参考系， { b } \{b\} {b} 是固定在刚体上的物体参考系。下面把刚体在时刻 $t$ 的位形写出来：

$$T_{sb}(t)=T(t)=\left[\begin{array}{cc}R(t)& p(t)\\ 0& 1\end{array}\right]$$

其中：

• $R(t)$ 表示 { b } \{b\} {b} 相对 { s } \{s\} {s} 的姿态；
• $p(t)$ 表示 { b } \{b\} {b} 原点在 { s } \{s\} {s} 中的位置；
• $\dot{p}(t)$ 是 { b } \{b\} {b} 原点的速度，并且用 { s } \{s\} {s} 表达。

对 $T$ 求导：

$$\dot{T}=\left[\begin{array}{cc}\dot{R}& \dot{p}\\ 0& 0\end{array}\right]$$

最后一行变成 $[0000]$，因为齐次变换矩阵最后一行 $[0001]$ 是常数。

原书接下来考察两个量：

$$T^{-1}\dot{T},\dot{T}{T}^{-1}$$

它们不是随便凑出来的。前者对应 $R^{-1}\dot{R}$，会得到物体坐标表达的速度；后者对应 $\dot{R}{R}^{-1}$，会得到空间坐标表达的速度。

二、$T^{-1}\dot{T}$：物体运动旋量

先看 $T^{-1}\dot{T}$。因为：

$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}p\\ 0& 1\end{array}\right]$$

所以：

$$T^{-1}\dot{T}=\left[\begin{array}{cc}{R}^T& -R^{T}p\\ 0& 1\end{array}\right]\left[\begin{array}{cc}\dot{R}& \dot{p}\\ 0& 0\end{array}\right]=\left[\begin{array}{cc}{R}^T\dot{R}& R^T\dot{p}\\ 0& 0\end{array}\right]$$

这里有两部分：

$$R^T\dot{R}=[{\omega }_b],v_b=R^T\dot{p}$$

${\omega }_b$ 是角速度在 { b } \{b\} {b} 中的表达。$v_b$ 是 { b } \{b\} {b} 原点速度在 { b } \{b\} {b} 中的表达，因为 $\dot{p}$ 原本用 { s } \{s\} {s} 表达，左乘 $R^T$ 后改写到了 { b } \{b\} {b} 中。

于是：

$$T^{-1}\dot{T}=\left[\begin{array}{cc}[{\omega }_b]& v_b\\ 0& 0\end{array}\right]=[V_b]$$

其中：

$$V_b=\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]\in {\mathbb{R}}^6$$

这里不是又推导出了一个新物理量，而是在做一个记号约定。矩阵 $[V_b]$ 里真正独立的信息只有 6 个数：左上角的反对称矩阵 $[{\omega }_b]$ 由 3 个数决定，右上角的 $v_b$ 也是 3 个数。把这两部分按“角速度在前、线速度在后”的顺序排成一列，就得到 $V_b$。

也就是说，下面两种写法表达的是同一个 twist：

$$[V_b]=\left[\begin{array}{cc}[{\omega }_b]& v_b\\ 0& 0\end{array}\right]⟺V_b=\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]$$

$V_b$ 就是 body twist，也就是物体坐标表达的运动旋量。

可以把它读成：

$V_b$ 描述同一个刚体运动，但所有速度量都站在当前物体参考系 { b } \{b\} {b} 里看。

三、$\dot{T}{T}^{-1}$：空间运动旋量

再看另一个乘法顺序：

$$\dot{T}{T}^{-1}=\left[\begin{array}{cc}\dot{R}& \dot{p}\\ 0& 0\end{array}\right]\left[\begin{array}{cc}{R}^T& -R^{T}p\\ 0& 1\end{array}\right]$$

展开得到：

$$\dot{T}{T}^{-1}=\left[\begin{array}{cc}\dot{R}{R}^T& \dot{p}-\dot{R}{R}^{T}p\\ 0& 0\end{array}\right]$$

其中：

$$\dot{R}{R}^T=[{\omega }_s]$$

这是角速度在固定参考系 { s } \{s\} {s} 中的表达。右上角定义为：

$$v_s=\dot{p}-[{\omega }_s]p=\dot{p}-{\omega }_s\times p=\dot{p}+{\omega }_s\times (-p)$$

于是：

$$\dot{T}{T}^{-1}=\left[\begin{array}{cc}[{\omega }_s]& v_s\\ 0& 0\end{array}\right]=[V_s]$$

其中：

$$V_s=\left[\begin{array}{c}{\omega }_s\\ v_s\end{array}\right]\in {\mathbb{R}}^6$$

$V_s$ 就是 spatial twist，也就是空间坐标表达的运动旋量。

四、为什么 $v_s$ 不是 $\dot{p}$

图 1：原书 Figure 3.17。实线是当前刚体位置，虚线是下一瞬间的位置。$\dot{p}$ 是 { b } \{b\} {b} 原点的速度，$v_s$ 是当前落在 { s } \{s\} {s} 原点处的那个刚体点速度。

这是本节最容易误解的地方。

{ b } \{b\} {b} 原点速度在 { s } \{s\} {s} 中的表达是 $\dot{p}$， { s } \{s\} {s} 原点相对 { b } \{b\} {b} 原点位移是 $-p$，角速度是 ${\omega }_s$。两点相对位置决定的速度差是 ${\omega }_s\times (-p)$，因此：

$$v_s=\dot{p}+{\omega }_s\times (-p)$$

也就是：

$$v_s=\dot{p}-{\omega }_s\times p$$

这正好和前面从矩阵乘法得到的右上角一致。

所以 $v_s$ 的物理意义就是原书说的：如果把运动刚体想象成无限大，那么 $v_s$ 是这个刚体上当前位于固定系原点的那个点的瞬时速度，并且用 { s } \{s\} {s} 表达。

这样看，$V_s$ 和 $V_b$ 的对应关系就清楚了：

• ${\omega }_s$：角速度用 { s } \{s\} {s} 表达；
• ${\omega }_b$：角速度用 { b } \{b\} {b} 表达；
• $v_s$：当前位于 { s } \{s\} {s} 原点的刚体点速度，用 { s } \{s\} {s} 表达；
• $v_b$：当前位于 { b } \{b\} {b} 原点的刚体点速度，用 { b } \{b\} {b} 表达。

五、$se(3)$：twist 的矩阵形式

不管是 $V_s$ 还是 $V_b$，只要写成：

$$V=\left[\begin{array}{c}\omega \\ v\end{array}\right]$$

它的矩阵形式就是：

$$[V]=\left[\begin{array}{cc}[\omega ]& v\\ 0& 0\end{array}\right]\in se(3)$$

展开为：

$$[V]=\left[\begin{array}{cccc}0& -{\omega }_3& {\omega }_2& v_1\\ {\omega }_3& 0& -{\omega }_1& v_2\\ -{\omega }_2& {\omega }_1& 0& v_3\\ 0& 0& 0& 0\end{array}\right]$$

这里的方括号有两层用法：

• 对三维向量 $\omega$，$[\omega ]$ 是 $3\times 3$ 反对称矩阵；
• 对六维向量 $V=(\omega ,v)$，$[V]$ 是 $4\times 4$ 矩阵。

可以把关系记成：

$$SO(3)\leftrightarrow so(3),SE(3)\leftrightarrow se(3)$$

$SE(3)$ 里放刚体位形，$se(3)$ 里放刚体速度的矩阵表示。

六、空间 twist 和物体 twist 怎样互相转换

同一个刚体运动，可以写成 $V_s$，也可以写成 $V_b$。它们不是两个运动，而是同一个运动的两套坐标。

从矩阵形式出发：

$$[V_s]=T[V_b]T^{-1},[V_b]=T^{-1}[V_s]T$$

把第一条展开成向量形式：

$$\left[\begin{array}{c}{\omega }_s\\ v_s\end{array}\right]=\left[\begin{array}{cc}R& 0\\ [p]R& R\end{array}\right]\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]$$

中间这个 $6\times 6$ 矩阵叫 伴随板换矩阵 (adjoint representation)：

$$[A{d}_T]=\left[\begin{array}{cc}R& 0\\ [p]R& R\end{array}\right]$$

这里的 $[p]$ 和 $[\omega ]$ 一样，表示由三维向量 $p$ 构造出的反对称矩阵。

因此：

$$V_s=[A{d}_{T_{sb}}]V_b,V_b=[A{d}_{T_{bs}}]V_s$$

如果把第二条写成完整矩阵，就是：

$$V_b=\left[\begin{array}{cc}{R}^T& 0\\ -R^T[p]& R^T\end{array}\right]V_s$$

更一般地，若同一个 twist 在 { d } \{d\} {d} 中的表达是 $V_d$，要改写成 { c } \{c\} {c} 中的表达，则：

$$V_c=[A{d}_{T_{cd}}]V_d$$

这条公式的下标读法和齐次变换矩阵一致：

$T_{cd}$ 表示 { d } \{d\} {d} 相对 { c } \{c\} {c} 的位形，所以它可以把 $V_d$ 改写成 $V_c$。

这里要注意，$A{d}_T$ 改变的是同一个 twist 的坐标表达，不是在创造一个新的刚体运动。

七、例 3.23：小车的两个 twist 表达

原书用一个三轮小车例子说明：同一个瞬时运动，看参考系不同，twist 的数字会不同。

图 2：原书 Figure 3.18。小车底盘的瞬时运动可以看成绕平面内某点 $r$ 的转动。

图中小车绕平面内一点 $r$ 做瞬时转动，角速度大小为：

$$w=2\text{rad/s}$$

固定系里：

$$r_s=(2,-1,0),{\omega }_s=(0,0,2)$$

物体系里：

$$r_b=(2,-1.4,0),{\omega }_b=(0,0,-2)$$

${\omega }_b$ 的第三个分量是负数，是因为图中 ${\hat{z}}_b$ 指向纸内，而 ${\hat{z}}_s$ 指向纸外。

对应的位形矩阵为：

$$T_{sb}=\left[\begin{array}{cccc}-1& 0& 0& 4\\ 0& 1& 0& 0.4\\ 0& 0& -1& 0\\ 0& 0& 0& 1\end{array}\right]$$

因为纯转动时 $v=-\omega \times r=r\times \omega$，所以：

$$v_s=r_s\times {\omega }_s=(-2,-4,0)$$

$$v_b=r_b\times {\omega }_b=(2.8,4,0)$$

于是：

$$V_s=\left[\begin{array}{c}0\\ 0\\ 2\\ -2\\ -4\\ 0\end{array}\right],V_b=\left[\begin{array}{c}0\\ 0\\ -2\\ 2.8\\ 4\\ 0\end{array}\right]$$

它们看起来不一样，但描述的是同一个小车瞬时运动。二者可以通过：

$$V_s=[A{d}_{T_{sb}}]V_b$$

互相验证。

八、twist 的螺旋轴解释

到这里，twist 已经可以作为“刚体速度”来用了。原书接着给出一个更几何的解释：twist 可以看成沿某条螺旋轴的运动速度。

角速度可以写成：

$$\omega =\hat{\omega }\dot{\theta }$$

其中 $\hat{\omega }$ 是单位旋转轴，$\dot{\theta }$ 是绕轴转动的角速度大小。

类似地，twist 也可以写成：

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

这里 $S$ 是螺旋轴，$\dot{\theta }$ 是沿这条螺旋轴运动的速度尺度。

图 3：原书 Figure 3.19。螺旋轴可以由轴上一点 $q$、单位方向 $\hat{s}$ 和螺距 $h$ 描述。

一条螺旋轴可以先用三部分描述：

• $q$：螺旋轴上的任意一点；
• $\hat{s}$：螺旋轴的单位方向；
• $h$：螺距，也就是沿轴方向线速度与绕轴角速度的比值。

如果刚体以角速度大小 $\dot{\theta }$ 绕这条轴运动，同时沿轴方向平移，那么对应的 twist 为：

$$V=\left[\begin{array}{c}\omega \\ v\end{array}\right]=\left[\begin{array}{c}\hat{s}\dot{\theta }\\ -(\hat{s}\dot{\theta })\times q+h\hat{s}\dot{\theta }\end{array}\right]$$

也就是说：

$$\omega =\hat{s}\dot{\theta },v=-(\hat{s}\dot{\theta })\times q+h\hat{s}\dot{\theta }$$

$v$ 是两部分之和：

• $h\hat{s}\dot{\theta }$：沿螺旋轴方向的平移速度；
• $-(\hat{s}\dot{\theta })\times q$：绕螺旋轴转动时，参考系原点产生的线速度。

第一项沿着 $\hat{s}$，第二项垂直于 $\hat{s}$。

反过来，对于任意一个一般 twist：

$$V=\left[\begin{array}{c}\omega \\ v\end{array}\right]$$

只要 $\omega \ne 0$，都可以找到一条等效螺旋轴和一个速度尺度：

$$\hat{s}=\frac{\omega }{\Vert \omega \Vert },\dot{\theta }=\Vert \omega \Vert ,h=\frac{{\hat{s}}^{T}v}{\dot{\theta }}=\frac{{\omega }^{T}v}{\Vert \omega {\Vert }^2}$$

如果 $\omega =0$，就是纯平移。这种情况看成螺距 $h$ 为无穷大：

$$\hat{s}=\frac{v}{\Vert v\Vert },\dot{\theta }=\Vert v\Vert$$

这里的 $\dot{\theta }$ 不再是角速度，而是沿 $\hat{s}$ 方向的线速度大小。

由于$h$可能无穷大，$q$也不具备唯一性，书中决定不使用 { q , s ^ , h } \{q,\hat s,h\} {q,s^,h}，把螺旋轴本身也写成一个 6 维向量 $S=(\omega ,v)$。这就是 normalized screw axis，也就是归一化螺旋轴。

归一化的目的，是把“轴的几何信息”和“速度大小”分开：

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

具体分两种情况。

如果一般 twist $V=(\omega ,v)$ 有旋转分量，即 $\omega \ne 0$，则：

$$S=\frac{V}{\Vert \omega \Vert }=\left[\begin{array}{c}\omega /\Vert \omega \Vert \\ v/\Vert \omega \Vert \end{array}\right],\dot{\theta }=\Vert \omega \Vert$$

也就是说，$S$ 的角速度部分已经被除成单位向量。为了贴合原书记号，接下来把归一化后的 $S$ 仍写成 $S=(\omega ,v)$，那么它满足：

$$\Vert \omega \Vert =1$$

如果一般 twist 是纯平移，即 $\omega =0$，则：

$$S=\frac{V}{\Vert v\Vert }=\left[\begin{array}{c}0\\ v/\Vert v\Vert \end{array}\right],\dot{\theta }=\Vert v\Vert$$

同样，如果把归一化后的 $S$ 仍写成 $S=(\omega ,v)$，它满足：

$$\omega =0,\Vert v\Vert =1$$

所以“单位螺旋轴”满足两种情况之一：

• $\Vert \omega \Vert =1$；
• $\omega =0$ 且 $\Vert v\Vert =1$。

这里也要注意一个符号习惯：原书用 $(\omega ,v)$ 同时表示一般 twist $V$ 和归一化螺旋轴 $S$。区别在上下文里判断：$V$ 不要求归一化，$S$ 必须满足上面的两种条件之一。

螺旋轴的矩阵形式和 twist 一样：

$$[S]=\left[\begin{array}{cc}[\omega ]& v\\ 0& 0\end{array}\right]\in se(3)$$

螺旋轴和 twist 一样，也可以用 adjoint 改写参考系。若 $S_b$ 是螺旋轴在 { b } \{b\} {b} 中的表达，要写成 { a } \{a\} {a} 中的表达，则：

$$S_a=[A{d}_{T_{ab}}]S_b$$

反过来：

$$S_b=[A{d}_{T_{ba}}]S_a$$

这和 twist 的坐标变换完全一致，因为螺旋轴本质上就是归一化后的 twist。

小结

第一，机器人学不用单独的 $\dot{T}$ 表示刚体速度，而是把它和 $T^{-1}$ 组合起来：

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

这样得到的 $[V_b]$ 和 $[V_s]$ 都是 $se(3)$ 中的矩阵表示。

第二，twist 本身是 6 维向量：

$$V=\left[\begin{array}{c}\omega \\ v\end{array}\right]$$

前三维是角速度，后三维是线速度项。$V_b$ 是物体坐标表达，$V_s$ 是空间坐标表达。

第三，最容易错的是 $v_s$。它不是 { b } \{b\} {b} 原点速度 $\dot{p}$，而是当前位于 { s } \{s\} {s} 原点处的那个刚体点速度：

$$v_s=\dot{p}-{\omega }_s\times p$$

这个结论要贴着 Figure 3.17 看：从 { b } \{b\} {b} 原点走到 { s } \{s\} {s} 原点的位移是 $-p$，所以多出来的旋转速度是 ${\omega }_s\times (-p)$。

第四，同一个刚体速度可以换参考系表达，换法由 adjoint 矩阵给出：

$$V_s=[A{d}_{T_{sb}}]V_b,[A{d}_T]=\left[\begin{array}{cc}R& 0\\ [p]R& R\end{array}\right]$$

第五，twist 还有一个螺旋轴解释：

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

也就是把“沿哪条螺旋轴运动”和“运动得多快”分开。归一化螺旋轴 $S$ 满足 $\Vert \omega \Vert =1$，或者在纯平移时满足 $\omega =0,\Vert v\Vert =1$。

如果只记一句话，可以记：

$T\in SE(3)$ 描述刚体位形，$V\in {\mathbb{R}}^6$ 描述刚体瞬时速度，$[V]\in se(3)$ 是 twist 的矩阵表示，$A{d}_T$ 负责在不同参考系之间改写 twist。

后面的 3.3.3 会继续沿着这条线走：既然 $V$ 是速度，那么把一个常量 twist 沿时间积分，就会得到刚体运动的指数坐标表示。

参考资料

1. Kevin M. Lynch and Frank C. Park, Modern Robotics: Mechanics, Planning, and Control, Section 3.3.2 Twists.
2. Lynch and Park, Modern Robotics 2019 preprint PDF. 文中图片为该 PDF 中 3.3.2 节相关原书图的局部截图。