---

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

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

❤ 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。。。看了之后我就觉得。。。以后用到了再看吧。。。

---

○ 齐次变换矩阵

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

齐次变换矩阵也可以写成这样的形式

$$T=\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]$$

其中旋转矩阵

$$R=\left[\begin{array}{ccc}{n}_x& o_x& a_x\\ n_y& o_y& a_y\\ n_z& o_z& a_z\end{array}\right]$$

中的列向量表示如下含义

→→→《机器人运动学精品教程》

如果一个坐标系除了有旋转变换还有平移变换时

---

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

---

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

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

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

$$\begin{array}{rl}{}_i^{i-1}A& =R_x\left({\alpha }_{i-1}\right)T_x\left(a_{i-1}\right)R_z\left({\theta }_i\right)T_z\left(d_i\right)\\ & =\left[\begin{array}{cccc}c{\theta }_i& -s{\theta }_i& 0& a_{i-1}\\ s{\theta }_{i}c{\alpha }_{i-1}& c{\theta }_{i}c{\alpha }_{i-1}& -s{\alpha }_{i-1}& -s{\alpha }_{i-1}{d}_i\\ s{\theta }_{i}s{\alpha }_{i-1}& c{\theta }_{i}s{\alpha }_{i-1}& c{\alpha }_{i-1}& c{\alpha }_{i-1}{d}_i\\ 0& 0& 0& 1\end{array}\right]\end{array}$$

---

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

※ 关于齐次变换矩阵的意义

齐次变换矩阵是对两个坐标系相对位置的描述。

可以理解成坐标系$o_{i-1}$经过齐次变换矩阵${}_i^{i-1}T$得到$o_i$。${}_i^{i-1}T$为坐标系$o_i$在坐标系$o_{i-1}$中的表示。

• 下面通过一个简单的例子来说明

如下图坐标系，假设z方向的距离和转角都为零，将空间变换转化为平面坐标中表示

这两个坐标系间的齐次变换矩阵为

$${}_i^{i-1}T=\left[\begin{array}{cccc}\cos \alpha & -\sin \alpha & 0& x_0\\ \sin \alpha & \cos \alpha & 0& y_0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

此时在$o_{i-1}$中有一点$P$，他在坐标系$o_{i-1}$中的坐标向量为$\overrightarrow{p_{i-1}}=[\begin{array}{ccc}{x}_1& y_1& 0\end{array}{]}^{\mathrm{T}}$，要想得到他在变换后的坐标系$o_i$中的坐标向量$\overrightarrow{p_i}[\begin{array}{ccc}{x}_2& y_2& 0\end{array}{]}^{\mathrm{T}}$

此时两个向量坐标之间的变化关系为

$$\{\begin{array}{c}{x}_1=x_2\cos \alpha -y_2\sin \alpha +x_0\\ y_1=x_2\sin \alpha +y_2\cos \alpha +y_0\end{array}$$

写成矩阵相乘的形式

$$\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\\ 1\end{array}\right]=\left[\begin{array}{cccc}\cos \alpha & -\sin \alpha & 0& x_0\\ \sin \alpha & \cos \alpha & 0& y_0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\\ 1\end{array}\right]$$

可以发现，这个向量变化关系可以写成

$$\overrightarrow{p_i}={}_i^{i-1}T\cdot \overrightarrow{p_{i-1}}$$

于是可以认为 【齐次变换矩阵与变换后的坐标矩阵相乘，可以得到变换前的坐标】。

如果此时坐标向量为机器人末端点在工具坐标系中的位置向量，那么将此向量依次左乘机器人从末端到基座的齐次变换矩阵，则会得到末端点在机器人基座坐标系中的坐标向量。

$$\overrightarrow{p_0}={}_1^{0}T{}_2^{1}T{}_3^{2}T{}_4^{3}T{}_5^{4}T{}_6^{5}T{}_t^{6}T\cdot \overrightarrow{p_t}={}_t^{0}T\cdot \overrightarrow{p_t}$$

此时${}_6^{0}T$即为机器人的齐次变换矩阵

$${}_t^{0}T={}_1^{0}T{}_2^{1}T{}_3^{2}T{}_4^{3}T{}_5^{4}T{}_6^{5}T{}_t^{6}T$$

【！！】齐次变换矩阵${}_i^{i-1}T$表示的意义是连杆坐标系$o_i$在连杆坐标系$o_{i-1}$中的描述
见→→→【齐次变换矩阵相乘的物理意义（欧拉角的矩阵相乘顺序）】

---

※ 关于齐次变换矩阵的左乘/右乘

简单来说，从数学上来说，左乘是对原矩阵进行行变换，右乘进行列变换；从变换矩阵的角度来说，左乘旋转矩阵绕固定坐标系旋转，右乘旋转矩阵绕自身坐标系旋转.

但是单纯的这样理解很容易让人误解。

我在查阅了很多资料后，发现这篇文章讲的比较清楚详细

→→→【旋转矩阵及左右乘的意义，看这一篇就够了】

文章比较长，我就不搬了，需要的同学自己跳转哈~

---

机器人正运动学

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

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

△ 正运动学位姿描述矩阵

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

$${}_1^{0}T=\left[\begin{array}{cccc}{c}_1& -s_1& 0& 0\\ s_1& c_1& 0& 0\\ 0& 0& 1& d_1\\ 0& 0& 0& 1\end{array}\right]$$

$${}_2^{1}T=\left[\begin{array}{cccc}{c}_2& -s_2& 0& a_1\\ 0& 0& -1& 0\\ s_2& c_2& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

$${}_3^{2}T=\left[\begin{array}{cccc}{c}_3& -s_3& 0& a_2\\ s_3& c_3& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

$${}_4^{3}T=\left[\begin{array}{cccc}{c}_4& -s_4& 0& a_3\\ 0& 0& -1& -d_4\\ s_4& c_4& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

$${}_5^{4}T=\left[\begin{array}{cccc}{c}_5& -s_5& 0& 0\\ 0& 0& 1& 0\\ -s_5& -c_5& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

$${}_6^{5}T=\left[\begin{array}{cccc}{c}_6& -s_6& 0& 0\\ 0& 0& -1& 0\\ s_6& c_6& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

$${}_t^{6}T=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_t\\ 0& 0& 0& 1\end{array}\right]$$

• matlab代码

syms Q1 Q2 Q3 Q4 Q5 Q6  d1 d4 dt a1 a2 a3 

%ZK-500连杆间齐次变换矩阵
T_01 =[ cos(Q1),   -sin(Q1),    0,      0
        sin(Q1),    cos(Q1),    0,      0
        0,          0,          1,      d1
        0,          0,          0,      1];
T_12 =[ cos(Q2),   -sin(Q2),    0,      a1
        0,          0,         -1,      0
        sin(Q2),    cos(Q2),    0,      0
        0,          0,          0,      1];
T_23 =[ cos(Q3),   -sin(Q3),    0,      a2
        sin(Q3),    cos(Q3),    0,      0
        0,          0,          1,      0
        0,          0,          0,      1];
T_34 =[ cos(Q4),   -sin(Q4),    0,      a3
        0,          0,         -1,     -d4
        sin(Q4),    cos(Q4),    0,      0
        0,          0,          0,      1];
T_45 =[ cos(Q5),   -sin(Q5),    0,      0
        0,          0,          1,      0
       -sin(Q5),   -cos(Q5),    0,      0
        0,          0,          0,      1];
T_56 =[ cos(Q6),   -sin(Q6),    0,      0
        0,          0,         -1,      0
        sin(Q6),    cos(Q6),    0,      0
        0,          0,          0,      1];
T_6t=[  1           0           0       0
        0           1           0       0
        0           0           1       dt
        0           0           0       1];

由于连杆间的齐次变换矩阵表示的是后一个坐标系在前一个坐标系中的表示（如${}_1^{0}T$表示的时杆1的坐标系在基座坐标系中的表示），于是将机器人各连杆间的齐次变换矩阵按从末端到基座的顺序连续左乘，即可得到末端相对于基座坐标系的位姿描述矩阵。

工具坐标系与杆6坐标系转角保持不变，只是沿$z_6$轴平移一定距离，在计算机器人整体齐次变换矩阵时不计算在内。

但是注意因为建立机器人改进DH模型时将4、5、6连杆的坐标系原点重合，因此连杆6的长度也被视为末端工具长度的一部分。

$${}_6^{0}T={{}_1^{0}T}_2^1{T}_3^2{T}_4^3{T}_5^4{T}_6^{5}T=\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]$$

其中各项元素为：

（简化表示：$s_1=\sin {\theta }_1$，$c_1=\cos {\theta }_1$，$s_{23}=\sin \left({\theta }_2+{\theta }_3\right)$，$c_{23}=\cos \left({\theta }_2+{\theta }_3\right)$）

$$\begin{array}{rl}{n}_x& =s_6\left(c_4{s}_1-s_4{c}_1{c}_{23}\right)-c_6\left(s_5{c}_1{s}_{23}-c_5\left(s_1{s}_4+c_4{c}_1{c}_{23}\right)\right)\\ n_y& =-c_6\left(s_5{s}_1{s}_{23}+c_5\left(c_1{s}_4-c_4{s}_1{c}_{23}\right)\right)-s_6\left(c_1{c}_4+s_4{s}_1{c}_{23}\right)\\ n_z& =c_6\left(s_5{c}_{23}+c_4{c}_5{s}_{23}\right)-s_4{s}_6{s}_{23}\\ o_x& =s_6\left(s_5{c}_1{s}_{23}-c_5\left(s_1{s}_4+c_4{c}_1{c}_{23}\right)\right)+c_6\left(c_4{s}_1-s_4{c}_1{c}_{23}\right)\\ o_y& =s_6\left(s_5{s}_1{s}_{23}+c_5\left(c_1{s}_4-c_4{s}_1{c}_{23}\right)\right)-c_6\left(c_1{c}_4+s_4{s}_1{c}_{23}\right)\\ o_z& =-s_6\left(s_5{c}_{23}+c_4{c}_5{s}_{23}\right)-c_6{s}_4{s}_{23}\\ a_x& =c_5{c}_1{s}_{23}+s_5\left(s_1{s}_4+c_4{c}_1{c}_{23}\right)\\ a_y& =c_5{s}_1{s}_{23}-s_5\left(c_1{s}_4-c_4{s}_1{c}_{23}\right)\\ a_z& =c_4{s}_5{s}_{23}-c_5{c}_{23}\\ p_x& =a_1{c}_1+a_3{c}_1{c}_{23}+d_4{c}_1{s}_{23}+a_2{c}_1{c}_2\\ p_y& =a_1{s}_1+a_3{s}_1{c}_{23}+d_4{s}_1{s}_{23}+a_2{c}_2{s}_1\\ p_z& =d_1+a_2{s}_2+a_3{s}_{23}-d_4{c}_{23}\end{array}$$

• matlab代码

%连杆间齐次变换矩阵相乘得到机器人齐次变换矩阵
T_0t=T_01*T_12*T_23*T_34*T_45*T_56;
nx=T_0t(1,1);
ny=T_0t(2,1);
nz=T_0t(3,1);
ox=T_0t(1,2);
oy=T_0t(2,2);
oz=T_0t(3,2);
ax=T_0t(1,3);
ay=T_0t(2,3);
az=T_0t(3,3);
px=T_0t(1,4);
py=T_0t(2,4);
pz=T_0t(3,4);

---

△ 正运动学验证

这里用机器人工具箱的正解函数来验证机器人正运动学的位姿描述矩阵。将相同的关节角同时带入位姿描述矩阵与机器人工具箱，代码如下

clc
clear all;

%这里的输入为角度
theta1=10;
theta2=10;
theta3=10;
theta4=10;
theta5=10;
theta6=10;

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

%将角度转化为弧度
Q1=theta1/180*pi;
Q2=theta2/180*pi+pi/2;
Q3=theta3/180*pi;
Q4=theta4/180*pi;
Q5=theta5/180*pi;
Q6=theta6/180*pi;


%ZK-500连杆间齐次变换矩阵
T_01 =[ cos(Q1),   -sin(Q1),    0,      0
        sin(Q1),    cos(Q1),    0,      0
        0,          0,          1,      d1
        0,          0,          0,      1];
T_12 =[ cos(Q2),   -sin(Q2),    0,      a1
        0,          0,         -1,      0
        sin(Q2),    cos(Q2),    0,      0
        0,          0,          0,      1];
T_23 =[ cos(Q3),   -sin(Q3),    0,      a2
        sin(Q3),    cos(Q3),    0,      0
        0,          0,          1,      0
        0,          0,          0,      1];
T_34 =[ cos(Q4),   -sin(Q4),    0,      a3
        0,          0,         -1,     -d4
        sin(Q4),    cos(Q4),    0,      0
        0,          0,          0,      1];
T_45 =[ cos(Q5),   -sin(Q5),    0,      0
        0,          0,          1,      0
       -sin(Q5),   -cos(Q5),    0,      0
        0,          0,          0,      1];
T_56 =[ cos(Q6),   -sin(Q6),    0,      0
        0,          0,         -1,      0
        sin(Q6),    cos(Q6),    0,      0
        0,          0,          0,      1];
T_6t=[  1           0           0       0
        0           1           0       0
        0           0           1       dt
        0           0           0       1];

%连杆间齐次变换矩阵相乘得到机器人齐次变换矩阵
T_06=T_01*T_12*T_23*T_34*T_45*T_56;
nx=T_06(1,1);
ny=T_06(2,1);
nz=T_06(3,1);
ox=T_06(1,2);
oy=T_06(2,2);
oz=T_06(3,2);
ax=T_06(1,3);
ay=T_06(2,3);
az=T_06(3,3);
px=T_06(1,4);
py=T_06(2,4);
pz=T_06(3,4);

%----------
%机器人工具箱建立机器人模型
%        theta(z)   d(z)     a(x)     alpha(x)  
L1=Link([  0       d1       0        0       ],'modified');
L2=Link([  0       0        a1       pi/2    ],'modified');
%将关节2的角度在初始值处补偿
%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       0        0        pi/2    ],'modified');
Robot_6=SerialLink([L1,L2,L3,L4,L5,L6],'name','ZK-500');


%----------
%同时输出结果
disp('齐次变换矩阵正解：');
T_06

disp('机器人工具箱正解：');
Robot_6.fkine([Q1 Q2 Q3 Q4 Q5 Q6])

注意这里轴2的转角$Q_2$要注意与轴1转角之间有90°的补偿量，需要在带入变换矩阵前添加进来，且在机器人工具箱中将补偿量去掉。

结果如下：

结果是一样的，但是变换矩阵的结果前面成了个1.0e+03的系数，怎么能把这个去掉呢。。。。【 ？】

好吧，正运动学就到这里。

---

❤ 2023.6.19 ❤

机器人逆运动学

CSDN提示我字数太多，一篇发不下，只好拆开，逆运动学的内容见

→→→【工业机器人运动学与Matlab正逆解算法学习笔记（用心总结一文全会）（二）】