【Matlab 六自由度机器人】运动学正解（附MATLAB机器人正解完整代码）

往期回顾

【汇总】

相关资源：

$\boxed{【Matlab六自由度机器人】系列文章汇总}$

【主线】

1. 定义标准型及改进型D-H参数，建立机器人模型。
2. 运动学正解
3. 基于蒙特卡罗方法(Monte Carlo Method)构建机器人工作空间

【补充说明】

1. 关于灵活工作空间与可达工作空间的理解
2. 关于改进型D-H参数(modified Denavit-Hartenberg)的详细建立步骤
3. 关于旋转的参数化（欧拉角、姿态角、四元数）的相关问题
4. 关于双变量函数atan2(x,y)的解释
5. 关于机器人运动学反解的有关问题

前言

本篇介绍机器人运动学正解的有关问题，介绍了如何理解正向运动学，并利用D-H参数求解机器人运动学正解。

---

以下是本篇文章正文内容，包含对正解的含义的理解和代码的分步解析。

正文

一、运动学正解

定义：已知各关节的运动参数，求末端执行器的相对参考坐标系的位姿。

求解步骤：

• 各连杆首尾相连；
• 确定各连杆间的齐次变换矩阵；
• 得到最后的总变换矩阵。

且该总变换矩阵内的未知数只有各轴的旋转角度，因此得到旋转角度即可得到六自由度机器人的末端笛卡尔空间坐标。

1. 齐次变换矩阵

在上一篇的Matlab建立六自由度机器人模型中详细解释了如何定义机器人的DH参数、如何设置$z_i$轴等等，在此只叙述部分内容。
对DH参数的符号约定

DH约定参数	符号约定
$\theta$	joint angle 关节转角
$d$	link offset 连杆偏移
$a$	link length 连杆长度
$\alpha (alpha)$	link twist 连杆扭角
制定DH参数的程序规则

• 参数$a$是轴$z_0$和轴$z_1$之间沿轴线$x_1$测得的距离；
• 角度$\alpha$是在垂直于$x_1$平面内测得的轴线$z_0$和$z_1$之间的夹角。角度$\alpha$的正向取值定义为从$z_0$到$z_1$，由右手规则来确定；
• 参数$d$为从原点$o_0$到轴线$x_1$与$z_0$交点之间的距离，该距离沿$z_0$轴线进行测量得到；
• $\theta$是在垂直于$z_0$的平面内测得的从$x_0$到$x_1$的角度。

理解了上述的符号约定及程序规则后，在此基础上，每个齐次变换矩阵$T_i$都可以表示为是个基本矩阵的乘积，

• 对于标准型D-H参数
  其乘积顺序如下：
  ${}^{i-1}{T}_i=Rot(z_{,}{\theta }_i)\times Trans(z，d_i)\times Trans(x_{，}{a}_i)\times Rot(x，{\alpha }_i)$
  通用齐次变换矩阵如下：
  ${}^{i-1}{T}_i=$$\left[\begin{array}{cccc}cos{\theta }_i& -sin{\theta }_{i}cos{\alpha }_i& sin{\theta }_{i}sin{\alpha }_i& a_{i}cos{\theta }_i\\ sin{\theta }_i& cos{\theta }_{i}cos{\alpha }_i& -cos{\theta }_{i}sin{\alpha }_i& a_{i}sin{\theta }_i\\ 0& sin{\alpha }_i& cos{\alpha }_i& d_i\\ 0& 0& 0& 1\end{array}\right]$
• 对于改进型D-H参数
  其乘积顺序如下：
  ${}^{i-1}{T}_i=Rot(x_i，{\alpha }_{i-1})\times Trans(x_i，a_{i-1})\times Rot(z_i,{\theta }_i)\times Trans(z_i，d_i)$
  通用齐次变换矩阵如下：
  ${}^{i-1}{T}_i=$$\left[\begin{array}{cccc}cos{\theta }_i& -sin{\theta }_i& 0& a_i\\ sin{\theta }_{i}cos{\alpha }_i& cos{\theta }_{i}cos{\alpha }_i& -sin{\alpha }_i& -sin{\alpha }_i{d}_i\\ sin{\theta }_{i}sin{\alpha }_i& cos{\theta }_{i}sin{\alpha }_i& cos{\alpha }_i& cos{\alpha }_i{d}_i\\ 0& 0& 0& 1\end{array}\right]$

2. 总变换

确定好DH参数建立方式并构建出各关节的DH参数后，代入各自的通用齐次变换矩阵，得到${}^0{T}_1$、${}^1{T}_2$、${}^2{T}_3$、${}^3{T}_4$、${}^4{T}_5$、${}^5{T}_6$共六个矩阵。在此作者选择的是改进型的D-H参数，因此各矩阵分别如下所示：
${}^0{T}_1=$$\left[\begin{array}{cccc}cos{\theta }_1& -sin{\theta }_1& 0& a_1\\ cos{\alpha }_{1}sin{\theta }_1& cos{\alpha }_{1}cos{\theta }_1& -sin{\alpha }_1& -d_{1}sin{\alpha }_1\\ sin{\alpha }_{1}sin{\theta }_1& sin{\alpha }_{1}cos{\theta }_1& cos{\alpha }_1& d_{1}cos{\alpha }_1\\ 0& 0& 0& 1\end{array}\right]$
${}^1{T}_2=$$\left[\begin{array}{cccc}cos{\theta }_2& -sin{\theta }_2& 0& a_2\\ cos{\alpha }_{2}sin{\theta }_2& cos{\alpha }_{2}cos{\theta }_2& -sin{\alpha }_2& -d_{2}sin{\alpha }_2\\ sin{\alpha }_{2}sin{\theta }_2& sin{\alpha }_{2}cos{\theta }_2& cos{\alpha }_2& d_{2}cos{\alpha }_2\\ 0& 0& 0& 1\end{array}\right]$
${}^2{T}_3=$$\left[\begin{array}{cccc}cos{\theta }_3& -sin{\theta }_3& 0& a_3\\ cos{\alpha }_{3}sin{\theta }_3& cos{\alpha }_{3}cos{\theta }_3& -sin{\alpha }_3& -d_{3}sin{\alpha }_3\\ sin{\alpha }_{3}sin{\theta }_3& sin{\alpha }_{3}cos{\theta }_3& cos{\alpha }_3& d_{3}cos{\alpha }_3\\ 0& 0& 0& 1\end{array}\right]$
${}^3{T}_4=$$\left[\begin{array}{cccc}cos{\theta }_4& -sin{\theta }_4& 0& a_4\\ cos{\alpha }_{4}sin{\theta }_4& cos{\alpha }_{4}cos{\theta }_4& -sin{\alpha }_4& -d_{4}sin{\alpha }_4\\ sin{\alpha }_{4}sin{\theta }_4& sin{\alpha }_{4}cos{\theta }_4& cos{\alpha }_4& d_{4}cos{\alpha }_4\\ 0& 0& 0& 1\end{array}\right]$
${}^4{T}_5=$$\left[\begin{array}{cccc}cos{\theta }_5& -sin{\theta }_5& 0& a_5\\ cos{\alpha }_{5}sin{\theta }_5& cos{\alpha }_{5}cos{\theta }_5& -sin{\alpha }_5& -d_{5}sin{\alpha }_5\\ sin{\alpha }_{5}sin{\theta }_5& sin{\alpha }_{5}cos{\theta }_5& cos{\alpha }_5& d_{5}cos{\alpha }_5\\ 0& 0& 0& 1\end{array}\right]$
${}^5{T}_6=$$\left[\begin{array}{cccc}cos{\theta }_6& -sin{\theta }_6& 0& a_6\\ cos{\alpha }_{6}sin{\theta }_6& cos{\alpha }_{6}cos{\theta }_6& -sin{\alpha }_6& -d_{6}sin{\alpha }_6\\ sin{\alpha }_{6}sin{\theta }_6& sin{\alpha }_{6}cos{\theta }_6& cos{\alpha }_6& d_{6}cos{\alpha }_6\\ 0& 0& 0& 1\end{array}\right]$

对该六个齐次变换矩阵按顺序相乘后得到六自由度机器人的总变换：
${}^0{T}_6=$${}^0{T}_1\times$${}^1{T}_2\times$${}^2{T}_3\times$${}^3{T}_4\times$${}^4{T}_5\times$${}^5{T}_6=$$\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$

二、代码实现

1. 定义各连杆参数

对连杆参数进行符号变量的定义有两种方式，代码如下：

第一种方式 是将所有参数定义为未知量

syms   theta1   d1    a1     alpha1;
syms   theta2   d2    a2     alpha2;
syms   theta3   d3    a3     alpha3;
syms   theta4   d4    a4     alpha4;
syms   theta5   d5    a5     alpha5;
syms   theta6   d6    a6     alpha6;

但由于设置太多的未知量会导致总变换的结果过于冗长，因此在此设置 第二种方式 来规避复杂的结果，以下参数请参考上一篇的Matlab建立六自由度机器人模型

%连杆偏移
d1 = 398;
d2 = -0.299;
d3 = 0;
d4 = 556.925;
d5 = 0;
d6 = 165;
%连杆长度
a1 = 0;
a2 = 168.3;
a3 = 650.979;
a4 = 156.240;
a5 = 0;
a6 = 0;
%连杆扭角
alpha1 = 0;
alpha2 = pi/2;
alpha3 = 0;
alpha4 = pi/2;
alpha5 = -pi/2;
alpha6 = pi/2;
%由于我们需要分析各轴θi所对应的机器人末端位置
%因此theta1 theta2 theta3 theta4 theta5 theta6仍设为未知量
syms theta1 theta2 theta3 theta4 theta5 theta6

有了以上参数的设置，接下来对参数进行齐次变换矩阵的设置。

2. 齐次变换矩阵及总变换

首先对各参数归纳到一个统一的矩阵中，方便后续对参数的引用

% 参数矩阵取名为MDH
MDH = [theta1          d1       a1       alpha1;
       theta2+pi/2     d2       a2       alpha2;    
       theta3          d3       a3       alpha3;
       theta4          d4       a4       alpha4;
       theta5          d5       a5       alpha5;
       theta6          d6       a6       alpha6];

注意！！！ 由于作者的六自由度机器人在第二个关节中有一个关节变量偏移量，该偏移量是导致MDH中的theta2需要加pi/2的的关键。

接下来在齐次变换矩阵中引用参数矩阵的数值，对各关节的齐次变换矩阵进行一个定义，代码如下：

T01=[cos(MDH(1,1))               -sin(MDH(1,1))                 0              MDH(1,3);
     sin(MDH(1,1))*cos(MDH(1,4))  cos(MDH(1,1))*cos(MDH(1,4))  -sin(MDH(1,4)) -sin(MDH(1,4))*MDH(1,2);
     sin(MDH(1,1))*sin(MDH(1,4))  cos(MDH(1,1))*sin(MDH(1,4))   cos(MDH(1,4))  cos(MDH(1,4))*MDH(1,2);
      0                           0                             0              1];
T12=[cos(MDH(2,1))               -sin(MDH(2,1))                 0              MDH(2,3);
     sin(MDH(2,1))*cos(MDH(2,4))  cos(MDH(2,1))*cos(MDH(2,4))  -sin(MDH(2,4)) -sin(MDH(2,4))*MDH(2,2);
     sin(MDH(2,1))*sin(MDH(2,4))  cos(MDH(2,1))*sin(MDH(2,4))   cos(MDH(2,4))  cos(MDH(2,4))*MDH(2,2);
      0                           0                             0              1];
T23=[cos(MDH(3,1))               -sin(MDH(3,1))                 0              MDH(3,3);
     sin(MDH(3,1))*cos(MDH(3,4))  cos(MDH(3,1))*cos(MDH(3,4))  -sin(MDH(3,4)) -sin(MDH(3,4))*MDH(3,2);
     sin(MDH(3,1))*sin(MDH(3,4))  cos(MDH(3,1))*sin(MDH(3,4))   cos(MDH(3,4))  cos(MDH(3,4))*MDH(3,2);
      0                           0                             0              1];
T34=[cos(MDH(4,1))               -sin(MDH(4,1))                 0              MDH(4,3);
     sin(MDH(4,1))*cos(MDH(4,4))  cos(MDH(4,1))*cos(MDH(4,4))  -sin(MDH(4,4)) -sin(MDH(4,4))*MDH(4,2);
     sin(MDH(4,1))*sin(MDH(4,4))  cos(MDH(4,1))*sin(MDH(4,4))   cos(MDH(4,4))  cos(MDH(4,4))*MDH(4,2);
      0                           0                             0              1];
T45=[cos(MDH(5,1))               -sin(MDH(5,1))                 0              MDH(5,3);
     sin(MDH(5,1))*cos(MDH(5,4))  cos(MDH(5,1))*cos(MDH(5,4))  -sin(MDH(5,4)) -sin(MDH(5,4))*MDH(5,2);
     sin(MDH(5,1))*sin(MDH(5,4))  cos(MDH(5,1))*sin(MDH(5,4))   cos(MDH(5,4))  cos(MDH(5,4))*MDH(5,2);
      0                           0                             0              1];
T56=[cos(MDH(6,1))               -sin(MDH(6,1))                 0              MDH(6,3);
     sin(MDH(6,1))*cos(MDH(6,4))  cos(MDH(6,1))*cos(MDH(6,4))  -sin(MDH(6,4)) -sin(MDH(6,4))*MDH(6,2);
     sin(MDH(6,1))*sin(MDH(6,4))  cos(MDH(6,1))*sin(MDH(6,4))   cos(MDH(6,4))  cos(MDH(6,4))*MDH(6,2);
      0                           0                             0              1];

通过上述，可知总变换代码如下：

T06 = T01*T12*T23*T34*T45*T56;

3. 代码运行结果

在代码实例中，我们将对${\theta }_i$进行如下赋值

theta1 = pi/3;
theta2 = pi/4;
theta3 = pi/5;
theta4 = pi/3;
theta5 = pi/4;
theta6 = pi/5;

运行程序后如下图所示

通过上篇文章的robot.teach() 进行仿真后，其结果如下图所示

---

总结

以上就是正向运动学的内容，本文详细介绍了如何理解正向运动学及代码的实现，机器人工具箱提供了如何处理连杆长度、连杆扭曲等函数和方法。

参考文献

1. Matlab机器人工具箱（1）——机器人的建立、绘制与正逆运动学
2. DH参数法建立机器人的运动学正解
3. 六轴机器人matlab写运动学正解函数（DH模型）
   
4. 【机器人学】平面2R机器人（一）——正运动学