工业机器人运动学与轨迹规划仿真

唐新星，王 平，范大川

(长春工业大学 机电工程学院，吉林 长春 130012)

0 前言

“中国制造2025”提出后，制造业普遍需要技术和设备升级改造，以增强竞争力，提高经济效益，工业机器人作为先进制造业中的重要装备和手段，具有极大的发展空间[1-3]。在三维空间内对工业机器人进行轨迹和速度规划，可进一步提高生产率、提高产品质量、降低劳动强度、改善劳动条件，在实际工作中具有重要的现实意义。

工业机器人轨迹规划以保证其运动轨迹精度和稳定性为前提，各关节位移、速度、加速度曲线光滑连续无突变，减少机械本体的磨损和振动冲击，提高作业效率[4]。文献[5]以M-10iA工业机器人为研究对象，选用D-H法则建立机器人数学模型，运动ADAMS软件进行仿真，研究连杆末端点相对于坐标原点的位置变化；文献[6]以六自由度机器人为例，运用MATLAB对机器人关节运动状态仿真分析；文献[7]将三次均匀B样条应用于工业机器人轨迹规划，但该轨迹方法不易得到平滑的加速度曲线，且不合适于空间任意分布的示教点；文献[8] 运用七次样条曲线进行轨迹规划，得到了光滑的速度和加速度曲线，但高次插值易发生“龙格”现象。

针对以上问题，本文根据自行研发的六自由度关节型工业机器人，利用SolidWorks建立工业机器人模型，采用五次插值多项式算法，在ADAMS仿真软件中建立该机器人的虚拟样机，进行运动学与轨迹规划仿真研究。

1 机器人手臂结构设计

运用SolidWorks软件对六自由度关节型工业机器人进行1∶1的三维建模，将各零部件模型进行装配后构建虚拟样机[9]。所设计的六自由度关节型工业机器人如图1所示。该机器人由基座、腰部、大臂、小臂、腕部和手部组成，共有6个关节。各关节均为转动关节，其中关节1～5装有交流伺服电机，关节6装有步进电机，在机器人控制系统的作用下，驱动各个关节按照预定的空间轨迹运动，从而实现末端执行器的作业要求。

图1 机器人实物图

2 机器人数学模型的建立

为了研究机器人的运动学问题，首先要对机器人建立数学模型。

2.1 空间坐标系的建立

机器人共有6个关节，根据机器人各关节位置与结构关系可将工业机器人空间坐标系分为基础坐标系、关节坐标系、用户坐标系和工具坐标系。按照D-H连杆坐标系建模方法，分别建立关节1～6坐标系，建立针对本机器人的坐标系如图2所示。

图2 机器人连杆坐标系

由D-H连杆坐标系可得到相应连杆参数如表1所示，其中，a1=100 mm，a2=222 mm，d4=253 mm，机器人末端坐标系到工具坐标系的距离dT=68 mm。

表1 工业机器人连杆参数

2.2 机器人运动学方程的正解

正向运动学主要解决机器人运动方程建立及手部位姿的求解，即已知各个关节的变量，求解手部的位姿[10]。根据所建立的D-H连杆坐标系，i-1系与i系间变换关系可用坐标系的平移、旋转来实现，其变换矩阵如式(1)所示。

Ai=Rot(z,θi)Trans(0,0,di)Trans(ai,0,0)Rot(x,αi)=

(1)

末端执行器在连杆坐标系中的位姿可通过式(2)所示矩阵T表示。

(2)

式中，nx=-c6[c1s5s23-c5(c1c4c23+s1s4)]-

s6(c1s4c23+s1c4)

ny=-c6[s1s5s23-c5(s1c4c23-c1s4)]-

s6(s1s4c23+c1c4)

nz=s4s6s23-c6(s5c23+c4c5s23)

ox=s6[c1s5s23-c5(s1s4+c1c4c23)]-

c6(c1s4c23-s1c4)

oy=s6[s1s5s23-c5(s1c4c23-c1s4)]-

c6(s1s4c23+c1c4)

oz=s23s4c6+s6(c4c5s23+c23s5)

ax=-c1c5s23-s5(c1c4c23+s1s4)

ay=-s1c5s23-s5(s1c4c23-c1s4)

az=c5c23+c4s5s23

px=c1(a1+a2c2-d4s23)-

dT[c1c5s23+s5(c1c4c23+s1s4)]

py=s1(a1+a2c2-d4s23)-

dT[s1c5s23+s5(s1c4c23-c1s4)]

pz=-a2s2-d4c23+d6(-c5c23+c4s5s23)

s1=sinθ1；s23=sin(θ2+θ3)；c1=cosθ1；c23=cos(θ2+θ3)，以此类推。

2.3 机器人运动学方程的反解

工业机器人在空间的轨迹是按照机器人的作业任务来规划的，轨迹上的各点分别对应他们各自关节变量，由计算得到的关节变量q=[θ1,θ2,θ3,θ4,θ5]T去控制工业机器人的各个关节运动，实现对规划轨迹的跟踪。工业机器人末端执行器坐标系原点相对于基座坐标系原点的位置关系式：

(3)

式(3)可进一步化简为，则有

(4)

对式(4)按照一阶泰勒级数展开，则有

(5)

选择合适的步长不断进行迭代，由式(5)则可以求得

(6)

在工业机器人的实际运用中，为了使得到的关节角更加合理，根据各关节之间的关系，可以应用约束：θ1∈[-165°～180°]，θ2∈[-132°～71°]，θ3∈[-123°～123°]，θ4∈[-170°～170°]，θ5∈[-120°～120°]。

3 轨迹规划与仿真

机器人轨迹规划是根据作业任务的要求，计算出预期的运动轨迹。为了满足对机器人运动轨迹的更高要求，本文设定机器人的初始位置和目标位置，根据五次多项式插值算法求解各个方程的轨迹方程[11]。

3.1 五次多项式插值算法

五次多项式样条中，两节点之间的曲线段由五次多项式拟合生成，整个曲线由一段五次多项式组成。五次多项式函数中共有6个未知系数，对其求一阶、求二阶导，可求出机器人末端的速度、加速度关于时间t的函数式为

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(7)

(8)

(9)

其中与起始和终止时间点t0、tf相对应的角位移θ0和θf为已知，且满足条件：

(10)

在每段多项式起始点和终止点处满足二阶连续，设置始末点的速度、加速度且满足条件：

(11)

每个点的角位移值为已知量，设定其角速度、角加速度为任意值，两点处共可得6个已知条件，将其代入式(7)、(8)、(9)联立方程组可分别解出6个未知系数a0～a5。

3.2 轨迹规划

关节1轨迹方程为

θ1(t)=1.932-1.667t3+1.250t4-0.250t5

(12)

关节2轨迹方程为

θ2(t)=0.289+1.252t3-0.939t4+0.188t5

(13)

关节3轨迹方程为

θ3(t)=-1.109+2.094t3-1.571t4+0.314t5

(14)

关节4轨迹方程为

θ4(t)=0.922+1.690t3-1.268t4+0.254t5

(15)

关节5轨迹方程为

θ5(t)=0.444+0.679t3-0.509t4+0.102t5

(16)

关节6轨迹方程为

θ6(t)=0.197+2.411t3-1.808t4+0.362t5

(17)

3.3 仿真分析

将建好的SolidWorks模型导入ADAMS中，根据机器人构件的相对运动关系在ADAMS中添加相应的约束和运动副，如图3所示[12]。检查机器人的自由度后，根据机器人的运动轨迹要求，设定各个关节的轨迹程。

图3 机器人虚拟样机模型

将上述关节轨迹方程分别输入到对应的Motion驱动中，设置终止时间是2 s，设置输出步数是200步，设置仿真类型为运动学仿真。

利用ADAMS仿真得到0～2 s内各关节及末端执行器的运动状态，如图4～ 10所示。按照轨迹规划要求，由图4可以看出关节1转动了76.41°，关节2转动了-57.38°，关节3转动了-96°，关节4转动了-77.47°，关节5转动了-31.12°，关节6转动了-110.52°。由图5、图6可以看出机器人在起始时间各关节角速度、角加速度均为0，其中关节1的最大角速度为71.63 rad/s，关节2的最大角速度为-33.63 rad/s，关节3的最大角速度为-25.44 rad/s，关节4的最大角速度为-57.46 rad/s，关节5的最大角速度为27.46 rad/s，关节6的最大角速度为-65.43 rad/s，在终止时间机器人各关节角速度、角加速度均为0。末端执行器的位移、速度和加速度变化直接影响着工业机器人的平稳性。图7～图9给出了机器人末端执行器的重心对机器人基坐标系分别在x、y、z方向的位移、速度和加速度变化曲线，图10给出了工业机器人末端执行器的重心的轨迹曲线。从图7～10中可以看出，机器人运动速度和加速度比较平稳，均无突变，从而保证了机器人工作时的平稳连续性。

图4 关节角度变化曲线

图5 关节角速度变化曲线

图6 关节角加速度变化曲线

图7 末端执行器位移变化曲线

图8 末端执行器速度变化曲线

图9 末端加速度变化曲线

图10 机器人末端运动轨迹

4 结束语

通过Solidworks三维建模软件构造工业机器人的虚拟样机，将三维模型导入ADAMS多体动力学软件中，根据设定机器人的初始位置和目标位置，经运动学反解获得各个关节的运动参数，利用五次多项式插值对工业机器人运动轨迹进行规划仿真，得到工业机器人各个关节角度、角速度和角加速度的变化曲线，末端执行器的运动轨迹、位置、速度和加速度变化曲线。仿真结果表明，机器人运动速度和加速度比较平稳，加速度无突变，能够保证工业机器人平稳连续运行，为进一步工业机器人实时控制提供了重要参考。

参考文献:

[1] 《中国制造2025》推动机器人发展[J]. 机器人技术与应用，2015 (03):31-33.

[2] 王田苗，陶永. 我国工业机器人技术现状与产业化发展战略[J]. 机械工程学报，2014，50(09):1-13.

[3] 骆敏舟，方健，赵江海. 工业机器人的技术发展及其应用[J]. 机械制造与自动化，2015，44(01):1-4.

[4] 乐英，岳艳波.六自由度机器人运动学仿真及轨迹规划[J].组合机床与自动化加工技术，2016(04)：89-92.

[5] 朱华炳，张娟，宋孝炳. 基于ADAMS的工业机器人运动学分析和仿真[J]. 机械设计与制造，2013(05):204-206.

[6] 王想实，周薇，高雅. 焊接机器人控制设计与仿真研究[J]. 机床与液压，2017 (17):32-35.

[7] 李林峰，马蕾. 三次均匀B样条在工业机器人轨迹规划中的应用研究[J]. 科学技术与工程，2013，13(13):3621-3625，3646.

[8] 刘松国，朱世强，吴文祥. 具有运动时间约束的机械手最优平滑轨迹规划[J]. 电机与控制学报，2009，13(06):897-902.

[9] 詹迪维.SolidWorks机械设计教程2012[M].机械工业出版社，2012.

[10] 蔡自兴.机器人学基础[M],机械工业出版社，2015.

[11] 张强. 基于UG和ADAMS的五自由度机器人手臂的空间轨迹优化设计[D].沈阳：沈阳理工大学，2016.

[12] 宋少云，尹芳.ADAMS在机械中的应用[M].国防工业出版社，2015.