基于B樣條和改進遺傳算法的機器人時間最優軌跡規劃

李國洪 王遠亮

(天津理工大學天津市復雜系統控制理論及應用重點實驗室 天津 300384)

0 引 言

工業機器人時間最優軌跡規劃問題長期以來備受專家學者們的廣泛關注，對此展開了大量的研究[1]。軌跡規劃就是按照某個性能指標產生機器人沿著該光滑路徑運動至各點處的速度和加速度的時間序列[2]。如何使工業機器人的末端執行器以最短的時間穩定地完成移動任務到達指定的位置，是提高工業機器人工作效率、縮短工作時間的核心問題[3]。關于軌跡規劃問題，Tondu等[4]提出使用特定折線連接相鄰路徑點，最終規劃出理想的關節軌跡。Bazaz等[5]通過3次多項式插值規劃關節軌跡，解決了曲線與關鍵點的擬合問題，但是卻忽略了加速度連續性的問題。李東潔等[6]運用3次多項式和標準遺傳算法相結合的方式優化關節軌跡，使得關節軌跡曲線準確經過路徑點，且縮短了軌跡運行時間，但同樣忽略了加速度連續的問題，易造成關節間的抖動。在關節空間中，3次多項式因其計算簡便深受歡迎，然而其存在的關節角加速度不連續的問題可能會導致機械系統產生一定的沖擊[7-9]。5次、7次等高階多項式雖然加速度平滑，避免了沖擊和振動，可是階數較高容易造成失真現象[10]。3次B樣條曲線同樣存在加速度不平滑的問題，容易造成跟蹤誤差的積累，甚至引起共振[11-13]。所以采用7次B樣條規劃的關節軌跡，保證了加加速度連續且光滑，速度、加速度值較小，軌跡規劃精度高，是一種穩定可靠的軌跡規劃方法[14]。3次多項式插值、5次多項式插值以及B樣條插值等僅僅優化了軌跡的擬合問題，使得軌跡曲線更加的圓滑，但是并沒有縮短機器人的工作時間[15]。雖然神經網絡、模糊控制等算法能夠提高工作效率，但是算法十分復雜[16]。而遺傳算法彌補了這一缺點，且具有全局尋優的特性，收斂速度較快。本文以標準遺傳算法為前提，通過聚類方法對遺傳算法的插值時間進行改進，大大提高了收斂速度。以安川六自由度工業機器人為例，在7次B樣條曲線連接相鄰路徑點的基礎上，采用改進遺傳算法優化各關節軌跡，使機器人可以在最短的時間里按照預定的軌跡到達指定位置，同時提出6關節時間同步的方法，實現最短時間軌跡規劃。最后通過MATLAB分析仿真結果。

1 基于B樣條插值的最短時間軌跡規劃

1.1 安川六自由度工業機器人

安川機器人主要由基座、腰部、大臂、小臂以及腕部組成，有6個轉軸，如圖1所示。借助坐標系來確定連桿的位置，基座的坐標系用{O}表示，連桿i的坐標系記為{i}。連桿參數定義如下：

(1)ai-1是從zi-1軸和xi軸交點到第i坐標系原點沿xi軸的偏置距離(即zi-1和zi兩軸間的最小距離)，稱為連桿長度；

(2)αi-1是繞xi軸(按右手規則)由軸zi-1轉向zi軸的偏角，稱為連桿扭角；

(3)di是從第(i-1)坐標系的原點到zi-1軸和xi軸的交點沿zi-1軸的距離，稱為兩連桿距離；

(4)θi是繞zi軸(按右手規則)由xi-1軸轉向xi軸的關節角，稱為兩連桿夾角。

圖1 六自由度工業機器人

連桿的參數如表1所示，其中：a2=0.432 10，a3=0.020 29，d2=0.149 12，d4=0.433 05。

表1 安川六自由度工業機器人連桿參數

1.2 B樣條曲線插值

1.2.1B樣條曲線的定義和性質

B樣條曲線曲面具有許多優良性質，是目前CAD系統常用的幾何表示方法，如今也被廣泛應用在機器人軌跡規劃等領域。

p次B樣條曲線的方程如下：

(1)

式中：di為控制點距離,表示為標量;{Ni,p(u)}為p次規范B樣條基函數；u是參數;C(u)也表示為標量。

B樣條基函數遞推法定義為:

(2)

式中：若分子分母相比為0/0時，則其結果定義為0;p表示B樣條次數;i為B樣條序號。可見，Ni,p(u)的支撐區間為[ui,ui+1]。B樣條基函數{Ni,p(u)}的節點向量為：

U=(a,…,a,up+1,…,um-p+1,b,…,b)

通常節點向量U隨機分布，且非均勻。向量U兩端節點重復度為p+1。除非另外聲明，在軌跡規劃過程中，為了確保生成的B樣條曲線經過首末控制點，一般設定a=0、b=1。

由B樣條基函數與B樣條曲線的定義可知：(1)p次B樣條曲線為p次多項式的線性組合。(2) B樣條曲線控制點的個數n+1與次數p、節點個數m+1滿足關系：m=n+p+1。(3) 隨機選取區間u∈[ui,ui+p+1]上的一點，p次B樣條Nj,p(u)(j=i-p,i-p+1,…,i)的個數不超過p+1個,故B樣條曲線可表示為：

(3)

這體現出p次B樣條曲線上定義域內參數為u∈[ui,ui+1]的一點C(u)最多與p+1個控制點dj(j=i-p,i-p+1,…,i)相關，與其他頂點無關。(4) B樣條曲線不經過控制點di。

上述性質對于編程實現B樣條曲線軌跡規劃十分重要。

1.2.2B樣條曲線示例

選取一些控制點，如表2所示。用這些控制點描繪出2次、7次B樣條曲線分別如圖2中實線所示，虛線為所選控制點的連線。

表2 曲線的控制點

圖2 B樣條曲線

可以看出，B樣條曲線沒有經過控制點di。并且，根據兩種B樣條曲線的比較能夠得出，2次B樣條的控制力稍強于7次B樣條曲線，即B樣條次數p的增加，反而會使控制點的控制力減弱。

由于圖2的p次B樣條曲線不經過控制點，為了解決這一問題，接下來對關鍵點進行全局曲線插值。

1.2.3B樣條曲線全局插值軌跡規劃

首先選定了一個合適的節點向量U={u0,u1,…,un}，建立一個(n+1)×(n+1)階線性方程組：

(4)

(5)

(6)

選取弦長參數化的方法可以解決數據點不均勻所導致的問題，比如打圈自交。

(7)

通過上述方法對表2數據進行3次B樣條全局曲線插值。

U=(0,0,0,0,0.492 3,0.644 2,0.757 6,0.861 7,1,1,1,1)

再由式(4)得出：

D={-46.53,246.89,-111.41,-2.49,33.01,71.81,97.33,113.03}

根據式(1)能夠得到如圖3所示的3次B樣條曲線及其各階導曲線。

由圖3(a)可見，B樣條曲線準確經過所有關鍵點，但是在(0，-46.53)和(0.33，48.19)兩點間出現過運動的現象。

由圖3(b)可見，3次B樣條曲線在(0，-46.53)和(0.33，48.19)兩點間出現過運動主要是因為導曲線的起點數過大。

根據圖3(c)、圖3(d)可知，加速度和加加速度曲線存在突變，且加速度曲線不連續，這會導致關節力矩不連續，不利于機器人的精確控制。

為解決這種問題，接著將選取指定端點導矢的7次B樣條曲線進行插值。

1.2.4端點導矢指定的全局B樣條曲線插值

在B樣條軌跡規劃中，為了使關節平穩、準確地運行，需要在兩條軌跡的連接點處有相同的角度、速度以及加速度，所以在指定軌跡起始角度和終止角度的同時，還需指定軌跡曲線端點處的一階、二階和三階導。這時就需知道B樣條曲線導矢與控制點D、節點向量U的關系。

令C(k)(u)表示C(u)的k階導矢。對于固定值u，C(k)(u)的計算公式為：

(8)

式中：

且U(k)={0,…,0,up+1,…,um-p+1,1,…,1},U(k)兩端0和1的個數均為p-k+1。

由式(8)可得B樣條曲線在端點處的一階導矢為：

(9)

二階導矢為：

(10)

在關節空間中，對機器人進行軌跡規劃時，為保證關節在運行過程中精確穩定，需設定軌跡端點處的各項參數，并使其滿足運動學約束條件。

(2) 求解節點矢量U；

(3) 通過求出基函數的值來建立以控制點為未知量的線性方程組；

(4) 求出該線性方程組的解。

可見，上述參數求解過程與B樣條曲線插值一致。但步驟(2)、步驟(3)卻略有差異。此時會多出一個線性方程，能夠由式(8)-式(10)得到。比如，令C′(0)=C′(1)=0，由式(9)可得：

D1=D0且Dn=Dn-1

(11)

此時，由于節點數量的變化，需要用式(12)替換式(7)來計算節點矢量U。

(12)

用端點各階導均為0的7次B樣條曲線插值表2所示參數和對應的關鍵點。根據式(12)確定節點向量為：

U={0，0，0，0，0，0，0，0，0.30，0.52，0.61，0.63，0.71，0.72，1，1，1，1，1，1，1，1}

n+7個控制點通過系矩陣為(n+7)×(n+7)階線性方程組的解得到：

D={-46.53，-46.53，-46.53，-46.53，61.30，224.77，-200.25，95.99，79.90，145.46，113.03，113.03，113.03，113.03}

圖4 7次B樣條曲線

可以看出，端點導矢為0的7次B樣條曲線準確經過全部關鍵點，各階導曲線連續，且端點值均為0，達到了預定的要求。

選取端點導矢指定的全局7次B樣條曲線規劃關節軌跡，在確保各關節速度、加速度、加加速度連續的同時，也保證了關節所受力矩沒有突變，降低了關節應力水平，有效地提高了軌跡跟蹤精度并減少了關節間的損耗。

2 優化函數

通過7次B樣條函數，優化各關節軌跡，增強軌跡對控制點的擬合度，使得軌跡曲線平穩光滑。接下來將對關節軌跡運行時間進行優化。

這里在6個關節都要滿足其運動學約束的前提下，即不超過各關節的速度、加速度、加加速度的上限，以求得總時間最短的時間序列節點為目標，這是一個非線性約束優化問題，優化的目標函數為：

(13)

式中：hi為相鄰兩個路徑點之間的時間間隔，且hi=ti+1-ti；f(x)為經過路徑點的總時間。

約束條件為：

(14)

3 時間最優軌跡規劃

遺傳算法(Genetic Algorithm，GA)是借鑒生物界自然選擇和自然遺傳機制，使用適者生存的原則，在潛在的解決方案中逐次產生一個近似最優解，是自然選擇和計算機科學相互滲透而成的隨機搜索算法。

這里使用改進的遺傳算法，以聚類方法為基礎，在時間上對完成7次B樣條插值后的關節軌跡進行優化。其工作原理如圖5所示。

圖5 遺傳算法工作原理圖

(1) 編碼。采用常見的二進制編碼，相對于實數編碼來說，雖然計算較為復雜，但是操作簡便，更容易實現后期的遺傳操作，更具優越性。

(2) 種群初始化。在時間間隔hi內，隨機產生一定數量的個體，種群的大小設為m，取值范圍設為20～200。

(3) 適應度函數的建立與標定。通過外罰函數法淘汰不滿足約束條件的個體，使得迭代點逐漸靠近可行域。其表達式如下：

(15)

(16)

通過以上分析，確定適應度函數為：

(17)

因為個體的適應度值會隨著算法的收斂而接近，導致尋優停滯不前，此時則需加大個體適應度值來增強選擇能力。

所以根據式(16)重新標定：

(18)

式中：F(t)為標定后的適應度值；Fit(t)min為Fit(t)的最小值；Fit(t)max為Fit(t)的最大值；δ∈(0,1)為正實數。

(19)

(20)

式中：e,f,g∈[1,2,…,2m/3]。

4 仿真實驗

4.1 創建機器人模型

采用機器人工具箱里的Link函數以及Robot函數建立機器人對象。

Link函數的格式為：

L=Link([alpha A theta D sigma offset],connvetion)

使用機器人工具箱進行正運動學求解，需調用fkine()函數，格式為：

TR=fkine(robot,q)

圖6 MATLAB建立的機器人模型

在機器人工具箱中利用ikine()函數進行逆運動學求解，格式如下：

Q=ikine(Robot,T)；

4.2 仿真結果

仿真實驗使用安川機器人作為數學模型，以安川機器人為例編寫MATLAB程序。表3、表4分別給出了路徑點各關節角度和機器人的約束條件。

表3 路徑點各個關節角度 °

表4 機器人速度、加速度、加加速度的約束條件

(21)

表5 各關節最優時間序列 s

由表5可見，運用改進后的遺傳算法進行仿真得到的最優軌跡規劃時間為[7.73,9.03,6.69,7.21,10.82] s，而運用標準遺傳算法進行仿真得到的最有軌跡規劃時間為[9.23,11.34,9.01,10.21,12.99] s，減少了[1.5,2.31,2.32,3,2.17] s，實現了在時間上對原有遺傳算法改進的目的。

將改進后的遺傳算法與標準遺傳算法的迭代圖進行對比，如圖7所示。

圖7 算法迭代收斂對比圖

可以看出，標準遺傳算法的收斂迭代次數高于改進后的遺傳算法。表明改進后的算法在后期有較快的搜索速度，有效減少了算法迭代收斂的時間。各個關節位置與時間的最優軌跡如圖8所示。

圖8 各關節時間最優軌跡

可以看出，關節3到達最終關節位置所需的時間是6.69 s，關節6到達最終關節位置卻需要10.82 s。

由于不同關節的約束條件相異，所以各關節到達目標位置的時間也不同。現在要求末端執行器在任務空間中準確經過路徑點，同時各關節在到達路徑點的轉動時間相同。這時就需對轉速較快的關節進行降速，可以通過如下步驟實現：

(1) 在相鄰路徑點之間，保證各軸轉動時間同步。以前兩個路徑點為例，關節6轉動時間最長，為3.57 s，則關節1到關節5在前兩個路徑點之間的轉動時間都延長到3.57 s，即h1=3.57 s。

(3) 由式(12)確定節點向量{U}，由式(4)確定控制點{D}。

(4) 通過式(1)得到新的軌跡曲線。

根據上述步驟，可以得到各關節位置-時間曲線、速度-時間曲線、加速度-時間曲線和加加速度-時間曲線分別如圖9-圖12所示。

圖9 位置-時間曲線

圖10 速度-時間曲線

圖11 加速度-時間曲線

圖12 加加速度-時間曲線

仿真結果顯示，各關節運行時間實現了同步，位置變化曲線以及速度、加速度、加加速度變化曲線連續光滑，且運動學參數均滿足約束條件，有效縮短了機器人軌跡運行時間。

5 結 語

以安川六自由度工業機器人為例，運用端點導矢指定的全局7次B樣條曲線連接相鄰路徑點，使其關節運動軌跡曲線更加平滑，保證了關節所受力矩沒有突變，降低了關節應力水平。采用聚類方法改進遺傳算子對個體適應度值大小進行分類，制定不同選擇方法，保留優良個體并兼顧種群多樣性。以新的變異操作對后期的最優個體進行調整，提高搜索效率，同時提出六關節運行時間同步方法。使用MATLAB進行仿真，結果表明，運用端點導矢指定的全局7次B樣條曲線插值和改進遺傳算法優化關節軌跡，使得各軸軌跡曲線、速度曲線、加速度曲線、加加速度曲線連續平滑，且其運動學參數均滿足約束條件，提高了軌跡跟蹤精度，減少了關節間的損耗，有效地縮短了軌跡運行時間。在后續的研究中，將從能量損耗的角度著手，爭取實現能耗和時間最優。