机械臂仿真：使用matlab机器人工具箱与标准DH法，实现6自由度机械臂的仿真运动

0 引言

在了解了如何描述末端执行器的位姿之后，接下来我们就可以实现对机械臂的数学建模。建模的工具有很多种，本文介绍一种基于标准DH参数法与Matlab的Robotics Toolbox - Peter Corke库来实现此过程，并尝试实现6自由度机器人的仿真运动。

强烈推荐b站教学视频MATLAB机器人工具箱10.4 机械臂仿真教学（未完结），并且强烈建议大家先提前了解机器人位姿相关知识。本文多数内容基于自己的理解与参考中的链接，不妥之处多多指正。

---

1 预备知识

不严格的说，机械臂大体上可以分成三种结构：连杆（link），关节（又叫轴，axis）与基座（base）。直觉上说，我们会把坐标系固定在关节上，而非连杆上；基座指的是机器人的底座，是不动的，它又被称为连杆0。

每个坐标系（除了基座上的）我们都可以视为一个“自由度”，这意味着这个连杆是可以运动的。一般来说一个连杆只有一个自由度，要么是旋转，要么是移动，也可能有两个自由度。对于不同的连杆，建模方法是有所差异，但大致相同。

---

1.1 标准DH建模法

1.1.1 基本概念

建模的主要目的之一是为了保存两个关节与他们之间连杆的信息（例如连杆长度、旋转角度等），有了这些描述，我们就可以从基座开始，一步步把机械臂重新画出来。

标准DH法是一种常用的定义连杆坐标系的规则，由J Denavit，RS Hartenberg于1956年提出。其具体规则为图2所示：

PS：实际建模过程中，坐标系$O_{i-1}$与坐标系$O_i$’ 重合，这意味着轴$x_i$与轴$z_{i-1}$总是垂直相交。这样，我们就能用更少的参数来表达这些关节和连杆（我所指的信息是指此处的$a、\alpha 、\theta$）；并且实际建模的过程，我们大多数情况只关心z轴与x轴上旋转、位移的信息。

与其叫做标准DH建模法，笔者认为叫做提线木偶可能更好理解一些。它的核心就是，需要把坐标系建立在连杆远离基座方向的关节的那一端（例如图1中的坐标系$O_i$和连杆$i$，以此**“牵引着”**这个连杆的旋转或者移动，就像使用提线木偶一样。更形象一点：使用左手抓住右手指尖，然后拉着指尖来控制右手臂的运动，你的手臂就是连杆，你的右手指尖的位置就是关节坐标系的位置，这就是标准DH建模法的“感觉”。

补充：改进DH建模法

还有一种方法叫做改进DH建模法，它的核心就和标准DH建模法相反，它把坐标系建在连杆的靠近基座的那一个关节上，它的感觉就比如：用右手的手腕，来主动控制右手掌的旋转。不过，现实建模过程区分它们最严谨的方法，还是它们获取数据的顺序。

---

1.1.2 DH参数表格

我们建模过程获得的信息是怎么样的？如果我们有$n$个连杆，我们获得的信息的排布可能是一个$n\times 4$（如果把基座看做连杆，例如此处的连杆0）大小的表格，就比如图3：
这四个参数分别是：

1.：$\theta$表明从上一个关节所对应的坐标系的z轴，该旋转多少角度（逆时针为正）；

2.：$d$表明沿着上一个关节所对应的坐标系的z轴，该移动多少距离；

3.：$a$表明沿着上一个关节所对应的坐标系的x轴，该移动多少距离（这个时候就已经到达）；

4.：$\alpha$表明沿着上一个关节所对应的坐标系的x轴，该旋转多少角度（它可能用来调整这个自由度的旋转方向）；

这个DH参数表格中包含了所有我们需要的描述机器人的完整信息，本质上就是机器人的各个坐标系之间的“变换矩阵”。这个信息可以帮助我们从基座的坐标系一步步的从参考坐标系一直推导到末端执行器的坐标系上，从而获得末端执行器的位姿，这也是正向运动学的核心。相关的推导会放到后续的章节中（剧透：就是四个齐次变换矩阵的矩阵乘法，获得相邻两坐标系的变换矩阵）。关于机器人位姿的理解，可以参考这篇文章：机械臂位姿描述。

1.1.3 使用matlab机器人工具箱通过标准DH参数表格建立机器人

本文使用机器人工具箱Robotics Toolbox - Peter Corke以及标准DH建模法来建立机器人。核心是先确定有多少个自由度，再确定机械臂的初始位姿，最后使用几个函数生成我们的机器人，获得一个对象。
对于每一个自由度，使用link函数来获取一个连杆对象，这个对象的属性如下：

Link 的类属性（读/写）： 
theta：D-H参数 
d:D-H参数 
a:D-H参数 
alpha:D-H参数 
sigma: 默认0，旋转关节；1，移动关节 
mdh: 默认0，标准D-H；1，改进D-H 
offset:关节变量偏移量 
qlim:关节变量范围
m: 质量 
r: 质心 
I: 惯性张量 
B: 粘性摩擦 
Tc: 静摩擦 
G: 减速比 
Jm: 转子惯量

例如，我们拿图3举例，我们建立第1根连杆及其坐标系，代码如下：

L(1) = Link('revolute', 'd', 0.216, 'a', 0, 'alpha', pi/2, 'qlim', [-150 ,150]/180*pi);

第一个参数表明它是一个旋转自由度，后面的$d、a、\alpha$为标准DH参数表中的内容。为什么没有在这里定义$\theta$这个参数呢？在matlab中，如果是旋转关节，那么需要给offset这个属性赋值$\theta$。并且如果选择改进DH建模法，需要在最后加上一个参数'modified'。
我们一步步把所有图3所有连杆及其对应的关节定义出来（可以有多种不同的定义方法）：

L(1) = Link('revolute', 'd', 0.216, 'a', 0, 'alpha', pi/2, 'qlim', [-150 ,150]/180*pi);
L(2) = Link('revolute', 'd', 0, 'a', 0.5, 'alpha', 0, 'offset', pi/2, 'qlim', [-100 ,90]/180*pi);
L(3) = Link('revolute', 'd', 0, 'a', sqrt(0.145^2+0.42746^2), 'alpha', 0, 'offset', -atan(427.46/145), 'qlim', [-90 ,90]/180*pi);
L(4) = Link('revolute', 'd', 0, 'a', 0, 'alpha', pi/2, 'offset', atan(427.46/145), 'qlim', [-100 ,100]/180*pi);
L(5) = Link('revolute', 'd', 0.258, 'a', 0, 'alpha', 0, 'qlim', [-180 ,180]/180*pi);

然后，我们可以使用SerialLink类来获得一个机械臂对象：

Five_dof = SerialLink(L, 'name', '5-dof');

其次，我们需要确定0号连杆及其关节坐标系的位置。连杆0也就是基座，对应于图3第一行信息（Link：0）。这个连杆0是固定的，所以在matlab中单独定义成机械臂对象的一个属性：

% transl函数获得一个4*4变换矩阵
% transl(a, b, c) = [1  0  0  a
%                    0  1  0  b
%                    0  0  1  c
%                    0  0  0  1]
Five_dof.base = transl(0, 0, 0.28);

这个变换矩阵是针对于世界坐标系的。在matlab中，默认世界坐标系在(0, 0, 0)处，因此可以通过base这个属性，从世界坐标系处获得0号坐标系以及连杆0（也就是基座）。
想要查看这个机械臂，使用teach属性：

Five_dof.teach;

---

1.2 正向运动学与逆向运动学

1.2.1 正向运动学

简单且不严谨地说，正向运动学是指通过机械臂的关节运动的参数推导出末端执行器的位置。推导需要几个基本条件：第一，我们需要完整的DH参数表格；其次，我们需要知道关节究竟发生了怎么样的运动。我们最终得到的是末端执行器的坐标系与参考坐标系（基坐标系）的变换矩阵。

标准DH参数表获得的变换矩阵

拿图3举例。Link0与Link1对应的两个坐标系之间的变换矩阵应该等于：

其中的四个矩阵的形状都应该为$4\times 4$的大小。矩阵中元素的具体取值，请参考最后一节的第4个链接。不过，${}^0{T}_1$仅仅只是相对关系。我们要使用这个变化矩阵必须提前知道0号坐标系的位置。
假定我们拥有一个六自由度机械臂，并且每个关节只有一个绕$z$轴旋转自由度。最终，我们通过一系列矩阵乘法，就可以获得从0号坐标系到末端执行器的变换矩阵：

${\theta }_i$的含义就是指的是每个关节DH参数的第一个信息值，也就是绕$z$轴旋转的角度。
有了标准DH参数表，我们可以获得末端执行器的位置，也可以获得每个关节的位置。

1.2.2 逆向运动学

简单来说，逆向运动学能获得末端执行器到达某个位姿所需的各个关节的角度。大多数情况下，末端执行器到达某个位姿的方式可以有很多种，也可能根本没有解。获得解的方式便是反向求解$公式2$中的${\theta }_i$参数。
具体求解方法涉及较复杂的矩阵运算和线性代数知识，可以参考网上资料，或者此链接：建模+正逆运动学+雅克比矩阵

1.2.3 matlab代码实现正逆运动学

正向运动学

这里使用图3所定义的机械臂来举例。我们只需要给出每个关节的角度即可。返回一个变换矩阵（也就是${}^0{T}_6$，对于图3而言是${}^0{T}_5$，因为它只有5个自由度）。

q = [pi/2 pi/2 0 0 0];
T = Five_dof.fkine(q);

在学习的时候产生了一些疑惑。例如我这里直接给出的角度q向量和我定义机械臂（一堆Link对象）的时候给出的$\theta$之间是什么关系呢？如果是旋转关节，q是在原来标准DH参数表格上，直接对$\theta$参数加上或减去某个角度；如果是移动关节，类比下来应该是对某个移动参数作出改变，但笔者还没有验证这一点。

逆向运动学

我们给出一个变换矩阵${}^0{T}_6$（代表末端执行器的位姿），传入机器人对象中的ikunc函数就可以快速求解各个关节角度：

T = transl(0, 0, 0.28) * trotx(180);
q = Five_dof.ikunc(T);

1.3 使用matlab完成轨迹规划

想象一个简单的场景：我们想让一个机械臂的末端执行器，不发生任何旋转的方式从某一个点直线移动到另一个点。这个过程该怎么描述呢？
首先就是位置，假定一个起点以及一个终点，它们可以分别使用一个变化矩阵来描述。在这里由于没有发生任何旋转，便可使用两个坐标来描述（相对于世界坐标系）。然后需要考虑此过程中的速度$v$和加速度$a$，因此要引入一个时间$t$这个参数方便描述以上参数的变化。
总之，我们简化这个过程为4个参数：$x（位置）、t（时间）、v（速度）、a（加速度）$。轨迹规划的含义简单来说，就是在确定位置后，设计几个良好的速度和加速度，保证运行过程稳定、柔和。
如果在我们假想场景中，我们运行速度保持不变，那么机器人在开始运动的时候，速度在短时间内从$0$突变到$v$，结束运动时速度又突变为$0$，在开始和结束的加速度很大，也就对机械臂的动态冲击很大，这显然是不利于机械臂的长期运作的。
在matlab提供了很多种方法，这里介绍一个5次多项式的插值方式获得轨迹的方法。简化一下，我们只看x坐标的变化并且从$x=0$的位置移动到$x=3$的位置，时间设置为$2s$。

t = linspace(0, 2, 51);
[P, dP, ddP] = tpoly(0, 3, t);
figure
subplot(1,3,1);
plot(t, P, '.-', 'LineWidth',1);
ylabel('x');
subplot(1,3,2);
plot(t, dP, '.-', 'LineWidth',1);
ylabel('v');
subplot(1,3,3);
plot(t, ddP, '.-', 'LineWidth',1);
ylabel('a');

相比于速度恒定的轨迹，五次多项式插值后的轨迹明显平缓且加速度没有突变情况，也就更加柔和一些。
我们同时考虑x、y、z坐标，相当于把我们原来的假设增加两个维度。在matlab中，实现也是非常方便的。假设在$2s$内，我们要从坐标$(1,2,3)$移动到$(2,3,4)$：
为了方便绘制，我们写一个函数，能一次性在同一幅图中画出三个坐标的变化：

function plot_traj(Traj, t)
    hold on
    plot(t, Traj(:, 1), '.-', 'LineWidth',1);
    plot(t, Traj(:, 2), '.-', 'LineWidth',1);
    plot(t, Traj(:, 3), '.-', 'LineWidth',1);
    grid on
    legend('x', 'y', 'z');
    xlabel('times')
    ylabel('position')
end

那么这三条轨迹的图像是：

t = linspace(0, 2, 51);
% @tpoly表明选择某个插值方式
[P ,dP ,ddP] = mtraj(@tpoly, [1, 2, 3], [2, 3, 4], t);
figure
subplot(1,3,1);
plot_traj(P, t);
ylabel('x');
subplot(1,3,2);
plot_traj(dP, t);
ylabel('v');
subplot(1,3,3);
plot_traj(ddP, t);
ylabel('a');

我们使用这些函数获得的是一系列坐标值，例如图6情形所代表的矩阵$P$，其长度应该是$51\times 3$。51行是因为我们选取的时间t向量的长度为51段，3的含义是指$x，y，z$三个坐标。我们要把这些坐标转化成变化矩阵（也就是末端执行器的位姿）：

T = zeros(4, 4, 51);
for i = 1:51
% 不带任何旋转
  T(:, :, i) = transl(P(i, :));
end
% 逆向运动学
q = Five_dof.ikunc(T);

使用matlab还可以实现更多不同的插值方式（例如lspb）、多维多段的轨迹规划（mstraj），在此不多赘述。

旋转运动轨迹规划

旋转矩阵的优点就是坐标系转换非常简单，但是不容易直接通过它去规划（插值）机械臂的运动。

现在我们已知起点和终点末端执行器的变换矩阵$T_0、T_1$，我们可以由此获得它们的旋转矩阵。旋转矩阵本质上就是坐标系的旋转变换，不过它是一个相对关系（相对于基坐标系），$T_0$和$T_1$都可以由另外三次旋转来完成。怎么旋转的方式有很多种，比如欧拉角和横滚-俯仰-偏航角：

这里的${\theta }_i$角就是我们想要的。例如$公式3$，我们将获得$T_0、T_1$分别对应的${\theta }_{11}、{\theta }_{12}、{\theta }_{13}$与${\theta }_{21}、{\theta }_{22}、{\theta }_{23}$，我们类比前面对于三维坐标的插值，把这两对角度看做两对坐标，用法就和前文完全一致。使用tr2rpy或者tr2eul函数来实现转化成“两对角度”，使用rpy2tr或者eul2tr重新获得一系列变换矩阵：

T0 = transl(1, 2, 3) * trotx(180);
% 绕x轴旋转-180度
T1 = transl(1, 2, 3);
rpy1 = tr2rpy(T0)/pi*180;
rpy2 = tr2rpy(T1)/pi*180;

% 时间
t = linspace(0, 2, 51);

% 多维五次多项式插值（规划）
rpy_traj = mtraj(@tpoly, rpy1, rpy2, t);

%重新转化成变换矩阵，指导机械臂运动
T_traj_rot = rpy2tr(rpy_traj);

值得注意的是，转换成角度后，我们丢失了“位姿”中的位置参数。所以我们在规划机械臂运动的时候，对于位置和姿态先分别规划，最后把它们获得的变化矩阵重新整合。例如如下代码：

% 起点和终点
T0 = transl(1, 2, 3) * trotx(180);
T1 = transl(2, 3, 4);
% 设置运动时间
t = linspace(0, 2, 51);

% 对于姿态
rpy1 = tr2rpy(T0)/pi*180;
rpy2 = tr2rpy(T1)/pi*180;
rpy_traj = mtraj(@tpoly, rpy1, rpy2, t);
T_traj_rot = rpy2tr(rpy_traj);

% 对于位置
P1 = transl(T0);
P2 = transl(T1);
P_traj = mtraj(@tpoly, P1', P2', t);
T_traj_transl = transl(P_traj);

% 整合
T_traj = zeros(4, 4, 51);
for i = 1:51
  T_traj(:,:,i) = T_traj_rot(:,:,i)*T_traj_transl(:,:,i);
end

% 让机械臂动起来
q_traj = Five_dof.ikunc(T_traj)
Five_dof.plot(q_traj);

但是对于角度的规划，求逆解的过程有误差。对于roll-pitch-yaw（横滚-俯仰-行偏角）的求解出的三个角度被限制在了一定范围。所以并不推荐以上的方法来获取旋转的插值（有可能产生不连续的情况）。

更简单的方法

matlab给了一个非常方便的函数trinterp，能直接传入两个变化矩阵（起点和终点），求解一个指定插值方式的轨迹（默认是线性）；以及笛卡尔轨迹生成函数ctraj。trinterp用法如下：

% S可以是一个值，也可以是向量，但必须所有的元素在[0, 1]范围之内
T = trinterp(T0, T1, S);

以下代码生成与图6一致的轨迹：

T0 = transl(1, 2, 3);
T1 = transl(2, 3, 4);
% 必须保证tpoly生成的向量中的元素都在[0, 1]之内
t = linspace(0, 2, 51);
T  = trinterp(T0, T1, tpoly(0, 2, t)/2); 
p = transl(T);
% 此函数在上文中定义过
plot_traj(p, t);

ctraj的用法与trinterp基本一致，它们的区别在于默认的插值方式不同。

当然，还可以先对我们的起点和终点的变换矩阵直接求逆解获得起点和终点的关节变量，然后对这两个关节变量进行轨迹规划。笔者认为，这种方法更加稳定。

q1 = Five_dof.ikunc(T0);
q2 = Five_dof.ikunc(T1);
t = linspace(0, 2, 51)/2;

% 使用jtraj函数对关节角度插值
q_Traj = jtraj(q1, q2, t);

Five_dof.plot(q_Traj);

---

2 控制六自由度机械臂写名字

我们选择名叫RB08的6自由度机械臂来实现上文中的所有内容。它的标准DH参数表如下：

它没有给出任何的${\theta }_i$角度，因此实际上机器人的初始位姿可以自行定义，此处就不定义了，给它默认值。

• 建模

%% modeling
L(1) = Link('revolute', 'd', 0.45,  'a', 0.17,      'alpha', pi/2, 'qlim', [-180, 180]/180*pi);
L(2) = Link('revolute', 'a', 0.56,  'qlim', [-135, 180]/180*pi);
L(3) = Link('revolute', 'a', 0.166, 'alpha', pi/2,    'qlim', [-90, 90]/180*pi);
L(4) = Link('revolute', 'd', 0.63,  'alpha', -1*pi/2, 'qlim', [-180, 180]/180*pi);
L(5) = Link('revolute', 'a', 0.11,  'alpha', pi/2,    'qlim', [-135, 135]/180*pi);
L(6) = Link('revolute', 'd', 0.095, 'qlim', [-180, 180]/180*pi);
Six_dof = SerialLink(L, 'name', 'RB08');
Six_dof.teach;

虽然由于没有${\theta }_i$参数看上去比较奇怪，但是各个自由度是非常正常的。

• 预设路径
  简单起见，让机器人只写两个字母（H与L）。这两个字母非常简单，只有横和竖。

%% 轨迹
n = 20;
t = linspace(0, 2, 20)/2;
t(20) = 1;
% H
% H-1
T1_0 = transl(1.2, -0.5, 0.8)*troty(90);
T1_1 = transl(1.2, -0.5, -0.2)*troty(90);
T_traj_1 = trinterp(T1_0, T1_1, t);
q_traj_1 = Six_dof.ikunc(T_traj_1);
% Six_dof.plot(q_traj_1, 'trail', 'b');

% H-2
T2_0 = transl(1.2, -0.5, 0.3)*troty(90);
T2_1 = transl(1.34, 0.35, 0.3)*troty(90);
% 到达第二笔初始位置
T_traj2s = trinterp(T1_1, T2_0, t);
q_traj2s = Six_dof.ikunc(T_traj2s);

T_traj_2 = trinterp(T2_0, T2_1, t);
q_traj_2 = Six_dof.ikunc(T_traj_2);

% H-3
T3_0 = transl(1.34, 0.35, 0.8)*troty(90);
T3_1 = transl(1.34, 0.35, -0.2)*troty(90);
% 到达第三笔的初始位置
T_traj3s = trinterp(T2_1, T3_0, t);
q_traj3s = Six_dof.ikunc(T_traj3s);

T_traj_3 = trinterp(T3_0, T3_1, t);
q_traj_3 = Six_dof.ikunc(T_traj_3);

% L-1
T4_0 = transl(1, 0.83, 0.8)*troty(90);
T4_1 = transl(1, 0.83, -0.2)*troty(90)*trotx(-60);
% 到达第四笔的初始位置
T_traj4s = trinterp(T3_1, T4_0, t);
q_traj4s = Six_dof.ikunc(T_traj4s);

T_traj_4 = trinterp(T4_0, T4_1, t);
q_traj_4 = Six_dof.ikunc(T_traj_4);

% L-2
T5_0 = transl(1, 0.83, -0.2)*troty(90)*trotx(-60);
T5_1 = transl(0.5, 1.2, -0.2)*troty(90)*trotx(-60);

T_traj_5 = trinterp(T5_0, T5_1, t);
q_traj_5 = Six_dof.ikunc(T_traj_5);

%% 最终轨迹
q = zeros(n*8, 6);
q(1:n, :) = q_traj_1;
q(n+1:n*2, :) = q_traj2s;
q(n*2+1:n*3, :) = q_traj_2;
q(n*3+1:n*4, :) = q_traj3s;
q(n*4+1:n*5, :) = q_traj_3;
q(n*5+1:n*6, :) = q_traj4s;
q(n*6+1:n*7, :) = q_traj_4;
q(n*7+1:end, :) = q_traj_5;
Six_dof.plot(q,'trail', 'b');

代码运行起来就像这样：

也许还有更好的方法绘制轨迹，这里暂时先用屎山代码放着。

---

3 matlab机器人工具箱常用函数

请阅读本文参考链接MATLAB机器人工具箱10.4 机械臂仿真教学（未完结）

# 参考

参考链接：

1. 运动学建模（标准DH法）
2. https://blog.csdn.net/yaked/article/details/73030668
3. https://blog.csdn.net/weixin_43332715/article/details/131347874
4. 机械臂位姿描述
5. 机器人工具箱常用函数、代码
6. 建模+正逆运动学+雅克比矩阵
7. 正运动学详解
8. MATLAB机器人工具箱10.4 机械臂仿真教学（未完结）