机器人运动轨迹规划分析与算法徐向荣〔华东冶金学院机械系马香峰马鞍山市)(北京钢铁学院机器人研究所)nu本文在Li和Lh等人工作的基础上提出了一种新的机器人CP运动轨迹规划算法在该算法中连续路T)被分成m段径由一组直角坐标下的参数方程来描述时间区间〔。并且各轨迹段的拟合多项式系数采用递推方式摘要求得这不仅易子计纤机实现而且计算全也较少另外本文对时间最优轨迹规划问题也作了分析和研究并提出了一种有效的算法关镜词操作器轨迹规划递推多项式1引言轨迹规划是机器人研究中一个非常重要课,到一种机器人CP运动的轨迹规划算法,在这种算法中各轨迹段的拟合多项式系数采用递题国内外已有不少学者在这方面做了大量工“「推方式求得这不仅易于计算机实现,而且计作题,’。’Kahn和Roth〔“’早在1971年就研究算量也较少2过机器人作PTP运动时时间最优轨迹规划问P机器人CP运动的轨迹规划算法但作了不少假设和近似,ual`“’曾提出机器人在于移动:CP运动也即连续轨迹运动的特点,一种机器人手臂沿空间直线段运动的关节轨迹规划方法他首先利用齐次变换矩阵将手部在我们不仅要求机器人到达目标点而且CP必须沿着我们所希望的路径在一定精度范围内直角坐标下的位置速度和加速度变换成各关,本文采用基于直角坐标方案来规划,节的位移速度和加速度然后用插补法将每,运动轨迹的这种方案就是,:轨迹是以机器人手t一轨迹段上关节的位移二次平滑函数法速度和加速度规划成但〔咭’的直角坐标位置和姿势对时间的函数来描述:,Pul方法易于理解和应用aTa为规划轨迹,我们进行如下考虑计算量非常大loyr改进了Pual的方,假定机器人完成CP运动所需时间为T区间〔。T将,他采用四元数表示法来描述手部位置为,〕分成m段(m与精度有关m越减小轨迹误差他将轨迹离散成一定数量的点然后再逐步计算出这些离散点上关节的位移大〔t则精度越高):〔工o。t=,〕,〔才:,公:〕,…,t、`〕r。=tT每个子区’eI长度d…,速度和加速度空间的线完成,后来Kim和,shin〔“’提出一种为:乙t二r、一亡``,(i,=`1。),在每时间最短轨迹规划方法括动力学约束这种方法是基于关节,一时刻t(亡、=艺`+尔),首先根据连续轨,并考虑了各种实际约束条件其中也包,迹的方程和手部姿势要求进行运动学反解计算次运动学反解计算;计算出机器人手部但这种方法较复杂n且只能离〔6坐标系相对基座参照坐标系的位置和姿势再m十后来iL和Luh等人提出了规划机器7’,以求得期望的关节转角,人CP运动轨迹的三次样条函数方法n,这,(或位移)和关节角增量l这样共要进行t、种方法的优点是可得到优化的关节运动规律但当轨迹中间路径点个数较多时计算量也就较大当求得在,和t;两点此法所需,时各关节变量值和速度值时拟合在区间〔t,,再用一多项式来另外这种方法还要求在轨迹“〕内关节的运动PTP因此规划前计算出整个路径上所有中间点这也使Lu,机器人CP运动轨迹的规划比运动轨迹规,得这种方法具有局限性人工作的基础上,本文在iLn和h等划所需的计算量要大得多置要求外通过对轨迹规划的研究1987得当机器人手部沿连续轨迹运动时除了位,收到本文的时间是年4月2。日往往还有速度和加速度要求因此为了使规划出的各关节运动规律还能满足手部卷期机器人运动轨迹规划分析与算法在直角坐标下的速度和加速度要求运动学速度和加速度反解计算先求出雅可比矩阵J,,还应进行:,甲i=切。+望少二望几T一般方法是(9)然后对其求逆和求导山X口Y`,、分别表示手部与基座参照坐标系再由下面两式进行计算将手部速度和加速度:Z轴的夹角或欧拉角的约束变换为对关节速度和加速度的约束g二Jq二J为求出各关节的运动规律1,q,二q,(约,j(T二`(Q)y+(1)J…,Nt,`N为自由度)对区间〔0〕中`’(q)下`(q)yy(2)每一时刻T(i=O1…m;t。二O,x￡t、二),先由式(a*3)~,(9)求出(y式中:J(q)为雅可比矩阵;丫为手部在直角坐z;尽`甲`)再据此计算出手部坐标二标下的速度矢量,速度矢量为手部在直角坐标下的加系相对基座参照坐标系的变换矩阵T寿i(1,o,…m),然后再由T介进行运动学反解计,由于求雅可比矩阵及其求导求逆运算极算求出对应的各关节变量值的多解性,由于运动学反解某复杂因此这种方法所需计算量很大,为此运动对每一个,,T寿可能有好几组关节变,本文采用一种近似方法来考虑机器人时手部有速度和加速度约束的情况连续轨迹可用参数方程表示cP量值与之对应这时可根据各关节工作范围将其条件是无意义解去掉值才、在剩下的几组有效解中…再选=:`并且参数能反映择一组与当前形态最接近的解作为各关节变矗口`一、手部速度和加速度的变化,q*2g;N另外当乙r`(-:现设连续轨迹是以如下参数方程表示l)很小时,可用下述近似公式计算各关节:esIesexs=f;()s(3)(4)速度和加速度叮`,夕=z二ffs:(s)3亡(q`*一召、:,)/乙t;=乙叮`,/乙t`(s)(t)(5)(10)q、,=(6)`*s(q、`,,一q、:,)“`/(乙忿)(11),其中::Xyz,是连续轨迹的直角坐标分量;它是用来反映手部速度和加,式中:爪,乙q口`,一g`:,;i=01…,是时间t的函数,j=1…N速度变化的其意义随具体情况而定可以是当求得在亡`:和,:`两点时的各关节的关节长度或角度等时,譬如当机器人手部沿直线运动,变量值和速度值时节运动,;便可规划〔亡,,t`〕内关可取s为运动路径长度为了减少计算量以及使规划出的关节,由于机器人CP运动时其手部姿势往往,运动平稳艺、`在每一子区间〔艺,:t`〕内用一多`也随着变化化为能准确描述机器人运动除了项式来拟合各关节运动并近似认为在〔公〕内机器人作PTP运动,,知道手部位置变化外a为(。,还应知道手部姿势的变(a,现设在〔t:*,,t`〕内设在连续轨迹的始点和终点手部的姿势角尽。拟合关节运动的多项式为q;,“甲。)月r,甲r),我们假式中q`,(七)“a;。+a门t+……+ai忿定姿势角随时间作线性变化因此在时刻仁:(12)。为无量纲丁一10(簇t,镇“T)手部姿势角为。时间l:二(T一“:`/(云)-`二“+”产`(7)`艺`亡亩艺入`任〔0,1〕儿、二乙玄;二“`二“。+五并与t、一艺、,;T(8)为实际时间月T任〔艺`,t`〕由于区间〔0T:〕共被分成m个子区间〔0188年七,〕,〔亡,,云2〕,…,〔tm,,才。〕因此对于一个关节来说需要构造3个多项式另一种是下面我们采得中间各轨迹段多项式系数叼(=用两种方法来规划各关节的运动,一种是4333段多项式系数的确定(镇2(②第艺):一1)。+`,七+a;:艺“+、,“a;3才一135轨迹规划法,3一34轨迹规划法2由于叮、口;已知而口,口,工又可用333一35轨迹的规划前面方法递推求出件及:因此该多项式系数可由条g`当对各关节的运动速度无约束时(始终点qq``(0)=q、,(1)=q、,q`(0)=q、:除外)合,可用这种方法来规划轨迹这种方案,(0)=口*,就是将最后一个轨迹段用一五次多项式来拟其余各轨迹段用三次多项式来拟合即a,3求得如下I::Je0第i段:q,(`)=a,。+a,;t+a12`+艺“el10=qf=q;1几q`(13)Q`2一二一)采1In了乙第i段:口,(。)=a,。+a门亡+a,:云2+a,。艺“a(14)。:门=口*一口、;一q;,h;一咬乙一g`,九若第竹段,:口二(:+os)3=a二。+4a。,`+a。。`“+③第m段多项式系数的确定(一5)口(艺)=a+:。茄才+a二艺`+at“+a,云+a:t“+a3艺3+由于关节在始点和终点的位移qo速度加速度a、艺4+a。艺5,qoqogf,qf,q,及各中间点的关节,由于口叮口口已知,而q二口,可变f值口q:…q;是已知的据此可递递推求出,据此可得:了/,1推求出各轨迹段多项式系数①第由于口:1段多项式系数的确定叮l,,(0)口q1:=q;q(0)=q;口口(0)==qqf:,(t)+=a,。+a,,艺+。、“+a,。t3(1)=二叼r,g(1)6=qr,(1)(亡)=(a+Za,:艺+3a;。艺2:)/孔;口,(公):由这6个条件可求出;口o个多项式系数如下(Zaq:,:6aq。,:忿):/h圣由已知条件g(0)qa,=qo=q1(0)=,口,(o)三口。;g,(1)=可得a。z=q1几各系数如下::一三石23一,、点一一一如0q阮a=`。“:一4“2+寸31“3!…!Iél19二口q口a02一一瓦a“4=7c6c2一一153cc2l一c一00口一几一一:2iq。衅c,一ll+;c3=qr一ao一al一a二2Z因此ql,口,=g,,(1),q,二q(1)再以2cZ“Qr耘一a1一Zaqlq和q:为已知参数,便可求得第轨c:=召fh一二Za2迹段多项式q。(t)的各系数以此递推下去可求出各轨迹段多项式系数后,关节的运动2卷6期机器人运动轨迹规划分析与算法也就规划出来了2243一3一.一334轨迹的规划,ai。=q;hi一Zq`+Za;o+a、a当对各关节在中间点时的速度有约束时③第m段多项式系数的确定q可采用这种轨迹规划法用三次多项式来拟合的约束,此法就是将起始段和,(才)=a。+a:云+aZt“+:a二。亡3+a`艺4结束段均用一四次多项式来拟合而中间各段该段多项式要满足的边界条件有q二(0a对机器人手端运动速度或(10)进行速度反解,)=q二,:;q二(1)=q,;可由式(,1)(0)=a;a。(1)=qr;:口二(1)二qr计算变换为对关节速度的约束关节速度进行限制运动速度的目的这样通过对各由这些条件可求得各系数如下a就可达到控制机器人手端0=下面利用已知条件导出求各a。qq二l1轨迹段多项式系数的递推公式=1凡二一一①第叮;1段多项式系数的确定=aaa2====6D4Dr23DSD,:x+0DZSD3ss(t)a,。+a;:t+a::艺“+a,“3才+a:`艺峨一,4第1段多项式的边界条件是qq:qr+ZD一D+05D3;(])(1)=qq、DD一qf一ao一alZ,=,=凡qr`a。lq,(0)=q。D3=嵘q,由此可得各系数如下Q20:求出各关节各轨迹段多项式系数后q节随时间的运动规律i(诊,各关,=qqo(i二1…N,a一z=o九1O2岌T()T3也就被确定,a,:=该4c1,2l步骤综上所述口瓦。可得机器人CP运动轨迹规划aaC、3==一c::算法(CPTA)如下24cZI一Q3c一。l(1)输入连续轨迹方程(直线11圆圆,z==Q一一Q一Q12弧或椭圆等)s:x=;sf(),,=:f(s)z=3sf()=s(七);:九1qz一azl一2a12(2)输入始终点的各关节速度和加速度i簇。一1)ai32(②第i段多项式系数的确定(并给定机器人沿连续轨迹运动时手部姿势变化规律:〔0Qi(玄)=ai。+ai,艺+:ai:t“+t“a=a(零),尽=尽(云),,=中(艺);T该段多项式的边界条件为qqi(3)输入运动时间Tqq;根据精度要求将艺;(0)(0)二qi,;(1)=qin个子区间〕分成l二;:〔0〕,〔t,,tZ〕,…,〔t才。〕(七=共T);;=q、,;i(1):=口`(4)置i0;由这四个条件可求得各系数如下口10=(5)根据连续轨迹方程和手部姿势变化qi1规律求出时刻(6)t`时手部坐标系相对基座参照坐标系的变换矩阵T么l口门=q卜h`由T介进行运动学反解计算q`1,,求出,Q币么=3叹、一3a￡o一Zaix一q`儿对应的各关节变量值q`:,…,a`。再机器人9年由式q门,或q`2,计算对应的各岁亩速度5动a,力矩变化的最大极限值分别为:V,;口qi对;mJ5m。(jq=1,2,…N);(7)利用33333一35或433……`为使规划的轨迹有效足:,`i和关节力矩:i须满4轨迹规划递推公式求出区间以,,亡、〕(艺二1…,m)内各关节轨迹段多项式1,系数,(8)}叮`,}毛V}(*二置艺二艺+如果云毛川则返回到}q`,ai。步骤25,否则向下继续;1口;s落J】j二二i=1,2,……,,、,(9)输出各关节运动规律41中速度3:`s}簇Qs=夕12`,,……NCPT几的特点下面再设法求出在区间〔`,CP,,〕内N个关节本文的机器人如下特点:运动轨迹规划算法具有加速度和跃动的最大值对于用43.,一334或3一33一,335方法规划轨(1)由于求轨迹段多项式系数的公式是,迹时除第,1段和第m段外中间各段都是3以递推形式给出的因此在规划轨迹前,不必,次多项式因此若设在第i区间〔才`最大加速度口,,玄`〕内第计算出所有中间点后边走边算,,而是仅算出前面几点然j关节的最大速度最大跃动和递推地求出后面各段的多项式系n最大力矩分别为q丁`s二`sq`,。q`,。数,这为实时计算提供了可能性;(2)本文算法比iL,则11口和Luh等人提出的三次样条函数方法所需计算量要少和Lu因为iL一nqmmax口】;i(t)1=h的方法要求在规划轨迹前计算出整个路ax〔}q,;s(云`,)}径上所有中间点(n数)一,而且需要解一个(n。)2xq】;(云`)艺=1】}q,`,(“)}〕,(16)“了)2的三对角方程组(灯为中间路径点个式中t任〔t2=*,,t`〕,2、,一m;1,故当,较大时所需计算量也较大,由…00于本文算法所需计算量相对较小又是以递推N;ti任〔t`,,t〕,且满足,口`s(t`)形式给出3因此很易于计算机实现:轨迹的优化处理及m段的确定首先我们定义由于在第2至第m,一1段区间中关节轨t在满足各关节速度加速迹是3次多项式所以关节加速度是时间的度迹跃动1k)(’ej和力矩约束条件下在运动线性函数qi。萝故过程中耗时最少的关节运动轨迹为时间最优轨=二mmax!q(t)(t`l=,时间最优轨迹的规划是一个非常复杂问目前国外有不少学者仍在致力于这个问题ax题〔lqt:`)!,!q;,(`,)!〕的研究为求这个问题:本文采用一种与〔6〕T:(17)不同的方法,万i才斗!同式中:t任〔t:、,〕由于跃动是加速度的变化速率P,故有,:设机器人完成Ct,运动所需时问为,与前〔艺。,将时间区间〔O,,了〕分成二。;段二甘f了m_一}q;;(`一、)一q百了几f`,(“)1一1〕〔tt:〕……,〔t,,t,,价〕运用前面〕内各关节—、—一—一1l歹[算法CPTAl,可规划出区间〔艺、口`i(t才`(几`=t一艺`:),(18)的运动规律:2…,)(i二12…m;j=机器人关节力矩可根据动力学方程求出器人动力学方程可表示成:而机N)现设各关节速度加速度跃卷期机器人运动轨迹规划分析与算法()(,q)+G(q)a,i。大于V6i。,则几=}q`i。/V…,,二l>式中了为N火l广义关节力矩(力)矢量;D(动;1文献〔,〕的处理方法是将所有子区间都扩久hZ为N又N惯性矩阵,Hq(,)1cx是lN冰1哥氏力大人倍时间区间变成以执久风〕和离心力矢量:关节最大力矩G(动是N重力矢量由于因此`而实际上除〔t,,`;〕外,其它区间不一定需机器人动力学方程是非线性和强藕合的。要变化则是:这样处理的结果往往导致规划出的关而本文处理方法;的求解十分复杂:本文不作节轨迹并不是时间最优轨迹`仅仅将区间〔t,,具体讨论矩为:`现假设第,艺轨迹段第了关节的最大力t〕间隔h`扩大成i。并令X从。`,其它区间不变化2,,因此新的时间区间就儿`*是执二/Vmh:…入瓦`,,,…,儿二〕:,这样=尹平maX〔q“,口二〕(19)处理易于使规划出的轨迹更逼近于时间最优轨迹=〔口`s/as。〕(20)(21)时间最优轨迹规划算法可概述如下步骤1:输入各种已知参数:,,并假设运动6d31=mm2,ax犷户〔q`m/J/Qj二口时间区间为〔h步骤节轨迹;hZ,…2h,〕;`、=axY矛〔了;mj口(222)2:利用第节cPTA算法规划各关、i二z,…,m;1…N=云,一步骤1,3,:利用公式(m);16)~(23)确定d(艺=;若时间区间以工,亡,〕间隔儿;(2…公;,)变化成dh;,则关节速度加速度l跃,步骤4:用区lbJ〔d内::,,d声:,…,d声〕动将分别增大或缩小工/J因此l/d“,/d倍、名`”’取代原区’lf〔hA步骤5:hZ,…,h〕;2,令;:,如果6侣,1“=1,…;,”;),d“maX〔d,`,d;宁,d;宁d;〕(23)则进行步骤6步骤m否则返回到步骤2(乞=12二。”,),,6:输出时间最优关节运动规律得到…儿nz),,d;后用新的时间区间(dh1d孔Z2,下面再讨论分段数邢的确定问题为确定d扭启取代原时间区间仇阮2…,我们可采用如下近似方法先任意取既为一值,,:得到新的时间区间后再用前面算法,设机器人运动时轨迹误差最大允许值为。。CPTAl,重新规划轨迹如此反复进行直至d(i之并将时间区间〔0,T〕2…,m)近似等于i1为止d、刘1意分成。段根据连续轨迹方程和手部姿势变化+:味着机器人在第矩已接近极限值,轨迹段内关节运动或关节力也即目标函数T=n规律可相应得到l1个路径点在直角坐标下p。,的位置和姿势矢量p印`);,…,氏〔p:;=艺h`已。a`,`,夕*,z、,a`,月`1,〕]学通过运)Zq。,,。介)〕反解计算可得到接近最小值迹。+组关节变量矢量。此时的关节轨迹就是时间最优轨6〕中…,。`〔q`=(al,玉…:,至此可利用如下算法来最后确定m本文求时间最优轨迹的方法与文献〔步骤子区间〔t1`:利用CPTA算法规划各关节在各q`i从和变化nLuh的方法不同之处在于:文献〔6〕中每;t`〕`l的轨迹:,“。)=1,2(乞一区间大小的变化会导致其它所有区间都发生而本文中每一区间大小的变化则是相对爪;夕=12:2N);独立的不影响其它区间大小的变化艺`步骤步骤置艺二:瓦=二0;譬如假3设在区间〔l嚣、〕中关节夕的最大速度置t1—Z〔不十刃少机器`人ea年(3)步骤…,计算“〔`,(t`),口`2(t:),皿sauMeasurmenteandCornrtol1。了1,。3q`对(艺`:)〕;1643一172lRPPM姐ipulaotnsaeCarte城anpathnean步骤步骤5用q“进行运动学计算p,得到直pIEEETra4tionscnsaSystmMedCynotrol角坐标下相应的路径点位置与姿势矢量6:*;计算`与实际运动路径之间误5l。了。;SMC。(21)702berneties一7一1aorannnanexeeuTyltr砚RHPligd桩onofsghtilne口anipulaotrtrajeeto过esIBMJournalo差则笠:6*;fRe努acrhandDe讹OIpmneoeuntxo7。;2342`c一连36uKimBKtmieShiProeKGoAingniif￡entmiimsieltinn垃步骤7`:*如果d簇d。,则进行步骤pa、8。;;,否robotpathplannedreanreaeondin在p,p;与p;之间增加一中间点),并)桩o6nsInf1054iAmer20。anonCotrolCo。`“(X(艺切`,Y(才)Z(艺,`),(艺`feerLinuneeSanDiegano1054一503FormnoCSonChdecngPfoRardLuhJYSo尽“)(t(t:`))瓦=丸+1;laitnanp七imtzesitfseubiepolymialACtranoji二不+1,艺ttrjatiotoirindut`alnrosbotIEEE步骤步骤43;8如果置m=艺