云模型遺傳蟻群算法的機器人逆運動學求解

顧嘉偉，吳功平，彭向陽，王 銳

(1.武漢大學動力與機械學院，湖北 武漢 430072；2.廣東電網有限責任公司電力科學研究院，廣東 廣州 510080)

1 引言

運動學逆解[1]即已知機器人末端在坐標空間中的位姿，計算各關節的角度值。機器人求逆影響到后序的運動規劃、運動控制等工作。

常見的求逆方法有封閉解法，迭代法和智能算法。又因為機器人的機構設計問題或者裝配時存在誤差，很多時候機器人的三個相鄰關節軸未交于一點或三軸線平行，即不滿足Pieper準則[2]，此時不具有封閉解。而常用的迭代算法因計算量大，且在奇異位置雅可比矩陣行列式為0，無法進行求逆運算。故智能算法具有明顯的優勢。常見的智能算法有遺傳算法，粒子群算法和神經網絡等。其中遺傳算法具有搜索能力強的特點，被廣泛的應用于優化問題當中。文獻[3]采用二次編碼的方法對PUMA560機器人進行求逆，并實現了在冗余機器人上的求逆運算。同時也有改進的遺傳算法，進行機器人求逆。文獻[5]采用多種群遺傳算法求逆，利用遺傳算法中潛在的并行性，來提升求解的精度和收斂速度，并在Comau NJ-220型機器人上進行了驗證。文獻[6]將遺傳算法與粒子群算法相結合，進行求逆優化。但是以上算法，對于非線性、高維問題的解決效果仍然不是十分理想。

針對高壓輸電線跨越越障式巡檢機器人，采用了一種基于云模型的遺傳網格蟻群算法。利用遺傳算法進行全局尋優，利用蟻群算法[7]進行局部搜索。與傳統遺傳算法相比，并采用了適應度值函數尺度變換方法[8]增加種群的多樣性以，使用云模型[9]實現算法中交叉和變異參數的自適應；同時，用基于網格劃分策略的連續域蟻群算法[10]進行局部尋優。從而使得算法局部尋優能力得到提升，從而提高了算法的精度及收斂速度。

2 逆運動學問題模型的建立

以高壓輸電線跨越越障式巡檢機器人為對象開展研究。該機器人實物圖及機構簡圖，如圖1所示。該機器人采用雙臂反對稱設計，行走輪在架空地線上運動，不同于穿越越障式巡檢機器人[11]。其對桿塔的改造很小，遇到懸垂線夾，輔助導軌[12]等障礙物時，需要單臂夾緊地線，另一機械臂離線完成跨越障礙物的動作。

圖1 機器人實物圖及機構簡圖Fig.1 Robot And Its Schematic Diagram of Mechanism

因為壓緊關節、俯仰關節與回轉關節三個關節軸未交于一點或者平行，如圖1(b)所示。因此不滿足Pieper準則[2]。機器人越障時，各關節坐標系按以下方式建立。

固定臂設置基坐標系｛1｝，在俯仰關節處設置坐標系｛2｝，旋轉關節處為坐標系｛3｝，如圖2所示。因為兩臂采用反對稱結構，離線臂的坐標系設置同固定臂。控制箱、行走輪機構和夾緊機構等對機器人的位姿無影響因此不予考慮。

并補充該機器人各關節的上下邊界：

可見機器人各個關節的邊界差距很大，因此在優化過程中各關節值采取歸一化的措施。

利用坐標變換法對機器人進行運動學建模。對變換矩陣進行連乘所得到的矩陣即可表達機器人與機器人基坐標系間的變換關系。

式中：a—末端進入物體的方向；o—從一個位置指向下一個位置的方向，法向量n與之前兩向量滿足右手定則。以上三向量均為單位向量；Px、Py、Pz—末端位置矢量的三個方向上的分量；R3×3—機器人末端坐標系相對于基坐標系的旋轉矩陣(稱姿態矩陣)；P—機器人末端坐標系相對于基坐標系的位置矢量。

圖2 機器人坐標系分布Fig.2 Distribution of Robotic Coordinate System

可將位姿矩陣分為位置函數與姿態函數，因此求逆問題轉化為在變量取值范圍內求解函數最值的問題。如下所示在變量域中找到一組θ使得與目標的位姿矩陣之間的誤差最小。

式中：Pcur—當前關節角下得到的機器人末端的位姿矩陣，而θi—機器人對應的7個關節變量；Pdes—末端理想位姿矩陣，利用二者之差來表示當前個體所對應的機器人末端位姿與理想位姿之間的誤差。式中ΔR3×3—姿態誤差，取矩陣的譜范數；ΔP—位置誤差，取向量的模。可按以下方式構造尋優函數：

在研究過程中發現，姿態誤差與位置誤差之間存在數量級上的差距，因此引入權值系數α=1500和β=1，使姿態誤差與位置誤差對整體的影響相當，X表示關節變量。

3 云模型遺傳蟻群算法的實現

將式(3)作為遺傳算法的適應度值函數。針對遺傳算法存在的“早熟”以及局部搜索能力差的現象。從以下4點進行改善。

3.1 適應度值尺度變換

通常地，遺傳算法采用的是固定的適應度函數，但該策略在算法運行的后期，隨著，種群中個體趨于接近，易造成種群多樣性的下降，進而帶來“早熟”問題。為了避免上述問題，采用了適應度值尺度變換的方法。變換的方法為：指數尺度變換[8]。

式中：F′—新的適應度值，式中系數γ—參數，γ越小產生的新的適應度值就會有更大的差別，從而增加了種群的多樣性。又因為采用了指數變化的方式，因此適當調節參數γ便可以取得較好的效果。

3.2 浮點數編碼及算數交叉

傳統的遺傳算法，采用的是二進制編碼方案[3]。采用浮點數編碼及算數交叉的方法以提升進化效果。

因待優化問題屬于連續域函數優化問題，使用二進制編碼方案時存在著連續變量離散化映射誤差，另外額外的編碼解碼也增加了程序的時間復雜度及空間復雜度。因此采用浮點數編碼方案。即針對解向量的每一維，根據機器人關節的范圍，在該范圍內取隨機數來生成染色體。算數交叉是將兩個體通過線性組合產生出新個體的方法。

式中：t—第t代個體，ε—一參數。這里采用隨機數的方法來生成該參數。即：每一次交叉運算，ε采用隨機生成的方法。

3.3 基于云模型的參數自適應

遺傳算法中交叉和變異概率通常取固定值，隨著迭代的進行易出現“早熟”現象。

云模型[9]基于模糊集合理論和概率論實現了概念和數據之間轉化，由3個數字特征描述：

期望Ex：區域空間上云滴的均值，表示定性概念的值。

熵En：量化了定性概念的不確定性，受自身模糊性和隨機性的影響。同時也是定性概念隨機性度量，反映了概念的云滴的離散程度。

超熵He：量化了熵的不確定性，受熵的隨機性和模糊性的影響。

En和He的改變會影響云滴的形狀。同時云模型滿足正態分布，云滴的中心為Ex，呈對稱分布，具有中間多，兩邊少的特點。引入正向正態云發生器，即：將定性的知識轉化為定量的數據表示，通過數字特征值生成云滴的過程。將個體的平均適應度作為期望，通過對En和He的控制調節交叉概率和變異概率，實現了基于云模型的參數自適應策略。

交叉概率Pc的生成算法：

變異概率Pm的生成算法：

式中：E′n～N(En，He)。fita、fitb—待操作的兩個體的適應度值；fitmax與fitmin—種群中適應度值的最大值和最小值。f′與—當前待操作個體的適應度值和種群中適應度值的平均值。m1-2，n1-2和t1-4—調節系數。通常t1和t2取概率的下限值，t3和t4取概率的上限值。

其中6≤m≤3×Population(Population為種群中個體總數)，5≤n≤15。

3.4 網格劃分策略的連續域蟻群算法

為了改善遺傳算法局部搜索能力不足的缺點，采用局部搜索性能強的蟻群算法進行彌補。通常地，蟻群算法的信息量操作及最優解的選擇都是通過離散變量來求解的；而對于連續變量，其變量分布在一定的區域內，而不是點集。因此采用了網格劃分策略[10]，將連續問題離散化，實現對解的進一步優化。

該算法的思路為：首先估計出所求變量的取值范圍xjlower≤xj≤xjupper(j=1，2，3，…，n)。在變量區域內畫網格，螞蟻只能從空間網格點移動至空間內的另一網格點，并根據各網格點的對應函數值留下不同大小的信息量，以影響之后螞蟻的移動，不斷循環至滿足截至條件。

將遺傳算法迭代中得到的最優個體作為蟻群算法的初始解，以它為中心在變量空間進行網格劃分。每一維變量，按照其取值范圍進行N等分。某只螞蟻在空間中的位置可以表示為(n1，n2，…，nn)ni∈［0，N］，且ni為自然數。螞蟻空間坐標(n1，n2，…，nn)與變量(x1，x2，…，xn)轉換公式為：

將(n1，n2，…，nn)稱為狀態，螞蟻從第1級到第n級之間進行狀態轉移時，其概率，如式(17)所示。

第j級第i個節點的信息量取為τij按式(18)計算：

式中：ρ—信息素揮發系數；Q—信息素強度，它影響了算法的收斂速度；f—目標函數值。

這里動態調整的方法，對信息素揮發系數進行動態變化，進而提高蟻群算法的求解效率。

改進后蟻群算法具體實現步驟如下：

(1)估計出解中各維變量的取值范圍，即：xjlower≤xj≤xjupper(j=1，2，…，n)，并將各變量N等分。

(2)循環次數itc=0，給τij矩陣賦相同的數值，設置ρ，Q以及itmax的初始值。

(3)設置螞蟻的數量為num_ant，每只螞蟻選取節點的依據為式(17)。

(4)按式(18)修改信息量，并使循環次數加1。

(5)若未達到最大迭代次數，則跳轉至(3)；否則找出τij每列最大的元素所對應的行(m1，m2，…，mn)，將其作為優良解輸出。

4 算法描述

該算法將遺傳算法、蟻群算法與云模型相結合，將遺傳算法中每一次進化得到的最優個體送入到蟻群算法中進行進一步尋優。將算法主要分為7個步驟，具體流程及描述如下。

(1)初始化種群。設置初始種群中的個體數M，交叉概率Pc和變異概率Pm。

(2)確定選擇策略。選擇算子參考了“自然選擇”過程，適應度值越高的個體存活下來的概率就越高。

具體而言采用輪盤賭法。其基本思想是：個體被選中的概率與其適應度值大小線性相關，對環境適應度更好的個體，其優良性狀保留到下一代的概率就會更大。設種群的大小為M，個體i的適應度Fitnessi，則個體i被選中的概率Pis為：

式中：Fitnessi—個體i對應的適應度值。

(3)確定交叉算子。由于采用了浮點數編碼方案，因此采用算數交叉的方法。具體方法如上所述，不再贅述，交叉概率滿足式(10)。

(4)確定變異算子。在每次迭代過程中，每個個體染色體上的基因均有一定的概率發生變異，概率滿足式(15)。若發生，則以該基因取值范圍內的隨機數進行替換。

(5)網格劃分策略的連續域蟻群算法局部尋優。找出每次迭代適應度值最高的個體，該個體的染色體作為蟻群算法的初始解Xbest。以Xbest為搜索中心，按照每一維的取值范圍依次進行N等分，向周圍空間進行網格劃分。設置初始蟻群總數為num_ant，最大循環次數itmax，信息素揮發系數ρ，信息素強度Q，每只螞蟻按照隨機數方法生成網格空間坐標。如上所述以最有個體為中心進行尋優。

(6)更新最優個體。當前文所述的最優個體Xbest經過網格蟻群算法后產生最優螞蟻BestAnt，BestAnt經過式(16)轉化后稱為個體X′best，計算兩個個體適應度值函。若Fitness(Xbest)≤Fitness(X′best)則表明經過網格蟻群算法后最優個體發生了改變，表明該算法在Xbest附近找到了比自身性狀更加優良的個體。并用X′best當作當前種群中的最優個體去替換當前種群中的最差個體。反之，若Fitness(Xbest)＞Fitness(X′best)，則最優個體未發生改變，將Xbest替換當前種群中的最差個體。

(7)進化結束輸出最優解。若遺傳算法不滿足截止條件時，繼續進行迭代，直至達到收斂條件，輸出最優解，即運動學方程的逆解。

圖3 基于云模型的遺傳蟻群算法流程圖Fig.3 Flow Chart of The Algorithm Based on Cloud Model Genetic and Ant Colony Algorithm

5 實驗驗證

驗證針對跨越越障式巡檢機器人開展，在MATLAB中進行比較(實驗設備為一般PC機，英特爾core i5處理器1.7 GHz，4 GB內存，Windows 10操作系統)。第一組，已知某一位姿分別采用傳統的遺傳算法和所用算法進行計算，驗證這里算法的收斂速度上的優勢；第二組實驗，利用跨越機器人在跨越輔助導軌時記錄下的各個關節角參數，利用正運動學公式，得到一系列目標位姿。利用基于云模型的遺傳蟻群算法及遺傳算法進行逆運動學求解，說明算法在精度上的提升。

5.1 算法收斂速度分析

本組實驗具體參數設置如下。

(1)遺傳算法：二進制編碼方案，交叉概率Pc=0.6，變異概率Pm=0.2，選擇操作采用輪盤賭法，單點交叉，進化代數為200代。

(2)云模型遺傳蟻群算法：浮點數編碼，算數交叉，參與計算的個體總數M=200，進化代數取200代，在進化代數達到80%(160次)時，采用適應度值函數尺度，變換取參數γ=0.5。云模型的參數取值為：

網格蟻群參數設置為：

圖4 算法收斂速度比較Fig.4 Comparison of Convergence Speed of Two Algorithms

5.2 算法收斂精度分析

圖5 算法的姿態誤差對比Fig.5 Comparison of Postural Error between Two Algorithms

本實驗數據來自機器人跨越輔助導軌時獲取的關節參數，經正運動學公式得到機器人的位姿矩陣。通常地，輔助導軌上的落點距離機器人坐標原點(500～600)mm。采用遺傳算法和基于云模型的遺傳蟻群算法進行求逆運算，比較其姿態精度和位置精度。進化代數取100代，兩算法的收斂條件同取個體適應度值小于等于0.95，其余參數同上一組實驗。

經過求運算，兩算法的姿態誤差及位置誤差對比如下所示。

圖6 算法的位置誤差對比Fig.6 Comparison of Position Error between Two Algorithms

采用基于云模型的遺傳蟻群算法進行跨越越障型機器人求逆時，位置精度和姿態精度均遠高于遺傳算法，如圖(5)、圖(6)所示。將上述50個實驗點的平均姿態誤差和位置誤差統計與表1中。這里的算法要比標準遺傳算法精度上顯著提升，如表1所示。

表1 對比實驗數據分析Tab.1 Data Analysis of Compared Experiments

6 結論

(1)提出了用于跨越越障型巡檢機器人逆運動學求解的基于云模型的遺傳蟻群算法。

(2)針對傳統的遺傳算法的編碼方式，適應度值函數計算方式，交叉變異操作存在的不足，分別采用了浮點數編碼，適應度值尺度變化，以及云模型參數自適應的方法實現優化。

(3)在跨越越障型巡檢機器人上開展對比實驗。結果表明：相比遺傳算法，基于云模型的遺傳蟻群算法改善了原有的局部收斂現象，且運算的精度有了較大的提升。