机器人数学基础

机械手是机器人系统的机械运动部分，它的执行机构是用来保证复杂空间运动的综合刚体，而它也往往作为统一体进行运动.因此，需要用一种简便的数学方法来描述单一刚体的位移、速度、加速度和运动学的问题.这种数学描述不是唯一的，不同的人可能采用不同的方法.这里采用矩阵法来描述机器人机械手的运动学问题.这种数学方法是以四阶方阵变换三维空间的齐次坐标为基础的，能够将运动、变换和映射与矩阵运算联系起来.接下来让我们看看是如何做到的.

1 位姿和坐标系描述

1.1 位置描述

对于直角坐标系{$A$}，空间任一点$p$的位置可用$3\times 1$的列矢量${}^A\mathbf{p}$表示，上标$A$代表参考坐标系{$A$}，称${}^A\mathbf{p}$为位置矢量.
$${}^A\mathbf{p}=\left[\begin{array}{r}{p}_x\\ p_y\\ p_z\end{array}\right]$$

1.2 方位描述

物体的方位(orientation)可由某个固接于此物体的坐标系描述.为了规定空间某刚体$B$的方位，设置一直角坐标系{$B$}与此刚体固接.用坐标系{$B$}的三个单位主矢量${\mathbf{x}}_B,{\mathbf{y}}_B,{\mathbf{z}}_B$相对于参考坐标系{$A$}的方向余弦组成的$3\times 3$旋转矩阵${}_B^A\mathbf{R}$来表示刚体$B$相对于坐标系{$A$}的方位.式中，上标$A$代表参考坐标系{$A$}，下标$B$代表被描述的坐标系{$B$}.
$${}_B^A\mathbf{R}=\left[\begin{array}{ccc}{}^A{\mathbf{x}}_B& {}^A{\mathbf{y}}_B& {}^A{\mathbf{z}}_B\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{23}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

${}_B^A\mathbf{R}$共有9个元素，但只有3个是独立的，由于${}_B^A\mathbf{R}$的三个列矢量${}^A{\mathbf{x}}_B{,}^A{\mathbf{y}}_B{和}^A{\mathbf{z}}_B$都是单位矢量，且两两正交，因而它的9个元素满足6个约束条件：
$${}^A{\mathbf{x}}_B\cdot {}^A{\mathbf{x}}_B={}^A{\mathbf{y}}_B\cdot {}^A{\mathbf{y}}_B={}^A{\mathbf{z}}_B\cdot {}^A{\mathbf{z}}_B=1$$

$${}^A{\mathbf{x}}_B\cdot {}^A{\mathbf{y}}_B={}^A{\mathbf{y}}_B\cdot {}^A{\mathbf{z}}_B={}^A{\mathbf{z}}_B\cdot {}^A{\mathbf{x}}_B=0$$

可见，旋转矩阵${}_B^A\mathbf{R}$是正交的，并且满足条件：
$$\begin{gathered} {}_B^A{\mathbf{R}}^{-1}={}_B^A{\mathbf{R}}^{\mathrm{T}} \\ \left|\begin{array}{c}{}_B^A\mathbf{R}\end{array}\right|=1 \end{gathered}$$
对应于轴$x,y或z$做转角$\theta$的旋转变换，其旋转矩阵分别如下，其中，$s$表示$\sin$，$c$表示$\cos$.
$$\begin{gathered} \mathbf{R}(x,\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& c\theta & -s\theta \\ 0& s\theta & c\theta \end{array}\right] \\ \mathbf{R}(y,\theta )=\left[\begin{array}{ccc}c\theta & 0& s\theta \\ 0& 1& 0\\ -s\theta & 0& c\theta \end{array}\right] \\ \mathbf{R}(z,\theta )=\left[\begin{array}{ccc}c\theta & -s\theta & 0\\ s\theta & c\theta & 0\\ 0& 0& 1\end{array}\right] \end{gathered}$$

1.3 位姿描述

要完全描述刚体$B$在空间的位姿（位置和姿态），通常将物体$B$与某一坐标系{$B$}固接。{$B$}的坐标原点一般选在物体$B$的的特征点，如质心等.相对参考系{$A$}，坐标系{$B$}的原点位置和坐标轴的方位分别由位置矢量${}^A{\mathbf{p}}_{B_o}$和旋转矩阵${}_B^A\mathbf{R}$描述.这样，刚体$B$的位姿可由坐标系{$B$}来描述，即有：
$$\left\{\begin{array}{r}B\end{array}\right\}=\left\{\begin{array}{rl}{}_B^A\mathbf{R}& {}^A{\mathbf{p}}_{B_o}\end{array}\right\}$$
当表示位置时，旋转矩阵${}_B^A\mathbf{R}=\mathbf{I}$(单位矩阵)；当表示方位时，位置矢量${}^A{\mathbf{p}}_{B_o}=0$.

2 平移和旋转直角坐标系变换

2.1 平移坐标变换

坐标系{$B$}与{$A$}有相同的方位，但二者坐标原点不同.用位置矢量${}^A{\mathbf{p}}_{B_o}$描述{$B$}相对于{$A$}的位置（平移矢量）.如果点$p$在坐标系{$B$}中位置${}^B\mathbf{p}$，那么点$p$相对于坐标系{$A$}的位置矢量${}^A\mathbf{p}$可由{$B$}坐标系的原点在{$A$}坐标系下的位置矢量${}^A{\mathbf{p}}_{B_o}$ 和 点$p$在坐标系{$B$}的位置矢量${}^B\mathbf{p}$相加得出，即有：
$${}^A\mathbf{p}={}^B\mathbf{p}+{}^A{\mathbf{p}}_{B_o}$$
称上式为坐标平移方程.

2.2 旋转坐标变换

坐标系{$B$}与{$A$}有共同的坐标原点，但二者方位不同.用旋转矩阵${}_B^A\mathbf{R}$描述{$B$}相对于{$A$}的方位.如果点$p$在坐标系{$B$}中位置${}^B\mathbf{p}$，那么点$p$相对于坐标系{$A$}的位置矢量${}^A\mathbf{p}$可由{$B$}坐标系相对于{$A$}坐标系的旋转矩阵${}_B^A\mathbf{R}$ 左乘 点$p$在坐标系{$B$}的位置矢量${}^B\mathbf{p}$得出，即有：

$${}^A\mathbf{p}={}_B^A\mathbf{R}{}^B\mathbf{p}$$
称上式为坐标旋转方程.

类似地用${}_A^B\mathbf{R}$描述坐标系{$A$}相对于{$B$}的方位. ${}_B^A\mathbf{R}$和${}_A^B\mathbf{R}$都是正交矩阵，两者互逆.根据正交矩阵的性质可得：
$${}_A^B\mathbf{R}={}_B^A{\mathbf{R}}^{-1}={}_B^A{\mathbf{R}}^{\mathrm{T}}$$

2.3 复合变换

对于最一般的情形，坐标系{$B$}的原点与坐标系{$A$}的原点并不重合，且{$B$}系的方位与{$A$}系的方位也不相同.用位置矢量${}^A{\mathbf{p}}_{B_o}$描述{$B$}相对于{$A$}的位置；用旋转矩阵${}_B^A\mathbf{R}$描述{$B$}相对于{$A$}的方位.对于任一点$P$，在两坐标系{$A$}和{$B$}中的描述${}^A\mathbf{p}{和}^B\mathbf{p}$具有以下变换关系：
$${}^A\mathbf{p}={}_B^A\mathbf{R}{}^B\mathbf{p}+{}^A{\mathbf{p}}_{B_o}$$
把上式看成坐标旋转和坐标平移的复合变换.实际上规定一个过渡坐标系{$C$}，使{$C$}的坐标原点与{$B$}的坐标原点重合，而{$C$}的方位与{$A$}的方位相同.据式(9)可得向过渡坐标系{$C$}的变换：
$${}^C\mathbf{p}={}_B^C\mathbf{R}{}^B\mathbf{p}={}_B^A\mathbf{R}{}^B\mathbf{p}$$
再由式(8)，可得复合变换：
$${}^A\mathbf{p}={}^C\mathbf{p}+{}^A{\mathbf{p}}_{C_o}={}_B^A\mathbf{R}{}^B\mathbf{p}+{}^A{\mathbf{p}}_{B_o}$$

2.4 案例

假设两坐标系{$A$}和{$B$}初始重合，首先{$B$}相对于{$A$}的$z_A$轴转$30°$，再沿着$x_A$轴平移12单位，再沿着$y_A$轴移动6单位。求位置矢量${}^A{\mathbf{p}}_{B_o}$和旋转矩阵${}_B^A\mathbf{R}$.假设点$p$在{$B$}中的描述为${}^B\mathbf{p}=[3,7,0{]}^{\mathrm{T}}$，求它在{$A$}中的描述${}^A\mathbf{p}$.

clear all;clc;
format short
% 旋转矩阵，绕{A}的z轴转30度
R_z30=[cos(pi/6) -sin(pi/6) 0;
       sin(pi/6) cos(pi/6) 0;
        0 0 1]

% {B}的原点在{A}的描述
pA_Bo=[12;6;0]

% 点p在{B}的描述
pB=[3;7;0];

% 点p在{A}的描述
pA=R_z30*pB+pA_Bo

结果：

R_z30 =
    0.8660   -0.5000         0
    0.5000    0.8660         0
         0         0    1.0000
pA_Bo =
    12
     6
     0
pA =
   11.0981
   13.5622
         0

下篇：机器人数学基础2