一、机械臂及运动学
1.1 机械臂构成
机械臂多采用关节式机械结构，一般具有6个自由度，其中3个用来确定末端执行器的位置，另外3个则用来确定末端执行装置的方向(姿态)。
如图所示，一个机械臂是由一组可做相对运动的关节连接的连杆结合体。第一个连杆固定，连接该机械臂的基座，而最后一个连杆连接的是它的末端执行器。
通常可将关节划分为两种：第一种称为转动关节(或称为旋转关节)，转动关节可绕基准轴转动，相应的转动量称为关节角：第二种称为移动关节，移动关节是沿着基准轴移动，相应的位移称为关节偏距。还有一种特殊的关节称为球关节，球关节拥有三个自由度。可以用三个转动关节和一个零长度的连杆来描述一个球关节。
位于机械臂固定基座的坐标系称为基坐标系；位于操作臂末端执行器的坐标系称为工具坐标系，通常用它来描述机械臂的位置。
1.2 机器人运动学介绍
机器人运动学只研究机器人运动，不关注机器人运动过程中各零部件的质量及相关力，也不关注关节驱动力和力矩。
（1）机器人正运动学：给定一组机器人关节变量（转角或位移），求解末端工具坐标系相对于基坐标系的位置和姿态。
（2）机器人逆运动学：给定机器人末端工具箱坐标系的位置和姿态，求解机器人各关节变量。
二、D-H参数法
D-H 参数全称为Denavit-Hartenberg参数，它使用连杆参数来描述机构运动关系。在DH参数法中，描述机械臂中的每一个连杆需要4个运动学参数：
● 连杆长度
a
i
−
1
a _ { i - 1 }
ai−1：关节轴
i
−
1
i-1
i−1与关节轴
i
i
i之间公垂线的长度；
● 连杆转角
α
i
−
1
\alpha _ { i-1 }
αi−1：第
i
−
1
i-1
i−1个关节轴和第
i
i
i个关节轴之间的夹角；
● 连杆偏距
d
i
d_{i}
di：沿两个相邻连杆公共轴线方向的距离；
● 关节角
θ
i
\theta_{i}
θi：两相邻连杆绕公共轴线旋转的夹角。
2.1 标准D-H法（SDH）
建模规则：
（1）找出各关节轴，画出关节轴的延长线；
（2）确定
Z
Z
Z轴：与关节轴线重合，其中
Z
i
−
1
Z_{i-1}
Zi−1轴与关节
i
i
i的轴线重合；
（3）确定
X
X
X轴：
Z
i
−
1
Z_{i-1}
Zi−1轴与
Z
i
Z_{i}
Zi轴的公垂线，方向由
Z
i
−
1
Z_{i-1}
Zi−1轴指向
Z
i
Z_{i}
Zi轴
1）如果
Z
i
−
1
Z_{i-1}
Zi−1轴与
Z
i
Z_{i}
Zi轴平行，选取与前一关节的公垂线共线的一条公垂线作为
X
X
X轴；
2）如果
Z
i
−
1
Z_{i-1}
Zi−1轴与
Z
i
Z_{i}
Zi轴相交，
X
X
X轴为
Z
i
−
1
Z_{i-1}
Zi−1轴与
Z
i
Z_{i}
Zi轴的叉积方向；
（4）确定
Y
Y
Y轴：右手定则。
D-H参数含义：
（1）
a
i
a_{i}
ai：关节轴线
i
−
1
i-1
i−1和关节轴线
i
i
i的公垂线长度
（2）
α
i
\alpha_{i}
αi：关节轴线
i
−
1
i-1
i−1和关节轴线
i
i
i的夹角，指向为从轴线
i
−
1
i-1
i−1到轴线
i
i
i；
（3）
d
i
d_{i}
di：关节
i
i
i上的两条公垂线
a
i
−
1
a_{i-1}
ai−1 与
a
i
a_{i}
ai之间的距离，沿关节轴线
i
i
i测；
（4）
θ
i
\theta_{i}
θi：连杆
i
i
i相对于连杆
i
−
1
i-1
i−1绕轴线
i
i
i的旋转角度。
齐次变换矩阵：
从坐标系
i
−
1
i-1
i−1到坐标系
i
i
i，先绕
Z
i
−
1
Z_{i-1}
Zi−1轴旋转角度
θ
i
\theta_{i}
θil，再沿
Z
i
−
1
Z_{i-1}
Zi−1轴移动
d
i
d_{i}
di，然后沿
X
i
X_i
Xi轴平移
a
i
a_i
ai，最后绕
X
i
X_i
Xi轴旋转
α
i
\alpha_i
αi。齐次变换矩阵
i
−
1
i
T
_ { i - 1 } ^ { i } T
i−1iT可以写为：
2.2 改进D-H法（MDH）
建模规则：
（1）找出各关节轴，画出关节轴的延长线；
（2）确定
Z
Z
Z轴：与关节轴线重合，其中
Z
i
Z_{i}
Zi轴与关节
i
i
i的轴线重合；
（3）确定
X
X
X轴：
Z
i
Z_{i}
Zi轴与
Z
i
+
1
Z_{i+1}
Zi+1轴的公垂线，方向由
Z
i
Z_{i}
Zi轴指向
Z
i
+
1
Z_{i+1}
Zi+1轴；
（4）确定
Y
Y
Y轴：右手定则。
D-H参数含义：
（1）
a
i
−
1
a_{i-1}
ai−1：沿
X
i
−
1
X_{i-1}
Xi−1轴，从
Z
i
−
1
Z_{i-1}
Zi−1轴移动到
Z
i
Z_{i}
Zi轴的距离；
（2）
α
i
−
1
\alpha_{i-1}
αi−1：绕
X
i
−
1
X_{i-1}
Xi−1轴，从
Z
i
−
1
Z_{i-1}
Zi−1轴旋转到
Z
i
Z_{i}
Zi轴的角度；
（3）
d
i
d_{i}
di：沿
Z
i
Z_{i}
Zi轴，从
X
i
−
1
X_{i-1}
Xi−1轴移动到
X
i
X_{i}
Xi轴的距离；
（4）
θ
i
\theta_{i}
θi：绕
Z
i
Z_{i}
Zi轴，从
X
i
−
1
X_{i-1}
Xi−1轴旋转到
X
i
X_{i}
Xi轴的角度；
齐次变换矩阵：
从坐标系
i
−
1
i-1
i−1到坐标系
i
i
i，先绕
X
i
−
1
X_{i-1}
Xi−1轴旋转角度
α
i
−
1
\alpha_{i-1}
αi−1，再沿
X
i
−
1
X_{i-1}
Xi−1轴平移
a
i
−
1
a_{i-1}
ai−1，然后绕
Z
i
Z_{i}
Zi轴旋转角度
θ
i
\theta_{i}
θi，最后沿
Z
i
Z_{i}
Zi轴移动
d
i
d_{i}
di。齐次变换矩阵
i
−
1
i
T
_ { i - 1 } ^ { i } T
i−1iT可以写为：
2.3 SDH和MDH的区别及适用场景
2.3.1 SDH和MDH的区别
（其实一直以来笔者都不太能区分这两种方法，笔者最开始学习机器人学是看的《机器人学导论》这本书，上面似乎是偏向于MDH方法，所以后续我都是习惯于用MDH去建模）
（1）区别一：连杆坐标系建立的位置不同。SDH将连杆
i
i
i的坐标系固定在连杆的远端；MDH将连杆
i
i
i的坐标系固定在连杆的近端。
（2）区别二：执行变换的顺序不同。
2.3.2 SDH和MDH的适用场景
对于树形结构或者闭链机构的机器人来说，按照SDH方法建立的连杆坐标系会产生歧义，因为SDH的建系原则是把连杆
i
i
i的坐标系建立在连杆的远端，如图(a)所示，导致连杆0上同时出现了两个坐标系。而MDH把连杆坐标系建立在每个连杆的近端，则不会坐标系重合的情况，如图(b)所示，这就克服了SDH方法建系的缺点。
总结：
（1）SDH适合应用于开链结构的机器人；
（2）当使用SDH表示树状或闭链结构的机器人时，会产生歧义；
（3）MDH法对开链、树状、闭链结构的机器人都适用。
三、机器人运动学
3.1 创建一个连杆对象
在机器人工具箱中，还用变量
σ
i
\sigma _ { i }
σi表示机器人的关节类型，
σ
i
=
0
\sigma _ { i } = 0
σi=0表示转动关节，
σ
i
=
1
\sigma _ { i } = 1
σi=1表示移动关节(若未指定该参数，默认为转动关节)。
在工具箱中，用函数Link( )可以创建一个机械臂对象，其中输入的参数顺序分别是：关节角
θ
i
\theta _ { i }
θi、连杆偏距
d
i
d_{i}
di、连杆长度
a
i
−
1
a_{i-1}
ai−1、连杆转角
α
i
−
1
\alpha_{i-1}
αi−1和关节类型。例如，创建一个关节角为30°，连杆偏距为0.2m，连杆长度为0.5m，连杆转角为60°，关节类型为旋转关节的连杆，代码如下所示：
L = Link([pi/6, 0.2, 0.5, pi/3, 0])
这里，Revolute表示转动关节，std表示标准D-H参数法，offset表示关节偏移量。可以通过以下命令获得连杆的各个参数：
● 获取连杆的关节类型：L.type( )（注意：老版本的工具箱是L.RP）;
● 获取连杆的关节角：L.theta;
● 获取连杆的连杆偏距：L.d;
● 获取连杆的连杆长度：L.a;
● 获取连杆的连杆转角：L.alpha。
● 获取改连杆的齐次变换矩阵：L.A(
θ
\theta
θ)，例如连杆转动了30°，其齐次变换矩阵T为：
3.2 创建一个平面3-DOF的机械臂
这里创建了一个平面三R机构（三个转动关节），该平面三连杆机构的DH参数表如下所示：
% standard-表示标准DH法
% theta(z) d(z) a(x) alpha(x)
RRR_L(1) = Link([ 0 0 1 0 ],'standard');
RRR_L(2) = Link([ 0 0 0.8 0 ],'standard');
RRR_L(3) = Link([ 0 0 0.6 0 ],'standard');
运行结果：
通过构造函数SerialLink( )可以给创建的机械臂对象命名，并显示出对象的信息。输入命令:
three_link = SerialLink(RRR_L, 'name', 'three-link-RRR')
运行结果：
用以下的命令可以获取已创建机械臂的各个参数：
运行结果：
同时也可以对创建的机械臂对象进行复制，如复制一个名称为“three_link2”的机械臂输人命令:
three_link2 = SerialLink(RRR_L, 'name', 'three-link-RRR_2')
运行结果：
使用teach( )函数，可以对创建的机器人进行示教，如图所示：
3.3 机器人正运动学
机器人正运动学即给定一组关节角，计算出机器人末端相对于基坐标系的位置和姿态，如下图：
机器人工具箱中，fkine( )可以进行正运动学计算，即给定关节变量，得到末端坐标系关于基坐标系的齐次变换矩阵（位置和姿态）。这里以3.2节的3-DOF平面机械臂为例，当机器人三个关节角都为0°时：
% RRR机械臂
clear;
close all;
clc;
% theta(z) d(z) a(x) alpha(x)
RRR_L(1) = Link([ 0 0 1 0 ],'standard');
RRR_L(2) = Link([ 0 0 0.8 0 ],'standard');
RRR_L(3) = Link([ 0 0 0.6 0 ],'standard');
three_link = SerialLink(RRR_L, 'name', 'three-link-RRR');
q0 = [0 0 0]'*pi/180;
T0 = three_link.fkine(q0)
运行结果：
当机器人一关节10°，二关节-30°，三关节60°时，机器人末端的位置和姿态如图：
可以用plot( )函数绘制此时机器人的状态，如图：
例子：给定平面3-RRR机械臂各个关节的角度如图所示，求解机器人运动过程中末端点的三维坐标
% RRR机械臂
clear;
close all;
clc;
% theta(z) d(z) a(x) alpha(x)
RRR_L(1) = Link([ 0 0 1 0 ],'standard');
RRR_L(2) = Link([ 0 0 0.8 0 ],'standard');
RRR_L(3) = Link([ 0 0 0.6 0 ],'standard');
three_link = SerialLink(RRR_L, 'name', '3-DOF');
t = 0:0.05:4;
m = length(t);
theta1 = 120*sin(4*pi*t/4);
theta2 = 60*sin(2*pi*t/4);
theta3 = 30*sin(2*pi*t/4);
q = [theta1;theta2;theta3]*pi/180;
for i = 1:m
T = three_link.fkine(q(:,i)');
x(i) = T.t(1);
y(i) = T.t(2);
z(i) = T.t(3);
subplot(3,3,[1 4 7])
hold on
plot3(x(i),y(i),z(i),'r*');
hold on
three_link.plot(q(:,i)');
subplot(3,3,2)
hold on
plot(t(i),q(1,i)*180/pi,'b.')
xlabel('time /s')
ylabel('\theta_1 /deg')
subplot(3,3,5)
hold on
plot(t(i),q(2,i)*180/pi,'r.')
xlabel('time /s')
ylabel('\theta_2 /deg')
subplot(3,3,8)
hold on
plot(t(i),q(3,i)*180/pi,'m.')
xlabel('time /s')
ylabel('\theta_3 /deg')
subplot(3,3,3)
hold on
plot(t(i),x(i),'b.')
xlabel('time /s')
ylabel('x /m')
subplot(3,3,6)
hold on
plot(t(i),y(i),'r.')
xlabel('time /s')
ylabel('y /m')
subplot(3,3,9)
hold on
plot(t(i),z(i),'m.')
xlabel('time /s')
ylabel('z /m')
end
运行结果：
3.4 机器人逆运动学（未完待续）
笔者最近有点忙。。。
