Matlab机器人运动学与正逆解算法学习笔记

---

关于机器人正逆解的原理、算法、代码什么的，在网上一找一大堆，其本身也是关于机器人的知识里面最基础的，其实没什么好写的，这里只是把我理解的内容整理一下，方便自己和需要的人查看。

另外这里要感谢赵而阳师弟，之前我在弄有限元分析的内容，把机器人相关的问题都丢给他了。

❤ 2023.6.6 ❤

※ 参考资料整理

关于机器人运动学的参考资料与文章挺多的，同时matlab实现机器人的正逆解有更简单的方法就是用“机器人工具箱（Robotics Toolbox）”，所以要说这里的资料是不是最好的最有价值的不一定，但至少有帮助吧。

首先是我之前整理的

→→→【Matlab机器人工具箱（Robotics Toolbox）学习笔记】
其实就是把B站一个将机器人工具箱的up的视频整理了一下，并且附上了自己的练习与验证内容

→→→【机器人标准DH建模与改进DH建模】
这篇文章用比较简练的语言和图形说明了标准DH和改进DH的区别

→→→【（3）机器人的DH参数建模详解】
这篇文章讲了我经常搞混的机器人关节和连杆的编号问题

→→→【【机械臂运动学教程】机械臂+旋转矩阵+变换矩阵+DH+逆解+轨迹规划+机器人+教程】
P4 4.机械臂几何法与DH表示法

→→→【工业六轴机器人常见的STD(标准)-DH模型建立方法】

→→→【SD-H模型运动学正解、逆解及姿态角的解算验证】

→→→【工业六轴机器人常见的MOD(改进)-DH模型建立方法】

→→→【MD-H模型运动学正解、逆解及姿态角的解算验证】

---

建立DH模型

这是由Danevit Harttnberg提出的故称为DH方法

关于机器人的DH建模的方法，在各种机器人参考书里都有，据说，外国的书用改进DH比较多，中国的书用标准DH比较多，其实我也没有仔细看过，但是根据我看过的书籍的内容，DH建模方法这部分都没有特别详细的讲解，可能是大家都觉得这个内容太基础了，没必要将这么详细吧。。。

总之，在最开始学习机器人时，大部分人对于该如何建立DH模型会感到很混乱，这里总结一下DH模型的建立方法。

首先是参考资料：

→→→【机器人标准DH建模与改进DH建模】
这篇文章用比较简练的语言和图形说明了标准DH和改进DH的区别

→→→【（3）机器人的DH参数建模详解】
这篇文章讲了我经常搞混的机器人关节和连杆的编号问题

△ 基本概念和标准DH/改进DH

○ 连杆与关节的编号

这里直接引用【（3）机器人的DH参数建模详解】中的内容

总结一下，就是从基座为0开始，第几个杆编号就是几，离基座近的一端的关节编号和杆相同，远的一端编号+1

---

○ 标准DH与改进DH

• 标准DH与改进DH建立方法的区别

这里引用【机器人标准DH建模与改进DH建模】：

总结一下：杆的坐标系建立在离底座远的一端的是标准DH，近的一端的是改进DH

• 标准DH与改进DH应用的区别

简单来说就是当一个关节连了两个杆的时候，标准DH会出现歧义，而改进DH不会

---

△ DH参数模型建立方法

这里主要引用

→→→【（3）机器人的DH参数建模详解】中的内容

原文里各种公式崩了，这里我用Latex重新整理了一下

○ 标准DH参数定义及方法简介

· 连杆坐标系建立方法

• 第一步：确定各坐标系的Z轴：

基本原则为：选取$z_i$轴为关节i+1的轴向（如上面图片所示，如z1在轴2上）（指向可以任选，但是通常将各平行的Z轴的指向取为相同）。另外，机器人远端没有关节n+1，这时可以选取$z_n$轴和$z_{n+1}$轴重合 。

• 第二步：确定各坐标系的原点：

基本原则为：选取原点$o_i$在过$z_{i-1}$轴和$z_i$轴的公法线上（即公法线与$z_i$轴的交点）。

注意：
① 当$z_{i-1}$轴和$z_i$轴平行时，两轴的公法线有无数条，若两轴重合则取$o_i=o_{i-1}$，若两轴平行且不重合则过$o_{i-1}$点做两轴的公法线，此公法线与$z_i$轴的交点即为$o_i$。
② 由于没有$z_{-1}$,故$o_0$的无法按上面的方法确定，若$z_0$与$z_1$相交则$o_0=o_1$，若$z_0$与$z_1$不相交取$o_0$在$z_0$与$z_1$的公法线上。

• 第三步：确定各坐标系的X轴：

基本原则为：选取$x_i$轴沿过$z_{i-1}$轴和$z_i$ 轴的公法线，方向从 $z_{i-1}$轴指向$z_i$ 轴。

注意：
① 当$z_{i-1}$轴与$z_i$ 轴 重合时（这时$o_i=o_{i-1}$），选取$x_i$轴满足在初始位置时$x_i$轴与$x_{i-1}$轴重合。
② 当$z_{i-1}$轴与$z_i$ 轴相交且不重合时，选择$x_i=\pm (z_{i-1}\times z_i)$。通常使所有平行的X轴有相同的指向。
③ 当i=0时，由于$o_0=o_1$或$o_0$在$z_0$与$z_1$的公法线上，选取在初始位置时$x_0$轴与$x_1$轴重合。

• 第四步：确定各坐标系的Y轴：

基本原则为：使$y_i=z_i\times x_i$构成右手坐标系。

---

· 标准DH参数含义

1、杆件长度$a_i$：定义为从$z_{i-1}$轴到$z_i$轴的距离，沿$x_i$轴的指向为正；

2、杆件扭角${\alpha }_i$：定义为从$z_{i-1}$轴到$z_i$轴的转角，绕$x_i$轴正向转动为正，且${\alpha }_i\in (-\pi ,\pi ]$；

3、关节距离$d_i$：定义为从$x_{i-1}$轴到$x_i$轴的距离，沿$z_{i-1}$轴的指向为正；

4、关节扭角${\theta }_i$：定义为从$x_{i-1}$轴到$x_i$轴的转角，绕$z_{i-1}$轴正向转动为正，且${\theta }_i\in (-\pi ,\pi ]$；

---

※ 关于DH参数以哪个轴的指向为准的问题

这个问题我开始有点乱，为什么$a_i$是以$x_i$轴的指向为正而不是$x_{i-1}$，而$d_i$就是以$z_{i-1}$轴的指向为正而不是$z_i$呢？

我想考了一下，我觉得这个是根据前面的建立各连杆的坐标系的方法决定的，其实抛开这些概念，将坐标系标在机器人结构图上就明白了，比如$a_i$表示的是连杆长，也就是z轴间的距离，只有$x_i$的方向才能表示出从$z_{i-1}$轴到$z_i$的长度，于是才选定$x_i$的方向为正。$d_i$同理

【！】这是我的理解，不知道是不是正确

---

○ 改进DH参数定义及方法简介

· 连杆坐标系建立方法

• 第一步：确定各坐标系的Z轴：

基本原则为：选取$z_i$轴为关节$i$的轴向 。

• 第二步：确定各坐标系的原点：

基本原则为：选取原点$o_i$在过$z_i$轴和$z_{i+1}$轴的公法线上。

注意：
当$z_i$轴和$z_{i+1}$轴平行时，两轴的公法线有无数条，若两轴重合则取$o_i=o_{i-1}$（【？？？】这里存疑，我是按照标准DH写的），若两轴平行且不重合则过$o_{i-1}$点做两轴的公法线，此公法线与$z_i$轴的交点即为$o_i$。

• 第三步：确定各坐标系的X轴：

基本原则为：选取$x_i$轴沿过$z_i$轴和$z_{i+1}$轴的公法线，方向从$z_i$轴指向$z_{i+1}$轴。

注意：
① 当$z_i$轴与$z_{i+1}$ 轴 重合时（这时$o_i=o_{i-1}$），选取$x_i$轴满足在初始位置时$x_i$轴与$x_{i-1}$轴重合。（【！！！】存疑，我是按照标准DH写的）
② 当$z_{i-1}$轴与$z_i$ 轴相交且不重合时，选择$x_i=\pm (z_{i-1}\times z_i)$。通常使所有平行的X轴有相同的指向。

• 第四步：确定各坐标系的Y轴：

基本原则为：使$y_i=z_i\times x_i$构成右手坐标系。

---

· 改进DH参数含义

1、杆件长度$a_{i-1}$：定义为从$z_{i-1}$轴到$z_i$轴的距离，沿$x_{i-1}$轴的指向为正；

2、杆件扭角${\alpha }_{i-1}$：定义为从$z_{i-1}$轴到$z_i$轴的转角，绕$x_{i-1}$轴正向转动为正，且${\alpha }_i\in (-\pi ,\pi ]$；

3、关节距离$d_i$：定义为从$x_{i-1}$轴到$x_i$轴的距离，沿$z_i$轴的指向为正；

4、关节扭角${\theta }_i$：定义为从$x_{i-1}$轴到$x_i$轴的转角，绕$z_i$轴正向转动为正，且${\theta }_i\in (-\pi ,\pi ]$；

---

※ 关于这里为什么是$a_{i-1}$与${\alpha }_{i-1}$

我的理解是，DH参数的作用是描述第$i$个连杆的起始位置的参数，由于改进DH的坐标是建立在连杆的前端，也就是驱动轴上，那么能描述第$i$个连杆的参数也就是该连杆的驱动轴与传动轴的距离（或者说关节偏置）$d_i$（对于转动关节来说），以及第$i$个连杆与第$i-1$个连杆的相对位置的参数就是第$i-1$个连杆的长度$a_{i-1}$和夹角${\alpha }_{i-1}$。

如【机械臂运动学教程】机械臂+旋转矩阵+变换矩阵+DH+逆解+轨迹规划+机器人+教程视频中所示

其实在用改进DH（M-DH）建立机器人模型时不需要考虑那么多，只要把连杆对应的这一行参数填进去就行。

---

※ 关于DH参数的作用

为什么需要四个DH参数，刚开始我还不太懂，后来看了这个视频，老师讲的很明白

→→→【【机械臂运动学教程】机械臂+旋转矩阵+变换矩阵+DH+逆解+轨迹规划+机器人+教程】
P4 4.机械臂几何法与DH表示法

用$a$和$\alpha$可以表示出同一个杆的驱动轴和传动轴之间的变化关系

当有多个杆件串联时，需要$d$和$\theta$来表示两个杆件之间的相对位置关系

其中，在旋转关节中，$\theta$是变量，在平移关节中，$d$是变量

---

△ ZK-500机器人DH建模过程及分析

○ 机器人尺寸参数

这个机器人时南通振康生产的500Kg重载机器人（国产哦），现在用来做机器人搅拌摩擦焊的实验

• 机器人的尺寸及工作范围如下

---

○ 标准DH（STD-DH）模型建立及验证

关于工业机器人标准DH（STD-DH）建模的参考结果可以见这篇文章

→→→【工业六轴机器人常见的STD(标准)-DH模型建立方法】
只是他的方法和我的z轴的方向不同

同时标准DH的正逆解验证也可以参考同一个作者的这篇文章

→→→【SD-H模型运动学正解、逆解及姿态角的解算验证】

· 标准DH模型建立过程

• 确定z轴方向

基本原则为：选取$z_i$轴为关节i+1的轴向（如上面图片所示，如z1在轴2上）（指向可以任选，但是通常将各平行的Z轴的指向取为相同）。另外，机器人远端没有关节n+1，这时可以选取$z_n$轴和$z_{n+1}$轴重合 。

---

• 确定原点位置

基本原则为：选取原点$o_i$在过$z_{i-1}$轴和$z_i$轴的公法线上（即公法线与$z_i$轴的交点）。
注意：
① 当$z_{i-1}$轴和$z_i$轴平行时，两轴的公法线有无数条，若两轴重合则取$o_i=o_{i-1}$，若两轴平行且不重合则过$o_{i-1}$点做两轴的公法线，此公法线与$z_i$轴的交点即为$o_i$。
② 由于没有$z_{-1}$,故$o_0$的无法按上面的方法确定，若$z_0$与$z_1$相交则$o_0=o_1$，若$z_0$与$z_1$不相交取$o_0$在$z_0$与$z_1$的公法线上。

---

• 确定x轴方向

基本原则为：选取$x_i$轴沿过$z_{i-1}$轴和$z_i$ 轴的公法线，方向从 $z_{i-1}$轴指向$z_i$ 轴。
注意：
① 当$z_{i-1}$轴与$z_i$ 轴 重合时（这时$o_i=o_{i-1}$），选取$x_i$轴满足在初始位置时$x_i$轴与$x_{i-1}$轴重合。
② 当$z_{i-1}$轴与$z_i$ 轴相交且不重合时，选择$x_i=\pm (z_{i-1}\times z_i)$。通常使所有平行的X轴有相同的指向。
③ 当i=0时，由于$o_0=o_1$或$o_0$在$z_0$与$z_1$的公法线上，选取在初始位置时$x_0$轴与$x_1$轴重合。

---

• 确定y轴方向

基本原则为：使$y_i=z_i\times x_i$构成右手坐标系。

---

· ZK-500机器人标准DH（STD-DH）参数

1、杆件长度$a_i$：定义为从$z_{i-1}$轴到$z_i$轴的距离，沿$x_i$轴的指向为正；
2、杆件扭角${\alpha }_i$：定义为从$z_{i-1}$轴到$z_i$轴的转角，绕$x_i$轴正向转动为正，且${\alpha }_i\in (-\pi ,\pi ]$；
3、关节距离$d_i$：定义为从$x_{i-1}$轴到$x_i$轴的距离，沿$z_{i-1}$轴的指向为正；
4、关节扭角${\theta }_i$：定义为从$x_{i-1}$轴到$x_i$轴的转角，绕$z_{i-1}$轴正向转动为正，且${\theta }_i\in (-\pi ,\pi ]$；

---

· 验证标准DH模型参数

使用matlab机器人工具箱验证标准DH模型

建立机器人模型

d1=1050;a1=500;a2=1300;a3=150;d4=1200;d6=390;

%        theta(z)   d(z)     a(x)     alpha(x)  
L1=Link([  0       d1       a1        pi/2     ],'standard');
L2=Link([  0       0        a2         0       ],'standard');L2.offset=pi/2;
L3=Link([  0       0        a3        pi/2     ],'standard');
L4=Link([  0       d4       0        -pi/2     ],'standard');
L5=Link([  0       0        0         pi/2     ],'standard');
L6=Link([  0       d6       0          0       ],'standard');
Robot_6=SerialLink([L1,L2,L3,L4,L5,L6],'name','ZK-500');
Robot_6.teach;

注意这里的L2.offset=pi/2;，是对应连杆2的90°的补偿，

运行结果

---

○ 改进DH（M-DH）模型建立过程及验证

关于工业机器人标准DH（STD-DH）建模的参考结果可以见这篇文章（和上面那篇同一个作者）

→→→【工业六轴机器人常见的MOD(改进)-DH模型建立方法】
只是他的方法和我的z轴的方向不同

同时改进DH的正逆解验证也可以参考同一个作者的这篇文章

→→→【MD-H模型运动学正解、逆解及姿态角的解算验证】

· 改进DH模型建立过程

• 确定z轴方向

根据原则，选取$z_i$轴为关节$i$的轴向 。

确定各关节z轴方向如图所示：

关于z轴方向的问题，理论上来说都可以，但是我看有的教程说要按照轴旋转的方向确定z轴的朝向，为此我还特地去查看了下机器人的各轴实际转动方向，但是转念一想，旋转方向如果实际和坐标不一致，那么只要在带入求解的时候加个负号就行了，但是如果在建立DH模型时就考虑方向，那么模型的通用型就会受到限制。

于是我决定按照：轴向沿竖直方向的轴（如1轴）方向向上，轴向沿水平方向的轴（如2、3、5轴）方向向外，轴向指向末端的轴（如4、6轴）方向指向末端的习惯来确定z轴的朝向。

---

• 确定原点位置

基本原则为：选取原点$o_i$在过$z_i$轴和$z_{i+1}$轴的公法线上。
注意：
当$z_{i-1}$轴和$z_i$轴平行时，两轴的公法线有无数条，若两轴重合则取$o_i=o_{i-1}$（【！！！】存疑），若两轴平行且不重合则过$o_{i-1}$点做两轴的公法线，此公法线与$z_i$轴的交点即为$o_i$。

按照以上原则，标注各连杆坐标系原点如图并适当平移了z轴的位置：

连杆4、5、6的坐标系的原点在同一个点，这里将5、6杆坐标系平移到不重叠的位置。

---

※ 关于6杆的原点位置的选定

按照传统的标准DH和改进DH模型，6杆的坐标原点都是应该建在机器人末端6轴的轴线位置的，但是如果将6杆的坐标原点移动到和4、5杆重合的位置，同时建立一个工具坐标系，将6杆的长度算作工具长度的一部分，则会大大简化逆解运算的复杂程度（听我师弟说的）。

原因我后面再讲（如果我写到后面能记起来的话。。。）

---

• 确定x轴方向

基本原则为：选取$x_i$轴沿过$z_i$轴和$z_{i+1}$轴的公法线，方向从$z_i$轴指向$z_{i+1}$轴。
注意：
① 当$z_i$轴与$z_{i+1}$ 轴 重合时（这时$o_i=o_{i-1}$），选取$x_i$轴满足在初始位置时$x_i$轴与$x_{i-1}$轴重合。（【！！！】存疑）
② 当$z_{i-1}$轴与$z_i$ 轴相交且不重合时，选择$x_i=\pm (z_{i-1}\times z_i)$。通常使所有平行的X轴有相同的指向。

顺便标上了基座坐标系和工具坐标系

---

• 确定y轴方向

基本原则为：使$y_i=z_i\times x_i$构成右手坐标系。

完成！

然后和大佬做的比较一下（其实我就是一边比较一边做的。。。）

简直一模一样

---

· ZK-500机器人改进DH（M-DH）参数

1、杆件长度$a_{i-1}$：定义为从$z_{i-1}$轴到$z_i$轴的距离，沿$x_{i-1}$轴的指向为正；
2、杆件扭角${\alpha }_{i-1}$：定义为从$z_{i-1}$轴到$z_i$轴的转角，绕$x_{i-1}$轴正向转动为正，且${\alpha }_i\in (-\pi ,\pi ]$；
3、关节距离$d_i$：定义为从$x_{i-1}$轴到$x_i$轴的距离，沿$z_i$轴的指向为正；
4、关节扭角${\theta }_i$：定义为从$x_{i-1}$轴到$x_i$轴的转角，绕$z_i$轴正向转动为正，且${\theta }_i\in (-\pi ,\pi ]$；

---

· 验证改进DH模型参数

使用matlab机器人工具箱验证改进DH模型

建立机器人模型

d1=1050;a1=500;a2=1300;a3=150;d4=1200;d6=390;

%        theta(z)   d(z)     a(x)     alpha(x)  
L1=Link([  0       d1       0        0       ],'modified');
L2=Link([  0       0        a1       pi/2    ],'modified');L2.offset=pi/2;
L3=Link([  0       0        a2       0       ],'modified');
L4=Link([  0       d4       a3       pi/2    ],'modified');
L5=Link([  0       0        0       -pi/2    ],'modified');
L6=Link([  0       d6       0        pi/2    ],'modified');
Robot_6=SerialLink([L1,L2,L3,L4,L5,L6],'name','ZK-500');
Robot_6.teach;

这里没有完全按照上面的表格来配置参数，也就是没有将杆6的偏置作为工具的长度，而是按照一般的方法将其带入了杆6的参数

结果如下

虽然长得有点奇怪，但是拖动了一下各个轴，结果还是比较正常的。

OK，至此完成了ZK-500机器人的DH建模

---

❤ 2023.6.14 ❤

※ 关于机器人的变换矩阵

在总结机器人的正逆运动学之前先简单介绍下机器人的变换矩阵。

其实之前学习相关内容的时候都是当做定义来记的，但是整理资料的时候发现里面忽略的内容还不少，这里就简单记录一下。

参考的资料如下：

→→→【关于机器人运动学中变换矩阵左乘右乘的理解】
作者以他自己的理解说明了变换矩阵左乘与右乘的区别与联系

→→→【如何通俗地解释欧拉角？之后为何要引入四元数？】
这是篇讲解欧拉角的文章，突出一个通俗和简单易懂

→→→【三维旋转之欧拉角】
这篇文章同样比较通俗的讲解了欧拉角的概念，同时给出了欧拉角旋转顺序的12钟不同组合

△ 空间变换的描述

○ 欧拉角

欧拉角的定义很简单，就是绕xyz三个轴旋转，旋转矩阵的推导也很简单

但是用欧拉角来描述一个物体的旋转运动会有不同的情况

• 旋转矩阵的左乘/右乘与外旋/内旋

根据【关于机器人运动学中变换矩阵左乘右乘的理解】的描述，左乘旋转矩阵是坐标系不动，即外旋；右乘旋转矩阵是坐标系随着旋转而变化，即内旋

【三维旋转之欧拉角】中的描述如下

---

• 欧拉角的旋转顺序

引用【如何通俗地解释欧拉角？之后为何要引入四元数？】中的内容：

如下图所示，xyz表示世界坐标系，XYZ表示物体局部坐标系，

将上图的运动进行分解

这是其中一种欧拉角的描述方法，根据不同的旋转顺序，一共有12种不同的描述方法，对应的旋转矩阵也不同

【三维旋转之欧拉角】记录如下

---

• 欧拉角描述存在的问题

用欧拉角描述一个旋转变换会出现“万向节锁死”的情况

具体还是看这里【如何通俗地解释欧拉角？之后为何要引入四元数？】

---

○ 四元数

关于什么是四元数，这是一个很复杂的数学概念，可以参考

→→→【四元数——基本概念】
这篇文章讲的还是比较通俗易懂的（但是我依然没看懂。。。）

→→→【带你探秘四维的神秘数字——四元数】
这是一个国内up的视频

→→→【四元数的可视化】——3Blue1Brown
这是一个外国的著名的讲解各种数学问题的账号

emmm。。。看了之后我就觉得。。。以后用到了再看吧。。。

---

○ 齐次变换矩阵

齐次变换矩阵就是把旋转矩阵与平移矩阵写在一个矩阵里

关于标准DH和改进DH的坐标系间的齐次变换矩阵为什么是这样定义的呢？
我也很疑惑，但是根据我在《机器人学、机器视觉与控制》这本书P156看到的内容，这个是发明和改进DH建模方法的大佬规定的。。。

---

· 标准DH坐标系间的齐次变换矩阵

---

· 改进DH坐标系间的齐次变换矩阵

【 ！！】以下这个变换矩阵是【（3）机器人的DH参数建模详解】这篇文章中给出的变换矩阵，但是根据前面的改进DH建模的定义，这里的$a_i$和${\alpha }_i$应该为$a_{i-1}$和${\alpha }_{i-1}$

改正后的坐标系间齐次变换矩阵（这个是《机器人学、机器视觉与控制》的内容，不会有错的！）

$_i^{i-1}A=R_x(\alpha_{i-1})T_x(a_{i-1})R_z(\theta_i)$

---

机器人正运动学

考虑到我看到的资料里大部分的分析都是在改进DH模型基础上进行的，所以这里我只用改进DH方法

还有一个原因，matlab机器人工具箱针对标准DH方法建立的机器人模型有预设的求逆解得函数，但是改进DH的求逆解函数并不能用（至少我之前学的时候是这样的），需要自己写。

△ 齐次变换矩阵

首先根据坐标系间的齐次变换矩阵，推导出完整的机器人的齐次变换矩阵。