基于坐标变换法的3关节机械臂运动学研究

沈浩宇， 江先志

(浙江理工大学 机械与自动控制学院， 浙江 杭州 310000)

在21世纪，机器人技术取得了巨大发展，机器人的应用遍及工业、农业和医疗等诸多领域。随着行业间的深度融合，机器人应用范围将不断得到拓展[1-2]。对机器人运动学分析时，首先要建立机器人坐标系，最为常用的方法之一是1965年Hartenberg等在文献[3]提到的D-H参数法。D-H参数法严格定义了每个坐标系的建立规则[4]，存在观察抽象、坐标系转换不灵活、参数容易混乱以及理论模型与实体模型出入大等缺点；另外对于2个相邻的平行关节，传统D-H参数法得到的齐次变换矩阵是一个奇异矩阵，无法对这部分关节进行运动学分析[5]。串联机械臂逆运动学比较复杂，原因是求解已知末端位姿的机械臂的关节角时，存在无解或者多解现象。机械臂运动只能有一组解，若在解的选择时出现错误，机械臂便会出现飞车现象[6-7]。

课题组以3关节机械臂为研究对象，对上述问题进行探索、优化和求解，并进行了验证。

1 基于坐标变换法的运动学分析

机器人运动学研究的是机器人的运动特性，可分为正运动学和逆运动学。正运动学方程描述的是机器人末端执行器的位姿与机器人基座之间的运动关系；逆运动学方程是正运动学的逆过程，是在已知末端位姿矩阵的条件下求解满足条件的关节角。逆运动学求解是对机器人进行轨迹控制的基础，是机器人控制领域特别重要的问题。图1体现了机械臂运动学在整个机械臂力位控制系统中的作用[8]。

图1 运动学在力位控制系统中的作用Figure 1 Role of kinematics in force position control system

3关节机械臂由基座、大臂、小臂和腕部组成，如图2所示。基座与大臂之间由肩部的外展/内收关节、肩部的前屈/后伸关节共同连接，大臂和小臂之间由肘关节连接。

图2 机械臂模型Figure 2 Model of robotic arm

1.1 建立坐标系

坐标变换法在3关节机械臂中建立的坐标系如图3所示。O0,O1,O2,O3和O4分别表示各个坐标系的原点，θ1，θ2和θ3分别表示3个关节的运动角度，逆时针为正向转动方向。坐标系{1}和固定坐标系{0}初始位姿重合，肩部的外展/内收关节的自由度由坐标系{1}绕其自身Y轴的旋转变换实现；坐标系{2}相对于坐标{1}的X，Y和Z轴正方向偏移dx1，dy1和dz1，肩部的前屈/后伸关节的自由度由绕其自身X轴的旋转变换实现；坐标系{3}相对于坐标{2}的X，Y和Z轴正方向偏移dx2，dy2和dz2，肘关节的自由度由绕其自身X轴的旋转变换实现；坐标系{4}相对于坐标{3}X，Y和Z轴正方向偏移分别为dx3，dy3和dz3，O4为机械臂末端点。

图3 坐标系示意图Figure 3 Coordinate system diagram

在肩外展/内收运动角度θ1=0，肩前屈/后伸运动角度θ2=0，肘关节运动角度θ3=0时，机械臂的姿态正好和图4一致。机械臂的主要技术参数如表1所示。

表1 3关节机械臂的主要技术参数Table 1 Main technical parameters of three-joint robotic arm

1.2 机械臂正运动学分析

坐标系{1}相对于{0}的齐次变换矩阵为：

式(1)中，s1=sinθ1，c1=cosθ1；同理，在后面的章节中，有s2=sinθ2,s3=sinθ3,c2=cosθ2,c3=cosθ3。另外Rot(x,θ)、Trans(x,dx)分别代表坐标系绕其自身的X轴逆时针旋转θ、沿着X轴移动dx的距离；同理，在后面的章节中出现的其他变量的旋转平移变换以此类推。

坐标系{2}相对于{1}的齐次变换矩阵为：

(2)

(3)

(4)

(5)

1.3 机械臂逆运动学分析

1.3.1 求解肩外展/内收运动角度θ1

根据式(5)有：

(6)

(7)

又有：

(8)

(9)

结合式(7)～(9)得到：

(10)

由(10)得到：

c1x-s1z=dx1+dx2+dx3。

(11)

(12)

1.3.2 求解肩前屈/后伸运动角度θ2

(13)

(14)

且有：

(15)

结合式(13)～(15)得到：

(16)

由式(16)得到：

(17)

因为θ1在上节已经求出，取：

(18)

得到

(19)

联立式(17)～(19)，消去θ3，得：

(20)

又取：

(21)

得到：

Cs2-Dc2=E。

(22)

(23)

1.3.3 求解肘关节角度θ3

由式(20)，取：

(24)

可得：

(25)

(26)

综上所述，式(12)、(23)和(26)即可求出机械臂的所有逆解。在对多解进行选择时，首先将超出取值范围的解舍弃，若依然存在多解，再代入正运动学方程验算后进行取舍。

2 对坐标变换法进行仿真验证

图4 MATLAB中建立的3关节模型Figure 4 Three-joint model established by MATLAB

表2 各连杆的D-H参数和关节变量Table 2 D-H parameters and joint variables of each link

(27)

其中dx1=30 mm，dx2=30 mm，dx3=0 mm；dy1=5 mm，dy2=-5 mm，dy3=0 mm；dz1=-10 mm，dz2=-70 mm，dz3=-85 mm。

P=Rot(y,-90°)Rot(z,-90°)P′。

(28)

2种方法初始角度均为[0 0 0〗T，经计算可得P=P′。坐标转换法与D-H法求得的机械臂末端坐标一致，因此建立的仿真模型可行。

2.1 验证坐标变换法的正运动学方程的正确性

设置关节变量,由q1=[0 67.981 114.591]到q2=[-181.373 5.79 71.486]进行变换，采用jtraj()函数在这2关节位置之间进行插值，其中q1到q2运动时间为10 s。如图5所示插值得到的数据通过函数plot(Robot,q)和函数plot3(x,y,z)进行图形输出。分别得到了基于D-H方法的实线轨迹和基于改进方法的虚线轨迹，其中实线的末端运动轨迹逐渐覆盖掉了预先显示虚线轨迹。

图5 机械臂关节插值轨迹Figure 5 Interpolated trajectory of robot joints

另外，运用函数plot()分别输出机械臂末端点在坐标系{0}的X，Y，Z不同方向上随时间t的变化曲线。如图6所示，2种方法得到的曲线完全重合，充分验证了改进的坐标变换法的正运动学方程的正确性。

图6 机械臂不同时间各方向运动轨迹Figure 6 Trajectory of robotic arm in various directions at different times

2.2 验证坐标变换法的逆运动学方程的正确性

设置机械臂执行末端由位姿T1到位姿T2进行变换：

采用ctraj()函数在位姿T1和T2之间进行插值。运用ikine()函数逆解出每个插值位姿的关节角度，其中T1到T2运动时间为10 s。机械臂模型的位姿插值直线轨迹如图7所示。

图7 机械臂直线插值轨迹Figure 7 Linear interpolation trajectory of robot

另外，运用plot()函数分别输出机械臂随时间t的各关节q1，q2和q3的角度变化曲线。如图8所示，2种方法逆解出的关节变化曲线完全重合，充分证明了改进的坐标变换法的逆运动学方程的正确性。

图8 机械臂不同时间各关节运动角度Figure 8 Motion angle of each joint of robotic arm at different times

2.3 机械臂的仿真工作空间验证

工作空间是机械手末端能够到达的空间范围，即末端能够到达的目标点集合。若给定末端位姿位于工作空间内，则反解存在，否则反解不存在[9-10]。根据表2中机械臂各关节变量的范围，采用蒙特卡洛法[11-13]求取机械臂的运动空间。运用MATLAB中的函数rand()，生成50 000组3关节量，代入函数fkine()，得到50 000个末端点，生成运动空间云点图，如图9所示。

图9 3关节机械臂的运动空间Figure 9 Motion space of three-joint robotic arm

将已经生成的50 000组3关节量，代入式(27)和(28)得到基于坐标变换法的机械臂空间云点图，新的云点图在图9的云点图上进行叠加，得到如图10所示的运动空间。新的云点完全准确地覆盖了之前基于D-H坐标法生成的每一个云点，验证了基于改进的坐标变换法的正运动学方程的正确性。

图10 运动空间验证Figure 10 Verification of sports space

3 机械臂样机实验验证

编写上位机软件进行机械臂样机的控制。

3.1 制作机械臂样机

图11所示为系统结构和机械臂样机。上位机的控制信息通过2个配对的蓝牙模块，无线传输至舵机控制板，控制板将上位机的指令变成舵机的运动角度和速度，实现机械臂的控制。机械臂按照设计尺寸进行3D打印，并运用3个舵机作为关节。电源采用2节18650型锂电池串联的方式，稳压至5 V给舵机控制板和蓝牙供电。

图11 系统结构简图和机械臂样机Figure 11 System structure diagram and robotic arm prototype

3.2 上位机控制实验

在Visual Studio 2017平台上采用VC++进行上位机编程，软件界面如图12所示，功能分为6部分。软件中调用MSComm控件进行串口通信，Teechart控件实时显示各个关节的角度曲线。将关节角度信息通过蓝牙无线串口传输给舵机控制板，实现机械臂的控制。通过样机实验，机械臂运动情况与坐标变换法所计算出的坐标完全一致。

图12 上位机软件界面Figure 12 PC software interface

4 结论

课题组运用坐标变换法完成了对3关节机械臂模型的正、逆运动学分析和运动空间的计算。同时采用D-H参数法，在MATLAB的Robotics Toolbox中搭建机械臂模型完成了对坐标变换法的验证。坐标变换法根据关节之间的相对位置关系，利用平移和旋转的齐次变换矩阵将坐标系建立在关节上或其他合适的位置，坐标轴的方向灵活选取。相比于D-H法观察抽象、坐标系转换不灵活、参数容易混乱以及所建立的连杆模型与实体模型出入大的缺点，坐标变换法具有灵活方便、形象直观以及便于计算的优点，且弥补了传统D-H法在分析平行关节时的缺陷。