多个坐标系下的速度和角速度分析

1. 前言

刚体运动与齐次变换中介绍了刚体运动的位置表示方法，主要用于机器人的正逆运动学分析中。在开发机器人的过程中，不可避免的需要分析各个关节输出的力矩、速度等变量，这就是机器人的动力学分析所涉及的内容。

本文介绍多个坐标系下的速度和角速度关系，是动力学分析的基础。在力学课程中我们学过对位移的求导就可以得到速度，对机器人这种多坐标系结构的分析也是如此。这里主要考虑速度和角速度在参考坐标系和随体坐标系之间的变换关系。

本文结构安排如下，首先介绍斜对称矩阵，它可以简化旋转矩阵的求导过程；接着分析定点运动时刚体的角速度和旋转矩阵之间的关系；进一步的将该分析扩展到多个旋转坐标系之间；最后考虑坐标系之间具有相对位移的情形，分析齐次坐标下的速度角速度关系。

2. 斜对称矩阵

在分析定点转动刚体的各点速度之前，我们先介绍一个重要的矩阵——斜对称矩阵。它可以简化旋转矩阵的求导过程，可以方便的表示刚体转动的角速度。

定义1：对于一个$n \times n$的矩阵$S$，当且仅当下式满足时，是斜对称的(skew symmetric)。 $$ \begin{equation} \label{d1} S^T + S = 0 \end{equation} $$ 根据斜对称矩阵的定义式，可以推出其中每一个元素$s_{i,j}, i,j=1,2,..,n$都有$s_{i,j} = -s_{j,i}$，当$i=j$时，$s_{i,j} = 0$。

我们把所有$3 \times 3$的斜对称矩阵构成的集合称为$so(3)$。若$S \in so(3)$，那么其中只有三个独立变量。对于一个三维向量$\boldsymbol{v} = (x, y, z)^T$可以相应的写成斜对称矩阵的形式： $$ \begin{equation} \label{d2} S(\boldsymbol{v}) = \begin{bmatrix} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \end{bmatrix} \end{equation} $$ 三维的斜对称矩阵和旋转矩阵具有如下的性质：

对于任意的三维向量$\boldsymbol{a}, \boldsymbol{b} \in \mathbb{R}^3$，以及标量$\alpha, \beta$，有: $$ \begin{equation} \label{d3} S\left(\alpha \boldsymbol{a} + \beta \boldsymbol{b}\right) = \alpha S\left(\boldsymbol{a}\right) + \beta S\left(\boldsymbol{b}\right) \end{equation} $$
对于任意三维向量$\boldsymbol{a}, \boldsymbol{b} \in \mathbb{R}^3$有： $$ \begin{equation} \label{d4} S\left(\boldsymbol{a}\right)\boldsymbol{b} = \boldsymbol{a} \times \boldsymbol{b} \end{equation} $$
对于三维旋转矩阵$R \in SO(3)$和任意三维向量$\boldsymbol{a}, \boldsymbol{b} \in \mathbb{R}^3$有： $$ \begin{equation} \label{d5} R\left(\boldsymbol{a} \times \boldsymbol{b}\right) = R\boldsymbol{a} \times R\boldsymbol{b} \end{equation} $$
对于三维旋转矩阵$R \in SO(3)$和任意三维向量$\boldsymbol{a} \in \mathbb{R}^3$有： $$ \begin{equation} \label{d6} RS(\boldsymbol{a})R^T = S(R\boldsymbol{a}) \end{equation} $$ 式($\ref{d6}$)的几何意义在于$S(\boldsymbol{a})$在旋转$R$后的坐标系中的表示与向量$\boldsymbol{a}$旋转$R$后的斜对称化是一样的。

3. 定点转动刚体的角速度分析

在刚体运动与齐次变换中我们介绍过，若刚体在空间中定点转动，在刚体上建立随体坐标系$O_1$，那么刚体上任意一点$p$在参考坐标系$O_0$下的坐标可以通过左乘一个旋转矩阵$R_1^0$得到。 $$ \begin{equation} \label{f1} \boldsymbol{p^0} = R_1^0 \boldsymbol{p^1} \end{equation} $$ 其中$\boldsymbol{p^0}, \boldsymbol{p^1}$分别为点$p$在$O_0$和$O_1$下的坐标。对式($\ref{f1}$)两端同时求导就可以得到点$p$的速度： $$ \boldsymbol{\dot{p^0}} = \dot{R_1^0}\boldsymbol{p^1} + R_1^0\boldsymbol{\dot{p^1}} $$ 而刚体中的各点相对于随体坐标系是不会发生位移的，即相对速度为0，由此可以得到点$p$的绝对速度与旋转矩阵的关系如下： $$ \begin{equation} \label{f2} \boldsymbol{\dot{p^0}} = \dot{R_1^0}\boldsymbol{p^1} \end{equation} $$ 我们刚体的每个定点转动瞬间都可以归结为绕一转轴$\boldsymbol{k}=[k_x, k_y, k_z]^T$转动了$\theta$角度。旋转矩阵则可以用来描述这一运动过程，是一种特殊的正交矩阵，即$R_1^0 \in SO(3)$，其逆矩阵就是它的转置。 $$ \begin{equation} \label{f3} R_1^0\left(\theta\right){R_1^0\left(\theta\right)}^T = I \end{equation} $$ 对式($\ref{f3}$)两端同时关于$\theta$求导： $$ \begin{equation} \label{f4} \frac{dR_1^0}{d\theta}{R_1^0\left(\theta\right)}^T + R_1^0\left(\theta\right)\frac{d{R_1^0}^T}{d\theta} = 0 \end{equation} $$ 定义矩阵$s$: $$ \begin{equation} \label{f5} s=\frac{dR_1^0}{d\theta}{R_1^0\left(\theta\right)}^T \end{equation} $$ 那么式($\ref{f4}$)可以写为： $$ \begin{equation} \label{f6} s+ s^T = 0 \end{equation} $$ 可见$s$是一个$3\times3$的斜对称矩阵(Skew Symmetric Matrix)，记为$s \in so(3)$。因此，我们有： $$ \begin{equation} \label{f7} \frac{dR_1^0}{d\theta} = sR_1^0(\theta) \end{equation} $$ 实际上对于任意转轴$\boldsymbol{k}=[k_x, k_y, k_z]^T$，都有 (实际上我没有证明过下面这个式子，不过看到一些参考书中都是这么用的，在微分运动中给出一个推导，不是严格的证明，可供参考) $$ s = S(\boldsymbol{k}) = \begin{bmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0 \end{bmatrix} $$ 将旋转矩阵$R_1^0$表示为绕$\boldsymbol{k}$轴转动$\theta$角的形式$R_{\boldsymbol{k},\theta}$，那么式($\ref{f7}$)改写为: $$ \begin{equation} \label{f8} \frac{dR_{\boldsymbol{k},\theta}}{d\theta} = S(\boldsymbol{k})R_{\boldsymbol{k},\theta} \end{equation} $$ 因此旋转矩阵相对于时间的导数可以按照如下的方式求得： $$ \begin{equation} \label{f9} \dot{R_{\boldsymbol{k},\theta}} = \frac{dR_{\boldsymbol{k},\theta}}{dt} = \frac{dR_{\boldsymbol{k},\theta}}{d\theta} \frac{d\theta}{dt} = \dot{\theta}S(\boldsymbol{k})R_{\boldsymbol{k},\theta} = S(\boldsymbol{\omega})R_{\boldsymbol{k},\theta} \end{equation} $$ 其中$\boldsymbol{\omega} = \dot{\theta}\boldsymbol{k}$为刚体的角速度，是一种自由向量。将式($\ref{f9}$)代入式($\ref{f2}$)得 $$ \begin{equation} \label{f10} \boldsymbol{\dot{p^0}} = S(\boldsymbol{\omega})R_{\boldsymbol{k},\theta}\boldsymbol{p^1} = S(\boldsymbol{\omega})\boldsymbol{p^0} = \boldsymbol{\omega} \times \boldsymbol{p^0} \end{equation} $$ 这正是角速度与速度之间的关系式。

4. 多个相对坐标系下的刚体转动角速度关系

在对机器人进行动力学分析时，往往会涉及到多个相对坐标系下刚体的角速度关系。我们现在分析两个相对坐标系$O_1$和$O_2$的情况，更多的相对坐标系都可以归结到这种情形。

根据旋转矩阵的链乘关系，$O_1,O_2$与参考坐标系$O_0$之间的存在如下关系： $$ R_2^0 = R_1^0R_2^1 $$ 对两边求导得到： $$ \begin{equation} \label{a1} \dot{R_2^0} = \dot{R_1^0}R_2^1 + R_1^0\dot{R_2^1} \end{equation} $$ 根据公式($\ref{f9}$)，上式的左边可以写作： $$ \begin{equation} \label{a2} \dot{R_2^0} = S(\boldsymbol{\omega_{0,2}^0})R_2^0 \end{equation} $$ 其中$\boldsymbol{\omega_{0,2}^0}$表示相对坐标系$O_2$在参考坐标系下的旋转角速度，是$R_2^1$和$R_1^0$共同作用的结果。考察式($\ref{a1}$)右边的第一项， $$ \begin{align} \label{a3} \dot{R_1^0}R_2^1 & = S(\boldsymbol{\omega_{0,1}^0})R_1^0R_2^1 \\ & = S(\boldsymbol{\omega_{0,1}^0})R_2^0 \end{align} $$ 其中$\boldsymbol{\omega_{0,1}^0}$表示相对坐标系$O_1$在参考坐标系下的旋转角速度。现在再考察式($\ref{a1}$)右边的第二项， $$ \begin{align} \label{a4} R_1^0\dot{R_2^1} & = R_1^0S(\boldsymbol{\omega_{1,2}^1})R_2^1 \\ & = R_1^0S(\boldsymbol{\omega_{1,2}^1}){R_1^0}^T R_1^0 R_2^1 \\ & = S(R_1^0\boldsymbol{\omega_{1,2}^1})R_2^0 \end{align} $$ 其中$\boldsymbol{\omega_{1,2}^1}$表示$O_2$系相对于$O_1$系运动的角速度矢量，左乘$R_1^0$则将该自由向量变换到$O_0$系下。联立公式($\ref{a1}, \ref{a2}, \ref{a3}, \ref{a4}$)，得到角速度关系： $$ \begin{align} \label{a5} S(\boldsymbol{\omega_{0,2}^0})R_2^0 & = \left(S(\boldsymbol{\omega_{0,1}^0}) + S(R_1^0\boldsymbol{\omega_{1,2}^1})\right)R_2^0 \\ \boldsymbol{\omega_{0,2}^0} & = \boldsymbol{\omega_{0,1}^0} + R_1^0\boldsymbol{\omega_{1,2}^1} \\ & = \boldsymbol{\omega_{0,1}^0} + \boldsymbol{\omega_{1,2}^0} \end{align} $$ 对于一个具有$n$个相对坐标系的系统，有角速度矢量关系： $$ \begin{align} \label{a6} \boldsymbol{\omega_{0,n}^0} & = \boldsymbol{\omega_{0,1}^0} + R_1^0\boldsymbol{\omega_{1,2}^1} + ... + R_{n-1}^0\boldsymbol{\omega_{n-1,n}^{n-1}} \\ & = \boldsymbol{\omega_{0,1}^0} + \boldsymbol{\omega_{1,2}^0} + ... + \boldsymbol{\omega_{n-1,n}^0} \end{align} $$

5. 齐次坐标系下刚体运动的速度和角速度分析

在本文的第3/4部分中，只讨论了各个坐标系之间的相对转动关系。本部分考虑各个坐标系之间的相对位移，借助齐次矩阵得到多坐标系系统下的速度和角速度的一般关系。

根据齐次变换我们知道，刚体上一点$p$在随体坐标系和参考坐标系下的坐标$p^1,p^0$具有如下关系： $$ \begin{equation} \label{b1} \begin{bmatrix} \boldsymbol{p^0} \\ 1 \end{bmatrix} = \begin{bmatrix} R_1^0 & \boldsymbol{d_1^0} \\ \boldsymbol{0} & 1 \end{bmatrix} \begin{bmatrix} \boldsymbol{p^1} \\ 1 \end{bmatrix} \end{equation} $$ 因此，点$p$在参考坐标系下的坐标可以写作: $$ \begin{equation} \label{b2} \boldsymbol{p^0} = R_1^0 \boldsymbol{p^1} + \boldsymbol{d_1^0} \end{equation} $$ 对两边求导可以得到： $$ \begin{align} \label{b3} \dot{\boldsymbol{p^0}} & = \dot{R_1^0} \boldsymbol{p^1} + \dot{\boldsymbol{d_1^0}} \\ & = S(\boldsymbol{\omega_{0,1}^0})R_1^0 \boldsymbol{p^1} + \dot{\boldsymbol{d_1^0}} \\ & = \boldsymbol{\omega_{0,1}^0} \times \boldsymbol{p^0} + \boldsymbol{v} \end{align} $$ 其中，$\boldsymbol{\omega_{0,1}^0}, \boldsymbol{v}$分别为刚体运动的角速度和随体坐标平移的速度。

6. 总结

通过以上的分析，我们知道了在三维空间下，左乘一个斜对称矩阵相当于和一个向量做叉积运算。而旋转矩阵的导数可以通过由角速度矢量构建的斜对称矩阵与原来的旋转矩阵相乘得到。在多个坐标系下的旋转角速度可以依次把相对角速度变换到全局的参考坐标系下求和得到。最后，我们分析了随体坐标系发生位移的情况，可以分为角速度与位移的叉积产生的线速度和坐标系平移速度两个部分。