MATLAB Robotics System Toolbox学习笔记（二）：机器人学数学基础

本教程基于 MATLAB R2020a 版本

参考书籍：《机器人仿真与编程技术》 杨辰光

1 三维空间中的位置与姿态

通常来说，机器人指的是至少包含有一个固定刚体和一个活动刚体的机器装置。其中，固定的刚体称为基座，而活动的刚体称为末端执行器。在两个部件之间会有若干连杆和关节来支撑末端执行器，并使其移动到一定的位置。

位姿： 机器人上每个关节在每一时刻的位置和姿态

1. 位置描述

   在三维空间中建立某一坐标系，于是空间中的任何一个点就可以通过一个 $3\times 1$ 的位置矢量来确定。
   $$P=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]$$
   在 MATLAB 中我们可以将一个三维空间的点表示出来，代码如下：

   plot3(1,1,3,'x') % plot3(1,1,3,'o') 
   % 后面的‘o’指使用圆圈画出这个点，‘x’指使用叉画出这个点
   grid on
   

   显示效果如下：

   

2. 姿态描述

   空间中的物体还需要描述它的姿态（也称为方位），这用固定在物体上的坐标系 { B } \{ B \} {B} 来描述。为了规定空间某刚体 $B$ 的方位，设一坐标系 { B } \{ B \} {B} 与此刚体固连； 用三个单位矢量来表示 ${\text{x}}_B,{\text{y}}_B,{\text{z}}_B$ 坐标系 $B$ 的主轴方向，因此物体相对于参考坐标系 { A } \{ A \} {A} 的姿态可以用矢量 ${\text{x}}_B,{\text{y}}_B,{\text{z}}_B$ 相对于参考坐标系{ A } 的方向余弦组成的3 X 3 矩阵来表示，这个矩阵 ${}_B^A\text{R}$ 称为旋转矩阵。
   $${}_B^A\text{R}=\left[\begin{array}{ccc}{\text{x}}_B& {\text{y}}_B& {\text{z}}_B\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$
   用矢量两两之间的余弦则表示为：
   $${}_B^A\text{R}=\left[\begin{array}{ccc}cos(x_A,x_B)& cos(x_A,y_B)& cos(x_A,z_B)\\ cos(y_A,x_B)& cos(y_A,y_B)& cos(y_A,z_B)\\ cos(z_A,x_B)& cos(z_A,y_B)& cos(z_A,z_B)\end{array}\right]$$

   对应于轴 $X,Y$ 或 $Z$ 作转角为 $\theta$ 的旋转变换，其旋转矩阵分别为：
   $$\begin{gathered} {\text{R}}_x(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\theta & -sin\theta \\ 0& sin\theta & cos\theta \end{array}\right] \\ {\text{R}}_y(\theta )=\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right] \\ {\text{R}}_z(\theta )=\left[\begin{array}{ccc}cos\theta & -sin\theta & 0\\ sin\theta & cos\theta & 0\\ 0& 0& 1\end{array}\right] \end{gathered}$$
   在机器人工具箱中，可分别用函数计算旋转 $\theta$ 的旋转矩阵，其中，在默认情况下， $\theta$ 用角度表示，输入参数类型为 double。

   代码如下：

   R = rotx(180) % 围绕 X 轴做转角为 180° 的旋转变换
   % roty(180) % 围绕 Y 轴做转角为 180° 的旋转变换
   % rotz(180) % 围绕 Y 轴做转角为 180° 的旋转变换
   

   输出结果如下：

   R =
   
        1     0     0
        0    -1     0
        0     0    -1
   

   另外，我们可以使用函数将我们所设置的体坐标系描绘出来：

   trplot(R)
   

   显示效果如下：

   

   拓展：

   • trplot(T)： 绘制由齐次变换矩阵 T 表示的三维坐标系，其中 T 为 4×4 的矩阵。

     函数可选参数如下：

     参数	意义
     ‘noaxes’	在绘图上不显示坐标轴
     ‘color’, C	设置轴的颜色， C 代表MATLAB 图形内置的颜色类型
     ‘text_opts’,opt	调整显示文本的字体大小等属性；例如{‘FontSize’, 10, 'FontWeight ',‘bold’ }
     ‘length’, s	坐标轴的长度（默认值1)
     ‘width’, w	箭头宽度（默认为1)
     '3d ’	在三维空间中使用浮雕图形绘制
     ‘dispar’, D	3d 显示差异（默认0.1)
     ‘labels’, L	使用字符串L 的第1个，第2个，第3个字符标记 X,Y,Z 轴
     ‘axis’, A	将图形显示的轴尺寸设置为A, 其中A =[ xmin xmax ymin ymax zmin zmax]
     ‘frame’,F	将绘制出来的坐标系命名为F, 并且 X,Y,Z 轴的下标含有F
     ‘view’, V	设置绘图视图参数V = [az el] 角度，或者对于坐标系的原点查看’auto’
     ‘arrow’	设置坐标轴的末端为箭头，而不是线段
     ‘thick’, t	线条粗细（默认0.5)
     ‘anaglyph’, A	将"3d" 的浮雕颜色指定为左右两个字符（默认颜色为"re"): 选自红，绿，蓝，青，品红
     ‘text’	启用在框架上显示X,Y,Z 标签
     ‘rgb’	以红色，绿色，蓝色分别显示X,Y,Z 轴

   我们可以使用如下函数展示旋转过程：

   tranimate(R)
   

   显示效果如下：

   

   拓展：

   • tranimate(x1, x2, options) ：展示3D 坐标系从姿态 x1 变换到姿态 x2 的动画效果。其中，姿态 x1 和x2 有三种表示方法： 一个4×4 的齐次矩阵，或一个3×3 的旋转矩阵，或一个四元数。

   • tranimate(x, options) ：展示了坐标系由上一个姿态变换到姿态 x 的动画效果。同样地，姿势 x 也有三种表示方法：一个4×4 的齐次矩阵，或一个3×3 的旋转矩阵，或一个四元数。

   • tranimate(xseq, options)： 展示了移动一段轨迹的动画效果。xseq 可以是一组 $4\times 4\times N$ 的齐次矩阵，或一组 $3\times 3\times N$ 的旋转矩阵，或是一组四元数向量 $(N\times 1)$ 。它包含的可选参数如下：

     参数	意义
     ‘fps’,fps	每秒显示的帧数（默认为10)
     ‘nsteps’, n	沿路径的步数（默认50)
     ‘axis’, A	设置三个轴的边界， A = [xmin,xmax, ymim, ymax, zmim, zmax]
     ‘movie’, M	将帧保存为文件夹M 中的文件， M 为文件的路径

2 坐标变换（相对于参考坐标系）

在三维空间中，任一坐标系均可作为描述一个物体位姿的参考坐标系。在机器人学的许多问题中，常常需要在不同的参考坐标系中表示同一个物体的位姿。

1. 平移坐标变换

   用 $4\times 4$ 矩阵表示平移变换矩阵 $\text{T}$ ，则
   $$\text{T}=\left[\begin{array}{cccc}1& 0& 0& p_x\\ 0& 1& 0& p_y\\ 0& 0& 1& p_z\\ 0& 0& 0& 1\end{array}\right]$$
   $p_x,p_y,p_z$ 是平移向量 $\text{P}$ 相对于参考坐标系 $X,Y,Z$ 轴的 3 个分量 。

   MATLAB 中使用 transl() 可以计算出相对应的平移变换矩阵：

   TL =transl(1,11,5); % 生成一个平移变换矩阵，代表着沿着参考坐标系的X轴移动1个单位，Y轴移动11各单位，Z轴移动5个单位。
   % TL =transl(x,y,z)：表示能够获取一个分别沿着x,y,z 轴平移一段距离得到的 4X4 齐次变换矩阵。
   2. **旋转坐标变换**
   tranimate(TL); % 展示姿态变换动画
   

   生成的平移变换矩阵为：

   TL =
        1     0     0     1
        0     1     0    11
        0     0     1     5
        0     0     0     1
   

   动画效果如下：

   

   拓展：

   • T = transl(p) ：表示由经过矩阵（或向量）p = [x,y,z] 的平移得到的齐次变换矩阵。如果p 为(M X3 ) 的矩阵，则 T 为一组齐次变换矩阵(4×4×M), 其中T(: , : , i) 对应于 p 的第 1 行。

   • [x,y,z] = transl(T) ： x,y,z 是齐次变换矩阵中的三个分量，是一个 1×M 的向量。

   • p = transl(T) ：p 是齐次变换矩阵中 T 的平移部分，是一个 3×M 的矩阵。

2. 旋转坐标变换

   1. MATLAB 中使用 trotx()、troty()、trotz() 可以计算出相对应的旋转变换矩阵，也是一个 4×4 的矩阵：

      TX = trotx(90); % 对应参考坐标系 X 轴旋转90度
      TY = troty(90); % 对应参考坐标系 Y 轴旋转90度
      TZ = trotz(90); % 对应参考坐标系 Z 轴旋转90度
      T = TX*TY*TZ; % 三种效果叠加，表示先Z，后Y，后X
      tranimate(T); % 展示动画效果
      

      生成最终矩阵 $\text{T}$：

      T =
           0     0     1     0
           0    -1     0     0
           1     0     0     0
           0     0     0     1
      

      动画效果如下：

      

      我们将上面的步骤拆开来：

      TZ = trotz(90); % 对应参考坐标系 Z 轴旋转90度
      T = TZ; 
      tranimate(T); % 展示动画效果
      
      TY = troty(90); % 对应参考坐标系 Y 轴旋转90度
      T = TY*T; % 两种效果叠加，先Z，后Y
      tranimate(TZ,T); % 展示动画效果
      
      TX = trotx(90); % 对应参考坐标系 X 轴旋转90度
      T = TX*T; % 三种效果叠加，表示先Z，后Y，后X
      tranimate(TY*TZ,T); % 展示动画效果
      

      动画效果如下：

      

      可以看到效果是相同的。

3. 齐次坐标变换

   将旋转变换和平移变换合在一起我们可以用一个齐次变换矩阵来表示。齐次变换过程如下：
   $$\left[\begin{array}{c}{}^{A}P\\ 1\end{array}\right]=\left[\begin{array}{cc}{}_B^A\text{R}& {}^A{\text{p}}_{Bo}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{}^{B}p\\ 1\end{array}\right]$$
   简化表示：
   $${}^A\text{p}{=}_B^A\text{T}\ast {}^B\text{p}$$
   将上式展开就可以得到：
   $$\begin{gathered} {}^A\text{p}{=}_B^A\text{R}{\ast }^B\text{p}{+}^A{\text{p}}_{Bo} \\ 1=1 \end{gathered}$$
   我们将1与2的变换合并：

   T = TL*T;
   

   得出最后齐次变换矩阵：

   T =
        0     0     1     1
        0    -1     0    11
        1     0     0     5
        0     0     0     1
   

   合并动画效果如下：

   

   • 此外，关于齐次变换矩阵 T , 机器人工具箱提供了函数 t2r() 可以提取旋转矩阵分量。

     R=t2r(T);
     

   • 而函数 r2t() 则可以将旋转矩阵转换成对应的齐次变换矩阵。如通过运行指令 r2t(R) 可以得到上面的矩阵 T 。

     T=r2t® 获取一个正交旋转矩阵R 等价的具有零平移分量的齐次变换矩阵。如果 R 是一个3X3 的矩阵，则T 是一个 4×4 的矩阵；如果R 是一个2×2 的矩阵，则 T 是一个 3×3 的矩阵。

   • 通过以下的命令可以提取平移变换分量。输入命令：

     p = tranl(T);
     

3 姿态的其他表示方法

1. 角坐标表示法

   • $X-Y-Z$ 固定角坐标系 (RPY角)

     机器人工具箱提供了简单的函数去计算等价的旋转矩阵，其中一种是使用之前提过的函数 rotx() 、roty() 和 rotz() 按顺序相乘；另外一种是使用函数rpy2r() 。

     R1 = rotz(pi/3) * roty(pi/4) * rotx(pi/6);
     % 等同于 R1 = rpy2r(pi/3,pi/4,pi/6,'zyx');  该函数默认的是XYZ, 应在后面的选项添加zyx 指定为ZYX 顺序。
     

     如果已知一个旋转矩阵，要求出对应的偏转角、俯仰角和回转角，根据理论计算很难得到所要求的值。在机器人工具箱中，可以根据函数 tr2rpy() 计算出一个给定旋转矩阵的偏转角、俯仰角和回转角。

     TB = tr2rpy(R1,'zyx');
     

   • $Z-Y-X$ 欧拉角坐标系与$Z-Y-Z$ 欧拉角坐标系

     机器人工具箱提供了函数 eul2r() 去计算对应的欧拉角。

     R2 = eul2r(90,90,90)
     

     运行结果：

     R2 =
         -1     0     0
          0     0     1
          0     1     0
     

     我们可以使用 tr2eul() 计算出给定旋转矩阵的欧拉角

     EA = tr2eul(R2) % 输出1 × 3的矩阵，输出为弧度值
     EA = EA.*(180/pi) % 角度与弧度运算
     

     运行结果：

     EA =
         90    90    90
     

     还有很多转换函数，等需要的时候再慢慢探索~

2. 向量表示法

   • 双向量表示法
   • 单向量旋转法