基于改進(jìn)GA的5R串聯(lián)機(jī)器人最優(yōu)時(shí)間軌跡規(guī)劃

周琪鈞，李國(guó)洪，吳金澤

（天津理工大學(xué) 天津市復(fù)雜系統(tǒng)控制理論及應(yīng)用重點(diǎn)實(shí)驗(yàn)室，天津 300384）

機(jī)器人軌跡規(guī)劃一直以來是相關(guān)科研人員的研究重點(diǎn)。 所謂軌跡規(guī)劃，是指機(jī)器人根據(jù)作業(yè)任務(wù)的實(shí)際要求，計(jì)算出其預(yù)期的運(yùn)動(dòng)軌跡。 所謂軌跡，是指機(jī)器人在運(yùn)動(dòng)過程中的位移、速度和加速度[1]。 不過，到目前為止國(guó)內(nèi)外尚未形成一套完備的理論體系與歸納。

在機(jī)器人的實(shí)際運(yùn)用中，不僅要考量其運(yùn)動(dòng)效果和任務(wù)需求，也要考量其工作效率、平穩(wěn)性和能耗等因素。 就工作效率而言，就是以時(shí)間最優(yōu)作為機(jī)器人的性能指標(biāo)對(duì)其完成軌跡規(guī)劃，從而達(dá)到提升工作效率的目的。 在國(guó)內(nèi)研究中，文獻(xiàn)[2]采取了一種基于DE、GA 和SQP 求解機(jī)器人最優(yōu)時(shí)間軌跡規(guī)劃的混合優(yōu)化方法，其首先采用五次多項(xiàng)式構(gòu)造關(guān)節(jié)空間插值曲線， 然后結(jié)合DE 和GA 算法獲得全局最優(yōu)解的初始估計(jì)值，最后通過序列二次規(guī)劃（SQP）獲取準(zhǔn)確的全局最優(yōu)解，仿真結(jié)果表明該方法具有不錯(cuò)的效果；文獻(xiàn)[3]采用五次非均勻B 樣條曲線構(gòu)造關(guān)節(jié)空間插值曲線，進(jìn)而采取量子行為粒子群優(yōu)化算法進(jìn)行最優(yōu)軌跡尋找，獲得了比傳統(tǒng)粒子群算法和差分進(jìn)化算法更短的時(shí)間。 在國(guó)外研究中，文獻(xiàn)[4]提出了一種新的速度剖面生成方法，該方法可以獲得非凸優(yōu)化問題的全局最優(yōu)解，并在三自由度機(jī)器人上驗(yàn)證了其方法；文獻(xiàn)[5]通過三次B 樣條曲線規(guī)劃?rùn)C(jī)器人運(yùn)動(dòng)軌跡， 考慮機(jī)器人的運(yùn)動(dòng)學(xué)約束來優(yōu)化時(shí)間用以提升機(jī)器人的工作效率。

通過大量調(diào)研發(fā)現(xiàn)，目前針對(duì)時(shí)間最優(yōu)軌跡規(guī)劃主要有3 種解決思路：①通過優(yōu)化算法在數(shù)種軌跡中尋找時(shí)間最優(yōu)軌跡；②在運(yùn)動(dòng)學(xué)約束條件下尋找最大速度，通過提速來縮短運(yùn)動(dòng)時(shí)間；③將時(shí)間最優(yōu)目標(biāo)轉(zhuǎn)化成其他更易表達(dá)的模型來尋找最優(yōu)解。 隨著各種算法的不斷研究，算法的通用性也在不斷完善中，因此對(duì)于軌跡規(guī)劃的研究是十分有意義且必要的[6]。

1 模型構(gòu)建

本文以工業(yè)自動(dòng)化中的5R（轉(zhuǎn)動(dòng)副）工業(yè)機(jī)器人為研究對(duì)象并搭建其實(shí)驗(yàn)?zāi)Ｐ汀?該機(jī)器人的5 個(gè)關(guān)節(jié)機(jī)構(gòu)是由轉(zhuǎn)動(dòng)副組成，如圖1 所示，所以末端執(zhí)行器具備5 個(gè)自由度，其所對(duì)應(yīng)的D-H 參數(shù)如表1 所示[7]。

圖1 5R 串聯(lián)機(jī)器人坐標(biāo)系機(jī)構(gòu)圖Fig.1 Coordinate system diagram of 5R series robot

表1 D-H 參數(shù)表Tab.1 D-H parameter

運(yùn)用MATLAB 中的Robotics Toolbox 編程構(gòu)造該機(jī)器人的運(yùn)動(dòng)模型及其關(guān)節(jié)角度控制界面，如圖2 所示。

圖2 5R 串聯(lián)機(jī)器人仿真實(shí)驗(yàn)平臺(tái)Fig.2 Simulation experiment platform of 5R series robot

模型搭建完畢后，可分別驗(yàn)算該機(jī)器人的正、逆運(yùn)動(dòng)學(xué)分析過程的正確性，為后文軌跡規(guī)劃仿真實(shí)驗(yàn)的順利實(shí)施打下基礎(chǔ)。

2 速度約束條件下的3-5-3 樣條插值的最優(yōu)時(shí)間問題

本文討論的軌跡規(guī)劃是在關(guān)節(jié)空間中進(jìn)行的，由于關(guān)節(jié)空間法是以關(guān)節(jié)角度的函數(shù)來描述機(jī)器人的軌跡，不必在直角坐標(biāo)系中描述兩個(gè)路徑點(diǎn)間的路徑形狀，計(jì)算相對(duì)于笛卡爾空間軌跡規(guī)劃容易簡(jiǎn)單[8]。 而且，由于關(guān)節(jié)空間與直角坐標(biāo)系之間不是連續(xù)的對(duì)應(yīng)關(guān)系，所以不會(huì)發(fā)生機(jī)構(gòu)的奇異性問題[9]。

若給定機(jī)器人的初始點(diǎn)、中間點(diǎn)1、中間點(diǎn)2 及終末點(diǎn)，則可根據(jù)運(yùn)動(dòng)學(xué)逆解求解其所對(duì)應(yīng)的各關(guān)節(jié)的角度。假定已知起始點(diǎn)θi0、中間點(diǎn)θi1與θi2及終末點(diǎn)θi3（i=1，2，3，4，5），根據(jù)已知條件，選擇機(jī)器人各個(gè)關(guān)節(jié)分別以3 次、5 次、3 次多項(xiàng)式的方式依次分段運(yùn)行，則第i 段關(guān)節(jié)的3-5-3 樣條多項(xiàng)式的通式是：

式中：hi1（t），hi2（t），hi3（t）分別為第i 個(gè)關(guān)節(jié)在3 次、5次、3 次插值下的多項(xiàng)式表示；ai1j，ai2j，ai3j分別為hi1（t），hi2（t），hi3（t）的第j 次項(xiàng)的系數(shù)。

根據(jù)實(shí)際生產(chǎn)情況，本文約定條件：①5R 機(jī)器人在起始點(diǎn)與終末點(diǎn)處的速度、加速度均為0；②中間點(diǎn)1 與中間點(diǎn)2 的位置、速度及加速度連續(xù)。

根據(jù)以上14 個(gè)邊界和過渡的約定條件， 可以推導(dǎo)出式（2）～式（4）：

式中：

式（2）中A 僅與第i 個(gè)關(guān)節(jié)的3 段多項(xiàng)式的插值時(shí)間ti1，ti2，ti3密切相關(guān)；式（3）表示第i 個(gè)關(guān)節(jié)角的位置矩陣；式（4）為多項(xiàng)式系數(shù)矩陣。

若想令機(jī)器人在速度限制的狀況下從起始點(diǎn)到終末點(diǎn)的運(yùn)行時(shí)間最短，即在滿足速度約束的同時(shí)并獲取各關(guān)節(jié)最短三段插值時(shí)間。 第i 個(gè)關(guān)節(jié)的目標(biāo)函數(shù)為

且要滿足速度約束：

式中：vi為第i 個(gè)關(guān)節(jié)運(yùn)行的實(shí)時(shí)速度；vimax為第i個(gè)關(guān)節(jié)約定的最大速度。

由上述內(nèi)容可知，多項(xiàng)式的系數(shù)和插值時(shí)間有關(guān)，所以，速度也與插值時(shí)間有關(guān)，但用傳統(tǒng)方法對(duì)于速度約束條件下的插值時(shí)間求取最優(yōu)解是極其不便且困難的[10]，所以本文采取遺傳算法對(duì)插值時(shí)間進(jìn)行優(yōu)化，并對(duì)算法進(jìn)行了適當(dāng)?shù)母倪M(jìn)。

3 最優(yōu)時(shí)間的遺傳算法求解過程

傳統(tǒng)遺傳算法是受到生物進(jìn)化學(xué)中的一些自然現(xiàn)象的啟發(fā)而逐步發(fā)展起來的，這些自然現(xiàn)象包含遺傳、突變、雜交以及自然選擇等，所以該算法是一種基于種群優(yōu)化的隨機(jī)并行搜索算法， 通過選擇、交叉、變異等操作使得種群最終收斂于全局最優(yōu)解，其基本求解流程如圖3 所示[11]。

圖3 傳統(tǒng)遺傳算法基本流程Fig.3 Basic flow chart of traditional genetic algorithm

對(duì)各個(gè)關(guān)節(jié)的各段插值時(shí)間分別進(jìn)行優(yōu)化，本文設(shè)第i 個(gè)關(guān)節(jié)的適應(yīng)度函數(shù)為

式中：Tmax是對(duì)于總插值時(shí)間的最大估計(jì)值，本文令Tmax=12。 適應(yīng)度函數(shù)的復(fù)雜度往往是遺傳算法復(fù)雜度的主要組成部分，所以適應(yīng)度函數(shù)的設(shè)計(jì)應(yīng)盡可能簡(jiǎn)單，使計(jì)算的時(shí)間復(fù)雜度優(yōu)化。 可以看出，插值時(shí)間越短的粒子，其適應(yīng)值越高，而超過最大估值的粒子，其適應(yīng)值為0。

運(yùn)用傳統(tǒng)遺傳算法對(duì)關(guān)節(jié)i 求取時(shí)間最優(yōu)的過程：

第一步：編碼。 編碼方式采用二進(jìn)制，此種方式編碼簡(jiǎn)單，解碼操作簡(jiǎn)便易行且遺傳操作簡(jiǎn)單。

第二步：初始化種群。 在搜索空間［0.1，4.0］隨機(jī)初始化種群。 種群規(guī)模設(shè)置為n=80；個(gè)體基因型用長(zhǎng)度為L(zhǎng)=10 的二進(jìn)制編碼符號(hào)串表示， 則其精度為

第三步：將種群中的每個(gè)個(gè)體計(jì)算其適應(yīng)度值，流程如圖4 所示。

圖4 計(jì)算種群中每個(gè)個(gè)體的適應(yīng)度值機(jī)制圖Fig.4 Graph of the mechanism for calculating the fitness value of each individual in the population

采取這種機(jī)制既能向種群中引入新個(gè)體以規(guī)避未成熟收斂，又能快速解決速度約束的問題。

第四步：選擇。 按照某種規(guī)則根據(jù)個(gè)體適應(yīng)度值的大小選擇將進(jìn)入下一代的個(gè)體。 本文采用輪盤賭法和錦標(biāo)賽選擇法相結(jié)合的方式，在保留最佳個(gè)體的同時(shí)，用輪盤賭法和錦標(biāo)賽選擇法各選擇一部分個(gè)體進(jìn)入下一代。 這樣做，算法的收斂速度略有降低，卻能夠明顯增加搜索的廣度，總體效果是比較好的。

第五步：交叉。 按概率pc選擇種群中的個(gè)體進(jìn)行交叉操作。 本文以單點(diǎn)交叉法的方式生成新個(gè)體。

第六步：變異。 按概率pm選擇種群中部分個(gè)體進(jìn)行突變操作。 本文以基本位變異法的方式進(jìn)行操作。

第七步：判斷終止條件是否滿足。 本文設(shè)置的終止條件為達(dá)到最大進(jìn)化代數(shù)（Gmax=50），若滿足，則進(jìn)化終止，否則，轉(zhuǎn)至第三步再次進(jìn)行迭代。

4 遺傳算法的改進(jìn)

通過大量的調(diào)研發(fā)現(xiàn)，遺傳算法自身參數(shù)的設(shè)定對(duì)于算法的搜索速度及算法的優(yōu)化效果有著顯著影響[12]。 通常，傳統(tǒng)的遺傳算法是將交叉概率pc和變異概率pm設(shè)置為常數(shù)，種群中個(gè)體的適當(dāng)交叉能增強(qiáng)種群的多樣性，但pc太小時(shí)難以向前搜索，太大又容易破壞適應(yīng)度值高的個(gè)體的結(jié)構(gòu)，通常取pc=0.25～0.75；變異的主要目的是維持種群中解的多樣性，但pm太小時(shí)難以產(chǎn)生新的基因結(jié)構(gòu)，太大時(shí)又容易導(dǎo)致丟失最優(yōu)解，也將使算法趨于純粹的隨機(jī)搜索，通常取pm=0.001～0.1。

針對(duì)以上問題，本文分別對(duì)交叉概率pc和變異概率pm設(shè)置一種自適應(yīng)調(diào)節(jié)機(jī)制，其具體實(shí)現(xiàn)如下：

（1）針對(duì)交叉概率pc的改進(jìn)機(jī)制

考慮到前期的進(jìn)化需要加快更新速度及擴(kuò)大搜尋范圍，所以設(shè)置較大的pc值，而進(jìn)化到了后期，由于種群基本趨于飽和，設(shè)置較小的pc值用以使較好的個(gè)體基因結(jié)構(gòu)不被破壞；與此同時(shí)，對(duì)于適應(yīng)度值較大的個(gè)體賦予較小的pc值，使其優(yōu)良的基因結(jié)構(gòu)得以保存，而對(duì)于適應(yīng)度值較小的個(gè)體則賦予較大的pc值，幫助其向最優(yōu)個(gè)體方向靠攏，避免陷入遲滯狀態(tài)。 本文具體調(diào)節(jié)機(jī)制設(shè)置如下：

式中：pcj是第j 個(gè)個(gè)體的交叉概率；令pcmin=0.4；f（j）是第j 個(gè)粒子的適應(yīng)度值；fmax是種群中最大的適應(yīng)度值；favg是種群平均適應(yīng)度值。

（2）針對(duì)變異概率pm的改進(jìn)機(jī)制

在進(jìn)化初期， 將個(gè)體變異率pm設(shè)置為較小的值，到了進(jìn)化后期，考慮到種群開始逐漸收斂，為了避免其陷入局部最優(yōu)， 對(duì)個(gè)體實(shí)施較大的變異，適當(dāng)?shù)脑龃髉m值，擴(kuò)大種群的多樣性；同時(shí)，對(duì)于適應(yīng)度值較小的個(gè)體實(shí)施較大概率的變異，反之則實(shí)施較小概率的變異。 具體調(diào)節(jié)機(jī)制設(shè)置如下：

式中：令pmmax=0.006；pmj是第j 個(gè)個(gè)體的變異概率。

（3）最佳個(gè)體保存

父代中的最佳個(gè)體在遺傳操作前進(jìn)行保留，將遺傳操作后種群中適應(yīng)度值最差的個(gè)體用上一代的最佳個(gè)體進(jìn)行替換，再將此次迭代的最佳個(gè)體進(jìn)行保存，便于進(jìn)行下一次替代，循環(huán)往復(fù)。 采用該策略可防止最佳個(gè)體由于交叉、 變異操作后被破壞，提高收斂速度，防止種群退化。

5 仿真實(shí)驗(yàn)與結(jié)果分析

5.1 實(shí)驗(yàn)過程

本文設(shè)置5R 機(jī)器人末端在直角坐標(biāo)系中的4 個(gè)路徑點(diǎn)坐標(biāo)：起始點(diǎn)（110，0，280）、中間點(diǎn)1（168.64，-17.75，226.12）、中 間 點(diǎn)2（77.18，-30.87，315.11）、終末點(diǎn)（-6.44，2.34，348.86）。 通過運(yùn)動(dòng)學(xué)逆解求出4 個(gè)路徑點(diǎn)在關(guān)節(jié)空間中的角度插值點(diǎn)，如表2 所示。

表2 關(guān)節(jié)空間中的角度插值點(diǎn)Tab.2 Angular interpolation points in joint space

以關(guān)節(jié)1 為例，根據(jù)改進(jìn)的遺傳算法對(duì)其進(jìn)行不同速度約束下的時(shí)間最優(yōu)求解，求得的三段最佳粒子pg位置進(jìn)化圖及其目標(biāo)函數(shù)收斂圖，如圖5所示。

圖5 不同速度下的關(guān)節(jié)1 最佳粒子pg 位置進(jìn)化圖及目標(biāo)函數(shù)收斂圖Fig.5 Optimal particle pg of joint 1 evolution figure and objective function convergence graph at different speeds

由圖5 可以看出，在4 種不同速度的約束條件下，關(guān)節(jié)1 的最佳粒子pg經(jīng)過35 次進(jìn)化迭代就快速收斂。 不過，在實(shí)際的工業(yè)生產(chǎn)中，工業(yè)機(jī)器人的關(guān)節(jié)電機(jī)運(yùn)轉(zhuǎn)速度不宜設(shè)置過高，否則機(jī)體本身會(huì)產(chǎn)生較強(qiáng)烈的機(jī)械振動(dòng)，不利于機(jī)器人長(zhǎng)期穩(wěn)定的運(yùn)行。 本文設(shè)置各關(guān)節(jié)的最大速度約束為vimax=20（°/s）（其中i=1，2，3，4，5）， 根據(jù)與關(guān)節(jié)1 相同的優(yōu)化辦法，可得到其余各關(guān)節(jié)在3-5-3 樣條插值時(shí)的各段最優(yōu)插值時(shí)間ti1、ti2、ti3，從20 組實(shí)驗(yàn)數(shù)據(jù)樣本中抽取一組最優(yōu)時(shí)間解，如表3 所示。

表3 各關(guān)節(jié)在速度vimax=20（°/s）下的最優(yōu)三段插值時(shí)間Tab.3 Optimal three-segment interpolation time of each joint at speed vimax=20（°/s）

關(guān)節(jié)5 主要用于調(diào)整末端執(zhí)行器的姿態(tài)，所以在機(jī)器人軌跡規(guī)劃的整個(gè)過程中始終保持不變。 由于機(jī)器人關(guān)節(jié)是要求在同一時(shí)間內(nèi)運(yùn)動(dòng)的，所以取3-5-3 樣條插值的每段插值時(shí)間為各關(guān)節(jié)在該段插值時(shí)間的最大值，即t1=max｛ti1｝，t2=max｛ti2｝，t3=max｛ti3｝（i=1，2，3，4，5），由表3 可知，t1=3.5735 s，t2=3.8243 s，t3=3.8536 s，則總運(yùn)行時(shí)間為tsum=t1+t2+t3=11.2514 s。

通過MATLAB 實(shí)時(shí)仿真繪制出上述情況的5R工業(yè)機(jī)器人關(guān)節(jié)位置、角速度、角加速度的實(shí)驗(yàn)曲線，如圖6～圖8 所示。

圖6 機(jī)器人關(guān)節(jié)位置曲線Fig.6 Robot joint position curve

圖7 機(jī)器人關(guān)節(jié)角速度曲線Fig.7 Robot joint angular velocity curve

圖8 機(jī)器人關(guān)節(jié)角加速度曲線Fig.8 Robot joint angular acceleration curve

5.2 結(jié)果分析

由圖6 可發(fā)現(xiàn)機(jī)器人按預(yù)定軌跡平滑連續(xù)運(yùn)動(dòng)。 圖7 和8 說明機(jī)器人各關(guān)節(jié)在起始點(diǎn)與終末點(diǎn)處的角速度、角加速度為0，關(guān)節(jié)速度限制在vimax=20（°/s）的條件范圍內(nèi)，加速度也平滑變化。 圖9 是機(jī)器人關(guān)節(jié)急動(dòng)度曲線，可以看出急動(dòng)度并不連續(xù)，存在突變現(xiàn)象，這是3-5-3 樣條插值的固有特性所決定的。

圖9 機(jī)器人關(guān)節(jié)急動(dòng)度曲線Fig.9 Robot joint jerk curve

本實(shí)驗(yàn)在實(shí)時(shí)采集關(guān)節(jié)角度插值點(diǎn)的同時(shí)也在計(jì)算該點(diǎn)在笛卡爾坐標(biāo)系下的空間坐標(biāo)， 圖10是機(jī)器人末端在笛卡爾坐標(biāo)系下的3D 立體軌跡曲線，可以看到實(shí)驗(yàn)曲線經(jīng)過預(yù)設(shè)的4 個(gè)點(diǎn)。 實(shí)驗(yàn)表明本改進(jìn)遺傳算法應(yīng)用在速度約束下的5R 串聯(lián)機(jī)器人最優(yōu)時(shí)間軌跡規(guī)劃的有效性與可行性。

圖10 機(jī)器人末端執(zhí)行器空間軌跡曲線Fig.10 Space trajectory curve of Robot’s end effector

6 結(jié)語

為了實(shí)現(xiàn)關(guān)節(jié)位置、角速度和角加速度的連續(xù)性，采用3-5-3 樣條函數(shù)對(duì)5R 機(jī)器人在關(guān)節(jié)空間中進(jìn)行軌跡擬合，對(duì)其求導(dǎo)得到2-4-2 樣條函數(shù)，在滿足速度約束條件的同時(shí)要使三段插值時(shí)間最短本身是件非常復(fù)雜的數(shù)理分析問題[13]，常規(guī)方法難以實(shí)現(xiàn)，本文采用一種改進(jìn)遺傳算法對(duì)三段插值時(shí)間進(jìn)行優(yōu)化使結(jié)果收斂于速度約束條件內(nèi)，有效地解決了這種難題，最后通過仿真結(jié)果進(jìn)行分析，實(shí)驗(yàn)表明了該種方法的有效性與可行性。 不過，采用的改進(jìn)遺傳算法尚缺乏與其他算法的橫向?qū)Ρ?，暫未表明該種方法的優(yōu)越性，所以接下來的工作就是進(jìn)行算法的橫向?qū)Ρ取?/p>

通過圖9 可以看到，本文研究的內(nèi)容暫未考慮急動(dòng)度連續(xù)的問題，在有些高端機(jī)器人工業(yè)應(yīng)用場(chǎng)合是需要將其考慮進(jìn)去的，用以實(shí)現(xiàn)更高精度的控制要求，并且可進(jìn)一步降低對(duì)機(jī)器人臂結(jié)構(gòu)所造成的潛在沖擊影響，所以接下來的工作也要考慮急動(dòng)度連續(xù)的優(yōu)良解決方案。