冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)問題的多策略貪婪蜂群求解

任云暉，賈仙魚

（1.江海職業(yè)技術(shù)學(xué)院，江蘇 揚(yáng)州 225101；2.揚(yáng)州大學(xué)，江蘇 揚(yáng)州 225000）

1 引言

冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)求解是指根據(jù)末端執(zhí)行器在笛卡爾空間的位姿求解相應(yīng)的關(guān)節(jié)空間角［1］，求解過程具有高度非線性、高度耦合性的特點(diǎn)。機(jī)械臂運(yùn)動(dòng)學(xué)逆解是機(jī)械臂進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃和軌跡跟蹤控制的基礎(chǔ)，逆運(yùn)動(dòng)學(xué)求解精度決定了機(jī)械臂對(duì)笛卡爾空間軌跡的跟蹤精度［2］，會(huì)進(jìn)一步影響機(jī)械臂的定位精度和生產(chǎn)質(zhì)量等。

因此，研究機(jī)械臂逆運(yùn)動(dòng)學(xué)求解問題具有重要意義。冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)求解方法可大致分為3類，分別為封閉解法、數(shù)值解法和智能求解法［3］。

封閉解法也稱為幾何法，該方法依據(jù)空間幾何知識(shí)進(jìn)行逆運(yùn)動(dòng)學(xué)求解，具有求解精度高、求解速度快的特點(diǎn)，但是此方法通用性差切只適用于滿足Pieper準(zhǔn)則的機(jī)械臂結(jié)構(gòu)。文獻(xiàn)［4］將幾何法和坐標(biāo)投影法相結(jié)合進(jìn)行逆運(yùn)動(dòng)學(xué)求解，經(jīng)分析得出了逆運(yùn)動(dòng)學(xué)最多8組封閉解的結(jié)論。數(shù)值求解法包括雅可比矩陣法、梯度投影法等。雅可比矩陣法在每一次迭代都要有一次求導(dǎo)和求逆，計(jì)算過程較為復(fù)雜。梯度投影法是利用迭代法求逆運(yùn)動(dòng)學(xué)近似解的過程。文獻(xiàn)［5］以目標(biāo)函數(shù)作為約束對(duì)任務(wù)空間進(jìn)行增廣，并構(gòu)造增廣雅可比矩陣，使用分解計(jì)算策略得到求解結(jié)果。文獻(xiàn)［6］對(duì)比了雅可比矩陣轉(zhuǎn)置法和雅可比矩陣求逆法，并提出了計(jì)算量小、可靠性高的Levenberg-Marquardt逆解法。

智能求解法是通過編碼和適應(yīng)度函數(shù)的設(shè)置將運(yùn)動(dòng)學(xué)逆解問題轉(zhuǎn)化為尋優(yōu)問題，常用方法包括神經(jīng)網(wǎng)絡(luò)、粒子群算法、遺傳算法等。文獻(xiàn)［7］針對(duì)不滿足Pieper準(zhǔn)則的機(jī)械臂，以位置誤差和姿態(tài)誤差作為優(yōu)化目標(biāo)，使用人工蜂群算法進(jìn)行求解，得到了逆運(yùn)動(dòng)學(xué)的最優(yōu)解。文獻(xiàn)［8］將布谷鳥算法和帝國主義算法進(jìn)行混合，混合算法的搜索性能好于任何單一算法，使用混合算法求解機(jī)械臂逆運(yùn)動(dòng)學(xué)問題，得到了較優(yōu)解。啟發(fā)式搜索算法容易陷入局部最優(yōu)，難以搜索空間最優(yōu)解，從而影響逆解質(zhì)量，因此仍有較大研究空間。

這里研究了冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)問題的求解方法，建立了以減小逆解的位置誤差和姿態(tài)誤差為目標(biāo)的優(yōu)化模型，并對(duì)標(biāo)準(zhǔn)蜂群算法進(jìn)行改進(jìn)提出了多策略貪婪蜂群算法，使用多策略貪婪蜂群算法進(jìn)行逆運(yùn)動(dòng)學(xué)求解，實(shí)現(xiàn)了提高求解精度的目的。

2 機(jī)械臂逆運(yùn)動(dòng)學(xué)建模

2.1 機(jī)械臂介紹

這里研究對(duì)象為KUKA LBR iiwa 七自由度機(jī)械臂，其實(shí)物圖及運(yùn)動(dòng)學(xué)示意圖，如圖1所示。圖1（b）中θ1～θ7為機(jī)械臂關(guān)節(jié)轉(zhuǎn)角。

圖1 KUKA LBR iiwa機(jī)械臂Fig.1 KUKA LBR iiwa Manipulator

機(jī)械臂的D-H運(yùn)動(dòng)學(xué)參數(shù)及各關(guān)節(jié)活動(dòng)范圍，如表1所示。

表1 KUKA LBR iiwa機(jī)械臂D-H參數(shù)Tab.1 KUKA LBR iiwa Manipulator D-H Parameters

表中：αi—連桿扭角；di—連桿偏距；ai—連桿長度；Δθi—關(guān)節(jié)角活動(dòng)范圍，也是這里中關(guān)節(jié)角θi的搜索范圍。

2.2 逆運(yùn)動(dòng)學(xué)問題描述與建模

機(jī)械臂逆運(yùn)動(dòng)學(xué)問題描述為：給定機(jī)械臂末端執(zhí)行器位姿(xex，yex，zex，αex，βex，γex)，其中(xex，yex，zex)為末端執(zhí)行器質(zhì)點(diǎn)在極坐標(biāo)系的位置，(αex，βex，γex)為末端執(zhí)行器在極坐標(biāo)系的歐拉角，要求求解出該位姿對(duì)應(yīng)的一個(gè)關(guān)節(jié)空間點(diǎn){θi}，i= 1～7。

基于啟發(fā)式智能算法的求解過程為：算法每更新一個(gè)關(guān)節(jié)空間點(diǎn)，則使用正運(yùn)動(dòng)學(xué)方程求解出對(duì)應(yīng)的末端執(zhí)行器位姿，并計(jì)算末端執(zhí)行器位姿誤差，位姿誤差最小的解即為最優(yōu)解。

由以上求解過程可以看出，仍有兩個(gè)問題需要解決：

（1）正運(yùn)動(dòng)學(xué)方程的建立；

（2）位姿誤差評(píng)價(jià)模型的建立。

基于歐拉角法建立坐標(biāo)軸之間的轉(zhuǎn)換矩陣，坐標(biāo)系之間通過平移使原點(diǎn)重合，并通過3次旋轉(zhuǎn)可以使3個(gè)坐標(biāo)軸重合。根據(jù)上述過程，假設(shè)機(jī)械臂的當(dāng)前關(guān)節(jié)角為{θi}，i= 1～7，則坐標(biāo)系i到坐標(biāo)系i-1的轉(zhuǎn)換矩陣為［9］：

式中：(px，py，pz)—末端執(zhí)行器質(zhì)心在基坐標(biāo)系的實(shí)際位置，末端執(zhí)行器在基坐標(biāo)系的姿態(tài)可以依據(jù)矩陣T07的元素求出，如式（3）所示。

式中：αre、βre、γre—末端執(zhí)行器在基坐標(biāo)系的實(shí)際歐拉角。

根據(jù)給定的末端執(zhí)行器位姿和關(guān)節(jié)空間點(diǎn)求解的末端執(zhí)行器實(shí)際位姿，得到位置和姿態(tài)誤差分別為：

式中：ΔP—末端執(zhí)行器位置誤差；ΔR-姿態(tài)誤差。

使用加權(quán)法將2個(gè)目標(biāo)統(tǒng)一為1個(gè)優(yōu)化目標(biāo)，為：

式中：fobj—優(yōu)化目標(biāo)；wp、wr—位置誤差系數(shù)和姿態(tài)誤差系數(shù)。在式（5）中，位置誤差與連桿長度、連桿偏距相關(guān)，而姿態(tài)誤差由于使用弧度制表示，也與連桿長度相關(guān)，因此位置誤差和姿態(tài)誤差具有一定的相關(guān)性和倍數(shù)關(guān)系。為了平衡位置誤差與姿態(tài)誤差，經(jīng)分析與實(shí)驗(yàn)，參數(shù)設(shè)置為：wp= 1、wr=180 π。

3 多策略貪婪蜂群算法

3.1 人工蜂群算法

在標(biāo)準(zhǔn)人工蜂群算法中，蜜蜂分為引領(lǐng)蜂、跟隨蜂和偵察蜂等3類。算法的實(shí)現(xiàn)大致包括蜜蜂初始化、引領(lǐng)蜂搜索階段、跟隨蜂搜索階段和偵察蜂搜索階段等步驟［10］。

（1）蜜蜂初始化。初始時(shí)刻，所有蜜蜂使用隨機(jī)方式進(jìn)行位置初始化：

式中：i∈[1，N]—蜜蜂編號(hào)；N—蜜蜂總數(shù)；j∈[1，J]—位置維度，J—位置維度；xij—蜜蜂i的初始化位置；rand(0，1)—隨機(jī)數(shù)；

（2）引領(lǐng)蜂搜索階段。所有蜜蜂進(jìn)行位置適應(yīng)度排序，適應(yīng)度靠前的NB個(gè)蜜蜂被設(shè)置為引領(lǐng)蜂，其余蜜蜂設(shè)置為跟隨蜂。引領(lǐng)蜂采用交叉方式進(jìn)行位置更新［11］，即：

（3）跟隨蜂搜索階段。引領(lǐng)蜂位置適應(yīng)度通過搖擺舞傳遞給跟隨蜂，跟隨蜂根據(jù)引領(lǐng)蜂各自適應(yīng)度確定選擇概率，為：

式中：pi—跟隨蜂選擇引領(lǐng)蜂i的概率；引領(lǐng)蜂i的位置適應(yīng)度值；引領(lǐng)蜂i的目標(biāo)函數(shù)。

跟隨蜂選擇完引領(lǐng)蜂后，與引領(lǐng)蜂一起進(jìn)行蜜源搜索，蜜源搜索方式與式（7）一致。

（4）偵察蜂搜索階段。當(dāng)引領(lǐng)蜂和跟隨蜂在某一位置搜索次數(shù)超過閾值，且蜜源適應(yīng)度沒有明顯提高時(shí)，蜜蜂放棄此位置，轉(zhuǎn)化為偵察蜂后進(jìn)行大范圍搜索，方法為：

分析標(biāo)準(zhǔn)人工蜂群算法可知：（1）跟隨蜂依據(jù)式（7）確定對(duì)引領(lǐng)蜂的選擇概率，但是當(dāng)引領(lǐng)蜂中出現(xiàn)適應(yīng)度極大的“超級(jí)蜜蜂”時(shí)，其超強(qiáng)競(jìng)爭(zhēng)力會(huì)給其他蜜蜂產(chǎn)生較大競(jìng)爭(zhēng)壓力，跟隨蜂會(huì)以極大概率選擇“超級(jí)蜜蜂”，蜜蜂多樣性必然遭到嚴(yán)重破壞；（2）跟隨蜂根據(jù)式（7）進(jìn)行蜜源位置更新，更新方式單一且缺少群體經(jīng)驗(yàn)引導(dǎo)，是一種低效的位置更新方法。下文針對(duì)上述兩點(diǎn)分析和缺陷進(jìn)行改進(jìn)。

3.2 選擇策略優(yōu)化

為了削弱適應(yīng)度極大的“超級(jí)蜜蜂”對(duì)算法多樣性的影響，減小不同引領(lǐng)蜂之間的被選概率差值，這里使用基礎(chǔ)概率與適應(yīng)度差值概率求和的方式，將引領(lǐng)蜂i的被選概率pi設(shè)置為：

3.3 多搜索方式-貪婪選擇策略

為了解決跟隨蜂搜索方式單一且缺少群體經(jīng)驗(yàn)引導(dǎo)的問題，參考粒子群算法和差分算法中的更新方法，設(shè)置3種位置更新方法，分別為：

最優(yōu)蜜蜂；xa、xb、xm、xn—從種群中隨機(jī)選擇的蜜蜂。

式（11）中給出的多搜索方式中，第一式為受粒子群算法啟發(fā)的更新方法，第二式和第三式分別類似于差分算法的DE/best/1和DE/best/2的更新方法。

跟隨蜂同時(shí)使用式（11）中的3種方式進(jìn)行位置更新，而后計(jì)算原蜜源位置、3種更新位置的蜜源適應(yīng)度，使用貪婪策略確定更新的蜜源位置。

3.4 基于多策略貪婪蜂群算法的逆運(yùn)動(dòng)學(xué)求解

根據(jù)上述分析及改進(jìn)措施原理，制定基于多策略貪婪蜂群算法的逆運(yùn)動(dòng)學(xué)求解步驟為：

（1）設(shè)置多策略貪婪蜂群算法參數(shù)，包括蜜蜂總數(shù)N、位置維度J、引領(lǐng)蜂數(shù)量NB、單位置搜索次數(shù)閾值；

（2）所有蜜蜂進(jìn)行位置初始化，并對(duì)蜜蜂適應(yīng)度值進(jìn)行排序，適應(yīng)度靠前的NB個(gè)蜜蜂設(shè)置為引領(lǐng)蜂；

（3）跟隨蜂按照式（10）計(jì)算對(duì)引領(lǐng)蜂的選擇概率；

（4）引領(lǐng)蜂與跟隨蜂按照式（11）的3種方式進(jìn)行蜜源搜索，并使用貪婪準(zhǔn)則確定更新蜜源位置；

（5）當(dāng)引領(lǐng)蜂與跟隨蜂在某位置搜索次數(shù)大于閾值次數(shù)，且蜜源適應(yīng)度未得到提高時(shí)，蜜蜂轉(zhuǎn)化為偵察蜂按照式（9）進(jìn)行蜜源搜索；

（6）判斷是否到達(dá)最大迭代次數(shù)，若否則轉(zhuǎn)至（4），若是則輸出最優(yōu)蜜蜂位置，算法結(jié)束。

4 仿真驗(yàn)證與分析

4.1 求解結(jié)果與算法性能分析

實(shí)驗(yàn)的操作系統(tǒng)為Windows 10系統(tǒng)，硬件條件為Intel Core i7-8700K @ 3.7GHz，內(nèi)存大小為16GB。以圖1 的KUKA LBR iiwa 機(jī)械臂為研究對(duì)象，將機(jī)械臂末端執(zhí)行器期望位姿設(shè)置為(400，500，600，π 2，π 4，π 3)。

為了比較算法的收斂精度、收斂速度、性能穩(wěn)定性等參數(shù)，分別使用標(biāo)準(zhǔn)人工蜂群算法、多策略貪婪蜂群算法對(duì)前文建立的優(yōu)化模型進(jìn)行求解，算法的參數(shù)設(shè)置為：蜜蜂總數(shù)N= 50、位置維度J= 7、引領(lǐng)蜂數(shù)量NB= 10、單位置搜索次數(shù)閾值為7。

為了減小隨機(jī)因素影響，兩種算法各自獨(dú)立求解10次，選擇10組中的最優(yōu)解進(jìn)行比較，兩種蜂群算法對(duì)最優(yōu)解的迭代過程，如圖2所示。

圖2 求解迭代過程Fig.2 Solving Iteration Process

由圖2所示結(jié)果可知，多策略貪婪算法迭代至32次時(shí)算法收斂完畢，標(biāo)準(zhǔn)蜂群算法迭代至61次時(shí)算法收斂完畢。

從收斂精度看，多策略貪婪算法收斂值為1.4×10-5，求得的關(guān)節(jié)空間點(diǎn)為（0.1277，0.9453，1.1840，1.3314，-1.4050，-1.1484，1.8500），對(duì)應(yīng)的實(shí)際位姿為（399.9999，500.0001，600.0000，1.5708，0.7854，1.0472），與設(shè)定位姿做差可知，位置誤差僅為10-4mm，姿態(tài)誤差為0；標(biāo)準(zhǔn)蜂群算法收斂值為1.59，逆解對(duì)應(yīng)的實(shí) 際 位 姿 為（399.9416，500.0091，599.9324，1.1780，0.9425，1.0473），與設(shè)定位姿做差可知，位置誤差在接受范圍內(nèi)，但是姿態(tài)誤差較大，無法滿足求解要求。

由上述分析可以看出，多策略貪婪蜂群算法的收斂精度和速度均好于標(biāo)準(zhǔn)蜂群算法，這是因?yàn)楦倪M(jìn)算法中的跟隨蜂使用了多種位置更新策略，并使用貪婪規(guī)則確定更新位置，提高了蜜蜂的局部搜索能力；

另外，改進(jìn)算法有效避免了“超級(jí)蜜蜂”對(duì)算法多樣性的破壞，有效維持了蜜蜂多樣性。因此多策略貪婪風(fēng)趣算法的收斂精度和收斂速度均好于標(biāo)準(zhǔn)蜂群算法。

4.2 多點(diǎn)求解精度測(cè)試

為了驗(yàn)證這里算法對(duì)連續(xù)多點(diǎn)的求解精度，在蝴蝶曲線上按固定步長選取500個(gè)點(diǎn)，蝴蝶曲線表達(dá)式為：

式中：t—時(shí)間。

機(jī)械臂末端執(zhí)行器的質(zhì)心位置即為蝴蝶曲線的采樣位置，末端執(zhí)行器的姿態(tài)規(guī)定為：X軸向與曲線切向一致，Y軸向在蝴蝶曲線平面內(nèi)與切向垂直，Z軸向成右手定則。

依據(jù)多策略貪婪蜂群算法對(duì)上述500個(gè)位姿點(diǎn)求運(yùn)動(dòng)學(xué)逆解，得到對(duì)應(yīng)的關(guān)節(jié)空間點(diǎn)，實(shí)際軌跡與求解位置，如圖3所示。

圖3 標(biāo)準(zhǔn)軌跡與計(jì)算軌跡Fig.3 Standard Trajectory and Computed Trajectory Points

統(tǒng)計(jì)圖3中500個(gè)位姿點(diǎn)的逆解誤差，末端執(zhí)行器質(zhì)心位置的最大誤差為2.3×10-4mm，最小誤差為1.2×10-6mm，平均誤差為1.3×10-5mm；姿態(tài)最大誤差為1.4×10-4rad，最小誤差為2.2×10-5rad，平均誤差為6.7×10-4rad。

由上述數(shù)據(jù)可知，多策略貪婪蜂群算法對(duì)連續(xù)多位姿點(diǎn)的逆運(yùn)動(dòng)學(xué)問題也具有較高的求解精度。以上實(shí)驗(yàn)結(jié)果和分析進(jìn)一步驗(yàn)證了多策略貪婪蜂群算法在機(jī)械臂逆運(yùn)動(dòng)學(xué)求解問題中的優(yōu)越性。

5 結(jié)論

這里研究了冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)的求解問題，建立了以減小逆解的位置誤差和姿態(tài)誤差為目標(biāo)的優(yōu)化模型，并提出了多策略貪婪蜂群算法的模型求解方法。

經(jīng)驗(yàn)證得出以下結(jié)論：

（1）多策略貪婪蜂群算法的收斂精度和收斂速度好于標(biāo)準(zhǔn)人工蜂群算法；

（2）在機(jī)械臂逆運(yùn)動(dòng)學(xué)求解中，多策略貪婪蜂群算法的求解質(zhì)量高于標(biāo)準(zhǔn)蜂群算法；

（3）在連續(xù)多個(gè)位姿點(diǎn)的逆運(yùn)動(dòng)學(xué)求解中，多策略貪婪蜂群算法求解的位置精度和姿態(tài)精度均較高，驗(yàn)證了該方法在逆運(yùn)動(dòng)學(xué)求解中的優(yōu)越性。