多約束條件下的機(jī)器人時(shí)間最優(yōu)軌跡規(guī)劃

錢東海，馬文羅，汪建偉，王偉東

QIAN Dong-hai1,MA Wen-luo1,WANG Jian-wei2,WANG Wei-dong1

（ 1. 上海大學(xué) 精密機(jī)械工程系 上海 200072；2. 浙江中煙工業(yè)有限責(zé)任公司，杭州 310008 ）

0 引言

機(jī)器人運(yùn)動(dòng)規(guī)劃通常分為路徑規(guī)劃和軌跡規(guī)劃上下兩級(jí)[1]。路徑規(guī)劃由機(jī)器人的任務(wù)規(guī)劃器完成，用于在笛卡爾空間中或關(guān)節(jié)空間中產(chǎn)生一無碰撞的幾何路徑。軌跡規(guī)劃則是按照一定的性能指標(biāo)確定沿著該幾何路徑運(yùn)動(dòng)時(shí)各關(guān)節(jié)的速度、加速度、加加速度、控制力矩的時(shí)間曲線。機(jī)器人的時(shí)間最優(yōu)軌跡規(guī)劃是指以時(shí)間最短作為性能指標(biāo)，并在滿足各種約束的條件下優(yōu)化機(jī)器人的運(yùn)動(dòng)軌跡，使機(jī)器人沿規(guī)定路徑運(yùn)動(dòng)的時(shí)間最短。

由于軌跡規(guī)劃是機(jī)械人控制的基礎(chǔ)，其性能對機(jī)器人的工作效率，運(yùn)動(dòng)平穩(wěn)性和能量消耗具有決定性意義，因此在過去十幾年中，軌跡規(guī)劃一直是機(jī)器人研究中的一大熱點(diǎn)。軌跡規(guī)劃主要分成以下幾類：時(shí)間最優(yōu)軌跡規(guī)劃，能量最優(yōu)軌跡規(guī)劃，沖擊最優(yōu)軌跡規(guī)劃，以及混合最優(yōu)軌跡規(guī)劃。

時(shí)間最優(yōu)軌跡規(guī)劃在文獻(xiàn)中是最早提出的，因?yàn)榭梢源蟠筇岣呱a(chǎn)的效率[2～5]。能量最優(yōu)適合于機(jī)器人電源供應(yīng)受到限制的場合，此時(shí)利用能量法進(jìn)行軌跡規(guī)劃，產(chǎn)生光滑軌跡是非常理想的[6,7]。機(jī)器人運(yùn)動(dòng)過程中的沖擊不但會(huì)影響運(yùn)動(dòng)軌跡的光滑性和精確度，而且會(huì)損耗零部件，降低使用壽命，因此沖擊最優(yōu)軌跡規(guī)劃的研究同樣也是非常有意義的[8,9]。綜合考慮以上時(shí)間、能量、沖擊的約束條件，則構(gòu)成混合最優(yōu)軌跡規(guī)劃，如時(shí)間-能量最優(yōu)、時(shí)間-沖擊最優(yōu)等。文獻(xiàn)[10～12]分別從時(shí)間能量合成最優(yōu)和時(shí)間沖擊最優(yōu)來規(guī)劃機(jī)器人的運(yùn)動(dòng)軌跡，但這種方法需要精確的動(dòng)力學(xué)數(shù)學(xué)模型，因動(dòng)力學(xué)方程求解計(jì)算量大，以及動(dòng)力學(xué)方程參數(shù)難以精確確定使得上述算法受限于實(shí)際應(yīng)用。

已有文獻(xiàn)針對上述優(yōu)化問題，在設(shè)定約束條件時(shí)，在笛卡爾空間中都只考慮各離散路徑點(diǎn)的位置信息，而沒有對離散路徑點(diǎn)處速度方向這一約束進(jìn)行相應(yīng)考慮，從而造成機(jī)器人實(shí)際運(yùn)動(dòng)路徑與理想所需運(yùn)動(dòng)路徑偏差較大。本文采用三次多項(xiàng)式對運(yùn)動(dòng)軌跡進(jìn)行插值，建立優(yōu)化模型時(shí)，除了在關(guān)節(jié)空間中考慮各關(guān)節(jié)的速度、加速度、加加速度等約束條件，同時(shí)考慮笛卡爾空間中機(jī)器人在各離散路徑點(diǎn)處速度方向所應(yīng)滿足的約束條件。實(shí)現(xiàn)了在不增加離散路徑點(diǎn)數(shù)量的條件下，使得規(guī)劃的路徑精度得到了很大的提高。最后，本文提出的算法被應(yīng)用于一剪帶機(jī)器人的軌跡規(guī)劃，驗(yàn)證了該算法的有效性和可行性。

1 利用三次樣條構(gòu)造機(jī)器人運(yùn)動(dòng)軌跡

本文研究最優(yōu)軌跡規(guī)劃策略時(shí)，假定機(jī)器人已通過路徑規(guī)劃，在笛卡爾空間中形成一無碰撞的運(yùn)動(dòng)路徑，并依據(jù)常規(guī)假定該路徑由一系列離散路徑點(diǎn)構(gòu)成。

利用機(jī)器人運(yùn)動(dòng)學(xué)逆解在關(guān)節(jié)空間中求得與離散路徑點(diǎn)相對應(yīng)的關(guān)節(jié)節(jié)點(diǎn)序列（假定共n個(gè)節(jié)點(diǎn)），通過一系列的三次樣條曲線來插值連接這些關(guān)節(jié)節(jié)點(diǎn)。假定機(jī)器人具有N個(gè)關(guān)節(jié)，并設(shè) qj,1,qj,2,…,qj,n為關(guān)節(jié)j(1≤j≤N)在各節(jié)點(diǎn)處的位移，{t1,t2,…,tn}為機(jī)器人運(yùn)動(dòng)至各節(jié)點(diǎn)處的時(shí)間序列。定義Qj,i(t)、Q'j,i(t)、Q''j,i(t)、Q'''j,i(t)分別為關(guān)節(jié)j在時(shí)間間隔[ti,ti+1]內(nèi)位移、速度、加速度、加加速度的數(shù)學(xué)表達(dá)式，并且關(guān)節(jié)j在初始時(shí)刻t=t1的關(guān)節(jié)速度值、加速度值以及終止時(shí)刻t=tn的關(guān)節(jié)速度值、加速度值都是已知的[5]。

由于Qj,i(t)是時(shí)間t的三次多項(xiàng)式，其二階導(dǎo)數(shù)Q''j,i(t)是時(shí)間t 的一次多項(xiàng)式，于是得到：

對式(1)的Q''j,i(t)表達(dá)式連續(xù)進(jìn)行積分兩次，并利用已知條件Qj,i(ti)=qj,i，Qj,i(ti+1)=qj,i+1，可確定積分過程中的兩個(gè)積分常數(shù)，整理后得到Qj,i(t)的表達(dá)式為：

對公式(2)進(jìn)行一次微分和三次微分，可以得到如下表達(dá)式：

對于公式(1)～(4)，只要求出公式中Q''j,i(ti)在各時(shí)間節(jié)點(diǎn)(t=t1,t2,...,tn)處的值，公式(1)～(4)的方程便可以唯一確定。在ti=t1、ti=tn兩端點(diǎn)處，速度、加速度為給定值，利用公式(3)，并基于速度連續(xù)性條件聯(lián)立方程，可方便地求出(t=t1,t2,...,tn-1)時(shí)Q''j,i(ti)的值。

公式(1)～(4)的方程確定以后，最優(yōu)軌跡規(guī)劃就轉(zhuǎn)化為滿足上述4個(gè)方程，并滿足關(guān)節(jié)空間和笛卡爾空間的約束條件，求取時(shí)間序列t1,t2,...,tn的值，使得總的運(yùn)動(dòng)時(shí)間（即tn）為最小。

2 最優(yōu)軌跡規(guī)劃模型建立

2.1 關(guān)節(jié)空間中所應(yīng)滿足的約束條件

關(guān)節(jié)空間中，考慮各關(guān)節(jié)所能達(dá)到的最大速度、最大加速度的物理限制，同時(shí)考慮各關(guān)節(jié)所能允許的最大沖擊限制，定義各關(guān)節(jié)的速度、加速度、加加速度的約束條件如下：

其中j=1,2,…,N，i=1,2,…,n-1。VCj、WCj、JCj分別表示機(jī)器人第j個(gè)關(guān)節(jié)的最大速度、加速度和加加速度的值。

2.2 笛卡爾空間中所應(yīng)滿足的約束條件

一般來講，時(shí)間最優(yōu)軌跡規(guī)劃時(shí)，均需滿足公式(5)所要求的約束條件，但是對于需要沿給定路徑運(yùn)動(dòng)的機(jī)器人，上述約束條件是不夠的。上述約束條件只從機(jī)器人各關(guān)節(jié)所能達(dá)到的極限運(yùn)動(dòng)狀態(tài)加以約束，并沒有考慮機(jī)器人運(yùn)動(dòng)路徑的精度。本文針對笛卡爾空間中沿給定路徑運(yùn)動(dòng)機(jī)器人的時(shí)間最優(yōu)軌跡規(guī)劃問題，分析此類機(jī)器人運(yùn)動(dòng)時(shí)所應(yīng)滿足的約束條件，改進(jìn)優(yōu)化設(shè)計(jì)的模型，解決現(xiàn)有時(shí)間最優(yōu)軌跡規(guī)劃算法在解決此類問題中存在的不足。

對于沿給定路徑運(yùn)動(dòng)的機(jī)器人，機(jī)器人在任意時(shí)刻運(yùn)動(dòng)速度的方向都應(yīng)與機(jī)器人當(dāng)前所在位置處路徑切線的方向一致，這樣才能保證機(jī)器人不偏離給定路徑。反應(yīng)在機(jī)器人時(shí)間最優(yōu)軌跡規(guī)劃上，應(yīng)將機(jī)器人瞬時(shí)速度與運(yùn)動(dòng)路徑切線之間存在的約束引入到時(shí)間最優(yōu)軌跡規(guī)劃數(shù)學(xué)模型中。

假定f1(x)、f2(x)為笛卡爾空間中的兩曲面，其中x=[x1,x2,x3]T。機(jī)器人的運(yùn)動(dòng)路徑由兩曲面的交線確定，曲面f1(x)、f2(x)的法線向量為：

機(jī)器人末端的瞬時(shí)速度為：

機(jī)器人沿由f1(x)、f2(x)所確定的路徑運(yùn)動(dòng)時(shí)，其速度方向與兩曲面的法線垂直，故存在：

式(6)構(gòu)成了笛卡爾空間中沿給定路徑運(yùn)動(dòng)機(jī)器人在進(jìn)行時(shí)間最優(yōu)軌跡規(guī)劃時(shí)所應(yīng)滿足的約束條件。

2.3 時(shí)間最優(yōu)軌跡規(guī)劃目標(biāo)的建立

機(jī)器人的運(yùn)動(dòng)速度直接影響到作業(yè)的效率，機(jī)器人時(shí)間最優(yōu)軌跡規(guī)劃就是在滿足約束的條件下，使得機(jī)器人從起點(diǎn)到終點(diǎn)的運(yùn)動(dòng)時(shí)間為最短。也即對于公式(1)～(4)中的hi，使得：

本文采用序列二次規(guī)劃法來求解式(1)～(7)所描述的非線性約束優(yōu)化問題。序列二次規(guī)劃的思想是通過構(gòu)造拉格朗日函數(shù)，將一個(gè)約束優(yōu)化問題轉(zhuǎn)化為一系列無約束優(yōu)化問題來求解。由于序列二次規(guī)劃方法是一種局部最優(yōu)算法，因此初始值的選擇很重要，初始值選擇不合理，不僅影響到優(yōu)化算法的搜索效率，并有可能得到非最優(yōu)解。序列二次規(guī)劃法在優(yōu)化設(shè)計(jì)已經(jīng)得到廣泛應(yīng)用，可利用成熟的軟件直接進(jìn)行求解。

3 算法實(shí)例驗(yàn)證

3.1 超二次曲線的數(shù)學(xué)描述

為驗(yàn)證本文提出的算法，將在二維平面中，驅(qū)動(dòng)直角坐標(biāo)機(jī)器人沿著圓角矩形運(yùn)動(dòng)，分析其沿特定路徑運(yùn)動(dòng)時(shí)的偏差，并和已有算法做一對比分析。

圓角矩形本質(zhì)為一超二次曲線，其方程為：

圓角矩形的形狀會(huì)隨著指數(shù) 的變化而變化，如圖1所示。

圖1 不同的指數(shù)對超二次曲線的影響

圖1中α、b分別為矩形的半長和半寬?？梢钥闯觯?dāng)指數(shù)ε變小時(shí)，圓角矩形的四個(gè)頂點(diǎn)變尖，當(dāng)ε趨向無窮小時(shí)，圓角矩形就變成矩形。本文中令ε=0.5、α=b=10，驅(qū)動(dòng)機(jī)器人沿圓角矩形x4+y4=104進(jìn)行運(yùn)動(dòng)，其中單位為厘米。

3.2 試驗(yàn)結(jié)果與分析

表1 關(guān)節(jié)位移序列

上述運(yùn)動(dòng)路徑中，取圓角矩形與坐標(biāo)軸的四個(gè)交點(diǎn)，以及圓角矩形的四個(gè)頂點(diǎn)，作為離散路徑點(diǎn)，控制機(jī)器人沿圓角矩形的運(yùn)動(dòng)。因采用的是直角坐標(biāo)機(jī)器人，笛卡爾空間和關(guān)節(jié)空間是統(tǒng)一的，8個(gè)離散路徑點(diǎn)在笛卡爾空間，也即關(guān)節(jié)空間中的位置如表1所示。

各關(guān)節(jié)的速度、加速度、加加速度的約束如表2所示。并假定機(jī)器人在起始點(diǎn)和終止點(diǎn)的速度、加速度的值均為0。

表2 機(jī)器人速度、加速度、加加速度約束

利用文獻(xiàn)[5]給出的算法對機(jī)器人進(jìn)行時(shí)間最優(yōu)軌跡規(guī)劃，得到的實(shí)際運(yùn)動(dòng)路徑與給定圓角矩形的最大偏差為4.32毫米，所需的運(yùn)動(dòng)時(shí)間為20.1754秒。

采用本文所提出的算法，確保機(jī)器人在各離散路徑點(diǎn)處滿足由曲線方程所決定的速度約束條件，得到的實(shí)際運(yùn)動(dòng)路徑與給定曲線方程的最大偏差為0.4514毫米，所需的運(yùn)動(dòng)時(shí)間為41.2479秒。

通過對比可以看出：通過引入由曲線方程所決定的速度約束條件，機(jī)器人實(shí)際運(yùn)動(dòng)路徑的偏差大大降低了，這在機(jī)器人弧焊、涂膠、切割等應(yīng)用中具有很強(qiáng)的應(yīng)用價(jià)值。通過比較，我們也發(fā)現(xiàn)采用本文提出的方法會(huì)使得運(yùn)動(dòng)的時(shí)間相應(yīng)地增加，但這在弧焊、涂膠、切割等應(yīng)用中不會(huì)成為應(yīng)用的瓶頸。

圖2 關(guān)節(jié)位移圖

圖3 關(guān)節(jié)速度圖

圖4 關(guān)節(jié)加速度圖

圖5 關(guān)節(jié)加加速度圖

圖6 實(shí)際運(yùn)動(dòng)路徑與給定路徑偏差（采用文獻(xiàn)[5]算法）

圖7 實(shí)際運(yùn)動(dòng)路徑與給定路徑偏差（采用本文算法）

圖2～5分別表示采用本文提出的算法進(jìn)行時(shí)間最優(yōu)軌跡規(guī)劃，各個(gè)關(guān)節(jié)位移、速度、加速度、加加速度的曲線圖。其中實(shí)線表示第一個(gè)關(guān)節(jié)，虛線表示第二個(gè)關(guān)節(jié)。圖6和圖7分別為采用文獻(xiàn)[5]算法和采用本文算法時(shí)，實(shí)際運(yùn)動(dòng)路徑與給定路徑的偏差，其中實(shí)線表示實(shí)際運(yùn)動(dòng)路徑，虛線表示給定的路徑。

本文提出的算法還在自行研制的剪帶機(jī)器人中得到了驗(yàn)證。如圖8所示。

圖8 剪帶機(jī)器人

剪帶機(jī)器人是一臺(tái)能對紙箱外打包帶進(jìn)行自動(dòng)剪斷和回收的專用設(shè)備。其本質(zhì)為一直角坐標(biāo)機(jī)器人，共有三個(gè)自由度：第一、第二關(guān)節(jié)分別是平面內(nèi)沿著X軸和Y軸方向進(jìn)行平移的直線運(yùn)動(dòng)關(guān)節(jié)；第三個(gè)關(guān)節(jié)是繞Z軸回轉(zhuǎn)的旋轉(zhuǎn)關(guān)節(jié)。

4 結(jié)論

本文提出了一種機(jī)器人時(shí)間最優(yōu)軌跡規(guī)劃算法，該算法除了滿足機(jī)器人常見的關(guān)節(jié)極限運(yùn)動(dòng)的約束條件，還保證機(jī)器人在各離散路徑點(diǎn)處滿足由給定路徑所決定的速度約束條件，有效地減小了機(jī)器人實(shí)際運(yùn)動(dòng)路徑與給定路徑之間的偏差。最后試驗(yàn)驗(yàn)證了本文所提出算法的有效性和可行性。

[1] 錢東海,譚偉,趙錫芳.基于B樣條路徑的機(jī)器人時(shí)間最優(yōu)軌跡規(guī)劃[J].上海交通大學(xué)學(xué)報(bào),1998,32(12)：29-32.

[2] Bobrow J E,Dubowsky S,Gibson J S.Time optimal control of robotic manipulators along specified paths[J].The international journal of robotics research,1985,4 (3)：3-17.

[3] Constantinescu D,Croft E A. Smooth and time-optimal trajectory planning for industrial manipulators along specified paths[J].Journal of robot systems,2000,17(5)：233-249.

[4] 楊國軍,崔平遠(yuǎn).機(jī)械手時(shí)間最優(yōu)軌跡規(guī)劃方法研究[J].中國機(jī)械工程,2002,13(20)：1715-1717.

[5] LIN C S,CHANG P R,LUH J Y S.Formulation and optimization of cubic polynomial joint trajectories for Industrial robots[J].IEEE Transactions on Automation Control,1983,28(12)：1066-1074.

[6] Martin B J,Bobrow J E.Minimum effort motions for open chain manipulators with task dependent end effector con- straints[J].The international journal of robotics research,1999,18(2)：213-224.

[7] Jha K.Minimum energy based interpolation for interactive incremental well path design[J].Engineering computations,2004,21：676-691.

[8] Huang P F,Chen K,Xu Y S.Motion trajectory planning of space manipulator for joint jerk minimization[C].IEEE Inter- national conference on mechatronics and automation,China： Harbin,2007：3543-3548.

[9] Piazzi A,Visioli A.Global minimum-jerk trajectory planning of robot manipulators[J].IEEE Transaction On Industrial Electronics,2000,2000,47(1)：140-149.

[10]Shiller Z.Time-energy optimal control of articulated systems with geometric path constraints[C].IEEE International Conference on Robotics and Automation,1994,4：2680-2685.

[11]Gasparetto A,Zanotto V.A technique for time-jerk opti-mal planning of robot trajectories[J].Robotics and computer Integrated Manufacturing,2008,24(6)：415-426.

[12]Verscheure D,Demeulenaere B,Swevers J.Time-energy optimal path tracking for robots：a numerically efficient opti- mization approach[C].International Workshop On Advanced Motion Control,2008,1：727-732.