1、第38卷第14期2008年7月数学的实践与认识MA THEMA T ICS I N PRACT ICE AND THEORYVol138No114July,2008六自由度机械臂的运动规划任洪广1,刘聪1,张检发2(1.国防科技大学 计算机学院,长沙410073)(2.国防科技大学 光电科学与工程学院,长沙410073)摘要:为了使六自由度机械臂完成特定的动作,需要设计计算相应的指令序列.首先计算了机械臂位姿与指尖位置之间的关系公式,然后针对机械臂的到达问题、沿曲线运动问题和避障问题,分别提出目标位姿预测、曲线离散到达和受限目标到达三种解决方法,其中涉及的关键算法是自适应搜索法,该方法具有效率
2、高、精度高、适用范围广的特点.在产生指令序列时采用贪心算法.通过以上方法得到的执行结果误差很小(018mm),同时搜索收敛速度也很快.关键词:机械臂;运动规划;自适应搜索;位姿预测1问题分析收稿日期:2008204201图1机器人的示意图该问题为一个六自由度机械臂的运动规划问题.如何为机械臂输入正确的指令序列以使其完成相应的任务是本问题要求解的目标.由于该机械臂有六个自由度,一个目标点(指尖到达的位置)可能对应机械臂的多种位姿,但机械臂的一个位姿只会对应一个目标点.因此,问题的关键就是如何利用高效的方法求出一个可行位姿,使得机械臂在该姿态下到达目标点.产生指令序列时,只需要采用贪心法,由初始位
3、姿过渡到目标位姿即可.对于机械臂的其他动作,可以将其转化为多个目标点的依次到达问题.在有障碍物时,它对于从起始点搜索到目的点的影响,就是要在搜索的过程中加入一系列的约束条件,使得机械臂可以不和障碍物相碰.2位姿与指尖的空间位置之间的关系机器人关于六个自由度的每一个组合(=(1,2,3,4,5,6),表示机械臂的一个位姿,显然每个位姿确定顶端指尖的空间位置X:f()X.如图2,对所给的机械臂在其各个关节点建立相应的笛卡尔坐标系.其中以A为原点建立的坐标系就是题中给出的坐标系,1坐标系和2坐标系的原点都建立在B点,3坐标系建在C点,4,5,6坐标系的原点都重合地建立在D点,每个坐标系的Z轴都和相应
4、关节旋转轴所在直线重合.各个连杆变换矩阵如下:图2机械臂各个关节处的笛卡尔坐标系0T1=-sin1-cos100cos1-sin100001l10001,1T2=-sin2cos2000010cos2sin2000001,2T3=-sin3cos30l2-cos3-sin30000100001,3T4=cos4-sin400001l3-sin2-cos2000001,4T5=cos5sin50000-10-sin5cos5000001,5T6=cos6-sin6000010-sin6-cos6000001其中jTi表示从关节i坐标系转换到关节j坐标系的转换矩阵.各连杆变换矩阵相乘,得到机械臂的
5、变换矩阵为:0T6=0T1(1)1T2(2)2T3(3)3T4(4)4T5(5)5T6(6)所以,第6个坐标系中的坐标(x6,y6,z6,1)T在以基座A为原点的坐标系的表示为:(x0,y0,z0,1)T=0T6(x6,y6,z6,1)T对E点,其在第六个坐标系中的坐标为(0,0,65,1)T,容易得到E点在基座A为原点的坐标系中的坐标,它与 6无关.3目标位姿预测到达问题对于给定的可达目标点,我们首先需要做的就是预测机械臂在到达目标点时的位姿,即各个旋转轴的角度.3.1目标位姿预测对目标位姿预测采用先粗调然后微调的自适应的搜索法.3.1.1粗调由图中位置关系及各个旋转轴的连杆方式(平行连杆、
6、垂直连杆)可知,A、B、C、D四点肯定位于同一平面内.由于平行连杆G的作用使得E点有可能不在ABCD所在平面中.假设某时刻整个机械臂在平面xoy中的投影如图3:27数学的实践与认识38卷图3机械臂在平面xoy中的投影由图3可知,1)由于DE长度相对其他部分来说比较短,所以在机械臂到达目标位置时的“大致方向”与BD方向基本重合,而BD方向是由平行连杆F确定的,所以在进行粗调时首先将F旋转到目标方向.由Ox,Oy可以确定.2)可以看出目标点到z轴的距离大致与D点到z轴的距离相当,而D点到z轴的距离是由旋转轴C确定,所以在进行粗调时可以先将旋转轴C的角度大致调正.3.1.2微调微调要对机械臂上的每个
7、旋转轴进行数次自适应并行微调遍历,算法流程图如图4:图4自适应算法(微调)流程图注:距离为当前位姿下指尖到目标点的距离.算法会在指尖到达距离目标点设定精度范围内跳出,或者在执行次数过多以至于可以认为不可达时跳出.其中回溯模块是指在某些情况下,由于初始设置的搜索方向不好,导致机械臂陷入了一个极限状态,在这个方向上指尖与目标点的距离已经到达了最小,可还是没能接触到目标点.在这种情况下,为了求得结果,需要进行回溯.当执行程序成功跳出时,指尖到达目标点,同时确定位姿(1,2,3,4,5)(每个旋转轴都必须在旋转范围内).3.2运动规划由前面确定的目标位姿以及当前位姿,求解由当前位姿向目标位姿过渡的方式
8、时我们采用贪心算法.即每个旋转轴总是向着离目标角度以尽可能大的步长前进,直至最终所有的旋转轴到达其目标角度.那么,各个旋转轴的变化过程的增减幅度就是最终的指令序列.3.3问题求解机械臂初始状态为(0,-90,0,0,-90),目标点为(20,-200,120)(见图5),根据目标位3714期任洪广,等:六自由度机械臂的运动规划姿预测可得机械臂的最终状态为(-174.2,-38.2,-116.9,-0.4,-124.6),指尖距离目图5机械臂与目标点位置关系标点0.055mm.指令序列示意:-2.0,2.0,-2.0,-0.4,-2.0-2.0,2.0,-2.0,0.0,-2.0-2.0,2.0
9、,-2.0,0.0,-2.0-2.0,2.0,-2.0,0.0,-2.0-2.0,2.0,-2.0,0.0,-2.04曲线离散到达沿线路运动问题要求指尖沿着预先指定的一条空间曲线x=x(s),y=y(s),z=z(s),asb移动,计算满足要求的指令序列.4.1曲线离散化对于空间曲线,由于机械臂的移动本身是离散的,所以不可能让机械臂指尖严格按照曲线的轨迹连续运动.可以将空间曲线划分为离散的点,只要机械臂依次到达这些点就可以了.将s的取值区间a,b划分为N段,每一段是ds.机械臂依次到达(x(s0),y(s0),z(s0),(x(s0+ds),y(s0+ds),z(s0+ds),(x(s0+2d
10、s),y(s0+2ds),z(s0+2ds),即可.其中ds的选取原则为:选取步长应以使得既能很好地逼近曲线,同时在ds变化的程度足够引起指尖的移动.4.2初始位姿选择的必要性与重要性由于机械臂在沿弧线移动的过程中始终是进行微调,如果弧线的长度过长或者是一个圈,而起始的大位姿没有选好,那么将很容易导致机械臂在弧线的牵引下进入很尴尬的境地,本来可达的点变成不可达,这就是局部的不可达.解决局部不可达的方法只有回溯,那将是非常耗时的.所以初始大位姿的确定是很重要的.所谓大位姿就是指在机械臂的运动过程中较长杆所处的大致姿态,类似绘画艺术中的轮廓的意思.一般说来,在进行位姿预测时,对于一个已知的弧线,选
11、择弧线的中央位置或者中央的上方是比较合理的过渡目标.过渡目标的作用是将机械臂伸展到合理的大位姿.流程如图6所示:图6沿曲线移动情况下的流程4.3问题求解如图7,平面与圆台外壁的交线是一个封闭的弧线.起始位姿与的起始位姿相同.我们选取圆台上表面中心作为过渡目标点,调整机械臂的位姿至(-90,-2313,-11318,0,47数学的实践与认识38卷图7平面与圆台外壁相交-12613),指尖到达上表面中心,然后再移动至焊接起始点(52.5,0,26.25),这时机械臂的位姿为(-90,-74.6,-138,0,-88.2).移动过程如图8所示.图8含大位姿预测过程注意,上面提到的“移动”都只是设想中
12、的移动,是为了得到一个合理的大位姿,使得机械臂伸展开,方便在弧线上移动.当得到机械臂弧线上一点的位姿时,那么我们就直接由初始状态向着这个位姿转变,指尖会直接向弧线移动,而不会先到圆台上方的正中心再移向弧线.在分解曲线至小段时,为了使得划分小段的长度尽量均匀,我们将曲线方程转换为参数方程:x=210+630cos()5+cos()y=630sin()5+cos()z=420-15755+cos()然后,经测试取d为0.2度时比较合理,即ds=?900,此时指尖的每次移动刚好可以用一条指令来完成,如果再小的话指尖对位置的变化就不敏感了.机械臂的运动分为两部分:a)从起始点位姿(0,-90,0,0,
13、-90)到焊接点位姿(-90,-74.6,-138,0,-88.2).b)从焊接点位姿(-90,-74.6,-138,0,-88.2)开始焊接整个弧线的过程.5714期任洪广,等:六自由度机械臂的运动规划5受限目标到达避障问题5.1避障方法的讨论对于有障碍物的情况,我们可以看作是在到达问题中含有约束的情形.考虑到障碍物形状大小情况繁多,不可能一一列举,只能具体问题具体分析.大致可分为两步:1)计算障碍物的空间形状,求出几何方程,对于形状不规则的可以进行合理的拟合.2)根据障碍物的空间几何方程,计算什么位姿下机械臂与障碍物不接触,以其作为约束.图9机械臂在圆台内作业3)利用解决目标到达问题的目标
14、位姿预测进行搜索,模拟机械臂的实际运动.注意在搜索过程中要加入机械臂与障碍物不能相碰的约束即受限的目标到达.机械臂在运动过程中必须满足两个条件才会执行该条指令:机械臂的此次运动离目标点的距离减小;机械臂没有与障碍物发生接触.5.2问题求解机械臂要深入到圆台内部完成相应焊接工作.如图9所示.若要整个机械臂都不碰壁,那么:1)对于E点,它要在工件内壁点焊四个小零件,故它必须在圆台内,对于E而言,应有:st.0 zE 180(xE-210)2+y2E 159.38372.5(xE-210)2+y2E+zE 2.5159.38372)对于D点,存在两种情况:D在圆台内.此时线段CD与圆台上底面有交点,
15、其坐标(x1,y1)为x1=180-zCzC-zD(xC-xD)+xC,y1=180-zCzC-zD(yC-yD)+yC要使得CD连杆不会碰壁,该交点必须在圆台上表面开口的圆内:st.(x1-210)2+y21 87.38372故D在圆台内,且CD连杆不会碰壁的条件是:st.0 zD 180(xD-210)2+y2D 159.38372.5(xD-210)2+y2D+zD 2.5159.3837(x1-210)2+y21=180.线段DE与圆台上底面交点坐标(x2,y2)为:x2=180-zDzC-zE(xD-xE)+xD,y2=180-zDzD-zE(yD-yE)+yD要使得DE和圆台不相碰
16、,同理也必须满足:st.(x2-210)2+y22=180(x2-210)2+y22 87.38372综上所述,机械臂不碰壁的条件是:st.0 zE 180(xE-210)2+y2E 159.38372.5(xE-210)2+y2E+zE 2.5159.38370 zD 180(xD-210)2+y2D 159.38372.5(xD-210)2+y2D+zD 2.5159.3837(x1-210)2+y21 87.38372或st.0 zE 180(xE-210)2+y2E 159.38372.5(xE-210)2+y2E+zE=180(x2-210)2+y22 87.38372机械臂在圆台内
17、移动时,每改变一个旋转轴的角度都有可能会导致机械臂的某个部位碰壁,所以每次移动都要判断是否碰壁然后决定是否移动.所以与前面的移动不同,我们并不做位姿预测,而选择直接进行向目标点移动.具体采用的是贪心算法,每一条指令只改变其中一个旋转轴的角度,该角度的改变满足:1)不会产生碰壁;2)以尽可能大的幅度向目标点逼近.圆台内壁的四个点坐标为:1、(320,-104,20.7802),2、(120,106,51.5961),3、(190,-125,82.7563),4、(255,88,152.135).按照1324顺时针顺序依次焊接各个点,求得最终的指令序列.6算法分析及结果分析6.1自适应搜索算法的适
18、用范围在对于某个目标点而言,算法采用先预测后行动,预测时又是先粗调后微调的自适应的搜索法,所以算法具有很好的通用性.对于自适应搜索法,一个很重要的缺点就是对初始值以及步长变化的依赖性较强,如果初始值或者步长选择不合适的话有可能会导致意外的不可达.所以初始值以及步长的选择很重要,要根据具体问题进行合理确定.6.2自适应搜索算法计算效率对于一个可达点,自适应搜索法是可以快速收敛的.定义步长d每变化一次为搜索算法执行的一次循环:对于例1,算法总共执行了87次循环就收敛到距离目标点小于0.2毫米的7714期任洪广,等:六自由度机械臂的运动规划范围.对于例2,算法沿弧线一周执行了1800次到达搜索循环次
19、数为7543次,就可以使得在每一小段上都是局部最优的.对于例3,尽管增加了对于碰壁的限制,每从一个目标点向下一个目标的搜索都是在100次循环以内的.如表1所示:表1针对各个问题的执行情况问题目标点个数性质方法循环次数11距离较远粗调+微调(步长自适应)8721800相邻目标点之间距离很小微调(步长为0.1)754334距离中等(考虑碰壁)微调(步长自适应)93+60+80+51=2846.3误差分析6.3.1算法误差由于 i的取值是离散的,精度为0.1?,那么指尖所能到达的范围也是离散的.对目标点而言,指尖只能尽可能地靠近,这之间肯定会存在误差,这个误差是 i取值的离散性引起,在算法的执行过程
20、中造成的.故将其称为算法误差.针对文中的三个问题,误差统计结果见表2.从表可以看出,误差范围在1毫米以内.表2算法误差问题目标点机械臂位姿(0)指尖位置(mm)误差距离(mm)例1(20,-200,120)(-174.2,-38.2,-116.9,-0.4,-124.6,0)(19.98,-200.01,120.16)0.055例2弧(00.8)(avg0.17)例3(320,-104,20.7802)(-108,-49.2,-105.7,0.2,7.3,0)(319.58,-103.87,20.82)0.44(190,-125,82.7563)(-114,-27.2,-127.7,-58.2
21、,-41.6,0)(190.40,-124.92,82.22)0.68(120,106,51.5961)(-66,-41.9,-133.7,51,-71,0)(120.27,105.83,51.48)0.34(255,88,152.135)(-84,-24.6,-115.7,71.9,-77.6,0)(255.03,87.48,152.32)0.556.3.2精度误差i的精度为0.1?,根据精度的定义,i的误差是小于0.05?.考虑在一个确定的位姿下,只改变其中一个旋转轴的角度,让它增大或者减小一个不小于0.05的角度,观察最终目标的变化情况.由于在不同位姿下,i的变化引起的指尖位置的变化幅度
22、是不同的,以例题中涉及的几个位姿为例,如表3.7结语针对六自由度机械臂的到达问题、沿曲线运动问题和避障问题,本文分别提出目标位姿预测、曲线离散到达和受限目标到达三种解决方法,其中目标位姿预测是后面两种方法的基础.它采用自适应搜索算法,调整各个转角时先粗调后微调,在搜索到达目标点,确定了目标位姿后,用贪心法产生指令序列.本文很好地解决了六自由度机械臂的路径规划问题,算法效率高,产生的指令序列可以87数学的实践与认识38卷很好地完成题中的任务,误差很小.表3分别改变i的大小,增减0.05,引起指尖的偏移情况位姿i改变i的大小,增减0.05,引起指尖的偏移(mm)123450,-90,0,0,-90
23、,00.4450590.4486590.2296450.0567230.056723-174.2,-38.2,-116.9,-0.4,-124.6,00.1753960.1762580.1959630.0466910.0567230,-5.3,-127.7,0,43,00.2400260.2499730.2668330.0386850.056723-108,-49.2,-105.7,0.2,7.3,00.2932440.3111410.2788860.0072080.056723-84,-24.6,-115.7,71.9,-77.6,00.2352860.2295690.235340.0554
24、0.056723注:由表2和表3可以看出,算法的误差与机械臂的精度误差基本处于同一数量级.参考文献:1朱道元.数学建模案例精选M.北京:科学出版社,2003.2蔡自兴.机器人学M.清华大学出版社,2000,9.3丁学恭.机器人控制研究M.浙江大学出版社,2006,9.4杨文茂,李全英.空间解析几何M.武汉大学出版社,1997,1.5阙嘉岚,丁贵涛.基于启发式节点增强策略的PRM路径规划方法J.机器人ROBOT,2003,25(6):5442549.6蔡健荣,赵杰文.水果收获机器人避障路径规划J.农业机械学报,2007,38(3):1022105,结束语转135.The Trajectory P
25、lann ing of a 6-DOFMan ipulatorREN Hong2guang1,L I U Cong1,ZHAN G Jian2fa2(1.School of Computer Science,Changsha 410073,China)(2.School of Optoelectronics Science and Engineering,N ationalU niversity ofDefense Technology,Changsha 410073,China)Abstract:To make the 62DOF manipulator to finish some kin
26、d of jobs,we need to prepare theinstruction sequences toinput.Firstly,this paper gives the function which decides therelationship between the pose of the manipulator and the position of the Fingertip.A nd then inorder to solve the three jobs:the problem of arriving at some point,the problem of movin
27、galong a special curve,and the problem of avoiding obstacles,we design three methods:posespeculation,discretely arriving,and constrained arriving.The main algorithmis adaptivesearch,which is highly efficient,highly accurate and it can be used in many situations.W hencalculating the instructions we use greedy A lgorithm.A ll of this results in very short ti mesearching and very little errors(0.8mm).Keywords:manipulator;trajectory planning;adaptive search;pose speculation9714期任洪广,等:六自由度机械臂的运动规划
