基于MATLAB的六自由度工业机器人运动分析和仿真

表 1 mate 机器人 D-H 参数
连杆 i
ai/mm
α i/(°)
di/mm
θ i/(°) 关节范围/(°)
1
210
-90
697
θ1
-165~165
2
600
0
0
θ2
-105~135
3
99
-90
0
θ3
-145~180
4
0
90
548
θ4
-190~190
5
0
-90
0
θ5
-140~140
6
0
0
119
θ6
ox c1 c23c4c5s6 s4c6 s23s5s6 s1s4c5s6 c4c6
oy s1 c23c4c5s6 s4c6 s23s5s6 c1s4c5s6 c4c6
oz s23c4c5s6 s4c6 s23s5s6
ax c1c23c4s5 s23c5 s1s4s5
ay s1c23c4s5 s23c5 s1s4s5
J4 J3
参数法建立坐标系，求出连杆之间的位姿矩阵，建
立工业机器人运动学方程。并在 MATLAB 环境下， 利用 RoboticsToolbox 进行建模仿真。
J6
J5
J2
2 FANUC ARC mate100 D-H 坐标系的建立 mate100
是 FANUC 公司生产的 6 自由度工业机器人，包括底
其中
ci cosi si sini cij cos i j sij sin i j
3. 2 运动学逆解 上述建立了 mate100 机器人的正运动学方程，给定各个关节的旋转角度就可以求出末
端的位姿。然而在实际操作的过程中，情况恰恰相反。我们通常需要在知道末端位姿的情况 下求出各个关节旋转角度，这就是机器人反向运动学的问题，也称为求运动学逆解，即由笛 卡尔空间到关节空间的变换。
基于 MATLAB 的六自由度工业机器人运动分析及仿真
摘要：
以 FANUC ARC mate100 工业机器人为研究对象，对其机构和连杆参数进行分析，采用
D-H 法对机器人进行正运动学和逆运动学分析，建立运动学方程。在 MATLAB 环境下，运用
机器人工具箱进行建模仿真，仿真结果证明了所建立的运动学正、逆解模型的合理性和正确
性。
关键词:FANUC ARC mate100 工业机器人; 运动学; MATLAB 建模仿真
1 引言
工业机器人技术是在控制工程、人工智能、计算机科学和机构学等多种学科的基础上发
展起来的一种综合性技术。经过多年的发展，该项技术已经取得了实质性的进步[1]。工业机
器人的发展水平随着科技的进步和工业自动化的需求有了很大的提高，同时工业机器人技术
(1) Zi 轴沿关节 i +1 的轴线方向。 (2) Xi 轴沿 Zi-1 和 Zi 轴的公法线方向，且指向背离 Zi-1 轴的方向。 (3) Yi 轴的方向必须满足 Yi= Zi*Xi，使坐标系为右手坐标系。 按照上述方法，建立坐标系如图 2 所示。
x3
x4 z4
x5
y3 o3 z3 d4
o4 y4 y5 o5 z5 d6
J1
座、机身、臂、手腕和末端执行器，每个自由度对
应一个旋转关节，如图 1 所示。
图 1FANUC ARC mate 100 机器人三维模型
DENAVIT 和 HARTENBERG 于 1955 年提出了一种为关节链中的每一个杆件建立坐标系的
矩阵方法，即 D-H 参数法，在机器人运动学分析得到了广泛运用。采用这种方法建立坐标系:
a3
x2 z2 o2 y2
3 4
y6
5 6 o6 z6
a2
x6
2
z1
a1
o1
x1
y1
d1
1
z0 y0
o0 x0
图 2 mate100D-H 坐标系
连杆参数的表示:
( 1) 连杆长度 ai 为沿 Xi 轴方向测量，Zi-1 到 Zi 公垂线的长度。 ( 2) 杆件扭角α i 为绕 Xi 轴正向转动为正，Zi-1 到 Zi 的转角。 ( 3) 关节距离 di 为沿 Zi-1 轴指向为正，Xi-1 到 Xi 的距离。 ( 4) 关节转角θ i 为绕 Zi-1 轴正向转动为正，Xi-1 到 Xi 的转角。 由此得出 FANUC ARC mate100 相应各杆件的结构参数和运动参数，如表 1 所示。
mate100 型机器人运动学方程可以写为:
nx ox ax px
n y n0z
oy oz 0
ax az 0
py pz 1
T1T2T3T4T5T6
(4)
在该运动学方程中，等式左边的矩阵中的元素 nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz 均为已知的， 而等式右边的 6 个矩阵是未知的，它们的值取决于关节变量θ 1，θ 2，…，θ 6 的大小。用 未知的连杆的逆变换左乘方程的两端，把关节变量分离出来，从而求出解，按照这种方法让
az c23c4s5 c23c5
px c1 c23c4s5d6 a3 s23c5d6 d4 a2c2 s1s4s5d6 a1c1
py s1 c23c4s5d6 a3 s23c5d6 d4 a2c2 c1s4s5d6 a1s1
pz s23c4s5d6 a3 c23c5d6 d4 a2s2 d1
T60
T1T2T3T4T5T6
n y n0z
oy oz 0
ay az 0
p
y
pz 1
(3)
式中
nx c1c23c4c5c6 s4s6 s23s5c6 s1s4c5c6 c4s6
ny s1c23c4c5c6 s4s6 s23s5c6 c1s4c5c6 c4s6
nz s23c4c5c6 s4s6 s23s5c6
于第一根连杆坐标系的位姿。根据表 1 mate100 机器人的 D-H 参数和齐次坐标变换矩阵公式
可以求出 Ti 为
c1 0 s1 a1c1 c2 s2 0 a2c2 c3 0 s3 a3c3
T1
s1 0
0 1
0
0
c1 0 0
a1s1
d1 1
T2
s2
0
0
c2 0 0
0 1
a2s2 0
T3
根据上述逆解求解公式，采用 MATLAB 编程计算，由机器人的各个关节转角范围，可得
到 8 组逆解。在 MATLAB 中利用 RoboticsToolbox 进行实例计算，由 q1=ikine(r,T,q)，计算各个
关节的转角，如表所示
计算解 仿真解
θ1 0.6332/1.5708
1.570796
θ2 -1.0472/1.69 95 -1.047197
通过建立 FANUC ARC mate100 工业机器人的 D-H 坐标系，求出了相邻连杆之间的位姿 矩阵和机器人运动学方程。基于运动学方程对机器人进行运动学分析，得到了机器人在关节 空间与笛卡尔空间之间的变换。利用 MATLAB 中 RoboticToolbox 进行建模仿真，仿真结果和 MATLAB 编程计算结果误差较小，证明了所建运动学模型的正确性，为机器人的路径规划做 铺垫。
矩阵左右两边的元素相等便可求出各个关节变量的值。
T61T T1T2T3T4T5
T51T61T T1T2T3T4
T41T51T61T T1T2T3
T31T41T51T61T T1T2
T21T31T41T51T61T T1
(5)
4 机器人运动学仿真 4.1 建立运动模型
通过 mate100 机器人的初始位姿的 D-H 参数，并根据 Link 函数建立 M 文件并命名 为 FANUC ARC mate100，连杆的前 4 个元素依次为连杆扭角、连杆长度、连杆转角、连杆距 离，最后一个为 0(转动关节)。利用机器人工具箱中的 drivebot( )命令构建 mate100 的滑块图 和三维模型图，在滑块图中可以通过手动调节滑块或者输入各个关节的数值的方式驱动滑块， 就能实时控制机器人运动，如图 3 所示。
θ3 -0.7854
-0.785398
θ4 1.0472
θ5 0.5236
θ6 -0.7854/1.057
1.047198 0.523599 -0.785398
由表 2 可知，计算解和仿真解误差很小，说明所建立的运动学方程和模型比较可靠。由 此可以看出，利用 RoboticToolbox 进行实例计算的结果与用逆方程得到的其中一组解几乎没 有误差，证明逆运动学方程完全正确。 5 结论
s3 0
0 1
0
1
0
0
c3 0 0
a3 s3
0
1
c4 0 s4 0 c5 0 s5 0 c6 s6 0 0
T4
s4 0
0
0 1 0
c4 0 0
0
d4 1
T5
s5
0
0
0 1 0
c5 0 0
0 0
T6
s6 0
1
0
Hale Waihona Puke c6 0 000
(2)
1 0
d6 1
nx ox ax px
针对机器人求逆解的问题，求解方法多种多样，其中应用最广的是封闭解法，因为求封 闭解计算速度快、效率高、便于实时控制。封闭解法包括两种方法: 一种是代数解法，另一 种是几何解法。目前已建立的一种系统化的代数解法为: 运用变换矩阵就可得出一个可求解 的三角函数方程式。重复上述过程，直到求解出所有的未知数。
此之外，还可将各个关节的转角代入机器人模型中得到图 5
图 5 滑块图和 MATLAB 三维模型
给定各个转角的实际值，根据运动学方程求出的解与用 MATLAB 中的ＲotboticToolbox
求出的末端位姿矩阵是一致的;同时通过建立的机器人模型，输入各个转角求出末端执行器 的位置，并与前面求出的解相同，由此说明所建立的正运动学方程合理、正确。 4.2.2 逆运动学方程验证 逆运动学方程验证流程见图
给定机器人 的位姿矩阵
代入逆方程求 解关节转角
利 用 MATLAB 求解转角矩阵
Y 相等
N
逆运动学 方程正确
逆运动学 方程错误
给定机器人的位姿矩阵：
0.5 0.8 0.4 5.15
T
0.2
0.4
0.8 0.5
0.9 0
124.52 135.67
T1T2T3T4T5T6
0
0
0
1
(7)
也得到了进一步的完善。工业机器人的运动学分析主要是通过工业机器人各个连杆和机构参
数，以确定末端执行器的位姿。工业机器人的运动学分析包括正运动学分析和逆运动学分析。
随着对焊接件要求的提高，弧焊等机器人的需
求越来越多。本文就以 FANUC ARC mate100 机器人 为研究对象，通过分析机构和连杆参数，运用 D-H
0.2
0.8
0.4 0.5
0.9 0
124.52 135.67
0
0
0
1
(6)
在 MATLAB 中进行实例计算：把 q=[pi/2,-pi/3,-pi/4,pi/3,pi/6,-pi/4]赋给各个关节，利用
T=fkine(r,q)求出位姿矩阵。
由 MATLAB 编程得到的运动正解与 MATLAB 工具箱中所建立模型解的结果完全一致。除
图 3 滑块图和 MATLAB 下的三维模型
4.2 运动方程的验证 为了验证建立的正、逆运动学方程的正确性，利用 MATLAB(RoboticToolbox)进行实例计
算。 4.2.1 正运动学方程验证
正运动学方程验证流程图如图所示
给定各个 关节转角
代入正方程求 解位姿矩阵
利 用 MATLAB 求解位姿矩阵
Y 相等
N
正运动学 方程正确
正运动学 方程错误
图 4 正运动学方程验证流程图
给各个关节赋值θ 1=π /2，θ 2=-π /3，θ 3=-π /4，θ 4=π /3，θ 5=π /6，θ 6=-π /4 带入 公式，由 MATLAB 得
0.5 0.8 0.4 5.15
T1T2T3T4T5T6
cos i sin i cos i sin i sin i ai cos i
sin i
cos i cos i
cos i sin i
ai
sin i
0
0
sin i 0
cos i 0
di 1
(1)
T1 描述了第一根连杆相对于某个坐标系 ( 如机身) 的位姿，T2 描述了第二根连杆相对
-270~270
3 运动学方程建立
3. 1 运动学正解
运动学正解是指由机器人的各个关节的旋转角度位姿变换矩阵求解机器人末端执行器
的位姿，进而实现关节空间到笛卡尔坐标空间的转换。相邻连杆齐次变换矩阵为：
Ti Rot(z,i )Trans(0,0, di )Trans(ai ,0,0)Rot(x, i )