李國洪 王遠亮

(天津理工大學天津市復(fù)雜系統(tǒng)控制理論及應(yīng)用重點實驗室 天津 300384)

0 引 言

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

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

1.1 安川六自由度工業(yè)機器人

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

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

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

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

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

圖1 六自由度工業(yè)機器人

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

表1 安川六自由度工業(yè)機器人連桿參數(shù)

1.2 B樣條曲線插值

1.2.1B樣條曲線的定義和性質(zhì)

B樣條曲線曲面具有許多優(yōu)良性質(zhì)，是目前CAD系統(tǒng)常用的幾何表示方法，如今也被廣泛應(yīng)用在機器人軌跡規(guī)劃等領(lǐng)域。

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

(1)

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

B樣條基函數(shù)遞推法定義為:

(2)

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

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

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

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

(3)

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

上述性質(zhì)對于編程實現(xiàn)B樣條曲線軌跡規(guī)劃十分重要。

1.2.2B樣條曲線示例

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

表2 曲線的控制點

圖2 B樣條曲線

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

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

1.2.3B樣條曲線全局插值軌跡規(guī)劃

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

(4)

(5)

(6)

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

(7)

通過上述方法對表2數(shù)據(jù)進行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}

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

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

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

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

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

1.2.4端點導(dǎo)矢指定的全局B樣條曲線插值

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

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

(8)

式中：

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

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

(9)

二階導(dǎo)矢為：

(10)

在關(guān)節(jié)空間中，對機器人進行軌跡規(guī)劃時，為保證關(guān)節(jié)在運行過程中精確穩(wěn)定，需設(shè)定軌跡端點處的各項參數(shù)，并使其滿足運動學約束條件。

(2) 求解節(jié)點矢量U；

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

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

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

D1=D0且Dn=Dn-1

(11)

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

(12)

用端點各階導(dǎo)均為0的7次B樣條曲線插值表2所示參數(shù)和對應(yīng)的關(guān)鍵點。根據(jù)式(12)確定節(jié)點向量為：

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樣條曲線

可以看出，端點導(dǎo)矢為0的7次B樣條曲線準確經(jīng)過全部關(guān)鍵點，各階導(dǎo)曲線連續(xù)，且端點值均為0，達到了預(yù)定的要求。

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

2 優(yōu)化函數(shù)

通過7次B樣條函數(shù)，優(yōu)化各關(guān)節(jié)軌跡，增強軌跡對控制點的擬合度，使得軌跡曲線平穩(wěn)光滑。接下來將對關(guān)節(jié)軌跡運行時間進行優(yōu)化。

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

(13)

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

約束條件為：

(14)

3 時間最優(yōu)軌跡規(guī)劃

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

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

圖5 遺傳算法工作原理圖

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

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

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

(15)

(16)

通過以上分析，確定適應(yīng)度函數(shù)為：

(17)

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

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

(18)

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

(19)

(20)

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

4 仿真實驗

4.1 創(chuàng)建機器人模型

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

Link函數(shù)的格式為：

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

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

TR=fkine(robot,q)

圖6 MATLAB建立的機器人模型

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

Q=ikine(Robot,T)；

4.2 仿真結(jié)果

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

表3 路徑點各個關(guān)節(jié)角度 °

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

(21)

表5 各關(guān)節(jié)最優(yōu)時間序列 s

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

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

圖7 算法迭代收斂對比圖

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

圖8 各關(guān)節(jié)時間最優(yōu)軌跡

可以看出，關(guān)節(jié)3到達最終關(guān)節(jié)位置所需的時間是6.69 s，關(guān)節(jié)6到達最終關(guān)節(jié)位置卻需要10.82 s。

由于不同關(guān)節(jié)的約束條件相異，所以各關(guān)節(jié)到達目標位置的時間也不同。現(xiàn)在要求末端執(zhí)行器在任務(wù)空間中準確經(jīng)過路徑點，同時各關(guān)節(jié)在到達路徑點的轉(zhuǎn)動時間相同。這時就需對轉(zhuǎn)速較快的關(guān)節(jié)進行降速，可以通過如下步驟實現(xiàn)：

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

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

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

根據(jù)上述步驟，可以得到各關(guān)節(jié)位置-時間曲線、速度-時間曲線、加速度-時間曲線和加加速度-時間曲線分別如圖9-圖12所示。

圖9 位置-時間曲線

圖10 速度-時間曲線

圖11 加速度-時間曲線

圖12 加加速度-時間曲線

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

5 結(jié) 語

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