机器人学的学与思 [ 3 ] ：旋转速度与刚体速度

科学家必须在庞杂的经验事实中抓住某些可用精密公式来表示的普遍特征，由此探求自然界的普遍原理。
——爱因斯坦

1 旋转速度

先考虑 ${\mathbb{R}}^3$ 中纯旋转的情况，运动轨迹的曲线为 $R_{ab}(t)\in SO(3)$ 。设 $B$ 系与 $A$ 系共原点并相对于 $A$ 系旋转，称 $A$ 系为空间（spatial）坐标系，$B$ 系为物体（body）坐标系。刚体上一点 $q$ 在物体坐标系中的表示为 $q_b$ ，$q_b$ 是固定不变的。其在空间坐标系中的运动轨迹为：
$$q_a(t)=R_{ab}(t)q_b$$
旋转矩阵 $R_{ab}$ 随时间变化。

对 $q_a(t)$ 求导，就会得到该点在空间坐标系下的速度为：
$$v_{q_a}(t)=\frac{d}{dt}{q}_a(t)={\dot{R}}_{ab}(t)q_b$$
${\dot{R}}_{ab}(t)$ 表示 $R_{ab}(t)$ 对 $t$ 的导数，以下符号的表述皆相同，某些式子会省略参数 $t$ 。

可将上式改写为：
$$v_{q_a}(t)={\dot{R}}_{ab}(t)R_{ab}^{-1}{R}_{ab}{q}_b={\dot{R}}_{ab}(t)R_{ab}^{-1}{q}_a$$
其中，${\dot{R}}_{ab}{R}_{ab}^{-1}$ 为反对称矩阵，即 ${\dot{R}}_{ab}(t)R_{ab}\in so(3)$ ，可由对 $R{R}^T=I$ 两边求导证明。

这说明三维矢量可以表示旋转速度，定义瞬时空间角速度 $w_{ab}^s\in {\mathbb{R}}^3$ 为：
$${\hat{w}}_{ab}^s={\dot{R}}_{ab}{R}_{ab}^{-1}$$
有瞬时空间角速度，就有瞬时物体角速度，定义瞬时物体角速度 $w_{ab}^b\in {\mathbb{R}}^3$ 为：
$${\hat{w}}_{ab}^b=R_{ab}^{-1}{\dot{R}}_{ab}$$
其可由 $v_{q_a}(t)=R_{ab}(t)v_{q_b}(t)$ 推导得出。由表达式可得两个角速度的关系为：
$${\hat{w}}_{ab}^s=R_{ab}{\hat{w}}_{ab}^b{R}_{ab}^{-1}\text{or}{w}_{ab}^s=R_{ab}{w}_{ab}^b$$
则该点相对于空间坐标系和物体坐标系的速度可以分别表示为：
$$\{\begin{array}{l}{v}_{q_a}(t)={\hat{w}}_{ab}^s(t)q_a(t)\\ v_{q_b}(t)={\hat{w}}_{ab}^b(t)q_b\end{array}$$

2 刚体速度

2.1 定义

现在考虑 ${\mathbb{R}}^3$ 中带有旋转和平移的刚体运动的情况，运动轨迹的曲线为 $g_{ab}(t)\in SE(3)$ 。类比 之前的式子，可以写出：
$$v_{q_a}(t)={\dot{g}}_{ab}(t)g_{ab}^{-1}{g}_{ab}{q}_b={\dot{g}}_{ab}(t)g_{ab}^{-1}{q}_a$$
我们可以定义：
$$\{\begin{array}{l}{\hat{V}}_{ab}^s={\dot{g}}_{ab}{g}_{ab}^{-1}\\ {\hat{V}}_{ab}^b=g_{ab}^{-1}{\dot{g}}_{ab}\end{array}$$
其也可由 $v_{q_a}(t)=g_{ab}(t)v_{q_b}(t)$ 推导得出。

类比 ${\hat{w}}_{ab}^s\in so(3)$ ，${\hat{V}}_{ab}^s\in se(3)$ ，即其与运动旋量形式类似， 可以通过 ${\dot{g}}_{ab}{g}_{ab}^{-1}$ 矩阵相乘证明。因此有：

$${\hat{V}}_{ab}^s=\left[\begin{array}{cc}{\hat{w}}_{ab}^s& v_{ab}^s\\ 0& 0\end{array}\right]=\left[\begin{array}{cc}{\dot{R}}_{ab}{R}_{ab}^T& -{\dot{R}}_{ab}{R}_{ab}^T{p}_{ab}+{\dot{p}}_{ab}\\ 0& 0\end{array}\right]V_{ab}^s=\left[\begin{array}{c}{v}_{ab}^s\\ w_{ab}^s\end{array}\right]$$

其具有与运动旋量 $\xi$ 相同的 $\vee ,\wedge$ 操作。注意内部成分的上下标符号与其一致。${\hat{w}}_{ab}^s$ 的表达式与 $(43)$ 式不谋而合。

同样地有：
$$\{\begin{array}{l}{v}_{q_a}(t)={\hat{V}}_{ab}^s(t)q_a(t)=w_{ab}^s(t)\times q_a(t)+v_{ab}^s(t)\\ v_{q_b}(t)={\hat{V}}_{ab}^b(t)q_b=w_{ab}^b(t)\times q_b+v_{ab}^b(t)\end{array}$$

2.2 伴随变换

类比之前的式子，刚体运动的空间速度和物体速度之间的关系为：
$${\hat{V}}_{ab}^s=g_{ab}{\hat{V}}_{ab}^b{g}_{ab}^{-1}$$
那么有没有类比 $(45)$ 式后半部分得出来的 $V_{ab}^s,V_{ab}^b\in {\mathbb{R}}^6$ 之间的关系呢？单纯类比写出式子我们会发现，$(4\times 4)$ 和 $(6\times 1)$ 的矩阵根本不能相乘。这就需要我们通过分析其矩阵内部成分之间的关系得出它们之间的关系。

可得：
$$\{\begin{array}{l}{\hat{w}}_{ab}^s={\dot{R}}_{ab}{R}_{ab}^T\\ v_{ab}^s=-{\dot{R}}_{ab}{R}_{ab}^T{p}_{ab}+{\dot{p}}_{ab}=-{\hat{w}}_{ab}^s{p}_{ab}+{\dot{p}}_{ab}=p_{ab}\times w_{ab}^s+{\dot{p}}_{ab}\end{array}$$
因为：
$$w_{ab}^s=R_{ab}{w}_{ab}^b$$

$${\dot{p}}_{ab}=v_{ab}^s=R_{ab}{v}_{ab}^b$$

因为 $p_{ab}$ 是表示在空间坐标系下的，对其求导得到 $v_{ab}^s$ 。上述两式最好单独理解。

所以：

$$v_{ab}^s=p_{ab}\times (R_{ab}{w}_{ab}^b)+R_{ab}{v}_{ab}^b={\hat{p}}_{ab}{R}_{ab}{w}_{ab}^b+R_{ab}{v}_{ab}^b$$

联立上述两式，表示为矩阵形式：

$$V_{ab}^s=\left[\begin{array}{c}{v}_{ab}^s\\ w_{ab}^s\end{array}\right]=\left[\begin{array}{c}{\hat{p}}_{ab}{R}_{ab}{w}_{ab}^b+R_{ab}{v}_{ab}^b\\ R_{ab}{w}_{ab}^b\end{array}\right]=\left[\begin{array}{cc}{R}_{ab}& {\hat{p}}_{ab}{R}_{ab}\\ 0& R_{ab}\end{array}\right]\left[\begin{array}{c}{v}_{ab}^b\\ w_{ab}^b\end{array}\right]$$

定义将运动旋量坐标进行坐标系变换的 $6\times 6$ 矩阵为：

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

称其为伴随变换，其与 $g$ 有关，且有 $A{d}_{g^{-1}}=A{d}_g^{-1}$ 。

所以有：

$$V_{ab}^s=A{d}_{g_{ab}}{V}_{ab}^b$$

我们还可以得到：

$$\{\begin{array}{l}(\hat{V}{)}^{\vee }=V\\ (g\hat{V}{g}^{-1}{)}^{\vee }=A{d}_{g}V\end{array}$$

即运动旋量在 ${\mathbb{R}}^{4\times 4}$ 中左乘一个 $g$ 再右乘一个 $g^{-1}$ 的作用跟在 ${\mathbb{R}}^6$ 中左乘一个 $A{d}_g$ 的效果一致。当然，此处的 $A{d}_g$ 是根据 $g$ 得出来的。

那么，在不同坐标系下的空间速度是否也符合链式法则呢？答案是否定的。设有 $A,B,C$ 三个坐标系，各坐标系中的空间速度存在下列关系：

$$V_{ac}^s=V_{ab}^s+A{d}_{g_{ab}}{V}_{bc}^s$$

证明如下：

$$\begin{gathered} {\hat{V}}_{ac}^s={\dot{g}}_{ac}{g}_{ac}^{-1}=\dot{(g_{ab}{g}_{bc})}(g_{ab}{g}_{bc}{)}^{-1} \\ =({\dot{g}}_{ab}{g}_{bc}+g_{ab}{\dot{g}}_{bc})(g_{bc}^{-1}{g}_{ab}^{-1}) \\ ={\dot{g}}_{ab}{g}_{bc}{g}_{bc}^{-1}{g}_{ab}^{-1}+g_{ab}{\dot{g}}_{bc}{g}_{bc}^{-1}{g}_{ab}^{-1} \\ ={\dot{g}}_{ab}{g}_{ab}^{-1}+g_{ab}{\dot{g}}_{bc}{g}_{bc}^{-1}{g}_{ab}^{-1} \\ ={\hat{V}}_{ab}^s+g_{ab}{\hat{V}}_{bc}^s{g}_{ab}^{-1} \end{gathered}$$

$$V_{ac}^s=({\hat{V}}_{ac}^s{)}^{\vee }=({\hat{V}}_{ab}^s{)}^{\vee }+(g_{ab}{\hat{V}}_{bc}^s{g}_{ab}^{-1}{)}^{\vee }=V_{ab}^s+A{d}_{g_{ab}}{V}_{bc}^s$$

空间速度和物体速度在多个坐标系之间的变换，也可以通过上述证明方法求解。假设 $B$ 系和 $C$ 系没有相对运动，可以得出 $V_{bc}^s=0$ 。

3 旋量运动的速度

我们还可以从另一个角度考虑刚体运动的空间速度和物体速度。考虑旋量运动：

$$g_{ab}(\theta )=e^{\hat{\xi }\theta }{g}_{ab}(0)$$

其中，$\hat{\xi }$ 为定常运动旋量。可以得到刚体运动的空间速度为：

$$\begin{gathered} {\hat{V}}_{ab}^s={\dot{g}}_{ab}(\theta )g_{ab}^{-1}(\theta )=\dot{(e^{\hat{\xi }\theta }{g}_{ab}(0))}(e^{\hat{\xi }\theta }{g}_{ab}(0){)}^{-1} \\ =(\hat{\xi }\dot{\theta }{e}^{\hat{\xi }\theta }{g}_{ab}(0))(g_{ab}^{-1}(0)e^{\widehat{-\xi }\theta }) \\ =\hat{\xi }\dot{\theta }{e}^{\hat{\xi }\theta }{e}^{\widehat{-\xi }\theta }=\hat{\xi }\dot{\theta } \end{gathered}$$

同样地，可以得到刚体运动的物体速度：
$$\begin{gathered} {\hat{V}}_{ab}^b=g_{ab}^{-1}(\theta ){\dot{g}}_{ab}(\theta )=(e^{\hat{\xi }\theta }{g}_{ab}(0){)}^{-1}\dot{(e^{\hat{\xi }\theta }{g}_{ab}(0))} \\ =(g_{ab}^{-1}(0)e^{\widehat{-\xi }\theta })(\hat{\xi }\dot{\theta }{e}^{\hat{\xi }\theta }{g}_{ab}(0)) \\ =(g_{ab}(0{)}^{-1}\hat{\xi }{g}_{ab}(0))\dot{\theta } \\ =(g_{ab}(0{)}^{-1}\hat{\xi }{g}_{ab}(0){)}^{\vee \wedge }\dot{\theta } \\ =(A{d}_{g_{ab}^{-1}(0)}\xi {)}^{\wedge }\dot{\theta } \end{gathered}$$