多自由度机械臂实时仿真系统笛卡尔空间轨迹规划的研究

刘海蓉

（黄冈职业技术学院电子信息学院，湖北黄冈438002）

多自由度机械臂实时仿真系统笛卡尔空间轨迹规划的研究

刘海蓉

（黄冈职业技术学院电子信息学院，湖北黄冈438002）

轨迹规划对于机器手臂而言是相当重要的，文章基于笛卡尔空间对关节型机器手臂的轨迹规划进行了分析，从空间直线插值和空间圆弧插值这两种最基本的算法着手，针对空间连续直线出现尖角的问题，提出了一种基于空间圆弧连接的均值连续空间直线插补算法，并通过改变拟合度的方法实现了不同的轨迹.最后借助于Matlab软件对算法进行了仿真，验证了文章所提出算法的稳定性，取得了不错的效果，具有一定的实用价值.

插补算法；笛卡尔空间；轨迹规划；Matlab仿真

机器手臂的轨迹是指手臂的各关节在空间运动时，随时间变化而形成位置、速度或加速度曲线.轨迹规划一般分为关节空间轨迹规划和笛卡尔空间轨迹规划.关节空间轨迹规划简单方便且不易出现特殊位形，但是末端轨迹却存在不直观的问题；相反，笛卡尔空间轨迹规划比较直观，但是却常出现特殊位形，可谓各有优缺点.目前，学者们对关节空间的轨迹规划算法进行了大量的研究，在较为简单的机器手臂轨迹规划中一般采用三次多项式和五次多项式插值法［1］，该类算法的优点是计算量小，并具有连续的速度曲线，但是却存在明显的波动；B样条曲线插值法则兼具连续速度和波动小的特点，被更为广泛的使用，不过B样条曲线的速度、加速度初值和终值不能同时为0.Miti等［2］通过对B样条曲线参数的优化，获得了约束条件下的最优规划轨迹；此外，Zhang Bin等［3］对时间进行规划，获得满足关节速度约束条件的规划轨迹.相比之下，对于笛卡尔空间的轨迹规划方面的研究较少，目前被广泛使用和讨论的有空间直线插值算法和空间圆弧插值算法［4-5］等.当机器手臂的轨迹为几段连续的直线轨迹时，会在连接处形成尖角，从而影响轨迹的平滑性.通常的做法是将连续直线进行分离，得到多段的独立直线，然后对各条直线进行单独规划.这样增加了轨迹的平滑性，但电机在各段直线间会不停地启动和停止，从而引起很大的振动和磨损［6-7］.为此，本文在空间直线插值算法和圆弧插值算法的基础上提出了一种改进的插补算法.

1　空间匀速直线插补

空间均匀直线插补，是指机器手臂末端从开始点沿着预定的直线轨迹匀速移动到终点的过程中，对直线上的中间点位置的确定方法［8］.设起点为P0（x0，y0，z0），终点为P1（x1，y1，z1），机器手臂移动的速度为V，则完成整个移动所需时间用式（1）表示

其中i取整数，取值范围为［0，M］.通过式（3）可求出各插值点的位置，然后根据逆运动学便可求出机器手臂的各个关节值，控制关节运动，从而使机器手臂在其末端获得直线的运动轨迹.

2　空间匀速圆弧插补

空间圆弧插补通常有4个步骤［9-10］：第1步是根据空间中的3个点的坐标确定圆弧圆心及半径；第2步实现空间圆弧到平面圆弧的变换，建立UVW坐标系；第3步是在新的UVW坐标系上实现圆弧插补；第4步则是将插补后的结果映射为原坐标系的值.

设机器手臂末端从起点P0经过中间点P1最终到达P2点.其中P0（x0，y0，z0），P1（x1，y1，z1），P2（x2，y2，z2）为空间上的3个不共线的点.则这3点可以确定一空间圆弧，设其圆心为O（a，b，c），则通过该空间圆弧的平面可以表示为

通过式（4）和式（5）便可以相继求出圆心O（a，b，c）和圆弧半径R.

以圆心O为坐标原点，建立新的坐标系UVW，选取O→P0的方向作为U轴正方向，与OP0和OP1组成的平面垂直的方向作为W轴，然后根据右手法则建立UVW坐标系（见图1）.

图1　XYZ-UVW坐标系图Fig.1XYZ-UVW coordinate system

在坐标系｛UVW｝中，对于P0，P1，P2和圆心O，则有a= b=c=0；w0=w1=w2=0；u0=R.

在变换后的｛UVW｝平面内，空间圆弧转变为一个平面圆弧，便可开始｛UVW｝坐标系下的圆弧规划了.设起点P0到终点P2扫过的圆弧角为θ，机器手末端线速度为v，起点到终点所需时间为T.令插补周期为Δt，则插值的个数M=T/Δt，第i个插补点的值θi=ωiΔt，其中w为圆弧运动的角速度.则可以得出｛UVW｝坐标下插值Pi（UWV）=（Rcosθi，Rsinθi，0），而转换到｛XYZ｝坐标下的坐标值Pi（XYZ）（x，y，z），则可用下式求出：

3　空间圆弧连接的匀速连续直线插补

空间直线插补法和圆弧插补法作为笛卡尔空间轨迹规划中最为基本的2种轨迹规划插补法.在一些简单环境工作时，无论采用哪一种单一的插补法都能较好的实现规划任务.而当机器手臂所在的工作空间比较复杂时，单一的插补法往往不能较好地完成任务.典型情况就是当机器手臂末端需要沿着2条直线连续运行时，如果简单地对2条直线进行连续的直线插值，将会使得机器手臂各关节驱动机构反复的启停动作，从而磨损机器手臂.

为此，本文提出了一种基于空间圆弧连接的匀速空间连续直线插补法，即通过空间的圆弧来实现对2条直线的衔接，从而消除直线简单连接所产生的接点尖角.

空间中的2条相交的直线可以确定一个二维平面，因此将空间中的直线转换到二维平面来分析是一个不错的选择.由图2可知，空间上的直线L1和L2交于点P0，在P0点处出现尖角，为了消除其对机器手臂的不利影响，可以选取一段同时相交于两直线的圆弧Cl（见图2）来实现两直线的连接，交点分别为P1和P2.则整个机器手臂的运行轨迹为从L1的起点出发经过P1点，然后通过圆弧Cl到达交点P2，最后沿直线L2到达终点.可以发现，圆弧圆心位置的确定以及圆弧半径的大小，将是轨迹规划的关键.

图2　直线连接图Fig.2Linear connection diagram

为了使直线到圆弧的过渡更为流畅，本文让所选定的圆弧和直线L1和L2同时相切，则圆弧和L1直线的切点为P1，与L2直线的相切点为P2.通过图2可以看出，由于圆弧的圆心可以自由选取，因此内切于L1和L2的空间圆弧有无限多条.为了描述各圆弧相较于原始轨迹的程度，本文定义了一个“拟合度”F. F被定义为圆弧连接后第1段直线轨迹长度与圆弧连接前该直线轨迹长度之比.选取不同的圆弧，将对应不同的F.因此，其取值范围为［0，1］.当直线与圆弧连接点与开始点重合时，即Ps点为圆弧起点，则第1段直线轨迹长度为0，F取“0”值；又有当P1点选为L1直线的中点时，则F为“0.5”.如果设P0到P1的距离为d10，直线L1的长度为，则d10=dL1×（1-F）.

在图2中，由于圆弧同时相切于两直线，因此对两直线分别作垂线，则两垂线交点O（a，b，c）即为圆弧的圆心，P1或P2到O点的距离则为圆弧的半径R.空间圆弧连接的匀速直线插补步骤如下.

步骤1：计算圆弧Cl和两直线的交点坐标；设定拟合度F，根据起始点Ps（xs，ys，zs）和点P0（x0，y0，z0）的三维坐标，可计算出连接点P1处的坐标.

而通过结束点Pe（xe，ye，ze）和点P0（x0，y0，z0）的三维坐标，又可以计算出P2点的坐标.

式中dL1和dL2分别为直线L1和L2的长度.

步骤2：对Ps到P1点之间的空间直线进行空间直线插值，实现这段的轨迹规划.

步骤3：计算圆弧半径R及圆点.根据起点Ps、终点Pe和两直线原始交点P0的坐标，利用式（4）求出平面方程.而P1和P2两点在圆弧上，因此，

由于L1和圆弧相切，OP1的长度为圆的半径R（见图3），根据勾股定律则有R2+d210=d2x，（13）其中d10为P1到P0的距离，dx为圆心O到P0的距离.

图3　圆弧和直线L1的切线Fig.3Tangent of arc and line L1

根据式（4）、（12）和（13）可以求出原点O（a，b，c）的坐标值和圆的半径R.

步骤4：构建UVW坐标系.以圆心O为坐标原点，构建新坐标系UVW，U轴的正方向选定位O→P1的方向，和OP1、OP2所组成的平面垂直的方向设为W轴，则可由右手法则得出V轴的方向.

步骤5：在｛UVW｝坐标系上完成空间圆弧插补，求出插补点Pk（uk，vk，wk），然后得出插补点在原始坐标系｛XYZ｝上的坐标值，从而完成该圆弧段的轨迹规划.

步骤6：对于P2点到L2终端的直线段，采用空间直线均匀插值法完成轨迹规划.

通过以上6步，则可以实现空间圆弧连接的匀速连续空间直线轨迹规划.

4　仿真验证

4.1空间匀速直线插补仿真

设空间内有一条由P1［200，220，10］和P2［250，50，170］两点连接的直线.其中P1点为机器手臂末端的起点，P2为末端终点，采用空间匀速直线插补算法绘制出笛卡尔空间轨迹规划（见图4）.

图4　空间直线轨迹规划图Fig.4Spatial linear trajectory planning

仿真结果表明，轨迹规划为直线，满足要求.通过各插值点对应的坐标，采用运动学逆解的方法可以求出机器手臂各关节需要移动的目标角度.

4.2空间匀速圆弧插补仿真

设空间内有不共线的任意3点P1［10，140，130］，P2［150，90，40］，P3［250，50，70］.则3个点在空间内必定形成一个圆弧，设P1点为起点，P2为中间点，P3为圆弧终点.则机器手臂末端沿着该圆弧从起点运动到终点.可以采用空间均匀圆弧插补来实现笛卡尔空间的轨迹规划（插值次数选为30），仿真效果如图5.仿真图形显示为圆弧轨迹，满足算法要求.

4.3基于空间圆弧连接的匀速连续空间直线插补法仿真

设空间内有任意3点：Ps［210，120，110］，P0［55，220，255］，Pe［48，28，70］，为了使机器手臂能够平滑运行，采用本文提出的空间圆弧连接连续空间直线插补算法，构建一段圆弧来代替原轨迹中的尖角.首先设定一个拟合度的初值F=0.7，然后通过Matlab软件来实现轨迹规划仿真，仿真效果如图6所示.

图5　空间圆弧轨迹规Fig.5Space circular arc path

图6　基于空间圆弧连接的匀速连续空间直线插补轨迹规划图（F=0.7）Fig.6A uniform continuous space linear interpolation trajectory planning（F=0.7）based on space circular connection

由图6可以看出，机器手臂末端将从起点出发直线运行到P1，然后经过由P1、P2构成的内切于两直线的圆弧，最后再沿直线方向到达终点.

若改变拟合度F=0.9，可得到另一个轨迹规划仿真图如图7所示.

图6的规划轨迹和图7类似，所不同的是2个切点的坐标发生了变化，同时圆弧的圆心坐标也发生了变化.

由图6和图7的仿真结果可以看出，本文提出的基于空间圆弧连接的匀速连续空间直线插补轨迹规划算法符合设计要求.该算法运行稳定，能够很好地解决连续直线中的尖角问题，使规划轨迹变得更加平滑，从而改善了机器手臂中各关节的磨损现象.另外，通过选取不同的拟合度，会形成不同的轨迹.实际规划中需要根据环境或规划要求，选择更为适合的拟合度.一般而言，需要高度拟合原始轨迹时，则F的取值应设置为较大值.反之，则可以将拟合度选择得较小，从而减少路程，减少时间和能量浪费.

图7　基于空间圆弧连接的匀速连续空间直线插补轨迹规划图（F=0.9）Fig.7A uniform continuous space linear interpolation trajectory planning（F=0.9）based on space circular connection

5　结论

为了消除机器手臂空间轨迹规划中采用一般插补法产生尖角的问题，结合空间直线插补和圆弧插补算法，提出一种新的圆弧连接均值直线插补法，并引入“拟合度”的概念，在Matlab环境下对算法进行了仿真.结果表明：通过该算法以及对拟合度的调整可以获得最优规划路径，该算法在机器手臂轨迹规划当中具有较好的可行性.

［1］孙秀萍.机器人手臂轨迹规划的研究［J］.内蒙古师范大学学报：自然科学汉文版，2009，38（2）：174-177.

［2］Miti R，Shinichiro N.Offline and online trajectory genera⁃tion with sequential physical constraints［C］//International Conference on Robotics and Biomimetics，February 21-26，2009，Asian Institute of Technology，Bangkok.Washington DC：IEEE，2008：578-583.

［3］Zhang B，Fang Q，Ke Y.Optimal time trajectory planning method for a kind of posture aligning ystem of large rigid bodies［J］.Chinese Journal of Mechanical Engineering，2008，44（8）：248-252.

［4］周刚.垂直关节型6自由度机器人笛卡尔空间轨迹规划的研究［D］.广州：华南理工大学，2011.

［5］林威，江五讲.工业机器人笛卡尔空间轨迹规划［J］.机械工程与自动化，2014，10（5）：141-143.

［6］林仕高.搬运机器人笛卡尔空间轨迹规划研究［D］.广州：华南理工大学，2013.

［7］李伟光，许阳.搬运机器人的轨迹规划［J］.控制与检测，2010（5）：83-85.

［8］陈伟华，张铁.喷涂机器人连续直线轨迹规划的研究［J］.机械设计与制造，2009（8）：178-180.

［9］衣云龙.改进BP神经网络算法在机器人轨迹优化中的研究［J］.沈阳工程学院学报：自然科学版，2012（1）：75-78.

［10］陈军.多自由度机械臂实时仿真系统研究［D］.哈尔滨：哈尔滨工程大学，2012.

责任编辑：刘红

Trajectory Planning of Robotic Arm in Cartesian Space Based on the Matlab Simulation

LIU Hairong
（School of Electronics and Information，Huanggang Polytechnic College，Huanggang 438002，China）

Trajectory planning for robotic arm is very important，the trajectory plan of articulated robot arm was analyzed based on Cartesian space in this paper，Using the two most basic algorithm including space linear interpolation and circular interpolation，for continuous space straight line angle problem，put forward a spatial arc connection mean continuous space linear interpolation algorithm，and by changing a proposed degree different trajectories were obtained.Finally，the algorithm was simulated by Matlab software，and the stability of the algorithm was verified.

interpolation algorithm；Cartesian space；trajectory planning；Matlab simulation

TP 24

A

1674-4942（2015）04-0385-05

2015-09-25