【机器人学】串联机械臂连杆的速度及加速度推导

1. 反对称矩阵及其性质

定义一个（$n$×$n$）的矩阵$\text{S}$，如果满足$\text{S}+{\text{S}}^T=\text{O}$，则矩阵$\text{S}$被称为反对称矩阵。
对于三维向量$\text{a}=(a_x,a_y,a_z{)}^T$，定义其对应的反对称矩阵$\text{S}(a)$为如下形式：$$\text{S}(\mathbf{a})=\left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right]$$反对称矩阵具有以下几个性质：

1. $\text{S}(\alpha \text{a}+\beta \text{b})=\alpha \text{S}(\text{a})+\beta \text{S}(\text{b})，\mathbf{a},\mathbf{b}\in {\mathbf{R}}^3$
2. $\text{S}(\text{a})\text{p}=\text{a}\times \text{p}，\text{a},\text{p}\in {\text{R}}^3$
3. $\text{R}\text{S}(\text{a}){\text{R}}^T=\text{S}(\text{Ra})，\text{a}\in {\text{R}}^3，\text{R}\in SO(3)$
4. ${\text{X}}^T\text{S}\text{X}=0，\text{X}\in {\text{R}}^n$

习惯上使用符号$SO(n)$表示$n$阶的$n\times n$的特殊正交群。对于任意的$\mathbf{R}\in \mathbf{S}\mathbf{O}(\mathbf{n})$，以下性质成立：

• ${\mathbf{R}}^{\mathbf{T}}={\text{R}}^{-1}\in \mathbf{S}\mathbf{O}(\mathbf{n})$
• $\mathbf{R}$ 的各列（各行）也是相互正交的
• $\mathbf{R}$ 的各列（各行）都是单位向量
• $det(\text{R})=1$

2. 旋转矩阵的导数

考虑时变的旋转矩阵$\text{R}(t)$，基于$\text{R}(t)$的正交性有：$$\text{R}(t)\text{R}(t{)}^T=\text{I}\text{\hspace{1em}(2-1)}$$求上式关于时间的导数有：$$\dot{\text{R}}(t)\text{R}(t{)}^T+\text{R}(t)\dot{\text{R}}(t{)}^T=\text{O}\text{\hspace{1em}(2-2)}$$令$$\text{S}(t)=\dot{\text{R}}(t)\text{R}(t{)}^T\text{\hspace{1em}(2-3)}$$由于$$\text{S}(t)+{\text{S}}^T(t)=\text{O}\text{\hspace{1em}(2-4)}$$因此$\text{S}$为（3×3）的反对称矩阵。将公式（2-3）两边同时右乘$\text{R}(t)$，有$$\dot{\text{R}}(t)=\text{S}(t)\text{R}(t)\text{\hspace{1em}(2-5)}$$即$\text{R}(t)$对时间的导数可以表示成$\text{R}(t)$自身的函数。
————————————————————————
将$\text{R}(t)$对时间的导数$\dot{\text{R}}(t)$写成以下形式：$$\dot{\text{R}}(t)=\text{S}(\text{ω}(t))\text{R}(t)\text{\hspace{1em}(2-5)}$$其中$\text{S}(\omega (t))$是反对称矩阵，为了证明向量$\text{ω}(t)$是$t$时刻旋转坐标系相对于固定坐标系的角速度。考虑固连在一个移动坐标系上的点$p$，点$p$相对于固定坐标系的坐标可以由${\text{p}}_1=\text{R}{\text{p}}_0$，对该表达式求微分，可以得到：$$\frac{d}{dt}{\text{p}}_1=\dot{\text{R}}{\text{p}}_0=\text{S}(\text{ω})\text{R}{\text{p}}_0=\text{ω}\times \text{R}{\text{p}}_0=\text{ω}\times {\text{p}}_1$$上式表明向量$\text{ω}(t)$就是传统意义上的角速度向量。

3. 连杆速度

3.1. 通用连杆线速度推导

考虑具有开式运动链的机械手的通用连杆$i$。连杆$i$连接关节$i$和关节$i+1$，坐标系$i$固连于连杆$i$，且其原点在关节$i$的轴上，坐标系$i+1$的原点在关节$i+1$的轴上，如图。

令${\text{P}}_i$和${\text{P}}_{i+1}$分别为坐标系$i$和坐标系$i+1$在基坐标系${\text{O}}_o$中的位置向量。令 ${\text{r}}_{i,i+1}^i$为坐标系$i+1$的原点关于坐标系$i$的位置在坐标系$i$中的表示。令 ${\text{R}}_i^0$ 为坐标系$i$相对于基坐标系${\text{O}}_o$的旋转矩阵。因此有以下关系成立：$${\text{P}}_{i+1}^0={\text{P}}_i^0+{\text{R}}_i^0{\text{r}}_{i,i+1}^i\text{\hspace{1em}(3-1)}$$结合反对称矩阵的相关性质对上式求导有：$$\begin{array}{rl}{\dot{\text{P}}}_{i+1}^0& ={\dot{\text{P}}}_i^0+{\dot{\text{R}}}_i^0{\text{r}}_{i,i+1}^i+{\text{R}}_i^0{\dot{\text{r}}}_{i,i+1}^i\\ & ={\dot{\text{P}}}_i^0+\text{S}({\text{ω}}_i^0){\text{R}}_i^0{\text{r}}_{i,i+1}^i+{\text{R}}_i^0{\dot{\text{r}}}_{i,i+1}^i\\ & ={\dot{\text{P}}}_i^0+{\text{ω}}_i^0\times {\text{R}}_i^0{\text{r}}_{i,i+1}^i+{\text{R}}_i^0{\dot{\text{r}}}_{i,i+1}^i\\ & ={\dot{\text{P}}}_i^0+{\text{ω}}_i^0\times {\text{r}}_{i,i+1}^0+{\text{R}}_i^0{\dot{\text{r}}}_{i,i+1}^i\end{array}\text{\hspace{1em}(3-2)}$$$$\begin{array}{rl}{\text{v}}_{i+1}^0& ={\text{v}}_i^0+{\text{ω}}_i^0\times {\text{r}}_{i,i+1}^0+{\text{v}}_{i,i+1}^0\end{array}\text{\hspace{1em}(3-3)}$$其中${\text{v}}_{i,i+1}^0$表示以基坐标系${\text{O}}_o$为参考，坐标系$i+1$的原点相对于坐标系$i$的原点的移动速度。

3.2. 通用连杆角速度推导

考虑以下旋转矩阵的转换关系：$${\text{R}}_{i+1}^0={\text{R}}_i^0{\text{R}}_{i+1}^i$$求其关于时间的导数：$${\dot{\text{R}}}_{i+1}^0={\dot{\text{R}}}_i^0{\text{R}}_{i+1}^i+{\text{R}}_i^0{\dot{\text{R}}}_{i+1}^i$$$$\text{S}({\text{ω}}_{i+1}^0){\text{R}}_{i+1}^0=\text{S}({\text{ω}}_i^0){\text{R}}_i^0{\text{R}}_{i+1}^i+{\text{R}}_i^0\text{S}({\text{ω}}_{i,i+1}^i){\text{R}}_{i+1}^i$$$$\text{S}({\text{ω}}_{i+1}^0){\text{R}}_{i+1}^0=\text{S}({\text{ω}}_i^0){\text{R}}_{i+1}^0+{\text{R}}_i^0\text{S}({\text{ω}}_{i,i+1}^i)({\text{R}}_i^0{)}^T{\text{R}}_i^0{\text{R}}_{i+1}^i$$$$\text{S}({\text{ω}}_{i+1}^0){\text{R}}_{i+1}^0=\text{S}({\text{ω}}_i^0){\text{R}}_{i+1}^0+{\text{R}}_i^0\text{S}({\text{ω}}_{i,i+1}^i)({\text{R}}_i^0{)}^T{\text{R}}_{i+1}^0$$$$\text{S}({\text{ω}}_{i+1}^0)=\text{S}({\text{ω}}_i^0)+{\text{R}}_i^0\text{S}({\text{ω}}_{i,i+1}^i)({\text{R}}_i^0{)}^T$$$$\text{S}({\text{ω}}_{i+1}^0)=\text{S}({\text{ω}}_i^0)+\text{S}({\text{R}}_i^0{\text{ω}}_{i,i+1}^i)$$$$\text{S}({\text{ω}}_{i+1}^0)=\text{S}({\text{ω}}_i^0)+\text{S}({\text{ω}}_{i,i+1}^0)$$$$\text{S}({\text{ω}}_{i+1}^0)=\text{S}({\text{ω}}_i^0+{\text{ω}}_{i,i+1}^0)$$从而可以将连杆$i+1$的角速度表示为连杆$i$的角速度以及连杆$i+1$关于连杆$i$的角速度的函数：$${\text{ω}}_{i+1}^0={\text{ω}}_i^0+{\text{ω}}_{i,i+1}^0\text{\hspace{1em}(3-4)}$$

3.3. 不同关节类型的连杆速度

对于移动关节$\text{(i+1)}$而言，坐标系$i+1$关于坐标系$i$的方向不变，因此有$${\text{ω}}_{i,i+1}^0=0$$$${\text{ω}}_{i+1}^0={\text{ω}}_i^0+{\text{ω}}_{i,i+1}^0={\text{ω}}_i^0$$进一步，线速度为$${\text{v}}_{i,i+1}^0={\dot{d}}_{i+1}{\text{z}}_{i+1}$$$$\begin{array}{rl}{\text{v}}_{i+1}^0& ={\text{v}}_i^0+{\text{ω}}_i^0\times {\text{r}}_{i,i+1}^0+{\text{v}}_{i,i+1}^0\\ & ={\text{v}}_i^0+{\text{ω}}_i^0\times {\text{r}}_{i,i+1}^0+{\dot{d}}_{i+1}{\text{z}}_{i+1}\end{array}$$
对于旋转关节$\text{(i+1)}$而言，角速度满足以下关系$${\text{ω}}_{i,i+1}^0={\dot{\theta }}_{i+1}{\text{z}}_{i+1}$$$${\text{ω}}_{i+1}^0={\text{ω}}_i^0+{\text{ω}}_{i,i+1}^0={\text{ω}}_i^0+{\dot{\theta }}_{i+1}{\text{z}}_{i+1}$$关节$i+1$的运动引起了坐标系$i+1$相对于坐标系$i$的旋转，但坐标系$i+1$的原点相对于坐标系$i$的原点并没有发生移动，移动速度为0，因此对于线速度有以下关系$${\text{v}}_{i,i+1}^0=0$$$${\text{v}}_{i+1}^0={\text{v}}_i^0+{\text{ω}}_i^0\times {\text{r}}_{i,i+1}^0$$

下文中在表示有关坐标系0时，将省略上标“0”.

综上，结合式(3-2),(3-3)，可得到如下的速度表达式$${\text{ω}}_{i+1}=\{\begin{array}{ll}{\text{ω}}_i+{\dot{\theta }}_{i+1}{\text{z}}_{i+1},& \text{if Ji+1 is revolute}\\ {\text{ω}}_i,& \text{if Ji+1 is prismatic}\end{array}\text{\hspace{1em}(3-5)}$$$${\text{v}}_{i+1}=\{\begin{array}{ll}{\text{v}}_i+{\text{ω}}_i\times {\text{r}}_{i,i+1},& \text{if Ji+1 is revolute}\\ {\text{v}}_i+{\text{ω}}_i\times {\text{r}}_{i,i+1}+{\dot{d}}_{i+1}{\text{z}}_{i+1},& \text{if Ji+1 is prismatic}\end{array}\text{\hspace{1em}(3-6)}$$
对上述的速度表达式两边同时左乘${\text{R}}_0^{i+1}$，可推导出相邻坐标系之间的速度转换关系：$${\text{ω}}_{i+1}^{i+1}=\{\begin{array}{ll}{\text{R}}_i^{i+1}{\text{ω}}_i^i+{\dot{\theta }}_{i+1}{\text{z}}_{i+1}^{i+1},& \text{if Ji+1 is revolute}\\ {\text{R}}_i^{i+1}{\text{ω}}_i^i,& \text{if Ji+1 is prismatic}\end{array}\text{\hspace{1em}(3-7)}$$$${\text{v}}_{i+1}^{i+1}=\{\begin{array}{ll}{\text{R}}_i^{i+1}({\text{v}}_i^i+{\text{ω}}_i^i\times {\text{r}}_{i+1}^i),& \text{if Ji+1 is revolute}\\ {\text{R}}_i^{i+1}({\text{v}}_i^i+{\text{ω}}_i^i\times {\text{r}}_{i+1}^i)+{\dot{d}}_{i+1}{\text{z}}_{i+1}^{i+1},& \text{if Ji+1 is prismatic}\end{array}\text{\hspace{1em}(3-8)}$$

4. 连杆加速度

关节$i+1$是旋转关节，${\text{ω}}_{i+1}^{i+1}={\text{R}}_i^{i+1}{\text{ω}}_i^i+{\dot{\theta }}_{i+1}{\text{z}}_{i+1}^{i+1}$，求其关于时间的导数：$$\begin{array}{rl}{\dot{\text{ω}}}_{i+1}^{i+1}& ={\text{R}}_i^{i+1}{\dot{\text{ω}}}_i^i+{\ddot{\theta }}_{i+1}{\text{z}}_{i+1}^{i+1}+{\dot{\theta }}_{i+1}{\dot{\text{z}}}_{i+1}^{i+1}\\ & ={\text{R}}_i^{i+1}{\dot{\text{ω}}}_i^i+{\ddot{\theta }}_{i+1}{\text{z}}_{i+1}^{i+1}+{\dot{\theta }}_{i+1}\text{S}({\text{ω}}_i^{i+1}){\text{z}}_{i+1}^{i+1}\\ & ={\text{R}}_i^{i+1}{\dot{\text{ω}}}_i^i+{\ddot{\theta }}_{i+1}{\text{z}}_{i+1}^{i+1}+{\dot{\theta }}_{i+1}({\text{R}}_i^{i+1}{\text{ω}}_i^i\times {\text{z}}_{i+1}^{i+1})\\ & ={\text{R}}_i^{i+1}{\dot{\text{ω}}}_i^i+{\ddot{\theta }}_{i+1}{\text{z}}_{i+1}^{i+1}+{\text{R}}_i^{i+1}{\text{ω}}_i^i\times {\dot{\theta }}_{i+1}{\text{z}}_{i+1}^{i+1}\end{array}\text{\hspace{1em}(4-1)}$$

关节$i+1$是旋转关节，${\text{v}}_{i+1}^{i+1}={\text{R}}_i^{i+1}({\text{v}}_i^i+{\text{ω}}_i^i\times {\text{r}}_{i+1}^i)$，求其关于时间的导数：$$\begin{array}{rl}{\dot{\text{v}}}_{i+1}^{i+1}& ={\text{R}}_i^{i+1}({\dot{\text{v}}}_i^i+{\dot{\text{ω}}}_i^i\times {\text{r}}_{i+1}^i+{\text{ω}}_i^i\times {\dot{\text{r}}}_{i+1}^i)\\ & ={\text{R}}_i^{i+1}({\dot{\text{v}}}_i^i+{\dot{\text{ω}}}_i^i\times {\text{r}}_{i+1}^i+{\text{ω}}_i^i\times \text{S}({\text{ω}}_i^i){\text{r}}_{i+1}^i)\\ & ={\text{R}}_i^{i+1}({\dot{\text{v}}}_i^i+{\dot{\text{ω}}}_i^i\times {\text{r}}_{i+1}^i+{\text{ω}}_i^i\times ({\text{ω}}_i^i\times {\text{r}}_{i+1}^i))\end{array}\text{\hspace{1em}(4-2)}$$

关节$i+1$是移动关节，${\text{ω}}_{i+1}^{i+1}={\text{R}}_i^{i+1}{\text{ω}}_i^i$，求其关于时间的导数：$${\dot{\text{ω}}}_{i+1}^{i+1}={\text{R}}_i^{i+1}{\dot{\text{ω}}}_i^i\text{\hspace{1em}(4-3)}$$

关节$i+1$是移动关节，${\text{v}}_{i+1}^{i+1}={\text{R}}_i^{i+1}({\text{v}}_i^i+{\text{ω}}_i^i\times {\text{r}}_{i+1}^i)+{\dot{d}}_{i+1}{\text{z}}_{i+1}^{i+1}$，求其关于时间的导数：$$\begin{array}{rl}{\dot{\text{v}}}_{i+1}^{i+1}& ={\text{R}}_i^{i+1}({\dot{\text{v}}}_i^i+{\dot{\text{ω}}}_i^i\times {\text{r}}_{i+1}^i+{\text{ω}}_i^i\times {\dot{\text{r}}}_{i+1}^i)+{\ddot{d}}_{i+1}{\text{z}}_{i+1}^{i+1}+{\dot{d}}_{i+1}{\dot{\text{z}}}_{i+1}^{i+1}\\ & ={\text{R}}_i^{i+1}({\dot{\text{v}}}_i^i+{\dot{\text{ω}}}_i^i\times {\text{r}}_{i+1}^i+{\text{ω}}_i^i\times (\text{S}({\text{ω}}_i^i){\text{r}}_{i+1}^i+{\dot{d}}_{i+1}{\text{z}}_{i+1}^i))+{\ddot{d}}_{i+1}{\text{z}}_{i+1}^{i+1}+{\dot{d}}_{i+1}(\text{S}({\text{ω}}_i^{i+1}){\text{z}}_{i+1}^{i+1})\\ & ={\text{R}}_i^{i+1}({\dot{\text{v}}}_i^i+{\dot{\text{ω}}}_i^i\times {\text{r}}_{i+1}^i+{\text{ω}}_i^i\times ({\text{ω}}_i^i\times {\text{r}}_{i+1}^i))+{\text{R}}_i^{i+1}({\text{ω}}_i^i\times {\dot{d}}_{i+1}{\text{z}}_{i+1}^{i+1})+{\ddot{d}}_{i+1}{\text{z}}_{i+1}^{i+1}+{\dot{d}}_{i+1}({\text{R}}_i^{i+1}{\text{ω}}_i^i\times {\text{z}}_{i+1}^{i+1})\\ & ={\text{R}}_i^{i+1}({\dot{\text{v}}}_i^i+{\dot{\text{ω}}}_i^i\times {\text{r}}_{i+1}^i+{\text{ω}}_i^i\times ({\text{ω}}_i^i\times {\text{r}}_{i+1}^i))+({\text{ω}}_{i+1}^{i+1}\times {\dot{d}}_{i+1}{\text{z}}_{i+1}^{i+1})+{\ddot{d}}_{i+1}{\text{z}}_{i+1}^{i+1}+({\text{ω}}_{i+1}^{i+1}\times {\dot{d}}_{i+1}{\text{z}}_{i+1}^{i+1})\\ & ={\text{R}}_i^{i+1}({\dot{\text{v}}}_i^i+{\dot{\text{ω}}}_i^i\times {\text{r}}_{i+1}^i+{\text{ω}}_i^i\times ({\text{ω}}_i^i\times {\text{r}}_{i+1}^i))+{\ddot{d}}_{i+1}{\text{z}}_{i+1}^{i+1}+2{\text{ω}}_{i+1}^{i+1}\times {\dot{d}}_{i+1}{\text{z}}_{i+1}^{i+1}\end{array}\text{\hspace{1em}(4-4)}$$
综上，可得到如下的加速度表达式$${\dot{\text{ω}}}_{i+1}^{i+1}=\{\begin{array}{ll}{\text{R}}_i^{i+1}{\dot{\text{ω}}}_i^i+{\ddot{\theta }}_{i+1}{\text{z}}_{i+1}^{i+1}+{\text{R}}_i^{i+1}{\text{ω}}_i^i\times {\dot{\theta }}_{i+1}{\text{z}}_{i+1}^{i+1},& \text{if Ji+1 is revolute}\\ {\text{R}}_i^{i+1}{\dot{\text{ω}}}_i^i,& \text{if Ji+1 is prismatic}\end{array}$$$${\dot{\text{v}}}_{i+1}^{i+1}=\{\begin{array}{ll}{\text{R}}_i^{i+1}({\dot{\text{v}}}_i^i+{\dot{\text{ω}}}_i^i\times {\text{r}}_{i+1}^i+{\text{ω}}_i^i\times ({\text{ω}}_i^i\times {\text{r}}_{i+1}^i)),& \text{if Ji+1 is revolute}\\ {\text{R}}_i^{i+1}({\dot{\text{v}}}_i^i+{\dot{\text{ω}}}_i^i\times {\text{r}}_{i+1}^i+{\text{ω}}_i^i\times ({\text{ω}}_i^i\times {\text{r}}_{i+1}^i))+{\ddot{d}}_{i+1}{\text{z}}_{i+1}^{i+1}+2{\text{ω}}_{i+1}^{i+1}\times {\dot{d}}_{i+1}{\text{z}}_{i+1}^{i+1},& \text{if Ji+1 is prismatic}\end{array}$$