【机器人】刚体运动的指数坐标表达学习心得与理解

1.前言

上一篇文章对旋转的指数坐标表示进行了总结，也就是旋转矩阵和指数坐标之间的关系。今天准备写一下包括旋转和平移内容的刚体运动的指数坐标表达学习心得和对其的理解。如有问题，欢迎留言。

2.运动旋量

运动旋量也被称为“空间速度”，当然前提都是要有个参考坐标系。对于一个$4\times 4$的表示$\{b\}$相对于$\{s\}$齐次变换矩阵
$${}^s{T}_b=\left[\begin{array}{cc}R(t)& p(t)\\ 0& 1\end{array}\right]$$
则有
$${}^s{T}_b^{-1}\dot{{}^s{T}_b}=[V_b]=\left[\begin{array}{cc}[{\omega }_b]& v_b\\ 0& 0\end{array}\right]$$
$${\dot{{}^s{T}_b}}^s{T}_b^{-1}=[V_s]=\left[\begin{array}{cc}[{\omega }_s]& v_s\\ 0& 0\end{array}\right]$$
$$V_b=\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]$$
$$V_s=\left[\begin{array}{c}{\omega }_s\\ v_s\end{array}\right]$$
上式中，$V_b$和$V_s$分别称为物体运动旋量和空间运动旋量，其区别就在于角速度和线速度所在的坐标系不同。

3.运动旋量的螺旋解释

将运动旋量和角速度旋转运动对比起来看，如下表所示

角速度	运动旋量
角速度$\omega$可以写成绕单位转轴$\hat{\omega }$以角速度$\dot{\theta }$旋转，即$\omega =\hat{\omega }\dot{\theta }$	运动旋量$V$可以写成绕螺旋轴$S$以角速度$\dot{\theta }$旋转运动，该旋转还会带上沿螺旋轴的平移，即$V=S\dot{\theta }$

运动旋量为什么可以表达螺旋运动呢？下图是螺旋运动的一个示意图

螺旋运动是绕螺旋轴的转动和平移的复合。其中$\hat{s}$为螺旋轴方向的单位向量，$h$为节距，即线速度和角速度的比值，如果只有旋转运动，则$h=0$，若只有平移运动，则$h=+\infty$，$q\in {\mathbb{R}}^3$是螺旋轴上的任意一点，上述参数都是在固定坐标系中表示。

将螺旋运动和运动旋量中的各个速度参数对应起来表示为
$$V_s=\left[\begin{array}{c}{\omega }_s\\ v_s\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]$$
式中，线速度有两个分量组成，一个是沿着螺旋轴方向的线速度$h\hat{s}\dot{\theta }$，还有一个则是参考坐标系的原点，即向量0，在绕螺旋轴的旋转中的线速度，因为线速度=角速度$\times$从转轴到目标点的矢径，因此该线速度分量为
$$v_q=\hat{s}\dot{\theta }\times (0-q)=-\hat{s}\dot{\theta }\times q$$
为什么是0-q？因为要得到一条从转轴沿着q向量方向到达原点的向量，则只能通过目标点-起始点获得，因为向量减法是指向被减向量。

可见，对于任意运动旋量$V=(\omega ,v)$，当$\omega \ne 0$的时候，必定存在一个等效螺旋轴和旋转速度$\dot{\theta }$，其中

• 当$\omega \ne 0$时，$\hat{s}=\frac{\omega }{\Vert \omega \Vert }，\dot{\theta }=\Vert \omega \Vert$，即此时的螺旋轴$S=\frac{V}{\Vert \omega \Vert }=(\frac{\omega }{\Vert \omega \Vert },\frac{v}{\Vert \omega \Vert })$，因为螺旋轴的角速度$\dot{\theta }=\Vert \omega \Vert$，故$S\dot{\theta }=V$

• 当$\omega =0$时，此时仅有平移运动，线速度$\dot{\theta }=\Vert v\Vert$

给定参考坐标系，螺旋轴$S$可以写为
$$S=\left[\begin{array}{c}\omega \\ v\end{array}\right]\in {\mathbb{R}}^6$$
$$[S]=\left[\begin{array}{cc}[\omega ]& v\\ 0& 0\end{array}\right]$$
注意，当描述螺旋轴时，其角速度$\omega$或者线速度$v$为单位矢量，而当描述运动旋量时，则不需要，因为$S\dot{\theta }=V$，运动旋量可以看作是螺旋轴与角速度的乘积，就像旋转运动是单位旋转轴和角速度的乘积，只不过运动旋律是使用了单位螺旋轴。

4.刚体运动的矩阵指数和矩阵对数

和旋转运动一样，刚体运动也有对应的指数坐标表达，将$[S]\theta$看作是刚体运动齐次变换矩阵的李代数，即e的指数，同样使用泰勒展开为
$$\begin{gathered} e^{[S]\theta }=I+[S]\theta +[S{]}^2\frac{{\theta }^2}{2!}+[S{]}^3\frac{{\theta }^3}{3!}+... \\ =\left[\begin{array}{cc}{e}^{[\omega ]\theta }& G(\theta )v\\ 0& 1\end{array}\right] \end{gathered}$$
其中
$$\begin{gathered} G(\theta )=I\theta +[\omega ]\frac{{\theta }^2}{2!}+[\omega {]}^2\frac{{\theta }^3}{3!}+... \\ =I\theta +(1-cos(\theta ))[\omega ]+(\theta -sin(\theta ))[\omega {]}^2 \end{gathered}$$
即刚体运动的指数坐标表示，即矩阵指数为
$$e^{[S]\theta }=\left[\begin{array}{cc}{e}^{[\omega ]\theta }& [I\theta +(1-cos(\theta ))[\omega ]+(\theta -sin(\theta ))[\omega {]}^2]v\\ 0& 1\end{array}\right]$$
反求矩阵对数就很简单了，书上公式直接套即可，不再赘述。