机器人学中的微分运动

写在前面

微分运动指机构（例如机器人）的微小运动，可以用它来推导不同部件之间的速度关系。依据定义，微分运动就是微小的运动。因此，如果能够在一个很小的时间段内测量或者计算这个运动，就能得到速度关系。

为啥想讲讲这个微分运动呢，其实我的目的很简单，和雅可比矩阵联系上，进而帮助解决机械臂的逆运动学问题。当机器人关节做微量运动时，机器人手坐标系也会产生微量运动。而前面博客讲机器人雅可比矩阵已经讲到，这是机器人关节速度与末端机械手（末端坐标系）速度之间的映射，由于微分运动除以dt即可得到速度，因此可以说雅可比矩阵是机器人关节微分运动与末端微分运动之间的映射。当计算得到机器人末端坐标系的微分运动时，通过雅可比矩阵，则能计算得到各关节的微分运动，能解决逆运动学及速度控制的问题。以六轴机器人为例，
$$\left[\begin{array}{c}dx\\ dy\\ dz\\ \delta x\\ \delta y\\ \delta z\end{array}\right]=\left[\begin{array}{c}\\ \\ 机器人\\ 雅可比\\ 矩阵\\ \end{array}\right]\left[\begin{array}{c}d{\theta }_1\\ d{\theta }_2\\ d{\theta }_3\\ d{\theta }_4\\ d{\theta }_5\\ d{\theta }_6\end{array}\right]或者D=\mathbf{J}d\mathbf{\theta }\dots \dots （1）$$
其中，$D=[dx,dy,dz,\delta x,\delta y,\delta z]$表示六轴机器人末端机械手坐标的微分运动，前三项表示机械手坐标沿x，y，z轴的微分平移，后三项表示绕这三个轴的微分旋转；$d\theta$表示各关节的微分运动。

坐标系的微分运动

此处探讨的微分运动是基于坐标系而言的，再细化一下，对于本博客而言，是基于机器人末端坐标系而言的。

坐标系的微分运动可以分为：微分平移、微分旋转、微分变换（平移和旋转的组合）。其中微分旋转还分为绕参考轴xyz的微分旋转和绕一般轴q的微分旋转。

微分平移

很简单，就是坐标系原点平移一个微分量，和之前的运动学公式一致，只是将参数改为微分量，可以用$Trans(dx,dy,dz)$来表示，其含义是坐标系原点沿着x，y，z轴做了微小平移。

绕参考轴xyz的微分旋转

同样，与运动学公式一致，通常用$Rot(q,d\theta )$（其中q为x，y，z，对应的$d\theta$为$\delta x,\delta y,\delta z$）来表示。此处注意一点，由于旋转角度是微小量，故存在近似式
$$\begin{gathered} sin\delta x=\delta x(用弧度来表示) \\ cos\delta x=1\dots \dots （2） \end{gathered}$$
故可以得到表示绕x，y，z轴的微分旋转矩阵为
$$\mathrm{Rot}(x,\delta x)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& -\delta x& 0\\ 0& \delta x& 1& 0\\ 0& 0& 0& 1\end{array}\right],\mathrm{Rot}(y,\delta y)=\left[\begin{array}{cccc}1& 0& \delta y& 0\\ 0& 1& 0& 0\\ -\delta y& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],\mathrm{Rot}(z,\delta z)=\left[\begin{array}{cccc}1& -\delta z& 0& 0\\ \delta z& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\dots \dots （3）$$
例如$(\delta x{)}^2,\delta x\delta y$这样的高阶微分可忽略不计，故可将其略去，因此通过计算可以看出，几个微分运动以不同的顺序相乘可以得到同样的结果。因此在微分运动中，可认为相乘的顺序不重要。这与前面大角度旋转顺序不能交换的原则不一样。

绕一般轴q的微分旋转

上面分析可知，在微分运动中，任意相乘的顺序都能得到同样的结果。因此，可认为绕一般轴q的微分旋转运动可以表示以任意顺序绕3个坐标轴的3个微分旋转构成，如下
$$Rot(q,d\theta )=Rot(x,\delta x)Rot(y,\delta y)Rot(z,\delta z)\dots \dots （4）$$
同样，忽略所有高阶微分后，可得到
$$Rot(q,\mathrm{d}\theta )=Rot(x,\delta x)Rot(y,\delta y)Rot(z,\delta z)=\left[\begin{array}{cccc}1& -\delta z& \delta y& 0\\ \delta z& 1& -\delta x& 0\\ -\delta y& \delta x& 1& 0\\ 0& 0& 0& 1\end{array}\right]\dots \dots （5）$$

坐标系的微分变换

坐标系的微分变换是微分平移和以任意次序进行微分旋转的合成。用T表示微分变换前的坐标系位姿，这里默认T为相对于固定参考坐标系（基系）的位姿，下同，如果用位姿间的增量来进行表示的话，即用dT表示由于微分变换所引起的原坐标系T的变化量，$[dx,dy,dz,\delta x,\delta y,\delta z{]}^T$是相对固定参考系（基系）的，则可以得到我们定义的位姿微分变化量，有
$$[T+dT]=Trans(dx,dy,dz)Rot(q,d\theta )[T]\to [dT]=[Trans(dx,dy,dz)Rot(q,d\theta )-\mathbf{I}][T]\dots \dots （6）$$
公式（5）可写为
$$\begin{gathered} [dT]=[\Delta ][T] \\ [\Delta ]=[Trans(dx,dy,dz)Rot(q,d\theta )-\mathbf{I}]\dots \dots （7） \end{gathered}$$
我们将$\Delta$称为微分运动算子，坐标系的变化可以用微分运动算子乘以坐标系来实现，这个算子并不是我们之前讲的变换矩阵或者坐标系，它只是一个算子，使坐标系实现微分变化。其公式及具体计算式为
$$\begin{array}{rl}\Delta & =\mathrm{Trans}(\mathrm{d}x,\mathrm{d}y,\mathrm{d}z)\times \mathrm{Rot}(q,\mathrm{d}\theta )-I\\ & =\left[\begin{array}{llll}1& 0& 0& \mathrm{d}x\\ 0& 1& 0& \mathrm{d}y\\ 0& 0& 1& \mathrm{d}z\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{rrrr}1& -\delta z& \delta y& 0\\ \delta z& 1& -\delta x& 0\\ -\delta y& \delta x& 1& 0\\ 0& 0& 0& 1\end{array}\right]-\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\\ & =\left[\begin{array}{cccc}0& -\delta z& \delta y& dx\\ \delta z& 0& -\delta x& dy\\ -\delta y& \delta x& 0& \mathrm{d}z\\ 0& 0& 0& 0\end{array}\right]\end{array}\dots \dots (8)$$
前面讲到的$dT$为通过微分变换后原坐标系T的变化量，则结合$\Delta$，可以计算得到经过微分变换后坐标系新位姿为
$$T_{new}=dT+T_{old}（原坐标系T）\dots \dots （9）$$

微分运动算子：相对于固定参考坐标系、相对于自身坐标系

通常，我们将上面提到的、也就是公式（7）计算得到的微分运动算子$\Delta$定义为相对于固定参考坐标系U的微分运动算子，将其记为${}^U\Delta$。还存在另一种微分运动算子，即定义相对于自身坐标系、使得可以在该坐标系中计算出同样的变化，将其记为${}^T\Delta$。 因为微分算子${}^T\Delta$是相对于当前坐标系的，为了求出该坐标系的变化，必须右乘${}^T\Delta$，因为这两者都描述的是该坐标系中的相同变化，只是相对的坐标系不一样而已，因此结果都是相同的。于是有：
$$\begin{array}{rl}& [\mathrm{d}T]=[\Delta ][T]=[T]\left[{}^T\Delta \right]\\ \to & \left[T^{-1}\right][\Delta ][T]=\left[T^{-1}\right][T]{[}^T\Delta ]\\ \to & \left[{}^T\Delta \right]=\left[T^{-1}\right][\Delta ][T]\end{array}\dots \dots （10）$$
由式(10)可以计算得到相对于当前坐标系（本身坐标系）的微分算子。

上面谈到的都是坐标系的微分变化，这个坐标系可以是任意坐标系，包括机器人末端坐标系。下面的问题就是找出机器人关节的微分运动与机器人末端坐标系的微分运动是如何相关联的。从公式(1)中可以看出，向量D和微分算子$\Delta$中元素相同，由此可以得到向量D，再通过雅可比取逆计算，则可得到机器人关节的微分运动。

至此，已将坐标系的微分运动与机器人的微分运动相互联系了起来。