基于改進GA的5R串聯機器人最優時間軌跡規劃

周琪鈞，李國洪，吳金澤

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

機器人軌跡規劃一直以來是相關科研人員的研究重點。 所謂軌跡規劃，是指機器人根據作業任務的實際要求，計算出其預期的運動軌跡。 所謂軌跡，是指機器人在運動過程中的位移、速度和加速度[1]。 不過，到目前為止國內外尚未形成一套完備的理論體系與歸納。

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

通過大量調研發現，目前針對時間最優軌跡規劃主要有3 種解決思路：①通過優化算法在數種軌跡中尋找時間最優軌跡；②在運動學約束條件下尋找最大速度，通過提速來縮短運動時間；③將時間最優目標轉化成其他更易表達的模型來尋找最優解。 隨著各種算法的不斷研究，算法的通用性也在不斷完善中，因此對于軌跡規劃的研究是十分有意義且必要的[6]。

1 模型構建

本文以工業自動化中的5R（轉動副）工業機器人為研究對象并搭建其實驗模型。 該機器人的5 個關節機構是由轉動副組成，如圖1 所示，所以末端執行器具備5 個自由度，其所對應的D-H 參數如表1 所示[7]。

圖1 5R 串聯機器人坐標系機構圖Fig.1 Coordinate system diagram of 5R series robot

表1 D-H 參數表Tab.1 D-H parameter

運用MATLAB 中的Robotics Toolbox 編程構造該機器人的運動模型及其關節角度控制界面，如圖2 所示。

圖2 5R 串聯機器人仿真實驗平臺Fig.2 Simulation experiment platform of 5R series robot

模型搭建完畢后，可分別驗算該機器人的正、逆運動學分析過程的正確性，為后文軌跡規劃仿真實驗的順利實施打下基礎。

2 速度約束條件下的3-5-3 樣條插值的最優時間問題

本文討論的軌跡規劃是在關節空間中進行的，由于關節空間法是以關節角度的函數來描述機器人的軌跡，不必在直角坐標系中描述兩個路徑點間的路徑形狀，計算相對于笛卡爾空間軌跡規劃容易簡單[8]。 而且，由于關節空間與直角坐標系之間不是連續的對應關系，所以不會發生機構的奇異性問題[9]。

若給定機器人的初始點、中間點1、中間點2 及終末點，則可根據運動學逆解求解其所對應的各關節的角度。假定已知起始點θi0、中間點θi1與θi2及終末點θi3（i=1，2，3，4，5），根據已知條件，選擇機器人各個關節分別以3 次、5 次、3 次多項式的方式依次分段運行，則第i 段關節的3-5-3 樣條多項式的通式是：

式中：hi1（t），hi2（t），hi3（t）分別為第i 個關節在3 次、5次、3 次插值下的多項式表示；ai1j，ai2j，ai3j分別為hi1（t），hi2（t），hi3（t）的第j 次項的系數。

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

根據以上14 個邊界和過渡的約定條件， 可以推導出式（2）～式（4）：

式中：

式（2）中A 僅與第i 個關節的3 段多項式的插值時間ti1，ti2，ti3密切相關；式（3）表示第i 個關節角的位置矩陣；式（4）為多項式系數矩陣。

若想令機器人在速度限制的狀況下從起始點到終末點的運行時間最短，即在滿足速度約束的同時并獲取各關節最短三段插值時間。 第i 個關節的目標函數為

且要滿足速度約束：

式中：vi為第i 個關節運行的實時速度；vimax為第i個關節約定的最大速度。

由上述內容可知，多項式的系數和插值時間有關，所以，速度也與插值時間有關，但用傳統方法對于速度約束條件下的插值時間求取最優解是極其不便且困難的[10]，所以本文采取遺傳算法對插值時間進行優化，并對算法進行了適當的改進。

3 最優時間的遺傳算法求解過程

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

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

對各個關節的各段插值時間分別進行優化，本文設第i 個關節的適應度函數為

式中：Tmax是對于總插值時間的最大估計值，本文令Tmax=12。 適應度函數的復雜度往往是遺傳算法復雜度的主要組成部分，所以適應度函數的設計應盡可能簡單，使計算的時間復雜度優化。 可以看出，插值時間越短的粒子，其適應值越高，而超過最大估值的粒子，其適應值為0。

運用傳統遺傳算法對關節i 求取時間最優的過程：

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

第二步：初始化種群。 在搜索空間［0.1，4.0］隨機初始化種群。 種群規模設置為n=80；個體基因型用長度為L=10 的二進制編碼符號串表示， 則其精度為

第三步：將種群中的每個個體計算其適應度值，流程如圖4 所示。

圖4 計算種群中每個個體的適應度值機制圖Fig.4 Graph of the mechanism for calculating the fitness value of each individual in the population

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

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

第五步：交叉。 按概率pc選擇種群中的個體進行交叉操作。 本文以單點交叉法的方式生成新個體。

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

第七步：判斷終止條件是否滿足。 本文設置的終止條件為達到最大進化代數（Gmax=50），若滿足，則進化終止，否則，轉至第三步再次進行迭代。

4 遺傳算法的改進

通過大量的調研發現，遺傳算法自身參數的設定對于算法的搜索速度及算法的優化效果有著顯著影響[12]。 通常，傳統的遺傳算法是將交叉概率pc和變異概率pm設置為常數，種群中個體的適當交叉能增強種群的多樣性，但pc太小時難以向前搜索，太大又容易破壞適應度值高的個體的結構，通常取pc=0.25～0.75；變異的主要目的是維持種群中解的多樣性，但pm太小時難以產生新的基因結構，太大時又容易導致丟失最優解，也將使算法趨于純粹的隨機搜索，通常取pm=0.001～0.1。

針對以上問題，本文分別對交叉概率pc和變異概率pm設置一種自適應調節機制，其具體實現如下：

（1）針對交叉概率pc的改進機制

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

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

（2）針對變異概率pm的改進機制

在進化初期， 將個體變異率pm設置為較小的值，到了進化后期，考慮到種群開始逐漸收斂，為了避免其陷入局部最優， 對個體實施較大的變異，適當的增大pm值，擴大種群的多樣性；同時，對于適應度值較小的個體實施較大概率的變異，反之則實施較小概率的變異。 具體調節機制設置如下：

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

（3）最佳個體保存

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

5 仿真實驗與結果分析

5.1 實驗過程

本文設置5R 機器人末端在直角坐標系中的4 個路徑點坐標：起始點（110，0，280）、中間點1（168.64，-17.75，226.12）、中 間 點2（77.18，-30.87，315.11）、終末點（-6.44，2.34，348.86）。 通過運動學逆解求出4 個路徑點在關節空間中的角度插值點，如表2 所示。

表2 關節空間中的角度插值點Tab.2 Angular interpolation points in joint space

以關節1 為例，根據改進的遺傳算法對其進行不同速度約束下的時間最優求解，求得的三段最佳粒子pg位置進化圖及其目標函數收斂圖，如圖5所示。

圖5 不同速度下的關節1 最佳粒子pg 位置進化圖及目標函數收斂圖Fig.5 Optimal particle pg of joint 1 evolution figure and objective function convergence graph at different speeds

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

表3 各關節在速度vimax=20（°/s）下的最優三段插值時間Tab.3 Optimal three-segment interpolation time of each joint at speed vimax=20（°/s）

關節5 主要用于調整末端執行器的姿態，所以在機器人軌跡規劃的整個過程中始終保持不變。 由于機器人關節是要求在同一時間內運動的，所以取3-5-3 樣條插值的每段插值時間為各關節在該段插值時間的最大值，即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，則總運行時間為tsum=t1+t2+t3=11.2514 s。

通過MATLAB 實時仿真繪制出上述情況的5R工業機器人關節位置、角速度、角加速度的實驗曲線，如圖6～圖8 所示。

圖6 機器人關節位置曲線Fig.6 Robot joint position curve

圖7 機器人關節角速度曲線Fig.7 Robot joint angular velocity curve

圖8 機器人關節角加速度曲線Fig.8 Robot joint angular acceleration curve

5.2 結果分析

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

圖9 機器人關節急動度曲線Fig.9 Robot joint jerk curve

本實驗在實時采集關節角度插值點的同時也在計算該點在笛卡爾坐標系下的空間坐標， 圖10是機器人末端在笛卡爾坐標系下的3D 立體軌跡曲線，可以看到實驗曲線經過預設的4 個點。 實驗表明本改進遺傳算法應用在速度約束下的5R 串聯機器人最優時間軌跡規劃的有效性與可行性。

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

6 結語

為了實現關節位置、角速度和角加速度的連續性，采用3-5-3 樣條函數對5R 機器人在關節空間中進行軌跡擬合，對其求導得到2-4-2 樣條函數，在滿足速度約束條件的同時要使三段插值時間最短本身是件非常復雜的數理分析問題[13]，常規方法難以實現，本文采用一種改進遺傳算法對三段插值時間進行優化使結果收斂于速度約束條件內，有效地解決了這種難題，最后通過仿真結果進行分析，實驗表明了該種方法的有效性與可行性。 不過，采用的改進遺傳算法尚缺乏與其他算法的橫向對比，暫未表明該種方法的優越性，所以接下來的工作就是進行算法的橫向對比。

通過圖9 可以看到，本文研究的內容暫未考慮急動度連續的問題，在有些高端機器人工業應用場合是需要將其考慮進去的，用以實現更高精度的控制要求，并且可進一步降低對機器人臂結構所造成的潛在沖擊影響，所以接下來的工作也要考慮急動度連續的優良解決方案。