郭瑞峰， 彭战奎， 张文辉， 彭光宇

(西安建筑科技大学机电工程学院，陕西西安 710055)

目前化肥行业常见的码垛方式有3种，分别是人工码垛、传统机械码垛、码垛机器人码垛[1]。虽然化肥码垛机器人的价格比前面2种要高，但在后期的使用过程中它更能节省企业的生产成本。因为码垛机器人有以下7个特点：(1)化肥码垛机器人工作8 h可代替人工3～4人，1年可节省人力成本数十万元。(2)能耗低。传统机械码垛的耗能约 26 kW，而码垛机器人的耗能仅需5 kW左右。(3)结构简单、零部件少。因此其故障率低、鲁棒性好、易于保养和维修[1]。(4)适应性好。当抓取物品的尺寸、形状、空间位置发生改变或末端操作器更换时，只要修改控制程序或重新标定即可实现，而不会影响正常的生产效率[2]。(5)人机界面，便于操作。(6)占用空间少，便于流水化生产线的布置。(7)码垛机器人码垛精度更高，可达±3 mm，减少塌垛现象。

综合以上因素，本研究以实验室自主设计的四自由度化肥码垛机器人为例，通过几何法与Modified-DH模型组合求解的方式，推导该机器人正运动学方程。在逆运动学求解时，通过Atan2函数及各关节运动范围，求出其可行解；针对逆解的多解问题，利用最短行程的原则进行优化，得到各关节角的最优解[3-4]。为了保证机器人精确、流畅、连续、平稳地码放化肥包装袋，采用“5-3-5”法进行轨迹规划。最后通过ADAMS仿真分析验证了理论方法的正确性及机械手设计的合理性，完成化肥码垛机器人的试制，结果表明该码垛机器人可以满足设计要求。

1 化肥码垛机器人结构分析

该机器人可码垛化肥包装袋尺寸参数为960 mm×600 mm，一次性抓取总质量达到160 kg，一次性抓取包装袋的数量为2袋。其结构示意见图1。

该化肥码垛机器人有4个自由度，具有如下优点[5-6]：(1)四边形机构具有误差自补偿作用，Ⅱ驱动轴的运动通过四边形机构传递到末端执行器会更精确，在一定程度上保证了机器人末端执行器定位位置的准确性；(2)整机质量分布得到了优化，增强了码垛机器人的稳定性；(3)伺服电机下移到回转平台上，增加了电机安装位置的灵活性，减小了臂部关节的重力和转动惯量，实现小臂轻量化；(4)在该机构的作用下，机器人的承载能力得以提高[7]。

2 Modified-DH模型

机器人运动学研究中，运动学正解的求解常使用 Denavit和Hartenberg在1955年提出的4参数DH模型[2]。Paul在1981年证明了其对机器人运动学分析的价值，并提出Paul系统，由于被机器人广泛使用而发展成为了标准DH模型[2-3]。然而标准DH模型主要是针对串行结构的机器人进行运动学分析，当用标准DH模型来处理树状结构或者闭环结构机器人的时候会产生歧义[4，8]。1984年Craig提出一种Modified-DH模型[4]，其使用Craig连杆约束，因此又称为Craig系统。该模型具有以下优点：(1)可以用统一的定义来处理串行结构、树结构和闭环结构，具有更好的通用性。(2)其使用修改的DH参数，从机械结构的简化模型方面来讲，其参数更加清晰、真实。但由于国内机器人行业普遍采用标准DH模型处理串联机器人，较少涉及树结构和闭环结构的机器人，因此Modified-DH模型的论述较少。

Modified-DH模型的连杆坐标系描述如图2所示，将它与标准DH模型进行对比得到其基本差异如下：(1)坐标系依附到连杆的位置不同。Modified-DH模型坐标系i-1固连于连杆i-1上，坐标原点Oi-1位于关节轴i-1上，而不像标准DH那样坐标系i-1固连于连杆i上，坐标原点Oi-1位于关节轴i上。(2)所执行的转换顺序不同。根据Craig的连杆约定描述，变换矩阵由以下操作顺序给出。

相邻连杆坐标系Oi-1-xi-1yi-1zi-1到Oi-xiyizi的齐次变换步骤如下：(1)绕xi-1轴旋转αi-1，使zi-1和zi平行；(2)沿xi-1轴平移ai-1，使zi-1和zi共线；(3)绕zi-1轴旋转θi，使xi-1与Oi和Oi+1之间的连线重合；(4)沿zi轴平移di，使xi-1与xi重合。

因此Modified-DH模型的连杆坐标系Oi-1-xi-1yi-1zi-1到Oi-xiyizi的齐次变换通式为

i-1Ti=Rot(xi-1,αi-1)Trans(xi-1,ai-1)Trans(zi,θi)Rot(zi,di)；

(1)

(2)

3 化肥码垛机器人运动学分析

3.1 化肥码垛机器人正运动学计算

由于Modified-DH模型可以用统一的定义来处理该化肥码垛机器人，因此本节采用Modified-DH模型求解的方式，推导该机器人正运动学方程。

机器人机构运动简图如图3所示，图中虚线所示为小臂处于水平位置时的机器人机构状态。主传动系统包括大臂前臂AF、短杆AD、大臂后臂DE和小臂EF组成的平行四边形机构ADEF，以及短杆AD、连杆CD和曲柄BC组成的铰链四杆机构ABCD2个并联机构。为将这2个并联机构串联化，须要知道E点相对于B点的坐标[7-9]，具体做法如下。

首先如图3所示建立局部坐标系B-xy。设各关节点坐标：A(300，300)，B(0，0)，C(xC，yC)，E(xE，yE)。根据几何法可得：

(3)

(4)

(5)

联立公式(3)～(5)，可得：

(6)

式中：α等于连杆CD与坐标系y的夹角，且α∈(0，90°)。

在铰链四杆机构ABCD中可得γ角的正余弦如下：

(7)

根据三角函数关系，消去变量γ得：

(8)

根据公式(8)及A、D2点坐标，求出夹角α的正余弦：

(9)

通过公式(6)、(9)可以得出E点相对于B点的关系式仅与关节角θ2及θ3相关，因此可以将并联机构串联化。

根据图3建立机器人Modified-DH连杆坐标系见图4，对于Modified-DH模型，基坐标系O-x0y0z0可以任意设定[4]，通常为了简化计算，设定z0轴沿D(xD,yD)关节轴1的方向。各关节Modified-DH模型连杆参数见表1。

表1 Modofied-DH模型连杆参数

将表1中Modified-DH模型连杆参数带入公式(2)，可得0A1、1A2、2A3、3A4、4AG。将各连杆矩阵连乘得到0TG。

0TG=0A1·1A2·2A3·3A4·4AG。

(10)

所以，机器人末端执行器的位置为下列矩阵的第4列：

(11)

机器人末端执行器的姿态矩阵为

(12)

式中：R0为基坐标系的初始姿态矩阵。

联立式(6)、(7)、(10)～(12)，可得化肥码垛机器人正运动学方程如下：

(13)

式(13)中：

nx=-s(θ1+θ4)；

ox=ny=c(θ1+θ4)；

oy=s(θ1+θ4)；

(14)

(15)

(16)

3.2 化肥码垛机器人逆运动学计算

运动学逆解时，文献[7]采用单变量反正切函数，这样不仅可能会造成解的丢失，而且角的精度也难以保证[9]。这里通过先确定各关节角的正弦和余弦，然后应用Atan2函数求关节角。

将公式(14)、(15)两边对应相除可得：

θ1=Atan2(Hy,Hx)。

(17)

将公式(14)、(16)移位化简，可得：

(18)

对公式(18)利用三角函数关系，可以消去变量θ3。结合公式(9)、(17)可求出sθ2、cθ2：

θ2=Atan2(sθ2,cθ2)。

(19)

此时公式(18)只有θ3仍是未知量，因此可以求出sθ3、cθ3：

θ3=Atan2(sθ3,cθ3)。

(20)

假设工作过程中末端执行器的姿态保持不变，则：

θ1+θ4=0；
θ4=-Atan2(Hy0,Hx0)。

(21)

4 轨迹规划及ADAMS仿真分析

4.1 轨迹规划

码垛作业如下：1 h码放900袋，从生产线指定位置A(2 500，0，1 200)抓取化肥包装袋、经提升位置B(2 500，0，1 200)、下降位置C(3 500，0，1 200)将化肥包装袋码放在位置D(3 500，0，800)，轨迹规划按照“PTP”的运动形式，要求整个码放过程连续、平稳运行。化肥码垛机器人尺寸参数[7]见表2。

码垛作业的时间分布如下：

ΓA-B=2 s、ΓB-C=4 s、ΓC-D=2 s。

通过Matlab编写逆解程序，得到各关节角的可行解见表3。

对表3中各关节角的可行解，按照最短行程的原则(每个关节总的移动量最小)进行优化，可以得到各关节角的最优解见表4。

表2 化肥码垛机器人尺寸参数

表3 各关节角的可行解

表4 各关节角的最优解

为了保证机器人精确、流畅、连续、平稳的码放化肥包装袋[5]，须要对表4中的数据进行连续化处理。本研究采用“5-3-5”法对轨迹进行插补运算[5，10]，最终得到各关节轴角位移曲线见图5。

4.2 ADAMS仿真分析

(1)利用SolidWorks建立机器人三维模型；然后导入ADAMS软件中进行装配并添加相应的约束[10-11]。(2)将图5中各关节角位移曲线的数据导入ADAMS中；利用AKISPL函数进行关节角位移拟合，然后添加到相应的关节角驱动中。(3)根据作业要求，设置求解器仿真时间为8 s，进行运动学仿真，仿真过程中的某一时刻状态见图6。(4)利用ADAMS的测量功能及后处理功能，得到机器人末端执行器的仿真位移曲线见图7-a。(5)将图5中的数据代入到公式(14)、(15)、(16)当中，计算得到理论位移曲线见图7-b。(6)将ADAMS仿真位移曲线与理论位移曲线进行对比得到图7-c。运用Modified-DH模型计算得到的理论位移曲线与ADAMS仿真位移曲线基本重合，验证了该化肥码垛机器人运动学求解的正确性。

5 结论

本研究以实验室自主设计的四自由度化肥码垛机器人为例，通过几何法与Modified-DH模型组合求解的方式，推导该机器人正运动学方程。在逆运动学求解时，通过Atan2函数及各关节运动范围，求出其可行解；针对逆解的多解问题，利用最短行程的原则进行优化，得到各关节角的最优解。为了保证机器人精确、流畅、连续、平稳地码放化肥包装袋采用“5-3-5”法进行轨迹规划；通过ADAMS进行单周期仿真分析，验证了理论方法的正确性及机械手设计的合理性。最后完成化肥码垛机器人的试制，结果表明该码垛机器人可以满足设计要求。