改進遺傳算法搜索中間點的機械臂軌跡規劃?

譚 燕

(重慶三峽職業學院機械與電子工程系重慶404155)

機械臂正朝著靈活、控制精確、高效的方向發展，可以越來越多地完成人類難以完成或具有危險的工作。機械臂工作軌跡直接決定了機械臂工作效率[1-2］，因此研究機械臂軌跡規劃問題對于提高經濟效益意義明顯。

機械臂軌跡規劃的常用方法是以邊界約束為限定條件，使用多項式函數[3］、拋物線過渡的線性函數、B樣條曲線[4］等進行擬合，使用待定系數法確定軌跡函數。文獻[5］以6自由度機械臂為研究對象，使用多項式插值法進行軌跡規劃，使用雜交算法尋優，得到了速度約束下的時間最優軌跡；文獻[6］使用牛頓下山法求出了軌跡關鍵點，使用三次樣條插值擬合了軌跡，實現了能量消耗最優；文獻[7］以距離最優為目標選擇了空間梯形軌跡關鍵點，使用5次多項式插值法進行擬合，實現了預期目標；文獻[8］在速度約束下，提出了粒子群優化的多項式插值軌跡規劃方法，得到了時間最優軌跡。以上研究成果都達到了各自的優化目標，但是目前沒有成熟統一的軌跡規劃方法。本文研究了避障同時的多目標優化軌跡規劃方法。

本文研究機械臂軌跡規劃問題，設置了3個目標:避障、軌跡長度最短、關節角變化量最小。給出了簡單高效的碰撞檢測策略，當發生碰撞時，使用改進遺傳算法尋得最優中間點，而后使用軌跡基元規劃起點至中間點、中間點至終點的軌跡，最終達到避障同時保證軌跡最短和關節角變化量最小的目的。

1 機械臂運動學建模

1.1 仿人手臂機械臂

本文研究對象為如圖1所示的仿人手臂機械臂，圖中L1＝120 mm為大臂長度，L2＝100 mm為小臂長度。肩關節具有兩個旋轉自由度。肘關節具有1個旋轉自由度，腕關節具有3個旋轉自由度，圖中為每個旋轉自由度建立了坐標系，z軸為其旋轉軸，按照圖中各坐標系的腳標，將坐標系記為坐標系1～6。

1.2 運動學建模

機械臂運動學模型是機械臂各關節角與末端執行器位姿互相求解問題。當各關節角已知，求解末端執行器位姿時稱為運動學正解；當末端執行器位姿已知，求解各機械臂關節角時稱為運動學逆解。

本文使用D-H法建立運動學正解模型，記坐標系n到坐標系n＋1的齊次變換矩陣為，經過兩次旋轉和兩次平移可由坐標系n轉換到坐標系n＋1，得矩陣形式為:

那么從基座坐標系(記為0坐標系)到末端執行器的齊次變換矩陣為:

式(2)即為機械臂運動學正解模型，當機械臂各關節角確定時，可以通過式(2)確定末端執行器的位姿。

機械臂運動學逆解與正解過程恰好相反，在式(2)兩端分別左乘()-1、()-1、…、()-1，再使用反正切函數可以求得θi，i＝ 1，2，…，6。 運動學逆解存在多解問題，一般需根據實際情況進一步確定。

2 插入最優中間點的軌跡規劃

2.1 插入最優中間點軌跡規劃原理

本文對機械臂軌跡規劃的目標包括3個:避障、軌跡最短和關節角變化量最小。軌跡規劃的整體思路是:根據機械臂任務確定軌跡起點和終點，使用軌跡基元進行軌跡規劃，然后對規劃出的軌跡進行碰撞檢測，若發生碰撞則插入最優中間點，再使用軌跡基元規劃從起點至中間點、中間點至終點的軌跡，在保證軌跡不發生碰撞的同時，保證軌跡最短且關節角變化量最小。機械臂避障軌跡規劃原理如圖2所示。

由圖2可知，還有3個問題需要明確:一是確定軌跡基元，二是碰撞檢測如何實現，三是最優中間點如何選取。軌跡基元為點與點之間擬合使用的曲線函數。

常用函數包括多項式函數、B樣條曲線等。本文選擇使用5次多項式作為軌跡基元。

2.2 碰撞檢測方法

機械臂軌跡規劃的首要要求是安全，即避開障礙物，因此必須提出機械臂與障礙物的碰撞檢測策略。為了提出簡單高效、利于推廣的碰撞檢測方法，本文將障礙物模型簡化為球形，球心為障礙物幾何中心，球半徑為障礙物最大半徑。將機械臂模型簡化為線段，線段長度與機械臂長度一致，為了實現避障，將機械臂包絡半徑疊加到障礙物半徑中，最終得到障礙物球形模型的膨脹半徑為R＝Rob＋Rlink，式中Rob為障礙物半徑，Rlink為連桿包絡半徑。

通過障礙物與機械臂簡化模型可以看出，機械臂與障礙物之間碰撞檢測問題可以簡化為球模型與線段的位置關系判斷問題。確定連桿線段的位姿，只需用到關節2的位姿、關節3的位姿、關節4的位姿，則兩連桿在三維空間的矢量表達式為:

式中:為大臂的矢量表達式；為小臂的矢量表達式；(1，4)表示矩陣第1行、第4列的元素。

記障礙物圓球模型的球心坐標為P＝[px，py，pz］，選取連桿線段上任意一點到球心的矢量表達為，則連桿與球心距離為:

式中:d1為大臂到球心的距離；d2為小臂到球心的距離。若di＞R，i＝1，2則機械臂與障礙物不會發生碰撞，否則會發生碰撞。

本文提出的碰撞檢測策略在一定程度上犧牲了部分機械臂工作空間，但是其簡單高效的特點使這種方法易于推廣使用。

3 最優中間點選取方法

3.1 建立適應度函數

本文規劃機械臂軌跡時，在避障的同時保證軌跡最短且各關節角變化量最小，因此需要從軌跡長度和關節角變化量兩個方面建立適應度函數。

記fQ為關節角變化量，fL為機械臂運動軌跡長度，使用插值法對這兩個量進行計算。在運動軌跡中插入n-1個點，從而將運動軌跡劃分為n段，通過求得相鄰插入點的關節角變化量和軌跡長度，再通過累加得到整個軌跡的關節角變化量和軌跡長度，為:

式中:fOB為表征碰撞檢測結果的二值變量，若機械臂與障礙物不發生碰撞則fOB＝1，若機械臂與障礙物發生碰撞則fOB＝0；η1、η2為權重因子，角度變化量與軌跡長度數量級相差較大，約為1 000倍，為保證兩個因素對適應度函數具有相同的影響程度，取η1＝1、η2＝0.001。

對于多障礙物環境時，需要設置與障礙物相同數量的中間點，此時適應度函數為各中間點適應度函數的累加和。

3.2 改進遺傳算法的尋優方法

遺傳算法模擬自然界遺傳和優勝劣汰的自然選擇過程，使種群適應度不斷提高，尋得最優解。遺傳算法包括染色體編碼、初始種群生成、遺傳操作(選擇、交叉、變異)、解碼等過程[9］。其中初始種群生成方法、交叉概率、變異概率對算法尋優效果影響極大[10］。本文對此3個方面進行改進。

(1)染色體編碼。使用實數編碼方式，由于1個中間點對應6個關節角，那么對于需要設置n個中間點的規劃問題，其需要6n個關節角。即編碼方式為(θ1，θ2，…，θ6n)。

(2)初始種群生成。在傳統遺傳算法中使用隨機方法產生初始種群，這種方法產生的初始種群適應度不高，沒有優勢。本文提出以均勻方式產生初始種群，將機械臂工作空間分成z個區域，在每個區域中使用均勻數值產生p個染色體，這樣就得到了z×p個染色體，依據適應度函數選擇最大的前N個個體作為初始種群。這種初始種群產生方式保證了初始種群在解空間的分散性，有利于全局搜索。

(3)選擇操作。以個體適應度為選擇依據，每個個體被選概率為f(i)/∑f(i)，式中f(i)為個體i的適應度值，∑f(i)為所有個體適應度之和。這種選擇方

式中:qij為第i段軌跡第j個關節角；fQ為整個運動軌跡的 6 個關節角變化量之和；(pi，x，pi，y，pi，z)為第i個插入點的三維坐標；fL為整個運動軌跡長度。

以軌跡長度和各關節角變化量為基礎，建立適應度函數fG為:式既保證了優秀個體被選可能性較大，也保證了任意個體被選可能性，保持了種群多樣性。

(4)交叉算子。交叉算子可以提高物種多樣性，提高物種進化能力。那么，對于物種多樣性較差的種群使用較大的交叉概率，提高物種多樣性；對于物種多樣性較好的種群使用較小的交叉概率，保持物種多樣性。本文使用適應度值方差和分布熵衡量物種多樣性，并提出相應的交叉概率。記種群規模為N，第i個個體適應度為fi，種群平均適應度為，則第j代種群適應度方差Gj為:

種群分布熵計算方法為:將可行解空間劃分為z個子空間，種群個體落在第i個子空間數量記為Ai，那么第j代種群的分布熵Hj為:

式中:pi＝Ai/N。

適應度值方差、種群分布熵越大，表示種群空間分布和適應度值分布越均勻，進化能力越強，此時使用較小交叉概率。由此提出交叉概率為:

(5)變異算子。變異算子可以破壞優良基因，也可以使劣質基因變異為優良個體，因此對于優良基因應使用較小的變異概率，對于劣質基因應使用較大的變異概率。記第i個個體適應度值與平均值之差為θ，則第i個個體的變異概率為:

式中:pi＝1/(1＋e-θ)。 分析式(8)可知，當個體適應度值較大時，變異概率較小，保護了優良基因；當個體適應度值較小時，變異概率較大，劣質基因具有更大機會變異為優良基因。

(6)解碼。解碼方法與式(2)一致，即通過6個關節角確定中間點的位姿。

4 仿真驗證

本文設置了兩組仿真，一是確定中間點數量的仿真實驗，二是為了驗證本文提出方法的路徑規劃效果，同時比較改進遺傳算法與傳統遺傳算法搜索最優中間點的性能。

4.1 中間點數量的選取

本文中提到，當機械臂軌跡與障礙物發生碰撞時，插入最優中間點，而后使用軌跡基元分別規劃起點至中間點、中間點至終點的軌跡，但是中間點數量問題一直未得到明確或驗證。

首先設置單障礙物情況，障礙物中心為P＝(50，100，50)，膨脹半徑為R＝30 mm，使用軌跡基元直接規劃機械臂軌跡會發生碰撞，因此需要插入中間點。當插入一個中間點時，插入中間點為θ＝[0.229，0.748，-0.841，0，0.407，0.383］，軌跡如圖 3a 所示。 當插入兩個中間點時，尋優得到的兩個中間點分別為:

插入兩個中間點時的軌跡如圖3b所示。

為了對比兩條機械臂軌跡的性能，計算兩條軌跡的適應度函數值如表1所示。

表1 單障礙物時不同數量中間點對比

表1中迭代次數是指最早出現最優解時的迭代次數。由表1可以看出，當插入兩個中間點時，適應度值相差-0.000 9，下降百分比為1.29%，說明增加一個中間點的優化效果不明顯，但是增加一個中間點時計算量和迭代次數明顯增加，因此存在一個障礙物時應當插入一個中間點。

接下來研究兩個障礙物的情況，障礙物中心分別為P1＝(150，50，30)、P2＝(50，100，50)，膨脹半徑為R＝30 mm。分別插入一個中間點和兩個中間點進行軌跡規劃，插入一個中間點時中間點為θ＝[0.493，0.996，-0.438，0，0.1472，0.716］， 插 入 兩 個中間點時，兩個中間點分別為:

這里不再給出軌跡規劃結果，計算兩條軌跡的適應度值，結果如表2所示。

表2 兩個障礙物時不同數量中間點對比

由表2可知，當工作環境中存在兩個障礙物需要躲避時，與插入一個中間點相比，插入兩個中間點時適應度值增加了0.006 4，增加百分比為10.4%，優化效果非常明顯，因此當存在兩個障礙物時應當插入兩個中間點。

基于對單個障礙物和兩個障礙物的情況分析，本文粗略地規定:當工作空間中存在N≥0個障礙物時，考慮到軌跡優劣和計算量大小，最佳的避障策略是插入相同數量的中間點。

4.2 改進遺傳算法與傳統算法對比

本節設置了包含兩個障礙物的工作環境，用于對比改進遺傳算法與傳統算法在中間點選取上的性能優劣。 兩個障礙物位置分別為P1＝(150，50，30)、P2＝(50，100，50)，膨脹半徑為R＝30 mm。 由于環境中包含2個障礙物，因此需要優化出2個中間點，遺傳算法編碼時需設置12個角度量。

使用改進遺傳算法尋優得到的中間點見式(9)，使用傳統遺傳算法尋優得到的兩個中間點為:

而后使用5次多項式分別規劃起點至中間點、中間點至中間點、中間點至終點的軌跡，計算兩條軌跡各自的適應度值，結果如表3所示。

表3 兩種遺傳算法的軌跡規劃結果

由表2可知，改進遺傳算法在迭代131次時出現最優個體，而傳統算法在196次時才出現；與傳統遺傳算法規劃的軌跡相比，改進遺傳算法尋得的中間點用于軌跡規劃時，軌跡長度減少了12.1%，關節角變化量減少了24.4%，適應度值增加了23.6%，軌跡優勢非常明顯。這是因為改進遺傳算法使用均勻方式產生初始種群，非常有利于全局搜索，而且交叉概率和變異概率的設置方法非常有利于種群進化，使得改進遺傳算法出現最優個體的迭代次數少，且軌跡適應度值更大。

對于本節設置的兩個障礙物工作環境，規劃出的最優軌跡如圖4所示，圖中實線表示實際軌跡，虛線表示點與點之間的連線。以上分析及仿真結果充分證明了插入最優點的機械臂軌跡規劃方法，在實現避障的同時，能夠規劃出長度最短、關節角變化量最小的工作軌跡。

5 結語

本文研究了仿人手臂機械臂避障軌跡規劃問題，在避障的同時，保證了關節角變化量最小和軌跡最短。得出了以下結論:(1)插入最優中間點避障算法能夠準確規劃出一條避障軌跡，且關節角變化量最小和軌跡最短；(2)通過改進遺傳算法的初始種群生成方法、交叉變異概率，有利于算法全局尋優能力，且加強了種群進化能力，與傳統遺傳算法相比，不僅尋優速度快，而且結果更優。