机器人工具箱学习（二）-编程知识

一、机械臂及运动学

1.1 机械臂构成

机械臂多采用关节式机械结构，一般具有6个自由度，其中3个用来确定末端执行器的位置，另外3个则用来确定末端执行装置的方向(姿态)。
如图所示，一个机械臂是由一组可做相对运动的关节连接的连杆结合体。第一个连杆固定，连接该机械臂的基座，而最后一个连杆连接的是它的末端执行器。
在这里插入图片描述

通常可将关节划分为两种：第一种称为转动关节(或称为旋转关节)，转动关节可绕基准轴转动，相应的转动量称为关节角：第二种称为移动关节，移动关节是沿着基准轴移动，相应的位移称为关节偏距。还有一种特殊的关节称为球关节，球关节拥有三个自由度。可以用三个转动关节和一个零长度的连杆来描述一个球关节。
位于机械臂固定基座的坐标系称为基坐标系；位于操作臂末端执行器的坐标系称为工具坐标系，通常用它来描述机械臂的位置。

1.2 机器人运动学介绍

机器人运动学只研究机器人运动，不关注机器人运动过程中各零部件的质量及相关力，也不关注关节驱动力和力矩。
（1）机器人正运动学：给定一组机器人关节变量（转角或位移），求解末端工具坐标系相对于基坐标系的位置和姿态。
（2）机器人逆运动学：给定机器人末端工具箱坐标系的位置和姿态，求解机器人各关节变量。

二、D-H参数法

D-H 参数全称为Denavit-Hartenberg参数，它使用连杆参数来描述机构运动关系。在DH参数法中，描述机械臂中的每一个连杆需要4个运动学参数：
● 连杆长度 $a _ { i - 1 }$ ：关节轴 $i - 1$ 与关节轴 $i$ 之间公垂线的长度；
● 连杆转角 $\alpha _ { i-1 }$ ：第 $i - 1$ 个关节轴和第 $i$ 个关节轴之间的夹角；
● 连杆偏距 $d_{i}$ ：沿两个相邻连杆公共轴线方向的距离；
● 关节角 $\theta_{i}$ ：两相邻连杆绕公共轴线旋转的夹角。
在这里插入图片描述

2.1 标准D-H法（SDH）

建模规则：
（1）找出各关节轴，画出关节轴的延长线；
（2）确定 $Z$ 轴：与关节轴线重合，其中 $Z_{i-1}$ 轴与关节 $i$ 的轴线重合；
（3）确定 $X$ 轴： $Z_{i-1}$ 轴与 $Z_{i}$ 轴的公垂线，方向由 $Z_{i-1}$ 轴指向 $Z_{i}$ 轴
1）如果 $Z_{i-1}$ 轴与 $Z_{i}$ 轴平行，选取与前一关节的公垂线共线的一条公垂线作为 $X$ 轴；
2）如果 $Z_{i-1}$ 轴与 $Z_{i}$ 轴相交， $X$ 轴为 $Z_{i-1}$ 轴与 $Z_{i}$ 轴的叉积方向；
（4）确定 $Y$ 轴：右手定则。
在这里插入图片描述

D-H参数含义：
（1） $a_{i}$ ：关节轴线 $i - 1$ 和关节轴线 $i$ 的公垂线长度
（2） $\alpha_{i}$ ：关节轴线 $i - 1$ 和关节轴线 $i$ 的夹角，指向为从轴线 $i - 1$ 到轴线 $i$ ；
（3） $d_{i}$ ：关节 $i$ 上的两条公垂线 $a_{i-1}$ 与 $a_{i}$ 之间的距离，沿关节轴线 $i$ 测；
（4） $\theta_{i}$ ：连杆 $i$ 相对于连杆 $i - 1$ 绕轴线 $i$ 的旋转角度。

齐次变换矩阵：
从坐标系 $i - 1$ 到坐标系 $i$ ，先绕 $Z_{i-1}$ 轴旋转角度 $\theta_{i}$ l，再沿 $Z_{i-1}$ 轴移动 $d_{i}$ ，然后沿 $X_i$ 轴平移 $a_i$ ，最后绕 $X_i$ 轴旋转 $\alpha_i$ 。齐次变换矩阵 $_ { i - 1 } ^ { i } T$ 可以写为：
在这里插入图片描述

2.2 改进D-H法（MDH）

建模规则：
（1）找出各关节轴，画出关节轴的延长线；
（2）确定 $Z$ 轴：与关节轴线重合，其中 $Z_{i}$ 轴与关节 $i$ 的轴线重合；
（3）确定 $X$ 轴： $Z_{i}$ 轴与 $Z_{i+1}$ 轴的公垂线，方向由 $Z_{i}$ 轴指向 $Z_{i+1}$ 轴；
（4）确定 $Y$ 轴：右手定则。
在这里插入图片描述

D-H参数含义：
（1） $a_{i-1}$ ：沿 $X_{i-1}$ 轴，从 $Z_{i-1}$ 轴移动到 $Z_{i}$ 轴的距离；
（2） $\alpha_{i-1}$ ：绕 $X_{i-1}$ 轴，从 $Z_{i-1}$ 轴旋转到 $Z_{i}$ 轴的角度；
（3） $d_{i}$ ：沿 $Z_{i}$ 轴，从 $X_{i-1}$ 轴移动到 $X_{i}$ 轴的距离；
（4） $\theta_{i}$ ：绕 $Z_{i}$ 轴，从 $X_{i-1}$ 轴旋转到 $X_{i}$ 轴的角度；

齐次变换矩阵：
从坐标系 $i - 1$ 到坐标系 $i$ ，先绕 $X_{i-1}$ 轴旋转角度 $\alpha_{i-1}$ ，再沿 $X_{i-1}$ 轴平移 $a_{i-1}$ ，然后绕 $Z_{i}$ 轴旋转角度 $\theta_{i}$ ，最后沿 $Z_{i}$ 轴移动 $d_{i}$ 。齐次变换矩阵 $_ { i - 1 } ^ { i } T$ 可以写为：
在这里插入图片描述

2.3 SDH和MDH的区别及适用场景

2.3.1 SDH和MDH的区别

（其实一直以来笔者都不太能区分这两种方法，笔者最开始学习机器人学是看的《机器人学导论》这本书，上面似乎是偏向于MDH方法，所以后续我都是习惯于用MDH去建模）
（1）区别一：连杆坐标系建立的位置不同。SDH将连杆 $i$ 的坐标系固定在连杆的远端；MDH将连杆 $i$ 的坐标系固定在连杆的近端。
在这里插入图片描述

（2）区别二：执行变换的顺序不同。

2.3.2 SDH和MDH的适用场景

对于树形结构或者闭链机构的机器人来说，按照SDH方法建立的连杆坐标系会产生歧义，因为SDH的建系原则是把连杆 $i$ 的坐标系建立在连杆的远端，如图(a)所示，导致连杆0上同时出现了两个坐标系。而MDH把连杆坐标系建立在每个连杆的近端，则不会坐标系重合的情况，如图(b)所示，这就克服了SDH方法建系的缺点。
在这里插入图片描述
总结：
（1）SDH适合应用于开链结构的机器人；
（2）当使用SDH表示树状或闭链结构的机器人时，会产生歧义；
（3）MDH法对开链、树状、闭链结构的机器人都适用。

三、机器人运动学

3.1 创建一个连杆对象

在机器人工具箱中，还用变量 $\sigma _ { i }$ 表示机器人的关节类型， $\sigma _ { i } = 0$ 表示转动关节， $\sigma _ { i } = 1$ 表示移动关节(若未指定该参数，默认为转动关节)。
在工具箱中，用函数Link( )可以创建一个机械臂对象，其中输入的参数顺序分别是：关节角 $\theta _ { i }$ 、连杆偏距 $d_{i}$ 、连杆长度 $a_{i-1}$ 、连杆转角 $\alpha_{i-1}$ 和关节类型。例如，创建一个关节角为30°，连杆偏距为0.2m，连杆长度为0.5m，连杆转角为60°，关节类型为旋转关节的连杆，代码如下所示：

L = Link([pi/6, 0.2, 0.5, pi/3, 0])

在这里插入图片描述

这里，Revolute表示转动关节，std表示标准D-H参数法，offset表示关节偏移量。可以通过以下命令获得连杆的各个参数：
● 获取连杆的关节类型：L.type( )（注意：老版本的工具箱是L.RP）;
● 获取连杆的关节角：L.theta;
● 获取连杆的连杆偏距：L.d;
● 获取连杆的连杆长度：L.a;
● 获取连杆的连杆转角：L.alpha。

在这里插入图片描述
● 获取改连杆的齐次变换矩阵：L.A( $\theta$ )，例如连杆转动了30°，其齐次变换矩阵T为：

在这里插入图片描述

3.2 创建一个平面3-DOF的机械臂

这里创建了一个平面三R机构（三个转动关节），该平面三连杆机构的DH参数表如下所示：
在这里插入图片描述

% standard-表示标准DH法
%               theta(z)   d(z)     a(x)     alpha(x)  
RRR_L(1) = Link([  0       0        1        0    ],'standard');
RRR_L(2) = Link([  0       0      0.8        0    ],'standard');
RRR_L(3) = Link([  0       0      0.6        0    ],'standard');

运行结果：
在这里插入图片描述

通过构造函数SerialLink( )可以给创建的机械臂对象命名，并显示出对象的信息。输入命令:

three_link = SerialLink(RRR_L, 'name', 'three-link-RRR')

运行结果：

在这里插入图片描述
用以下的命令可以获取已创建机械臂的各个参数：

在这里插入图片描述
运行结果：

在这里插入图片描述

同时也可以对创建的机械臂对象进行复制，如复制一个名称为“three_link2”的机械臂输人命令:

three_link2 = SerialLink(RRR_L, 'name', 'three-link-RRR_2')

运行结果：
在这里插入图片描述

使用teach( )函数，可以对创建的机器人进行示教，如图所示：

在这里插入图片描述

3.3 机器人正运动学

机器人正运动学即给定一组关节角，计算出机器人末端相对于基坐标系的位置和姿态，如下图：
在这里插入图片描述

机器人工具箱中，fkine( )可以进行正运动学计算，即给定关节变量，得到末端坐标系关于基坐标系的齐次变换矩阵（位置和姿态）。这里以3.2节的3-DOF平面机械臂为例，当机器人三个关节角都为0°时：

% RRR机械臂
clear;
close all;
clc;%               theta(z)   d(z)     a(x)     alpha(x)  
RRR_L(1) = Link([  0       0        1        0    ],'standard');
RRR_L(2) = Link([  0       0      0.8        0    ],'standard');
RRR_L(3) = Link([  0       0      0.6        0    ],'standard');three_link = SerialLink(RRR_L, 'name', 'three-link-RRR');
q0 = [0 0 0]'*pi/180;
T0 = three_link.fkine(q0)

运行结果：

在这里插入图片描述

当机器人一关节10°，二关节-30°，三关节60°时，机器人末端的位置和姿态如图：

在这里插入图片描述
可以用plot( )函数绘制此时机器人的状态，如图：

例子：给定平面3-RRR机械臂各个关节的角度如图所示，求解机器人运动过程中末端点的三维坐标

在这里插入图片描述

% RRR机械臂
clear;
close all;
clc;%               theta(z)   d(z)     a(x)     alpha(x)  
RRR_L(1) = Link([  0       0        1        0    ],'standard');
RRR_L(2) = Link([  0       0      0.8        0    ],'standard');
RRR_L(3) = Link([  0       0      0.6        0    ],'standard');three_link = SerialLink(RRR_L, 'name', '3-DOF');t = 0:0.05:4;
m = length(t);
theta1 = 120*sin(4*pi*t/4);
theta2 = 60*sin(2*pi*t/4);
theta3 = 30*sin(2*pi*t/4);q = [theta1;theta2;theta3]*pi/180;for i = 1:mT = three_link.fkine(q(:,i)');x(i) = T.t(1);y(i) = T.t(2);z(i) = T.t(3);subplot(3,3,[1 4 7])hold onplot3(x(i),y(i),z(i),'r*');hold onthree_link.plot(q(:,i)');subplot(3,3,2)hold onplot(t(i),q(1,i)*180/pi,'b.')xlabel('time /s')ylabel('\theta_1 /deg')subplot(3,3,5)hold onplot(t(i),q(2,i)*180/pi,'r.')xlabel('time /s')ylabel('\theta_2 /deg')subplot(3,3,8)hold onplot(t(i),q(3,i)*180/pi,'m.')xlabel('time /s')ylabel('\theta_3 /deg')subplot(3,3,3)hold onplot(t(i),x(i),'b.')xlabel('time /s')ylabel('x /m')subplot(3,3,6)hold onplot(t(i),y(i),'r.')xlabel('time /s')ylabel('y /m')subplot(3,3,9)hold onplot(t(i),z(i),'m.')xlabel('time /s')ylabel('z /m')
end