六自由度机器人运动学仿真及轨迹规划*

乐　英 ,岳艳波

(华北电力大学 能源动力与机械工程学院，河北 保定　071000)



六自由度机器人运动学仿真及轨迹规划*

乐英 ,岳艳波

(华北电力大学 能源动力与机械工程学院，河北 保定071000)

为减小机器人运动过程中的振动和冲击，延长机器人使用寿命，提高轨迹规划的精度，以六自由度工业机器人为研究对象，针对采用五次多项式样条规划的轨迹因加加速度有突变而导致的轨迹精度不高的问题，采用五次和七次非均匀B样条对机器人轨迹进行规划。机器人的运动学建模和轨迹规划的仿真通过MATLAB平台实现。仿真结果表明,采用五次和七次非均匀B样条规划的运动轨迹速度、加速度光滑，加加速度连续没有突变，机器人轨迹精度较好，运动性能优于五次多项式样条。

工业机器人；非均匀B样条；轨迹规划；MATLAB

0　引言

机器人轨迹规划的一个目标是研究一种可靠的方法提高机器人运动轨迹的精度和稳定性，规划的各关节位移、速度、加速度、加加速度曲线光滑连续无突变[1]。避免由于突变引发的的机械磨损和振动冲击[2]。

机器人轨迹规划方法主要是利用多项式、抛物线、样条曲线以及不同曲线相结合进行规划。在实际工程应用中，常采用三次多项式对关节轨迹进行规划。但是应用三次多项式进行轨迹规划得不到光滑的加速度曲线。增加多项式次数，可以提高运动轨迹和控制精度，但是随之计算量也会加大，而且高次插值易发生“龙格”现象。文献[3]应用五次多项式样条进行轨迹规划，用MATLAB仿真验证表明，五次多项式样条轨迹规划方法效果优于三次多项式样条。文献[4]分析比较了三、五、七次多项式样条轨迹规划方法，文献[5]采用三次非均匀B样条对机器人轨迹进行规划，但都没有考虑加加速度的影响。在机械系统中，进给运动的加加速度对系统的稳定性有重要影响。如果加加速度有突变，在阶跃变化的上升沿和下降沿, 机械系统会产生振动和冲击, 影响运行平稳性, 进而影响加工质量[6]。

采用五次多项式样条规划的轨迹加加速度有突变，精度不够高，而七次多项式样条约束条件比较多。针对这些问题，本文以REBOT-V-6R机器人为研究对象，采用五次和七次非均匀B样条对机器人进行轨迹规划，并在MATLAB平台上进行仿真，验证轨迹规划方法的有效性和可靠性。

1　关节空间轨迹规划

采用五次多项式样条、五次和七次非均匀B样条在关节空间对机器人轨迹进行规划。根据已知的工作空间原始路径点，反求出对应的关节空间各关节位移。利用这些求出的关节位移值，加上设定的所需速度、加速度值，分别用五次多项式样条、五次和七次非均匀B样条插值求出相应的关节轨迹曲线。基于规划出的关节轨迹曲线，再正向求解出机器人末端规划后的运动轨迹。

1.1五次多项式样条

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

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

(1)

(2)

(3)

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

(4)

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

(5)

每个点的角位移值为已知量，设定其角速度和角加速度为0(可设定为其它值)，两点处共可得到6个已知条件。将已知条件代入公式(1)、(2)、(3)中并联立求解，可解出6个未知的系数。

1.2五次非均匀B样条

非均匀B样条的公式如下，

(6)

式中,di为控制顶点，Ni,k(u)为k次规范B样条基函数,且:

(7)

式中,k表示非均匀B样条次数,i表示序号。

非均匀B样条插值曲线由n+k个控制顶点di(i=0,1,2,…n+k-1)定义，节点矢量相应为U=[u0,u1,…un+2k]。

设机器人的总运动时间为tall=tn-t0，采用累计弦长参数化方法对时间节点ti归一化，得到节点矢量U=[u0,u1,…un+2k]。且有：

i=k+1,k+2,…,n+k-1

(8)

用于插值n+1个数据点pi的k次非均匀B样条曲线方程可写为：

(9)

将曲线定义域[uk,un+k]内的节点值依次代入方程，得到满足插值条件的n+1个方程：

ui+k∈[uk,un+k],i=0,1,…n

(10)

对于k次非均匀B样条开曲线还需增加k-1个通常由边界条件给定的附加方程,附加方程通过切矢边界条件得到，因取两端节点重复度为k,其首末控制顶点就是首末数据点,因此有:

(11)

同理可得：

(12)

将已知的始末点角位移和设定的角速度、角加速度值代入公式(10)、(11)、(12)中，并联立求解出非均匀B样条插值曲线的控制顶点[8],同理可求出七次非均匀B样条的插值曲线。对求出的非均匀B样条曲线依次求导，可分别得到机器人运动轨迹的速度、加速度、加加速度曲线。

2　机器人建模与仿真

以REBOT-V-6R机器人为研究对象，通过D-H法建立机器人的数学模型，该机器人各关节及连杆参数如表1所示。

表1　六自由度机器人的D-H参数表

其中Li为各关节编号，theta为关节变量，d为连杆偏移量，a为连杆长度，alpha连杆扭角。

图1　 机器人示教界面

通过MATLAB中的robotics toolbox工具箱[9]建立机器人的模型，通过工具箱中的LINK函数建立机器人的各连杆模型，

L1=Link([0 10 10 pi/2 0 0]);

将机器人命名为“bot”，通过Teach函数，可在三维空间中通过拖动滑块，观察机器人模型的运动状况。如图1所示。

teach(bot)

通过工具箱中的fkine函数，设定初始关节变量q0，可以求出机器人末端在工作空间的绝对坐标。

q0=[0 0 0 0 0 0];

T0=bot.fkine(q0);

求出机器人工作初始姿态下，末端在笛卡尔空间中的坐标点为p1=[41 -4 41.1]。

在机器人工作空间中，可以直观的观察到机器人末端的运动情况，但是规划的轨迹容易产生奇异点[10]，通过工具箱中jocb函数，可以将机器人末端的速度映射为每个关节的角速度。

q=[0 0 0 0 0 0];

J0=bot.jacob0(q);

Qd=[qd1qd2qd3qd4qd5qd6]′;

V=J0*Qd;%V=[dxdydzwxwywz]′

通过工具箱中的bot.A函数，可以求出机器人每个运动杆件的运动情况，避免机器人各部件与周边物件相碰撞。

A3=bot.A(1 2 3,q1)%经q1角度变化后，3关节的空间坐标。

A5.1=bot.A([2 3 4 5],q1)%经q1变化后，5关节相对1关节的空间坐标。

以建立的机器人模型为平台，以机器人末端为作业点，使末端在三维空间中画一个矩形，画图过程中机器人末端的姿态保持不变。为便于观察，以p1为矩形的一个顶点，其它三个顶点的坐标依次为：p2=[41 26 41.1];

p3=[11 26 41.1];p4=[11 -4 41.1];

在矩形的每条边上，等距离的取10个中间点，四条边共40个点。40个点的坐标信息如下：

X=[41 41 41 41 41 41 41 41 41 41 41 38 35 32 29 26 23 20 17 14 11 11 11 11 11 11 11 11 11 11 11 14 17 20 23 26 29 32 35 38];

Y=[-4 -1 2 5 8 11 14 17 20 23 26 26 26 26 26 26 26 26 26 26 26 23 20 17 14 11 8 5 2 -1 -4 -4 -4 -4 -4 -4 -4 -4 -4 -4];

Z方向坐标值均为41.1。

通过工具箱中的ikine函数，可以反求出与工作空间相对应的关节空间中机器人各关节角度值。

Q=bot.ikine(T,q);

由40个工作空间坐标点反求出40×6个关节位移值，以这40×6个位移值为已知条件，分别以五次样条和五次、七次非均匀B样条规划机器人末端运动轨迹，并设定末端经过每个路径点的速度和加速度均为0(可设定为其它值)。

通过MATLAB编程求出与机器人各关节相对应的五次样条和五次、七次非均匀B样条各6条。三种样条均为关于时间t的函数，总时间t取为12s，各样条曲线之间相互独立。

对求出的三种样条曲线依次求导，得到各关节运动轨迹的角速度、角加速度、角加加速度曲线。画出机器人各关节的运动曲线，为便于比较，可由图中任意截取一段(机器人末端在每个路径点处的速度、加速度均为0)，这里取t=[2.5,3.5]时间段，只列出基于三种曲线规划的1、2、3关节运动曲线。分别如图2～图4所示。

图2　截取的五次多项式样条规划1-3关节运动曲线

图3　截取的五次非均匀B样条规划1-3关节运动曲线

图4　截取的七次非均匀B样条规划1-3关节运动曲线

由图2～图4中可以看出，由两种样条曲线规划出的速度、加速度区别不是很大，都比较连续光滑。但五次多项式规划的运动轨迹在中间路径点处加加速度有突变。而五次非均匀B样条规划的运动轨迹在中间点处的加加速度是连续的，七次非均匀B样条规划的加加速度连续且光滑。在加加速度有突变的地方，会产生振动和冲击，因此，基于五次和七次非均匀B样条的规划方法可使机器人性能更稳定，寿命更长。

在规划出的五次多项式样条和非均匀B样条上以等时间间隔0.05s采集各关节位移数据，可采集到240组关节位移数据。通过工具箱中的fkine函数，可求出与240组关节位移数据相对应的240个工作空间坐标点。T1=bot.fkine(q1);

这些坐标点均为机器人末端规划路径点。将其在三维空间中展示出，如图5所示。

图5　基于五次多项式样条的机器人末端轨迹

规划的轨迹在X、Y、Z方向上均有误差，这里以Z方向上的误差为例进行分析，非均匀B样条规划的轨迹也有类似的结果。将三种曲线规划的轨迹在Z方向的摆动做成误差曲线，如图6～图8所示。

图6　五次多项式样条轨迹误差

图7　 五次非均匀B样条轨迹误差

通过比较可以看出，基于五次非均匀B样条规划的轨迹误差比五次多项式样条要小，七次非均匀B样条规划的轨迹误差最小。说明五次非均匀B样条规划的轨迹要优于五次样条，七次非均匀B样条规划的轨迹要优于五次非均匀B样条。

进一步的可以验证，如果单纯增加规划路径点采集数量，虽可以使机器人运动路径更加平稳光滑，但并不能减小误差，且运算量也会随之增大。要减小误差，只能增加原始路径点的数量(既增加矩形每条边上的路径点)或采用其它规划方法。

图8　 七次非均匀B样条轨迹误差

3　结论

为使机器人运动平稳，延长机器人使用寿命，提高轨迹规划精度，以REBOT-V-6R机器人为研究对象，通过MATLAB建立机器人三维模型。分别采用五次多项式样条、五次和七次非均匀B样条对机器人轨迹进行规划，通过MATLAB仿真比较三种曲线规划的轨迹运动曲线和误差曲线，结果表明五次、七次非均匀B样条曲线规划的轨迹优于五次多项式样条。

采用五次非均匀B样条规划的轨迹加加速度连续，可使机器人运动平稳，轨迹光滑，提高机器人运动性能。在不考虑计算量的状况下，七次非均匀B样条优于非均匀五次B样条，加加速度连续且光滑，规划的速度、加速度值较小，轨迹规划精度更高，是一种稳定可靠的轨迹规划方法。

[1] 勾治践，王成，付威.机器人轨迹规划的有效方法[J].煤矿机械，2013,34(8):66-69.

[2] 杨锦涛，姜文刚，林永才.工业机器人冲击最优的轨迹规划算法[J].科学技术与工程，2014,14(28)：64-69.

[3] 王宁，张新敏.基于MATLAB的六自由度机器人轨迹规划与仿真[J].制造业自动化，2014,36(8):95-97.

[4] 王学林,邢仁鹏,肖永飞,等.机械臂途经N路径点的连续轨迹插补算法研究[J].组合机床与自动化加工技术，2014(11):92-96.

[5] 赖永林，林茂松，梁艳阳.基于三次非均匀B样条曲线的机器人轨迹规划算法研究[J].科学技术与工程，2013,13(35):10511-10517.

[6] 赵国勇，赵玉刚，赵庆志.数控运动中加加速度连续的加减速方法[J].计算机集成制造系统，2011,17(2):16-21.

[7] 林仕高，刘晓麟，欧元贤.机械手笛卡尔空间轨迹规划研究[J].机械设计与制造，2013(3):49-52.

[8] 李小霞，汪木兰，刘坤,等.基于五次B样条的机械手关节空间平滑轨迹规划[J].组合机床与自动化加工技术，2012(8):39-42.

[9] 王智兴，樊文欣，张保成.基于MATLAB的工业机器人运动学分析与仿真[J].机电工程，2012,29(1):33-37.

[10] 计时鸣，黄希欢.工业机器人技术的发展与应用综述[J].机电工程，2015,32(1):1-13.

(编辑赵蓉)

The Kinematics Simulation and Trajectory Planning of Six-DOF Robot

YUE Ying ,YUE Yan-bo

(School of Energy,Power and Mechanical Engineering,North China Electric Power University,Baoding Hebei 071000 ,China )

In order to reduce the vibration and shock in the movement of robot , extend the service life , and improve the accuracy of trajectory planning, to solve the problem of trajectory inaccurate based on quintic polynomial caused by the curve of jerk occurs some breaking points,this thesis is concerning with trajectory planning study based on six-DOF industrial robot by means of quintic and septuple non-uniform B-spline.The modeling and simulation of the procedure for trajectory planning is realized by taking advantage of software MATLAB.The simulation results show that the proposed method could achieve smoother velocity and precision,the continuity of jerk is well,kinematics performance is better than quintic polynomial.

industrial robot; non-uniform B-spline; trajectory planning; MATLAB

1001-2265(2016)04-0089-04DOI:10.13462/j.cnki.mmtamt.2016.04.024

2015-06-05

河北省自然科学基金项目(E2014502042);中央高校基本科研业务费专项资金项目(11QJ61)

乐英(1971—)，女，河南信阳人，华北电力大学(保定)副教授，博士，研究方向为机器人运动控制技术，(E-mail)yueying71@163.com。

TH166;TG659

A