基于改進(jìn)自適應(yīng)小生境遺傳算法的機(jī)械臂逆運(yùn)動(dòng)學(xué)求解

楊惠珍, 劉西洋

(1.西北工業(yè)大學(xué) 航海學(xué)院, 陜西 西安 710072; 2.水下信息與控制重點(diǎn)實(shí)驗(yàn)室, 陜西 西安 710072)

機(jī)械臂軌跡規(guī)劃和位姿控制的關(guān)鍵問題之一是逆運(yùn)動(dòng)學(xué)求解,即由機(jī)械臂末端抓手位姿矩陣求所需關(guān)節(jié)角矩陣。逆運(yùn)動(dòng)學(xué)是笛卡爾空間到關(guān)節(jié)空間的一種復(fù)雜非線性映射關(guān)系,通常具有多個(gè)解,如何從解集中選取出所需的最優(yōu)解也是機(jī)械臂軌跡和位姿控制需要解決的難點(diǎn)之一。

機(jī)械臂逆運(yùn)動(dòng)學(xué)求解的傳統(tǒng)方法包括:解析法,幾何法和數(shù)值法等[1]。針對(duì)不同款六自由度工業(yè)機(jī)械臂,Kucuk等[2]將數(shù)值法與解析法結(jié)合,求得了封閉解;Liu等[3]提出矩陣分塊化簡(jiǎn)的解析法,解決了逆運(yùn)動(dòng)學(xué)的奇異性問題,并以PUMA560為對(duì)象進(jìn)行了驗(yàn)證;對(duì)于具有冗余關(guān)節(jié)的機(jī)械臂,Pfurner[4]利用數(shù)值解法求得了完整封閉解;魏延輝等[5]采用構(gòu)形平面法將機(jī)械臂工作構(gòu)形進(jìn)行位置和姿態(tài)匹配,然后采用半解析的方式求得逆運(yùn)動(dòng)學(xué)解,解決了奇異解問題;徐文福等[6]提出了逆運(yùn)動(dòng)學(xué)求解的關(guān)節(jié)角參數(shù)化及臂型角參數(shù)化2種方法,求得了解析解,并通過仿真算例對(duì)所提算法進(jìn)行了有效性校驗(yàn)。T?rdal等[7]采用共形幾何代數(shù)法求得了其解析解,為逆運(yùn)動(dòng)學(xué)求解提供了一種新方法。

在數(shù)值法研究方面,有學(xué)者利用神經(jīng)網(wǎng)絡(luò)算法來求解機(jī)械臂的逆運(yùn)動(dòng)學(xué)問題。Almusawi等[8]采用人工神經(jīng)網(wǎng)絡(luò)算法,K?ker[9]采用模擬退火算法優(yōu)化的神經(jīng)網(wǎng)絡(luò)算法,Yuan等[10]提出誤差逆?zhèn)鞑?BP)與徑向基(RBF)并行的神經(jīng)網(wǎng)絡(luò)算法,解決機(jī)械臂求逆問題。神經(jīng)網(wǎng)絡(luò)算法通過正運(yùn)動(dòng)學(xué)獲取訓(xùn)練及測(cè)試數(shù)據(jù),將位姿矩陣和關(guān)節(jié)角矩陣分別作為網(wǎng)絡(luò)的輸入和輸出,建立兩者的映射關(guān)系,完成求逆運(yùn)算。然而上述文獻(xiàn)中,運(yùn)用神經(jīng)網(wǎng)絡(luò)建立逆運(yùn)動(dòng)學(xué)模型時(shí)對(duì)訓(xùn)練樣本進(jìn)行了限制,使其成為單解問題,因此可以得到收斂的最優(yōu)解。機(jī)械臂運(yùn)動(dòng)學(xué)是笛卡爾空間與關(guān)節(jié)空間之間的非一對(duì)一映射關(guān)系,訓(xùn)練樣本應(yīng)盡可能代表機(jī)械臂工作空間。利用神經(jīng)網(wǎng)絡(luò)算法無法對(duì)這種非一對(duì)一的映射關(guān)系進(jìn)行準(zhǔn)確建模,求解逆運(yùn)動(dòng)學(xué)時(shí)存在缺陷。

逆運(yùn)動(dòng)學(xué)是一個(gè)多元多峰函數(shù)最值求取問題,遺傳算法(genetic algorithm,GA)等智能優(yōu)化算法也可用于求解該類問題。?avdar[11]提出了基于人工蜂群的逆運(yùn)動(dòng)學(xué)求解算法,并與粒子群算法,和諧搜索算法進(jìn)行了對(duì)比分析。Huang等[12]基于混合Taguchi DNA群體智能算法對(duì)逆運(yùn)動(dòng)學(xué)進(jìn)行了求解,并與GA進(jìn)行了精度對(duì)比。Tabandeh等[13]通過自適應(yīng)小生境遺傳算法(adaptive niched genetic algorithms,ANGA)對(duì)逆運(yùn)動(dòng)學(xué)問題進(jìn)行了分析,求得了六自由度機(jī)械臂的8組逆解。智能優(yōu)化算法在解決逆運(yùn)動(dòng)學(xué)問題時(shí)以最小化位姿誤差為目標(biāo),對(duì)機(jī)械臂構(gòu)型無特殊要求,通用性強(qiáng)。

本文對(duì)基于ANGA的逆運(yùn)動(dòng)學(xué)求解方法進(jìn)行了研究,主要做了以下幾點(diǎn)改進(jìn):(1)提出了融合位姿誤差和“最柔順”原則的適應(yīng)度函數(shù),求得了最柔順即能耗最小的最優(yōu)解。(2)引入減法聚類算法解決ANGA需準(zhǔn)確已知逆解個(gè)數(shù)的限制,提升了算法通用性。(3)遺傳算法的選擇算子采用最優(yōu)保存策略,交叉操作基于君主方案,進(jìn)行多點(diǎn)非均勻變異,提升算法的收斂速度及精度。

1 機(jī)械臂運(yùn)動(dòng)學(xué)求解問題

根據(jù)Denavit和Hartenberg提出的D-H法,連桿坐標(biāo)系i和坐標(biāo)系i-1之間的關(guān)系由以下4個(gè)變量進(jìn)行描述:關(guān)節(jié)角θi、連桿扭角αi-1、連桿偏移di、連桿長(zhǎng)度ai-1,可得連桿坐標(biāo)系和坐標(biāo)系之間的齊次變換矩陣如(1)式所示

(1)

式中,c代表角度余弦,s代表角度正弦。θi,αi-1,di,ai-1即D-H參數(shù),由機(jī)械臂的結(jié)構(gòu)決定。

對(duì)于n自由度的機(jī)械臂來說,正運(yùn)動(dòng)學(xué)是由關(guān)節(jié)角矩陣求位姿矩陣0Tn的過程,如(2)式所示。

(2)

2 遺傳算法適應(yīng)度函數(shù)

適應(yīng)度值是GA 選擇優(yōu)良個(gè)體的衡量標(biāo)準(zhǔn),被選中個(gè)體完成后續(xù)的交叉、變異操作。適應(yīng)度函數(shù)常由目標(biāo)函數(shù)變換而來,用以計(jì)算個(gè)體的適應(yīng)度值。

2.1 基本適應(yīng)度函數(shù)

為了保證機(jī)械臂達(dá)到期望位姿的誤差最小,目標(biāo)函數(shù)選為個(gè)體位姿與期望位姿之間的誤差

f(P,O)=min(ωpPe+ωoOe)

(3)

式中,Pe表示位置誤差,Oe表示姿態(tài)誤差。位置誤差用歐氏距離來計(jì)算

(4)

姿態(tài)誤差用橫滾,俯仰和偏轉(zhuǎn)角RPY各角度差值的絕對(duì)值之和來表示

Oe=|(R-φ)|+|(P-θ)|+|(Y-φ)|

(5)

式中：R,P,Y分別代表期望點(diǎn)的橫滾角、俯仰角和偏轉(zhuǎn)角；φ,θ,φ代表GA中個(gè)體的橫滾角、俯仰角和偏轉(zhuǎn)角。由于位置與姿態(tài)誤差描述所采用的單位不同,因此在兩者疊加時(shí)需進(jìn)行歸一化操作,歸一化系數(shù)ωp和ωo如(6)～(7)式所示

(6)

(7)

式中：Pmax為機(jī)械臂所能達(dá)到的最大工作距離；θmax和θmin分別為關(guān)節(jié)工作范圍的最大最小值。

對(duì)于逆運(yùn)動(dòng)學(xué)求解,(3)式所得結(jié)果越小,代表該個(gè)體具有更高的求解精度,其更應(yīng)被遺傳算法選中。因此,適應(yīng)度函數(shù)定義為

(8)

2.2 融合“最柔順”原則的適應(yīng)度函數(shù)

“最柔順”原則即機(jī)械臂從當(dāng)前位姿點(diǎn)到期望點(diǎn)之間各關(guān)節(jié)轉(zhuǎn)角之和盡量小,大臂運(yùn)動(dòng)盡量少,因此建立如下的目標(biāo)函數(shù)

(9)

將最柔順目標(biāo)與位姿目標(biāo)相結(jié)合,構(gòu)建目標(biāo)函數(shù)及適應(yīng)度函數(shù)分別如下所示

f(P,O,θ)=γf(P,O)+(1-γ)f(θ)

(10)

γ=0.5·e-f(P,O)+0.5

(11)

(12)

γ是位、姿誤差動(dòng)態(tài)權(quán)重值,用于調(diào)節(jié)進(jìn)化方向。遺傳進(jìn)化開始時(shí),f(P,O)較大,γ接近于0.5。隨著個(gè)體的進(jìn)化,f(P,O)逐漸減小,γ向1靠近。適應(yīng)度函數(shù)如此設(shè)置,可使f(P,O)與f(θ)在進(jìn)化過程中占有合適的權(quán)重,使得進(jìn)化方向更加合理。

3 基于改進(jìn)自適應(yīng)小生境遺傳算法的逆運(yùn)動(dòng)學(xué)求解

3.1 自適應(yīng)小生境遺傳算法

小生境遺傳算法(niched genetic algorithms,NGA)可以更好地保持解的多樣性,同時(shí)具有更好的全局尋優(yōu)能力和收斂速度,特別適合于復(fù)雜多峰函數(shù)的最值求取問題[14]。NGA在解決多峰函數(shù)的最值求取問題時(shí),需準(zhǔn)確設(shè)定小生境半徑dmin,對(duì)逆運(yùn)動(dòng)學(xué)求解而言,dmin無法準(zhǔn)確獲得。由Goldberg和Wang提出的ANGA基于經(jīng)濟(jì)學(xué)的壟斷競(jìng)爭(zhēng)模型[15],將個(gè)體分為商家和顧客兩大群體,商家以最大化小生境半徑范圍內(nèi)顧客數(shù)量且更接近其他商家為目標(biāo),顧客以最接近商家而又最不擁擠為目標(biāo),通過兩者的共同進(jìn)化實(shí)現(xiàn)多峰問題求解。ANGA的小生境半徑由個(gè)體間歐氏距離確定,而后逐步更新,并依群體進(jìn)化目標(biāo)自適應(yīng)調(diào)整。對(duì)于逆運(yùn)動(dòng)學(xué)求解來說,商家就是最優(yōu)關(guān)節(jié)角組合的代表,顧客則是遺傳算法迭代操作的個(gè)體。

3.2 自適應(yīng)小生境遺傳算法改進(jìn)

ANGA在求解多峰函數(shù)問題時(shí),需通過先驗(yàn)知識(shí)準(zhǔn)確設(shè)定商家個(gè)數(shù)。由于僅通過正運(yùn)動(dòng)學(xué)無法推知逆解個(gè)數(shù),故引入聚類算法對(duì)ANGA運(yùn)算結(jié)果進(jìn)行聚類分析。本文采用減法聚類算法。減法聚類是根據(jù)周圍數(shù)據(jù)點(diǎn)的密度尋找最佳數(shù)據(jù)點(diǎn)以定義聚類質(zhì)心的方法,通過聚類分析使得利用改進(jìn)ANGA求逆時(shí)無需準(zhǔn)確設(shè)定逆解個(gè)數(shù)。其參數(shù)設(shè)定在下述步驟9中詳細(xì)說明。

遺傳算子的設(shè)定對(duì)GA的收斂速度及搜索能力具有決定性作用。標(biāo)準(zhǔn)遺傳算法的選擇操作基于“輪盤賭”法,依概率對(duì)個(gè)體進(jìn)行選擇,存在一定的隨機(jī)性,會(huì)使部分優(yōu)良個(gè)體缺失,選擇誤差較大。本文選擇算子采用最優(yōu)保存策略,即個(gè)體依適應(yīng)度值高低進(jìn)行排序,選取適應(yīng)度值高的一半個(gè)體作為父代,進(jìn)行交叉、變異操作產(chǎn)生子代,更利于優(yōu)良個(gè)體信息的保持,提升算法的收斂速度及精度。交叉操作采用君主方案,即取最優(yōu)個(gè)體與所有偶數(shù)位個(gè)體進(jìn)行交叉操作產(chǎn)生新個(gè)體,使得新個(gè)體都含有最優(yōu)個(gè)體部分信息,使得進(jìn)化更具方向性;變異操作基于非均勻多點(diǎn)變異,與均勻變異相比能夠增強(qiáng)重點(diǎn)區(qū)域的局部搜索能力,改善標(biāo)準(zhǔn)遺傳算法局部搜索能力差的問題。

3.3 改進(jìn)ANGA算法流程

Step 1 構(gòu)建種群:隨機(jī)產(chǎn)生相互獨(dú)立的商家和顧客種群,每個(gè)個(gè)體都由n個(gè)關(guān)節(jié)角度值組成。

B=[θ1,θ2,…,θn]T

(13)

C=[θ1,θ2,…,θn]T

(14)

式中,B和C分別代表商家和顧客種群,每個(gè)關(guān)節(jié)角度值在其工作范圍內(nèi)隨機(jī)產(chǎn)生。對(duì)于個(gè)體θi=[θi1,θi2,…,θim],用歐式距離來衡量個(gè)體間差距。

dij=‖θi-θj‖i,j=1,2,…,m

(15)

Step 2 計(jì)算小生境半徑(dmin):dmin與適應(yīng)度值及評(píng)定新商家是否更優(yōu)相關(guān),初值常由(17)式計(jì)算而得

(16)

(17)

式中,di為個(gè)體i與其他個(gè)體之間的最小歐式距離,m為個(gè)體總數(shù),k通常取1。對(duì)于滿足dij≤dmin的個(gè)體,將其加入到對(duì)應(yīng)的小生境群體中。

(18)

式中,fC依(8)式或(12)式計(jì)算而得,CB代表商家B所服務(wù)的顧客的集合。簡(jiǎn)而言之,同一小生境范圍內(nèi)的顧客共享適應(yīng)度值。

商家的適應(yīng)度值是其所服務(wù)顧客適應(yīng)度值的總和

(19)

Step 5 交叉、變異操作:交叉操作采用君主方案,從父群體中選取最優(yōu)個(gè)體與其他所有偶數(shù)位的個(gè)體進(jìn)行交叉,產(chǎn)生新群體;完成交叉操作后,進(jìn)行非均勻多點(diǎn)變異操作,產(chǎn)生子群體。

Step 7 更新小生境半徑(dmin):dmin初值取di的平均值可盡量多地包含個(gè)體,保證小生境系統(tǒng)的多樣性。迭代過程中,應(yīng)逐漸減小dmin。隨著dmin的減小,顧客集中區(qū)域的商家仍保有顧客,而分散區(qū)域的商家將被迫尋找顧客集中度更高的區(qū)域,使最終結(jié)果更為接近最優(yōu)解。在每步迭代中,dmin按照(20)式逐步較小

(20)

式中,G為最大迭代步數(shù),Gt為當(dāng)前迭代步數(shù),λ通常取0.2～0.8之間的常數(shù)。

Step 8 終止判定:判定適應(yīng)度值是否小于設(shè)定誤差范圍或迭代次數(shù)是否已滿足要求,若是則退出循環(huán)輸出結(jié)果,否則返回步驟5,繼續(xù)循環(huán)。

Step 9 減法聚類分析:減法聚類將聚類中心數(shù)據(jù)集中的每個(gè)點(diǎn)都視作潛在的聚類中心,根據(jù)周圍數(shù)據(jù)集密度指定聚類中心。對(duì)于M維的數(shù)據(jù)集{x1,x2,…,xn},樣本xi的密度值為

(21)

選取數(shù)據(jù)集中密度值最高的點(diǎn)xc1作為第一個(gè)聚類中心。假定xck為第k次選定的聚類中心,按(22)式對(duì)其他點(diǎn)進(jìn)行密度修正

(22)

選取密度值最大的點(diǎn)為新聚類中心xck+1。一般取Rb=(1.2～1.5)Ra,定義為一個(gè)密度指標(biāo)函數(shù)顯著減小的鄰域。由(23)式判斷xck+1是否滿足聚類中心要求

(23)

若(23)式不成立,則xck+1設(shè)定為聚類中心,通過(22)式尋找新聚類中心;若成立則聚類結(jié)束,輸出結(jié)果。其中0<δ<1,取值越小則聚類數(shù)越多。

4 仿真分析

以經(jīng)典工業(yè)機(jī)械臂PUMA560為例,在Matlab環(huán)境下進(jìn)行以下仿真實(shí)驗(yàn):

仿真實(shí)驗(yàn)一:采用改進(jìn)ANGA,適應(yīng)度函數(shù)取(12)式,求得“最柔順”原則下機(jī)械臂逆運(yùn)動(dòng)學(xué)的最優(yōu)解。

仿真實(shí)驗(yàn)二:采用改進(jìn)ANGA,適應(yīng)度函數(shù)取(8)式,求得機(jī)械臂逆運(yùn)動(dòng)學(xué)的8個(gè)解。

仿真實(shí)驗(yàn)三:適應(yīng)度函數(shù)取(12)式,對(duì)比改進(jìn)ANGA與傳統(tǒng)ANGA在求解速度及精度上的差異。

PUMA560具有6個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié),前三關(guān)節(jié)確定腕部參考點(diǎn)位置,后三關(guān)節(jié)決定腕部方位。其D-H坐標(biāo)系如圖1所示。

由圖1可得表1所示的PUMA560 D-H參數(shù)表。

圖1 PUMA560 D-H坐標(biāo)系

連桿iθi/radαi-1/radai-1/mdi/m 1θ1000 2θ2-1.570 800.149 09 3θ300.43180 4θ4-1.570 80.203 20.433 07 5θ51.570 800 6θ6-1.570 800.562 50

為了體現(xiàn)全部8組解,將其各關(guān)節(jié)工作范圍統(tǒng)一為[-π,π] rad。ANGA算法參數(shù)設(shè)置如表2所示。

表2 算法參數(shù)設(shè)置

[1.344 2,0.030 1,0.481 3,1.370 8,1.048 2,-2.965 5]rad。求得了“最柔順”原則下的最優(yōu)解。

表3 不考慮柔順的逆運(yùn)動(dòng)學(xué)求解結(jié)果 rad

對(duì)應(yīng)的起始臂形及目標(biāo)位姿,所得8組逆解臂形及末端抓手朝向分別如圖2,3所示。以圓心坐標(biāo)為(0.2,0.2,0.4) m,半徑為0.05 m的球體代表目標(biāo)物所處位置,以Xe-Ye-Ze坐標(biāo)系描述目標(biāo)物的RPY運(yùn)動(dòng)姿態(tài)。以X-Y-Z坐標(biāo)系描述末端抓手的RPY運(yùn)動(dòng)姿態(tài)。

對(duì)于求得的8組逆解,由正運(yùn)動(dòng)學(xué)計(jì)算其所對(duì)應(yīng)的抓手位置坐標(biāo)和RPY角,以檢驗(yàn)算法的求解精度。結(jié)果如表4所示。

表4 逆解結(jié)果對(duì)應(yīng)位置及RPY角

圖2 起始臂形及目標(biāo)位姿圖

圖3 逆解臂形及目標(biāo)位姿圖

圖3中a)～h)分別對(duì)應(yīng)表4的第1～8組結(jié)果。由表4可知,本文所提出的逆運(yùn)動(dòng)學(xué)求解算法可求PUMA560固定位姿對(duì)應(yīng)的8組逆解,位置誤差在2 mm以內(nèi),角度誤差在0.2°以內(nèi),具有較高的求解精度。

由上述實(shí)驗(yàn)結(jié)果可以看出,實(shí)驗(yàn)一所得最優(yōu)解與表4所示第1組及圖3a)非常接近,而其在8組數(shù)據(jù)中更滿足“最柔順”原則。因此實(shí)驗(yàn)一所得結(jié)果即為“最柔順”原則下的最優(yōu)解。

仿真實(shí)驗(yàn)三中改進(jìn)ANGA與ANGA的適應(yīng)度函數(shù)均取(12)式,算法參數(shù)設(shè)定及初始條件同表2,對(duì)比適應(yīng)度值變化曲線,結(jié)果如圖4所示。并各進(jìn)行了300次求解,對(duì)迭代結(jié)束時(shí)的最終適應(yīng)度值進(jìn)行了對(duì)比,如圖5所示。

圖4 適應(yīng)度進(jìn)化曲線對(duì)比圖

圖5 多次求解最終適應(yīng)度值對(duì)比圖

由圖4可得,采用改進(jìn)ANGA的適應(yīng)度值進(jìn)化速度遠(yuǎn)高于傳統(tǒng)ANGA。圖5表明,改進(jìn)ANGA在300次求解過程中,幾乎每次的最終適應(yīng)度值都大于傳統(tǒng)ANGA,由(12)式可知,適應(yīng)度值越大表明求解結(jié)果更為精確。因此,改進(jìn)ANGA在求解速度及精度上性能更優(yōu)。

5 結(jié) 論

機(jī)械臂運(yùn)動(dòng)學(xué)是笛卡爾空間與關(guān)節(jié)空間之間的非一對(duì)一映射關(guān)系,逆運(yùn)動(dòng)學(xué)求解屬于多元多峰函數(shù)最優(yōu)解求取問題。在對(duì)逆運(yùn)動(dòng)學(xué)進(jìn)行深入分析后,提出了基于改進(jìn)ANGA的機(jī)械臂求逆算法,算法以目標(biāo)位置及RPY角作為輸入,適用于視覺等傳感器伺服的機(jī)械臂中。融合“最柔順”原則,求得了最優(yōu)解,為機(jī)械臂的軌跡規(guī)劃提供了基礎(chǔ)。仿真結(jié)果表明所提出的改進(jìn)算法具有較高的求解精度,收斂速度快,無機(jī)械臂構(gòu)型限制。下一步將進(jìn)行該算法的實(shí)驗(yàn)研究,針對(duì)實(shí)際機(jī)械臂系統(tǒng)進(jìn)行軌跡規(guī)劃的應(yīng)用研究。