双手互换机械臂的运动学建模及控制研究

田志彬，崔顺风

(苏州大学 机电工程学院，江苏 苏州 215100)

0 引言

目前越来越多的机器人正在取代人工，机械手臂作为现代化生产线的重要组成部分，已被广泛应用于工件加工、货物搬运和产品组装等环节[1-2]。汪满新等[3]提出了一种3-SPR并联机械手结构，基于量纲-尺寸参数相对应的方法对机构的运动学及性能做了相关研究。梁辉等[4]提出一种具有9自由度的上肢康复机构，通过运动仿真，验证了该康复机构运行的稳定性和可行性。当前在简单工件的正反冲压作业中，大部分采用人工翻面处理为主，如果将这部分工作交给机器人完成，不仅会解放更多的劳动力，还会大大提高生产效率。因此，设计了4自由度的双手机械臂，通过两个手爪的交替使用，实现冲压工件的翻面处理。通过CODESYS软控制器对运行轨迹进行规划，在三维空间上实现了工件的自动正反面冲压处理，且具有较快的速度和平滑的轨迹。

1 机械系统及运动学建模

1)三维模型

双手机械臂的三维模型如图1所示，1关节、2关节为旋转副，实现二维空间的定位；3关节为上下移动的模组，实现上下运动；4关节为活动手爪8的旋转运动，实现两手的转换，固定手爪7固定在模组上面；手爪的末端固定吸盘，实现对工件的吸取搬运。

图1 双手机械臂三维实体模型

2)运动学建模

运用D-H参数方法建立机械臂的连杆坐标系如图2所示。其中，x0z0为其基坐标系，基坐标系原点位于第一关节的轴线与底座的交点处。两个连杆参数分别为a和d,a为两个轴关节之间的连杆的长度，d为相邻者两个连杆之间的偏移距离。另外还有两个变量用来描述连杆本身或连杆之间的相互关系，其中一个为连杆连接两端轴线的夹角度数，另一个则是两个相邻连杆之间的夹角度数。不过对于本机器人来说，它有3个旋转轴以及1个移动轴，轴线之间的夹角都是90°或者0°。

图2 双手机械臂的连杆坐标系

当确定了各个连杆参数之后就可以画出连杆参数表。关节4作为附加轴，独立于轴组之外，单独控制，工件末端的位置由前3个关节决定。设θi为杆件i相对于杆件i-1的角度，di为杆件i相对于杆件i-1的偏移量，ai为杆件i的长度，αi为杆件i的扭转角度。

表1 机械臂的连杆参数

依据参数表，根据D-H法，求出坐标系i相对于坐标系i-1的齐次变换矩阵i-1Ti，可得

0T3=0T11T22T3=

(1)

式(1)中矩阵对应元素相等，得出

(2)

上式为机器人运动学正解，可将关节角度转变成机器人末端位置。

由于本机械臂没有末端姿态的旋转，由末端位置即可反解出θ1、θ2和d3。

(3)

3)运动学模型初步验证

利用MATLAB的Robotics工具，进行运动学建模的验证。机械臂的规格为l1=200，l2=200。利用Link函数指定连杆，SerialLink函数连接连杆，bot. fkine和bot. ikine两个函数分别实现正运动学和逆运动学的计算和验证。输入2个轨迹点的轴坐标(-41.234,75.829,40,0),(-33.563,73.472,60,180)，正解出的位置坐标分别为(315.042，-18.277，-40)、(320.069,17.742，-60)，手爪分别为闭合状态和分离状态。进而利用得到的位置坐标，进行运动学逆解，结果与两个点的轴坐标吻合，运动学模型初步得以验证。

4)运动模拟

借助SolidWorks实现对双手机械臂的作业机制的模拟仿真。

图3 运动模拟图

作业机制如下：首先机械臂两手爪呈分离状态，固定手爪抓取正面已经加工完毕的工件；机械臂改变位置，固定手爪将工件转移到活动手爪；活动手爪转动180°，即实现了工件的翻面处理，通过位置控制将工件放到工位上。

2 控制系统搭建及运动学仿真

1)控制系统搭建

工控机选用海康威视视觉工控机MV-VC2210，其中一个网卡安装Ethercat驱动，用于和驱动器通信；驱动器采用新时达MS-R四轴驱动器，支持Ethercat通信；驱动器连接4个带有绝对编码器的伺服电机。配置工控机为软控制器，在CODESYS软控制器上面建立双手机械臂模型，实现对机械臂的运动学仿真。控制系统如图4所示。

图4 控制系统构成

2)运动学仿真

根据正逆运动学，前3个轴构建为轴组，为其构造正逆解模块用于运动控制，第4个轴单独控制。CODESYS中的虚拟模型为双手机械臂，仿真的实体为SCARA机器人。因为SCARA机器人的大臂和小臂以及上下轴与本机械臂的结构大致相同。CODESYS可视化只显示二维位置模型，另外两个轴的位置信息用Ethercat捕捉，显示其运动轨迹即可。dArmLength1、dArmLength2分别为200mm、200mm。

利用双手机械臂的正逆解模块进行编程，通过梯形速度规划实现对双手机械臂的运动仿真，工控机作为Ethercat主站，驱动器作为从站，利用直线插补将机器人从(300,-200)运动到(300,50)，跟踪4个轴的轨迹，每隔4ms捕捉一次位置、速度、加速度信息及所有点构成平稳的位置和速度曲线。虽然实体机器人运行平稳，但是一轴、二轴的加速度有突变，会对机器人寿命产生影响。运动学逆解的关节曲线和对应的运动学正解的空间位置曲线如图5、图6所示。

图5 双手机械臂一轴、二轴的位置和速度轨迹曲线

图6 双手机械臂空间位置xy变化曲线

3 轨迹规划

由于梯形速度规划在实际中会引起加速的突变，于是在关节空间进行轨迹规划，利用机械臂工作时必经的5个点，反解出每个轴的参数，对每个轴进行7次多项式插值。给定n+1个点位(ti,pi),利用分段7次多项式插值，使得分段多项式经过所有点序列。

起点处一阶导数估计：

v0=(p1-p0)/(t1-t0)

(4)

终点处一阶导数估计:

vn(pn-pn-1)/(tn-tn-1)

(5)

中间点处一阶导数估计：

vk=[(pk+1-pk)/(tk+1-tk)+(pk-pk-1)/(tk-tk-1)]/2

(6)

二阶导数和三阶导数同理。

设每段的7次多项式方程为

(7)

利用7次多项式方程、一阶导数、二阶导数、三阶导数的起点和终点，构成8个约束方程：p(ts)=ps；p′(ts)=vs；p″(ts)=as；p‴(ts)=js；p(te)=pe；p′(te)=ve；p″(te)=ae；p‴(te)=je。

根据约束方程即可求解每一时间段的7次多项式系数a0，…，a7。

(8)

对机械臂的一轴进行轨迹规划，通过机器臂的5个位置反解出一轴的5个角度[-41.234, -33.563, -15.680, 2.566, -45.785]，每个节点对应的时间点为[0, 2, 4, 8, 10]，dt设为 0.001，利用7次多项式分段插值得到每个时间点的位置、速度、加速度和加加速度，绘制出第一个轴的位置、速度、加速度、加加速度曲线，如图7所示。与梯形速度规划的加速度曲线相比，7次多项式分段插值轨迹规划加速度连续平滑，解决了加速度突变的问题。

图7 一轴的位置、速度、加速度、加加速度曲线

4 结语

设计了一种双手互换实现工件正反面加工的搬运机械臂，首先推导出运动学方程并通过D-H法建立机械臂的运动学模型，解决了在工作空间内从 A位置精准运动到 B位置的问题，误差在0.1mm以内；然后通过Ethercat通信协议搭建机械臂的控制平台，在CODESYS上建立机械臂模型，仿真得到机械臂在 0～30s内各关节角度和末端执行机构质心的变化曲线以及速度曲线。通过MATLAB进行轨迹研究，利用7次多项式插值实现更平滑的轨迹曲线，改善了加速度突变的问题，得到了良好的控制效果。仿真结果表明：机械臂的结构设计十分可靠，推导出的运动学理论方程正确，求解出的工作空间直观、准确。