云模型遺傳蟻群算法的機(jī)器人逆運(yùn)動學(xué)求解

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

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

1 引言

運(yùn)動學(xué)逆解[1]即已知機(jī)器人末端在坐標(biāo)空間中的位姿，計(jì)算各關(guān)節(jié)的角度值。機(jī)器人求逆影響到后序的運(yùn)動規(guī)劃、運(yùn)動控制等工作。

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

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

2 逆運(yùn)動學(xué)問題模型的建立

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

圖1 機(jī)器人實(shí)物圖及機(jī)構(gòu)簡圖Fig.1 Robot And Its Schematic Diagram of Mechanism

因?yàn)閴壕o關(guān)節(jié)、俯仰關(guān)節(jié)與回轉(zhuǎn)關(guān)節(jié)三個(gè)關(guān)節(jié)軸未交于一點(diǎn)或者平行，如圖1(b)所示。因此不滿足Pieper準(zhǔn)則[2]。機(jī)器人越障時(shí)，各關(guān)節(jié)坐標(biāo)系按以下方式建立。

固定臂設(shè)置基坐標(biāo)系｛1｝，在俯仰關(guān)節(jié)處設(shè)置坐標(biāo)系｛2｝，旋轉(zhuǎn)關(guān)節(jié)處為坐標(biāo)系｛3｝，如圖2所示。因?yàn)閮杀鄄捎梅磳ΨQ結(jié)構(gòu)，離線臂的坐標(biāo)系設(shè)置同固定臂。控制箱、行走輪機(jī)構(gòu)和夾緊機(jī)構(gòu)等對機(jī)器人的位姿無影響因此不予考慮。

并補(bǔ)充該機(jī)器人各關(guān)節(jié)的上下邊界：

可見機(jī)器人各個(gè)關(guān)節(jié)的邊界差距很大，因此在優(yōu)化過程中各關(guān)節(jié)值采取歸一化的措施。

利用坐標(biāo)變換法對機(jī)器人進(jìn)行運(yùn)動學(xué)建模。對變換矩陣進(jìn)行連乘所得到的矩陣即可表達(dá)機(jī)器人與機(jī)器人基坐標(biāo)系間的變換關(guān)系。

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

圖2 機(jī)器人坐標(biāo)系分布Fig.2 Distribution of Robotic Coordinate System

可將位姿矩陣分為位置函數(shù)與姿態(tài)函數(shù)，因此求逆問題轉(zhuǎn)化為在變量取值范圍內(nèi)求解函數(shù)最值的問題。如下所示在變量域中找到一組θ使得與目標(biāo)的位姿矩陣之間的誤差最小。

式中：Pcur—當(dāng)前關(guān)節(jié)角下得到的機(jī)器人末端的位姿矩陣，而θi—機(jī)器人對應(yīng)的7個(gè)關(guān)節(jié)變量；Pdes—末端理想位姿矩陣，利用二者之差來表示當(dāng)前個(gè)體所對應(yīng)的機(jī)器人末端位姿與理想位姿之間的誤差。式中ΔR3×3—姿態(tài)誤差，取矩陣的譜范數(shù)；ΔP—位置誤差，取向量的模。可按以下方式構(gòu)造尋優(yōu)函數(shù)：

在研究過程中發(fā)現(xiàn)，姿態(tài)誤差與位置誤差之間存在數(shù)量級上的差距，因此引入權(quán)值系數(shù)α=1500和β=1，使姿態(tài)誤差與位置誤差對整體的影響相當(dāng)，X表示關(guān)節(jié)變量。

3 云模型遺傳蟻群算法的實(shí)現(xiàn)

將式(3)作為遺傳算法的適應(yīng)度值函數(shù)。針對遺傳算法存在的“早熟”以及局部搜索能力差的現(xiàn)象。從以下4點(diǎn)進(jìn)行改善。

3.1 適應(yīng)度值尺度變換

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

式中：F′—新的適應(yīng)度值，式中系數(shù)γ—參數(shù)，γ越小產(chǎn)生的新的適應(yīng)度值就會有更大的差別，從而增加了種群的多樣性。又因?yàn)椴捎昧酥笖?shù)變化的方式，因此適當(dāng)調(diào)節(jié)參數(shù)γ便可以取得較好的效果。

3.2 浮點(diǎn)數(shù)編碼及算數(shù)交叉

傳統(tǒng)的遺傳算法，采用的是二進(jìn)制編碼方案[3]。采用浮點(diǎn)數(shù)編碼及算數(shù)交叉的方法以提升進(jìn)化效果。

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

式中：t—第t代個(gè)體，ε—一參數(shù)。這里采用隨機(jī)數(shù)的方法來生成該參數(shù)。即：每一次交叉運(yùn)算，ε采用隨機(jī)生成的方法。

3.3 基于云模型的參數(shù)自適應(yīng)

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

云模型[9]基于模糊集合理論和概率論實(shí)現(xiàn)了概念和數(shù)據(jù)之間轉(zhuǎn)化，由3個(gè)數(shù)字特征描述：

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

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

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

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

交叉概率Pc的生成算法：

變異概率Pm的生成算法：

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

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

3.4 網(wǎng)格劃分策略的連續(xù)域蟻群算法

為了改善遺傳算法局部搜索能力不足的缺點(diǎn)，采用局部搜索性能強(qiáng)的蟻群算法進(jìn)行彌補(bǔ)。通常地，蟻群算法的信息量操作及最優(yōu)解的選擇都是通過離散變量來求解的；而對于連續(xù)變量，其變量分布在一定的區(qū)域內(nèi)，而不是點(diǎn)集。因此采用了網(wǎng)格劃分策略[10]，將連續(xù)問題離散化，實(shí)現(xiàn)對解的進(jìn)一步優(yōu)化。

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

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

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

第j級第i個(gè)節(jié)點(diǎn)的信息量取為τij按式(18)計(jì)算：

式中：ρ—信息素?fù)]發(fā)系數(shù)；Q—信息素強(qiáng)度，它影響了算法的收斂速度；f—目標(biāo)函數(shù)值。

這里動態(tài)調(diào)整的方法，對信息素?fù)]發(fā)系數(shù)進(jìn)行動態(tài)變化，進(jìn)而提高蟻群算法的求解效率。

改進(jìn)后蟻群算法具體實(shí)現(xiàn)步驟如下：

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

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

(3)設(shè)置螞蟻的數(shù)量為num_ant，每只螞蟻選取節(jié)點(diǎn)的依據(jù)為式(17)。

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

(5)若未達(dá)到最大迭代次數(shù)，則跳轉(zhuǎn)至(3)；否則找出τij每列最大的元素所對應(yīng)的行(m1，m2，…，mn)，將其作為優(yōu)良解輸出。

4 算法描述

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

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

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

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

式中：Fitnessi—個(gè)體i對應(yīng)的適應(yīng)度值。

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

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

(5)網(wǎng)格劃分策略的連續(xù)域蟻群算法局部尋優(yōu)。找出每次迭代適應(yīng)度值最高的個(gè)體，該個(gè)體的染色體作為蟻群算法的初始解Xbest。以Xbest為搜索中心，按照每一維的取值范圍依次進(jìn)行N等分，向周圍空間進(jìn)行網(wǎng)格劃分。設(shè)置初始蟻群總數(shù)為num_ant，最大循環(huán)次數(shù)itmax，信息素?fù)]發(fā)系數(shù)ρ，信息素強(qiáng)度Q，每只螞蟻按照隨機(jī)數(shù)方法生成網(wǎng)格空間坐標(biāo)。如上所述以最有個(gè)體為中心進(jìn)行尋優(yōu)。

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

(7)進(jìn)化結(jié)束輸出最優(yōu)解。若遺傳算法不滿足截止條件時(shí)，繼續(xù)進(jìn)行迭代，直至達(dá)到收斂條件，輸出最優(yōu)解，即運(yùn)動學(xué)方程的逆解。

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

5 實(shí)驗(yàn)驗(yàn)證

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

5.1 算法收斂速度分析

本組實(shí)驗(yàn)具體參數(shù)設(shè)置如下。

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

(2)云模型遺傳蟻群算法：浮點(diǎn)數(shù)編碼，算數(shù)交叉，參與計(jì)算的個(gè)體總數(shù)M=200，進(jìn)化代數(shù)取200代，在進(jìn)化代數(shù)達(dá)到80%(160次)時(shí)，采用適應(yīng)度值函數(shù)尺度，變換取參數(shù)γ=0.5。云模型的參數(shù)取值為：

網(wǎng)格蟻群參數(shù)設(shè)置為：

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

5.2 算法收斂精度分析

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

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

經(jīng)過求運(yùn)算，兩算法的姿態(tài)誤差及位置誤差對比如下所示。

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

采用基于云模型的遺傳蟻群算法進(jìn)行跨越越障型機(jī)器人求逆時(shí)，位置精度和姿態(tài)精度均遠(yuǎn)高于遺傳算法，如圖(5)、圖(6)所示。將上述50個(gè)實(shí)驗(yàn)點(diǎn)的平均姿態(tài)誤差和位置誤差統(tǒng)計(jì)與表1中。這里的算法要比標(biāo)準(zhǔn)遺傳算法精度上顯著提升，如表1所示。

表1 對比實(shí)驗(yàn)數(shù)據(jù)分析Tab.1 Data Analysis of Compared Experiments

6 結(jié)論

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

(2)針對傳統(tǒng)的遺傳算法的編碼方式，適應(yīng)度值函數(shù)計(jì)算方式，交叉變異操作存在的不足，分別采用了浮點(diǎn)數(shù)編碼，適應(yīng)度值尺度變化，以及云模型參數(shù)自適應(yīng)的方法實(shí)現(xiàn)優(yōu)化。

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