多約束條件下的機器人時間最優軌跡規劃

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

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

（ 1. 上海大學 精密機械工程系 上海 200072；2. 浙江中煙工業有限責任公司，杭州 310008 ）

0 引言

機器人運動規劃通常分為路徑規劃和軌跡規劃上下兩級[1]。路徑規劃由機器人的任務規劃器完成，用于在笛卡爾空間中或關節空間中產生一無碰撞的幾何路徑。軌跡規劃則是按照一定的性能指標確定沿著該幾何路徑運動時各關節的速度、加速度、加加速度、控制力矩的時間曲線。機器人的時間最優軌跡規劃是指以時間最短作為性能指標，并在滿足各種約束的條件下優化機器人的運動軌跡，使機器人沿規定路徑運動的時間最短。

由于軌跡規劃是機械人控制的基礎，其性能對機器人的工作效率，運動平穩性和能量消耗具有決定性意義，因此在過去十幾年中，軌跡規劃一直是機器人研究中的一大熱點。軌跡規劃主要分成以下幾類：時間最優軌跡規劃，能量最優軌跡規劃，沖擊最優軌跡規劃，以及混合最優軌跡規劃。

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

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

1 利用三次樣條構造機器人運動軌跡

本文研究最優軌跡規劃策略時，假定機器人已通過路徑規劃，在笛卡爾空間中形成一無碰撞的運動路徑，并依據常規假定該路徑由一系列離散路徑點構成。

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

由于Qj,i(t)是時間t的三次多項式，其二階導數Q''j,i(t)是時間t 的一次多項式，于是得到：

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

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

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

公式(1)～(4)的方程確定以后，最優軌跡規劃就轉化為滿足上述4個方程，并滿足關節空間和笛卡爾空間的約束條件，求取時間序列t1,t2,...,tn的值，使得總的運動時間（即tn）為最小。

2 最優軌跡規劃模型建立

2.1 關節空間中所應滿足的約束條件

關節空間中，考慮各關節所能達到的最大速度、最大加速度的物理限制，同時考慮各關節所能允許的最大沖擊限制，定義各關節的速度、加速度、加加速度的約束條件如下：

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

2.2 笛卡爾空間中所應滿足的約束條件

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

對于沿給定路徑運動的機器人，機器人在任意時刻運動速度的方向都應與機器人當前所在位置處路徑切線的方向一致，這樣才能保證機器人不偏離給定路徑。反應在機器人時間最優軌跡規劃上，應將機器人瞬時速度與運動路徑切線之間存在的約束引入到時間最優軌跡規劃數學模型中。

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

機器人末端的瞬時速度為：

機器人沿由f1(x)、f2(x)所確定的路徑運動時，其速度方向與兩曲面的法線垂直，故存在：

式(6)構成了笛卡爾空間中沿給定路徑運動機器人在進行時間最優軌跡規劃時所應滿足的約束條件。

2.3 時間最優軌跡規劃目標的建立

機器人的運動速度直接影響到作業的效率，機器人時間最優軌跡規劃就是在滿足約束的條件下，使得機器人從起點到終點的運動時間為最短。也即對于公式(1)～(4)中的hi，使得：

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

3 算法實例驗證

3.1 超二次曲線的數學描述

為驗證本文提出的算法，將在二維平面中，驅動直角坐標機器人沿著圓角矩形運動，分析其沿特定路徑運動時的偏差，并和已有算法做一對比分析。

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

圓角矩形的形狀會隨著指數 的變化而變化，如圖1所示。

圖1 不同的指數對超二次曲線的影響

圖1中α、b分別為矩形的半長和半寬?？梢钥闯?，當指數ε變小時，圓角矩形的四個頂點變尖，當ε趨向無窮小時，圓角矩形就變成矩形。本文中令ε=0.5、α=b=10，驅動機器人沿圓角矩形x4+y4=104進行運動，其中單位為厘米。

3.2 試驗結果與分析

表1 關節位移序列

上述運動路徑中，取圓角矩形與坐標軸的四個交點，以及圓角矩形的四個頂點，作為離散路徑點，控制機器人沿圓角矩形的運動。因采用的是直角坐標機器人，笛卡爾空間和關節空間是統一的，8個離散路徑點在笛卡爾空間，也即關節空間中的位置如表1所示。

各關節的速度、加速度、加加速度的約束如表2所示。并假定機器人在起始點和終止點的速度、加速度的值均為0。

表2 機器人速度、加速度、加加速度約束

利用文獻[5]給出的算法對機器人進行時間最優軌跡規劃，得到的實際運動路徑與給定圓角矩形的最大偏差為4.32毫米，所需的運動時間為20.1754秒。

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

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

圖2 關節位移圖

圖3 關節速度圖

圖4 關節加速度圖

圖5 關節加加速度圖

圖6 實際運動路徑與給定路徑偏差（采用文獻[5]算法）

圖7 實際運動路徑與給定路徑偏差（采用本文算法）

圖2～5分別表示采用本文提出的算法進行時間最優軌跡規劃，各個關節位移、速度、加速度、加加速度的曲線圖。其中實線表示第一個關節，虛線表示第二個關節。圖6和圖7分別為采用文獻[5]算法和采用本文算法時，實際運動路徑與給定路徑的偏差，其中實線表示實際運動路徑，虛線表示給定的路徑。

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

圖8 剪帶機器人

剪帶機器人是一臺能對紙箱外打包帶進行自動剪斷和回收的專用設備。其本質為一直角坐標機器人，共有三個自由度：第一、第二關節分別是平面內沿著X軸和Y軸方向進行平移的直線運動關節；第三個關節是繞Z軸回轉的旋轉關節。

4 結論

本文提出了一種機器人時間最優軌跡規劃算法，該算法除了滿足機器人常見的關節極限運動的約束條件，還保證機器人在各離散路徑點處滿足由給定路徑所決定的速度約束條件，有效地減小了機器人實際運動路徑與給定路徑之間的偏差。最后試驗驗證了本文所提出算法的有效性和可行性。

[1] 錢東海,譚偉,趙錫芳.基于B樣條路徑的機器人時間最優軌跡規劃[J].上海交通大學學報,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] 楊國軍,崔平遠.機械手時間最優軌跡規劃方法研究[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.