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

摘要：

以FANUC ARC mate100工业机器人为研究对象，对其机构和连杆参数进行分析，采用D-H法对机器人进行正运动学和逆运动学分析，建立运动学方程。在MATLAB环境下，运用机器人工具箱进行建模仿真，仿真结果证明了所建立的运动学正、逆解模型的合理性和正确性。

关键词:FANUC ARC mate100工业机器人; 运动学; MATLAB建模仿真 1引言

工业机器人技术是在控制工程、人工智能、计算机科学和机构学等多种学科的基础上发展起来的一种综合性技术。经过多年的发展，该项技术已经取得了实质性的进步[1]。工业机器人的发展水平随着科技的进步和工业自动化的需求有了很大的提高，同时工业机器人技术也得到了进一步的完善。工业机器人的运动学分析主要是通过工业机器人各个连杆和机构参数，以确定末端执行器的位姿。工业机器人的运动学分析包括正运动学分析和逆运动学分析。

随着对焊接件要求的提高，弧焊等机器人的需求越来越多。本文就以FANUC ARC mate100机器人J4 为研究对象，通过分析机构和连杆参数，运用D-HJ3 参数法建立坐标系，求出连杆之间的位姿矩阵，建

J2

立工业机器人运动学方程。并在MATLAB环境下，

J6 J5

利用RoboticsToolbox进行建模仿真。

2 FANUC ARC mate100 D-H坐标系的建立mate100是FANUC公司生产的6自由度工业机器人，包括底J1 座、机身、臂、手腕和末端执行器，每个自由度对应一个旋转关节，如图1所示。

图1FANUC ARC mate 100机器人三维模型

DENAVIT和HARTENBERG于1955年提出了一种为关节链中的每一个杆件建立坐标系的矩阵方法，即D-H参数法，在机器人运动学分析得到了广泛运用。采用这种方法建立坐标系:

(1) Zi轴沿关节i +1的轴线方向。

(2) Xi轴沿Zi-1和Zi轴的公法线方向，且指向背离Zi-1轴的方向。 (3) Yi轴的方向必须满足Yi= Zi*Xi，使坐标系为右手坐标系。 按照上述方法，建立坐标系如图 2 所示。

x 3 y3 o3 z3 d 4 a3 a2 a1 d1 x2 o2 z 2 y2 x4 z4 x5 y5 o5 z5

o4 y4

d6 y6

3 4 2 o1 y1 z 1 x1 5 6 o6 x 6 z 6

1 z0 o0 y 0 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所示。

表1 mate机器人D-H参数

连杆i 1 2 3 4 5 6

ai/mm210 600 99 0 0 0

αi/(°) -90 0 -90 90 -90 0

di/mm697 0 0 548 0 119

θi/(°) θ1 θθθθθ

2 3 4 5 6

关节范围/(°) -165~165 -105~135 -145~180 -190~190 -140~140 -270~270

3运动学方程建立 3. 1运动学正解

运动学正解是指由机器人的各个关节的旋转角度位姿变换矩阵求解机器人末端执行器的位姿，进而实现关节空间到笛卡尔坐标空间的转换。相邻连杆齐次变换矩阵为：

TiRot(z,i)Trans(0,0,di)Trans(ai,0,0)Rot(x,i)cosisini00aicosiaisinidi001 (1)

T1描述了第一根连杆相对于某个坐标系 ( 如机身) 的位姿，T2描述了第二根连杆相对

sinicosicosicosisinisinisinicosisinicosi于第一根连杆坐标系的位姿。根据表1 mate100机器人的D-H参数和齐次坐标变换矩阵公式可以求出Ti为

c10s1a1c1c2s2s10s2c2c1as11T2T10100d10000100c4s4T4000s40c410000a2c2c30s3a3c3s300a2s2c3as33T30101000100010c6s6s6c60T600010000(2) 1d601000c50s5s500c5T5010d41000oxoyoz0axayaz0nxnT60T1T2T3T4T5T6ynz0式中

pxpypz1 (3)

nxc1c23c4c5c6s4s6s23s5c6s1s4c5c6c4s6 nys1c23c4c5c6s4s6s23s5c6c1s4c5c6c4s6

nzs23c4c5c6s4s6s23s5c6

oxc1c23c4c5s6s4c6s23s5s6s1s4c5s6c4c6 oys1c23c4c5s6s4c6s23s5s6c1s4c5s6c4c6

ozs23c4c5s6s4c6s23s5s6

axc1c23c4s5s23c5s1s4s5 ays1c23c4s5s23c5s1s4s5

azc23c4s5c23c5

pxc1c23c4s5d6a3s23c5d6d4a2c2s1s4s5d6a1c1

pys1c23c4s5d6a3s23c5d6d4a2c2c1s4s5d6a1s1

pzs23c4s5d6a3c23c5d6d4a2s2d1

其中

cicosisisinicijcosijsijsinij

3. 2运动学逆解

上述建立了mate100机器人的正运动学方程，给定各个关节的旋转角度就可以求出末端的位姿。然而在实际操作的过程中，情况恰恰相反。我们通常需要在知道末端位姿的情况下求出各个关节旋转角度，这就是机器人反向运动学的问题，也称为求运动学逆解，即由笛卡尔空间到关节空间的变换。

针对机器人求逆解的问题，求解方法多种多样，其中应用最广的是封闭解法，因为求封闭解计算速度快、效率高、便于实时控制。封闭解法包括两种方法: 一种是代数解法，另一种是几何解法。目前已建立的一种系统化的代数解法为: 运用变换矩阵就可得出一个可求解的三角函数方程式。重复上述过程，直到求解出所有的未知数。

mate100型机器人运动学方程可以写为:

nxnynz0oxoyoz0axaxaz0pxpyT1T2T3T4T5T6 pz1 (4)

在该运动学方程中，等式左边的矩阵中的元素nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz均为已知的，

而等式右边的6个矩阵是未知的，它们的值取决于关节变量θ1，θ2，…，θ6的大小。用未知的连杆的逆变换左乘方程的两端，把关节变量分离出来，从而求出解，按照这种方法让矩阵左右两边的元素相等便可求出各个关节变量的值。

T61TT1T2T3T4T5

T51T61TT1T2T3T4 T41T51T61TT1T2T3 T31T41T51T61TT1T2

T21T31T41T51T61TT1

(5)

4机器人运动学仿真 4.1建立运动模型

通过mate100机器人的初始位姿的D-H参数，并根据Link函数建立M文件并命名为FANUC ARC mate100，连杆的前4个元素依次为连杆扭角、连杆长度、连杆转角、连杆距离，最后一个为0(转动关节)。利用机器人工具箱中的drivebot( )命令构建mate100的滑块图和三维模型图，在滑块图中可以通过手动调节滑块或者输入各个关节的数值的方式驱动滑块，就能实时控制机器人运动，如图3所示。

图3滑块图和MATLAB下的三维模型

4.2运动方程的验证

为了验证建立的正、逆运动学方程的正确性，利用MATLAB(RoboticToolbox)进行实例计算。

4.2.1正运动学方程验证

正运动学方程验证流程图如图所示

代入正方程求解位姿矩阵 给定各个关节转角 相等 利用MATLAB求解位姿矩阵 N 正运动学方程错误 正运动学方程正确 Y 图4正运动学方程验证流程图

给各个关节赋值θ1=π/2，θ2=-π/3，θ3=-π/4，θ4=π/3，θ5=π/6，θ6=-π/4带入公式，由MATLAB得

0.50.80.45.150.20.40.9124.52T1T2T3T4T5T6

0.80.50135.67001 (6)0在MATLAB中进行实例计算：把q=[pi/2,-pi/3,-pi/4,pi/3,pi/6,-pi/4]赋给各个关节，利用

T=fkine(r,q)求出位姿矩阵。

由MATLAB编程得到的运动正解与MATLAB工具箱中所建立模型解的结果完全一致。除此之外，还可将各个关节的转角代入机器人模型中得到图5

图5滑块图和MATLAB三维模型

给定各个转角的实际值，根据运动学方程求出的解与用MATLAB中的ＲotboticToolbox

求出的末端位姿矩阵是一致的;同时通过建立的机器人模型，输入各个转角求出末端执行器的位置，并与前面求出的解相同，由此说明所建立的正运动学方程合理、正确。 4.2.2 逆运动学方程验证 逆运动学方程验证流程见图

代入逆方程求解关节转角 给定机器人的位姿矩阵 相等 利用MATLAB求解转角矩阵 N Y 逆运动学方程正确 逆运动学方程错误

给定机器人的位姿矩阵：

0.50.80.45.150.20.40.9124.52T1T2T3T4T5T6 T0.80.50135.670001 (7)

根据上述逆解求解公式，采用MATLAB编程计算，由机器人的各个关节转角范围，可得

到8组逆解。在MATLAB中利用RoboticsToolbox进行实例计算，由q1=ikine(r,T,q)，计算各个关节的转角，如表所示 计算解 仿真解

由表 2 可知，计算解和仿真解误差很小，说明所建立的运动学方程和模型比较可靠。由此可以看出，利用RoboticToolbox进行实例计算的结果与用逆方程得到的其中一组解几乎没有误差，证明逆运动学方程完全正确。 5结论

通过建立FANUC ARC mate100工业机器人的D-H坐标系，求出了相邻连杆之间的位姿矩阵和机器人运动学方程。基于运动学方程对机器人进行运动学分析，得到了机器人在关节空间与笛卡尔空间之间的变换。利用MATLAB中RoboticToolbox进行建模仿真，仿真结果和MATLAB编程计算结果误差较小，证明了所建运动学模型的正确性，为机器人的路径规划做铺垫。

θ1

0.6332/1.5708 1.570796

θ2 -1.0472/1.6995

-1.047197

θ3 -0.7854 -0.785398

θ4 1.0472

θ5 0.5236

θ6

-0.7854/1.057

1.047198 0.523599 -0.785398