中正平和的机器人学笔记——1. 机械臂正运动学（附MATLAB代码）

中正平和的机器人学笔记——1.正运动学

0. 何为运动学？何为正运动学？

运动学主要研究的就是机械臂的运动特性（位置、速度、加速度等），但是不考虑使机械臂产生运动时施加的力和力矩（这部分是动力学）。而正运动学的研究即是将关节变量作为自变量，进而研究操作臂末端执行器的位姿与基座间的函数关系。

1. D-H参数法

想要研究一个物体就要有一种模型/一种方法去描述它，在这个基础上再进行研究。毋庸多言，直接给出最经典的D-H参数法，DH是两位大佬（Denavit和Hartenberg）名字的缩写。另外DH参数法有标准形式有改进形式，如果四个参数的下标是相同的“i”则是标准形式，如果有a和α下标是“i-1”而d和θ是“i”则是改进型是，改进形式记起来麻烦但是实际应用时则更为方便。

D-H参数法采用4个运动学参数来描述一个连杆：

由上图可以看到，四个参数分别为：
连杆长度（link length） a_i-1：关节轴i-1和关节轴i之间公垂线的长度；
连杆扭转角（link twist）α_i-1：关节轴i-1和关节轴i投影到垂直于 a_i-1的平面后构成的角（遵从右手法则，从i-1轴绕a_i-1转向i轴）；
连杆偏距（link offset）d_i：沿i轴方向，a_i-1和a_i的距离，即a_i-1与关节轴i的交点到a_i与关节轴i的交点的距离；
关节角（joint angle）θ_i：平移a_i-1和a_i，绕关节轴i旋转成的夹角。
通过D-H参数，我们可以有效地描述一个机构（连杆），例如对于6自由度（6个关节全是旋转关节）机械臂，我们需要6组18个参数就可以完全描述固定的运动学参数（a，α，d），θ是自变量。

2. 连杆坐标系

通常按照如下方式确定连杆上的坐标系：坐标系{i}的Z轴称为Z_i，并与i轴重合；原点位于连杆长a_i与关节轴i的交点处，X_i沿a_i方向由关节i指向关节i+1；Y_i则由右手定则确定。建立好的坐标系如下图：

根据上图坐标系，D-H参数也可定义为：
a_i-1：沿X_i-1轴，从Z_i-1移动到Z_i的距离；
α_i-1：绕X_i-1轴，从Z_i-1旋转到Z_i的角度；
d_i： 沿Z_i-1轴，从X_i-1移动到X_i的距离；
θ_i： 绕Z_i-1轴，从X_i-1旋转到X_i的角度。

这样理解D-H参数后，不论是使用还是记忆起来也会更加方便。
这里想说一点，在机器人学的学习过程中，一定要注意不能仅仅记忆公式，要通过联想空间中坐标系的变化取理解，死记硬背是不行的（这一点在动力学部分更是如此）。

3. 连杆变换的推导—结合实例Puma560机械臂

数学工具我们都已经准备好了，现在就着手开始进行正运动学的推导！
首先介绍一下Puma560机械臂。它是一个六自由度的机械臂，且所有关节均为转动关节，即RRRRRR，6R机构（R代表该关节是旋转关节，P代表该关节是位移关节）。这款机械臂大概是所有机器人学教材都会采用的一个非常经典的例子。人们对其分析已经很多很清晰了。

我们的分析步骤如下：

1. 建立6个连杆坐标系；
2. 分析D-H参数；
3. 进行连杆变换推导；

最终我们就可以得到运动学方程。

3.1 坐标系的建立

坐标系的建立规则如下：

1. 找出各个关节轴，为了便于分析可以画出其延长线。每次分析时仅考虑相邻两个轴（i和i+1）；
2. 找出轴i和轴i+1的公垂线或者i轴和i+1轴的交点，以轴和公垂线的交点或者两轴的交点为连杆坐标系{i}的原点，注意，上述第二种情况（i轴和i+1轴相交，这在工业机器人中是非常常见的），在这种情况下a_i-1显然为0；
3. 规定Z_i的指向；
4. 规定X_i的指向；如果轴i和轴i+1相交，则规定X_i垂直于轴i和轴i+1构成的平面；
5. 按右手定则确定Y_i的方向；
6. 当第一个关节变量为0时，规定坐标系{0}和坐标系{1}重合。
   
   以上两图即是建立好的坐标系，希望大家可以动手画一画，自己试着建立连杆坐标系。包括后面正运动学和逆运动学的推导，都希望大家拿出纸笔自己试着画一画，算一算，不要只凭脑补。

3.2 D-H参数的确立

这里参照第1节，大家可以推导出Puma560 6自由度机器人的D-H参数表。如果觉得太难，可以自己画一个简单的平面两连杆机械臂来推导D-H参数，练练手后再来看这一节。
大家可以根据上表来检查一下自己推导的结果是否正确，同时也可以参照这个表，加深对D-H参数和连杆坐标系的理解。

3.3 连杆变换推导

这里就要用到齐次变换矩阵的知识，不熟悉的同学可以搜一搜相关知识再来推导。
首先是坐标系{0}到坐标系{1}的变换，显然是最简单的旋转变换；
坐标系{1}到坐标系{2}的变换，根据D-H参数表是绕X₁旋转了-90°；
坐标系{2}到坐标系{3}的变换，是有位移也有旋转；
后面的同理，大家可以根据位姿变换的原理推导出变换矩阵如下：

对于齐次变换矩阵的理解希望大家能够结合实例深入理解。

比如坐标系{3}到坐标系{4}的变换，根据齐次变换矩阵可以看出：
{3}的原点沿X₃方向移动了a₃，沿Y₃方向移动了d₄，然后{4}中一点P(x,y,z)的z就是{3}中的y，当{4}沿轴4旋转时，P的x和y则由{3}的x和z表示。

这个可能第一次看有点绕，因此更需要大家自行推导，结合实例去理解齐次变换矩阵。至少要达到看到齐次变换矩阵就能想到空间中两个坐标系位姿的图来，这样才有利于理解后面的知识。

实际上到这一步我们的任务基本上就完成了。
将上述变换矩阵连乘。最终就可以得到末端坐标下一点在基座标系下的位姿。

最后这个矩阵就是正运动学方程，矩阵中自变量是6个关节角，因此当确定了6个关节的角度以后，我们就可以确定机械臂末端位姿在基座标系下的位姿！

4. 坐标系的标准命名

最后我们再完成一点收尾工作，对于一些坐标系的命名我们需要明确一下。

根据上图，我们给几个常用的坐标系说明一下：
基座标系（the base frame）{B}：位于操作臂的基座上，就是{0}。

固定坐标系（the station frame）{S}：与任务相关，通常位于机器人工作台上的一角，通常根据{B}确定，有时也称之为任务坐标系（the task frame）、通用坐标系（the universe frame）、世界坐标系（the world frame）。

腕部坐标系（The wrist frame）{W}：原点位于操作臂手腕上，随着操作臂的末端连杆移动。

工具坐标系（The tool frame）{T}：原点定义在机器人抓持工具的末端。

目标坐标系（The goal frame）{G}：对机器人移动工具到达的位置描述。特指在机器人运动结束后时，工具坐标系应当与目标坐标系重合。

5. 正运动学MATLAB的实现

这里不得不提一下Prof. Peter Corke的工具箱¹:
Robotics Toolbox和Machine Vision Toolbox。
这两个工具箱非常便捷，当然现在MATLAB官方的Robotics工具箱也很好，大家可以都尝试一下。
这里我就以最简单的两连杆为例，利用工具箱实现其正运动学。

function T = FK_position(a1, q1, a2, q2)
%a1：连杆1的长度
%q1：连杆1的关节角
%a2：连杆2的长度
%q2：连杆2的关节角

L(1) = Link([0 0 a1 0]); %建立连杆1，输入D-H参数θ、d、α、a
L(2) = Link([0 0 a2 0]);
two_link = SerialLink(L, 'name', 'two link');%建立完整的机械臂对象

TOOL = two_link.fkine([q1 q2]) 
%fkine函数是foward kinematics的缩写，即正运动学求解
%返回末端位姿的齐次变换矩阵
x = TOOL.t(1);
y = TOOL.t(2);
q0 = rad2deg(atan(y/x));
T = [x;y;q0];

two_link.plot([q1 q2])  %作图显示两连杆机械臂

正运动学部分就讲完啦，当然，讲得很简略，毕竟只是我个人学习过程的一点记录，而且我总不能把自己演算的步骤也一步步贴上来，那未免太过无味冗长。这个系列我还会继续写下去，希望大家能多提意见，谢谢！

---

1. www.petercorke.com/RVC ↩︎