基于組合優(yōu)化算法的6R機(jī)器人逆運(yùn)動(dòng)學(xué)求解

吉陽珍 侯 力 羅 嵐 羅 培 劉旭檳 梁 爽

四川大學(xué)機(jī)械工程學(xué)院，成都，610065

0 引言

工業(yè)機(jī)器人是現(xiàn)代制造業(yè)中一種重要的機(jī)電一體化設(shè)備，已被廣泛應(yīng)用于各個(gè)行業(yè),其中，6R機(jī)器人因具有工作空間大、運(yùn)動(dòng)靈活、非冗余等特點(diǎn)而成為應(yīng)用和研究的首要目標(biāo)[1]。逆運(yùn)動(dòng)學(xué)求解是根據(jù)給定的末端執(zhí)行器位姿(位置和姿態(tài))求取各關(guān)節(jié)變量，其方程組具有高維、非線性的特點(diǎn)，求解復(fù)雜且不易求出[2]。逆解問題是機(jī)器人研究的重點(diǎn)問題之一，是機(jī)器人軌跡規(guī)劃與運(yùn)動(dòng)控制的基礎(chǔ)。

目前6R機(jī)器人逆運(yùn)動(dòng)學(xué)求解的方法主要分為解析法[3]、數(shù)值解法[4]與人工智能法[5-8]。這些方法雖得到了一些成功的應(yīng)用，但仍存在一定的局限性。解析法的缺點(diǎn)是運(yùn)算量大，涉及符號(hào)運(yùn)算，求解過程復(fù)雜，對(duì)工程人員的數(shù)學(xué)基礎(chǔ)有很高的要求，且受限于機(jī)器人構(gòu)型，不具備通用性，只有在其幾何結(jié)構(gòu)滿足Pieper準(zhǔn)則[9]時(shí)才可求得封閉解。數(shù)值解法對(duì)機(jī)器人構(gòu)型沒有特別要求，通用性很高，其缺點(diǎn)是對(duì)初始條件敏感，若迭代初值設(shè)置不合理，可能會(huì)出現(xiàn)無解的情況。智能算法應(yīng)用于解決逆解問題時(shí)存在計(jì)算成本高、隨機(jī)性強(qiáng)、精度達(dá)不到要求等不足，在應(yīng)用中受到了很大的限制，如神經(jīng)網(wǎng)絡(luò)算法需要大量已知樣本進(jìn)行訓(xùn)練，泛化能力較弱，而遺傳算法實(shí)現(xiàn)過程復(fù)雜，求解效率低。

相比其他元啟發(fā)式算法，鯨魚優(yōu)化算法[10](whale optimization algorithm,WOA)具有原理簡單、參數(shù)設(shè)置少、易于編程實(shí)現(xiàn)、不需要梯度信息以及尋優(yōu)性能強(qiáng)等[11]優(yōu)點(diǎn)，已被成功應(yīng)用于求解多種優(yōu)化問題[12-13]。目前，還沒有學(xué)者將鯨魚算法與機(jī)器人逆運(yùn)動(dòng)學(xué)求解相結(jié)合進(jìn)行研究。

由于利用智能優(yōu)化算法求得的逆解本質(zhì)上也屬于數(shù)值解，故本文將改進(jìn)鯨魚算法與Newton-Raphson數(shù)值迭代法結(jié)合得到一種通用的組合優(yōu)化算法(combinatorial optimization algorithm，COA) ，并應(yīng)用于6R工業(yè)機(jī)器人的逆解問題，最后在MATLAB環(huán)境下開展了4組仿真試驗(yàn)，測試了改進(jìn)鯨魚算法和組合算法求逆解的性能。

1 機(jī)器人逆運(yùn)動(dòng)學(xué)分析

1.1 機(jī)器人模型

本文以IRB 4600-40/2.55型工業(yè)機(jī)器人為主要研究對(duì)象，它是符合Pieper準(zhǔn)則的六自由度關(guān)節(jié)型機(jī)械臂。根據(jù)表1所示的機(jī)器人關(guān)節(jié)參數(shù)，采用標(biāo)準(zhǔn)D -H參數(shù)法[14]建立的連桿坐標(biāo)系如圖1所示，其中，Σr(orxryrzr)和Σe(oexeyeze)分別是固連于基座和末端執(zhí)行器的參考坐標(biāo)系。

表1 IRB 4600型機(jī)器人的關(guān)節(jié)參數(shù)

圖1 IRB 4600-40/2.55型機(jī)器人連桿坐標(biāo)系Fig.1 Linkage coordinate system of IRB4600-40/2.55 robot

從Σi-1(第i-1個(gè)坐標(biāo)系)到Σi(第i個(gè)坐標(biāo)系)的通用齊次變換矩陣為

i-1Ti=

(1)

將6個(gè)變換矩陣從左到右依次相乘，即可得到機(jī)器人末端坐標(biāo)系相對(duì)于基坐標(biāo)系的位姿矩陣0T6。對(duì)于給定的機(jī)器人結(jié)構(gòu)，末端位姿T是關(guān)節(jié)變量θ=(θ1,θ2,θ3,θ4,θ5,θ6)的函數(shù)，有

(2)

式(2)即為機(jī)器人的正運(yùn)動(dòng)學(xué)方程。其中，R3×3、p分別為末端執(zhí)行器坐標(biāo)系相對(duì)于基坐標(biāo)系的姿態(tài)矩陣和位置矢量。

1.2 逆運(yùn)動(dòng)學(xué)求解目標(biāo)函數(shù)

組合優(yōu)化算法求機(jī)器人逆運(yùn)動(dòng)學(xué)解的思想是將逆解問題轉(zhuǎn)化為優(yōu)化控制問題來解決，以最小化位姿誤差為優(yōu)化目標(biāo)，其目標(biāo)函數(shù)的構(gòu)造過程如下。

假設(shè)機(jī)械人末端執(zhí)行器的期望位姿矩陣為

(3)

假設(shè)當(dāng)前關(guān)節(jié)角度值為θc，代入式(2)可求出當(dāng)前位姿矩陣:

(4)

為了使末端執(zhí)行器達(dá)到期望位姿，需要使當(dāng)前位姿矩陣與期望位姿矩陣相等，即應(yīng)滿足

‖Tc-Td‖=0

(5)

此時(shí)滿足條件的關(guān)節(jié)角度值即為運(yùn)動(dòng)學(xué)逆解。

各關(guān)節(jié)變量的邊界條件表示為

θil≤θi≤θiui=1,2,…,6

(6)

式中，θiu、θil分別為第i個(gè)關(guān)節(jié)對(duì)應(yīng)關(guān)節(jié)變量的上邊界和下邊界。

考慮工程應(yīng)用實(shí)際，需要限制多解，且末端執(zhí)行器從當(dāng)前位姿運(yùn)動(dòng)到目標(biāo)位姿的過程中，各關(guān)節(jié)轉(zhuǎn)角變化應(yīng)平穩(wěn)連續(xù)，沒有突變。假設(shè)機(jī)器人在連續(xù)軌跡規(guī)劃過程中所經(jīng)過的節(jié)點(diǎn)為Pk(k=1,2,…)，經(jīng)求逆得到該點(diǎn)位姿對(duì)應(yīng)的關(guān)節(jié)矢量值為θk，為確保當(dāng)前解始終與上一組解是最接近的，應(yīng)滿足

min ‖Δθ*‖=‖θk+1-θk‖k=1,2,…

(7)

從而將逆解問題轉(zhuǎn)化為多目標(biāo)約束優(yōu)化問題，則經(jīng)組合算法求得的逆解將會(huì)是滿足多約束條件的唯一解，不再具有隨機(jī)性。優(yōu)化模型如下：

(8)

式(8)即為本文機(jī)器人逆解問題的目標(biāo)函數(shù)，包含兩個(gè)非線性矢量方程和一個(gè)約束方程。

1.3 適應(yīng)度函數(shù)的設(shè)計(jì)

由于上述構(gòu)造的目標(biāo)函數(shù)為復(fù)雜的方程組，不適合直接代入鯨魚算法進(jìn)行計(jì)算，故采用如下方式設(shè)計(jì)一個(gè)求解最小值的適應(yīng)度函數(shù)。

將式(8)中的第一個(gè)非線性矢量方程表示為函數(shù)形式并分解成姿態(tài)誤差函數(shù)f1(θ)和位置誤差函數(shù)f2(θ)：

f1(θ)=‖ΔR3×3‖=‖R3×3,c-R3×3,d‖

(9)

f2(θ)=‖Δp‖=‖pc-pd‖

(10)

式中， ΔR3×3為姿態(tài)誤差，表示機(jī)器人末端執(zhí)行器實(shí)際姿態(tài)與期望姿態(tài)的差值；Δp為位置誤差，表示末端執(zhí)行器實(shí)際位置與期望位置的差值。

將式(8)中的第二個(gè)非線性矢量方程表示為函數(shù)形式：

f3(θ)=‖Δθ*‖=‖θk+1-θk‖

(11)

借鑒線性加權(quán)評(píng)價(jià)函數(shù)法[15]處理多目標(biāo)優(yōu)化問題的思想，分別賦予f1(θ)、f2(θ)和f3(θ)獨(dú)立的權(quán)值系數(shù)α、β和γ，將三者進(jìn)行線性組合。以式(6)作為變量定義域，最終構(gòu)造的適應(yīng)度函數(shù)如下：

F=α‖ΔR3×3‖+β‖Δp‖+γ‖Δθ*‖

(12)

由機(jī)器人變換矩陣式(1)可看出，姿態(tài)矩陣主要由正余弦函數(shù)組成，因此‖ΔR3×3‖的數(shù)量級(jí)為100；由機(jī)器人連桿參數(shù)可知，位置誤差的歐氏范數(shù)數(shù)量級(jí)在103；計(jì)算過程中關(guān)節(jié)轉(zhuǎn)角采用弧度單位制，而相鄰兩次轉(zhuǎn)角之差變化較小，故‖Δθ*‖的數(shù)量級(jí)在100。由此，需通過對(duì)各權(quán)值系數(shù)取不同的值來協(xié)調(diào)求解過程中適應(yīng)度函數(shù)各子項(xiàng)的收斂速度，避免因數(shù)量級(jí)差別過大導(dǎo)致某些子項(xiàng)的變化被忽略而對(duì)計(jì)算造成影響。實(shí)際取值根據(jù)具體的機(jī)器人參數(shù)確定。

2 鯨魚優(yōu)化算法的改進(jìn)

2.1 標(biāo)準(zhǔn)鯨魚優(yōu)化算法

在WOA中，群體中的任一個(gè)體在搜索空間中的位置都代表了優(yōu)化問題的一個(gè)可行解。假設(shè)鯨魚種群數(shù)量為N，第t次迭代時(shí)的種群X(t)={Xi(t),i=1,2,…，N}，Xi(t)為種群中第i個(gè)鯨魚個(gè)體的位置，其3種位置更新方式描述如下。

(1)包圍獵物階段的位置更新公式為

Xi(t+1)=Xbest(t)-AD

(13)

D=|CXbest(t)-Xi(t)|

A=2ar1-a

C=2r2

式中，D為當(dāng)前鯨魚個(gè)體與最優(yōu)位置之間的距離；A、C為系數(shù)變量；Xbest(t)為當(dāng)前群體最優(yōu)位置；r1、r2為[0,1]范圍內(nèi)的隨機(jī)數(shù)；a為收斂因子，按a=2(1-t/T)更新；T為最大迭代次數(shù)。

(2)發(fā)泡網(wǎng)攻擊階段，以相同概率選擇兩種方式更新。①收縮包圍機(jī)制：按式(13)進(jìn)行位置更新。②螺旋更新位置公式為

Xi(t+1)=D′eblcos(2πl(wèi))+Xbest(t)

(14)

D′=|Xbest(t)-Xi(t)|

式中，D′為鯨魚個(gè)體與當(dāng)前最優(yōu)位置之間的距離；b為對(duì)數(shù)螺旋形狀常數(shù)；l為[-1,1]上的隨機(jī)數(shù)。

(3)隨機(jī)搜索階段的位置更新公式為

Xi(t+1)=Xrand(t)-A|CXrand(t)-Xi(t)|

(15)

式中，Xrand(t)為當(dāng)前種群中隨機(jī)一個(gè)鯨魚個(gè)體的位置。

2.2 鯨魚算法的改進(jìn)

針對(duì)鯨魚算法求解高維、非線性、多峰值復(fù)雜優(yōu)化問題時(shí)存在的收斂速度慢、易陷入局部最優(yōu)、收斂精度低等缺點(diǎn)，本文提出了4點(diǎn)改進(jìn)措施，以改善WOA的尋優(yōu)性能，并將經(jīng)混合策略改進(jìn)后的鯨魚算法命名為MSWOA (improved whale optimization algorithm based on mixed strategy)。

2.2.1混沌映射初始化種群

利用混沌映射來初始化種群，增加種群的多樣性和分布均勻性，可在一定程度上改善解的質(zhì)量及算法的尋優(yōu)性能。文獻(xiàn)[16]使用多種混沌映射改進(jìn)鯨魚算法，結(jié)果表明Tent混沌映射對(duì)WOA性能提高的效果最好，故本文采用Tent映射來初始化鯨魚種群的位置，其表達(dá)式如下：

(16)

φn∈[0,1]

2.2.2收斂因子非線性更新策略

WOA中參數(shù)A的值在很大程度上依賴于收斂因子a的變化，a隨迭代次數(shù)的增加線性遞減，不能較好地體現(xiàn)出實(shí)際的尋優(yōu)過程，因此在調(diào)節(jié)全局探索和局部開發(fā)能力方面效果較差[17]。

本文對(duì)收斂因子a采用非線性時(shí)變更新策略，使得a在迭代初期具有較大的衰減速度，有利于算法快速收斂，隨著迭代次數(shù)增加，a的衰減速度逐漸變慢，有利于后期進(jìn)行精細(xì)的局部搜索。保持a的取值范圍不變，更新公式為

(17)

a隨迭代次數(shù)的變化趨勢見圖2(取最大迭代次數(shù)T=500)。

圖2 不同更新策略下a的變化趨勢Fig.2 The change trends of a under differentupdate strategies

2.2.3自適應(yīng)慣性權(quán)重位置調(diào)整

設(shè)計(jì)合理的慣性權(quán)重是避免算法陷入局部最優(yōu)并高效搜索的關(guān)鍵[18]。本文在WOA中引入權(quán)重因子w，并依據(jù)早熟收斂程度和適應(yīng)度對(duì)其值進(jìn)行自適應(yīng)調(diào)整。

設(shè)定鯨魚個(gè)體Xi(t)的適應(yīng)度為fi，分別計(jì)算鯨魚群的平均適應(yīng)度favg，優(yōu)于或等于favg的鯨魚個(gè)體適應(yīng)度平均值favg1，差于favg的個(gè)體適應(yīng)度平均值favg2，依據(jù)favg1、favg2將群體分為3個(gè)子群，分別選擇不同的慣性權(quán)重取值。

(1)fi≤favg1，表示當(dāng)前個(gè)體的適應(yīng)度優(yōu)于本輪迭代中較優(yōu)群體的平均適應(yīng)度，該個(gè)體位置較優(yōu)，此時(shí)應(yīng)減小其慣性權(quán)重，有利于提高局部搜索能力，w取值為

(18)

(2)favg1

(3)fi≥favg2，表示當(dāng)前個(gè)體的適應(yīng)度差于本輪迭代中較差群體的平均適應(yīng)度，該個(gè)體位置不理想，此時(shí)需跳出當(dāng)前區(qū)域，應(yīng)增大慣性權(quán)重，有利于增強(qiáng)全局尋優(yōu)能力，w取最大值，w=wmax。

將權(quán)重因子w引入式(13)～式(15)后，得到新的鯨魚個(gè)體位置更新公式分別為

Xi(t+1)=wXbest(t)-AD

(19)

Xi(t+1)=D′eblcos(2πl(wèi))+wXbest(t)

(20)

Xi(t+1)=wXrand(t)-A|CXrand(t)-Xi(t)|

(21)

2.2.4模擬退火策略

本文將模擬退火策略[19]引入WOA，使得其在搜索時(shí)增加適當(dāng)?shù)碾S機(jī)因素，以一定的概率接受使目標(biāo)函數(shù)變差的解，可以有效地避免陷入局部最優(yōu)，改善全局尋優(yōu)能力,具體操作如下。

在每輪迭代中，當(dāng)WOA求解完成之后，產(chǎn)生一個(gè)新的鯨魚種群用于退火處理。為了區(qū)別于產(chǎn)生初始種群的方法，采用自相關(guān)性較好的Chebyshev混沌序列[20]來初始化新種群，其表達(dá)式為

yn+1=cos(ψarccosyn)yn∈[-1,1]

(22)

式中，ψ為Chebyshev映射的階數(shù)，值取5。

(23)

溫度tc的更新公式為

tc=μtc

(24)

式中，μ為退溫系數(shù)。

3 機(jī)器人逆運(yùn)動(dòng)學(xué)組合優(yōu)化算法

3.1 逆運(yùn)動(dòng)學(xué)數(shù)值迭代算法

本文采用一種基于Newton-Raphson迭代法的數(shù)值解法(簡稱“N-R算法”)[14]，其核心思想是：假設(shè)機(jī)器人位姿與關(guān)節(jié)轉(zhuǎn)角的映射關(guān)系在關(guān)節(jié)轉(zhuǎn)角的空間鄰域內(nèi)是線性的，利用機(jī)器人的雅可比矩陣J(θ)建立機(jī)器人微分運(yùn)動(dòng)矢量和關(guān)節(jié)轉(zhuǎn)角增量之間的關(guān)系，從而構(gòu)造迭代表達(dá)式。對(duì)于迭代過程中可能產(chǎn)生雅可比矩陣奇異的情況，可利用其廣義逆矩陣J*(θn)代替逆矩陣J-1(θn)的方法解決。算法步驟如下：

(1)設(shè)置機(jī)器人的期望末端位姿Td、初始關(guān)節(jié)角θ0、初始化迭代次數(shù)n=0、最大迭代次數(shù)N0以及關(guān)節(jié)角θcn=θc(n)=θ0；設(shè)定迭代收斂條件ε，即末端位置和姿態(tài)誤差的閾值εp、εq。

(2)根據(jù)正運(yùn)動(dòng)學(xué)方程計(jì)算對(duì)應(yīng)于θcn的末端位姿矩陣Tc(θcn)以及雅可比矩陣J(θcn)。

(3)根據(jù)式(25)、式(26)計(jì)算從Tc(θcn)到Td的位置運(yùn)動(dòng)增量Δpn和姿態(tài)運(yùn)動(dòng)增量Δqn：

Δp(θc)=pc-pd

(25)

(26)

(4)判斷迭代條件‖Δpn‖≤εp且‖Δqn‖≤εq是否滿足，若條件滿足或迭代次數(shù)已達(dá)N0則循環(huán)結(jié)束，轉(zhuǎn)步驟(8)；否則，繼續(xù)下面的步驟。

(5)計(jì)算雅可比矩陣J(θcn)的廣義逆矩陣J*(θcn)，根據(jù)下式計(jì)算關(guān)節(jié)角增量：

(27)

(6)按下式計(jì)算下一個(gè)迭代周期的關(guān)節(jié)角：

θc(n+1)=θc(n)+Δθn

(28)

(7)令n←n+1，轉(zhuǎn)步驟(2)。

(8)輸出θc(n)，即為對(duì)應(yīng)于初始關(guān)節(jié)角θ0及期望的末端位姿Td的運(yùn)動(dòng)學(xué)反解。算法結(jié)束。

3.2 逆運(yùn)動(dòng)學(xué)組合優(yōu)化算法

機(jī)器人逆運(yùn)動(dòng)學(xué)問題本質(zhì)上屬于多元多峰函數(shù)最優(yōu)解求取問題，由于其目標(biāo)函數(shù)是帶約束條件的復(fù)雜非線性方程組，若根據(jù)前文設(shè)計(jì)的適應(yīng)度函數(shù)直接利用改進(jìn)鯨魚算法在解空間中進(jìn)行尋優(yōu)搜索，需要較大的種群數(shù)量和較多的迭代次數(shù)才可以收斂到較高質(zhì)量的解，所需的計(jì)算成本將非常高，無法滿足機(jī)器人實(shí)時(shí)控制的要求，又由于鯨魚算法在求解高維、非線性、多峰復(fù)雜問題時(shí)存在收斂精度低的固有缺陷，因此在很大概率上無法搜索到可以直接滿足機(jī)器人控制精度要求的解。

利用N-R算法求逆運(yùn)動(dòng)學(xué)解，優(yōu)勢在于程序簡單且收斂速度快(收斂階數(shù)為2)，缺點(diǎn)在于局部收斂性強(qiáng)，對(duì)迭代初值的要求很高，且不能提供算法收斂的先驗(yàn)條件。

逆運(yùn)動(dòng)學(xué)組合優(yōu)化算法旨在將上述兩種算法的優(yōu)勢有機(jī)結(jié)合，在充分協(xié)調(diào)計(jì)算成本的前提下，先利用MSWOA搜索到一個(gè)相對(duì)接近精確解或具有收斂趨勢的初始解，以此作為N-R算法的迭代初值，再通過少數(shù)幾次迭代后收斂到高精度的運(yùn)動(dòng)學(xué)逆解，極大地縮短求解時(shí)間。組合算法求機(jī)器人逆解的流程描述如下：

(1)輸入機(jī)器人結(jié)構(gòu)參數(shù)；

(2)輸入機(jī)器人末端執(zhí)行器的期望位姿Td；

(3)輸入當(dāng)次求解的參考關(guān)節(jié)矢量θk；

(4)設(shè)置MSWOA及N-R算法的基本參數(shù)；

(5)設(shè)置N-R算法的迭代收斂條件ε；

(6)將適應(yīng)度函數(shù)代入MSWOA求解逆運(yùn)動(dòng)學(xué)初始解；

(7)將上一步得到的初始解賦值給N-R算法的初始關(guān)節(jié)角θ0；

(8)利用N-R算法迭代求解；

(9)輸出θc，逆運(yùn)動(dòng)學(xué)求解結(jié)束。

需說明的是：對(duì)于連續(xù)軌跡起點(diǎn)或單點(diǎn)求逆時(shí)，參考矢量θk可隨機(jī)指定，通?？扇ˇ萲=(0°,0°,0°,0°,0°,0°)，如有其他已知條件，可相應(yīng)選取合適的θk值；N-R算法的迭代收斂條件ε決定了最終解的精度；若要求得某一位姿對(duì)應(yīng)的所有逆解，需指定不同的參考矢量θk并進(jìn)行多次運(yùn)算。

4 試驗(yàn)驗(yàn)證及結(jié)果分析

為檢驗(yàn)改進(jìn)鯨魚算法及組合優(yōu)化算法求運(yùn)動(dòng)學(xué)逆解的性能，在MATLAB仿真環(huán)境下開展了4組試驗(yàn)。其中，試驗(yàn)1用來分析改進(jìn)鯨魚算法的性能；試驗(yàn)2～4用來分析組合優(yōu)化算法的性能。試驗(yàn)的硬件環(huán)境為Inter(R) Core(TM) i7-4800MQ CPU @ 2.70GHz，運(yùn)行內(nèi)存8GB，操作系統(tǒng)為Windows 7(64位)旗艦版，仿真環(huán)境為MATLAB R2017b。

4.1 試驗(yàn)1

本組試驗(yàn)直接利用WOA、基于自適應(yīng)權(quán)重和模擬退火的鯨魚優(yōu)化算法(W-SA-WOA)[21]與MSWOA三種鯨魚算法分別對(duì)兩段連續(xù)軌跡進(jìn)行求逆，以對(duì)比三種算法的收斂速度及求解穩(wěn)定性差異，同時(shí)通過計(jì)算軌跡上各位姿點(diǎn)的姿態(tài)誤差與位置誤差，對(duì)比三種算法在求解精度方面的優(yōu)劣。

三種算法的公共參數(shù)取值如表2所示。經(jīng)預(yù)先多次試驗(yàn)后確定MSWOA的其他參數(shù)最優(yōu)取值為：慣性權(quán)重因子上限wmax=1.6，下限wmin=0.4，模擬退火階段初始溫度tc=100，退溫系數(shù)μ=0.95，常數(shù)b=1。WOA和W-SA-WOA算法的其他參數(shù)按對(duì)應(yīng)參考文獻(xiàn)設(shè)置(W-SA-WOA中，模擬退火初始溫度同取t=100)。

表2 算法對(duì)比試驗(yàn)公共參數(shù)取值

試驗(yàn)所用的兩段連續(xù)軌跡均是利用Robotics Toolbox機(jī)器人工具箱在笛卡兒空間下生成的，操作如下：在機(jī)器人工作空間內(nèi)任取四點(diǎn)A、B、C、D，分別以A、C點(diǎn)作為起點(diǎn)位置，B、D點(diǎn)作為終點(diǎn)位置規(guī)劃運(yùn)動(dòng)軌跡，使用jtraj()函數(shù)生成50個(gè)插值點(diǎn)，形成兩段五次多項(xiàng)式軌跡，如圖3所示。各端點(diǎn)對(duì)應(yīng)的位置矢量及一組整數(shù)關(guān)節(jié)矢量解具體為

pA=(1454.47 mm,-961.222 mm，479.38 mm)T
θA=(-30°,15°,-55°,-20°,45°,120°)
pB=(-1669.65 mm，824.10 mm,-94.34 mm)T
θB=(165°,75°,-15°,-75°,120°,-135°)
pC=(-1356.31 mm，1502.29 mm,471.52 mm)T
θC=(123°,32°,10°,-99°,-48°,76°)
pD=(-963.99 mm,-2132.97 mm,-901.73 mm)T
θD=(-113°,105°,-39°,-172°,63°,108°)

圖3 試驗(yàn)所用兩段五次多項(xiàng)式軌跡Fig.3 The quintic polynomial interpolation trajectoriesused in the tests

適應(yīng)度函數(shù)均采用式(11)的形式，為平衡各子項(xiàng)收斂速度的差異，三個(gè)子項(xiàng)與各自權(quán)值系數(shù)的乘積應(yīng)保持?jǐn)?shù)量級(jí)一致。先取權(quán)值系數(shù)α=1，經(jīng)100組測試計(jì)算得出‖ΔR3×3‖/‖Δp‖的均值為0.000 967，‖ΔR3×3‖/‖Δθ*‖的均值為0.8834，并發(fā)現(xiàn)權(quán)值系數(shù)取值在一定范圍內(nèi)的波動(dòng)不影響算法求解的性能和結(jié)果，因此，為簡化計(jì)算，確定其余兩系數(shù)取值為：β=0.001，γ=1。

由于求解對(duì)象為連續(xù)軌跡，故在計(jì)算過程中，可將上一點(diǎn)的求解結(jié)果作為當(dāng)次插值點(diǎn)計(jì)算時(shí)的參考關(guān)節(jié)矢量θk。為更好地保證各關(guān)節(jié)角度值平穩(wěn)變化，可將每次計(jì)算時(shí)的算法搜索空間作實(shí)時(shí)動(dòng)態(tài)調(diào)整，即以上一點(diǎn)求解得到的關(guān)節(jié)矢量為中心，將各關(guān)節(jié)角度值分別向上和向下擴(kuò)張一定角度范圍Δθk后作為當(dāng)次插值點(diǎn)計(jì)算時(shí)的搜索空間。為方便計(jì)算，設(shè)置統(tǒng)一取值為Δθk=10°，從而每點(diǎn)計(jì)算時(shí)的搜索空間表示為[θki-10°,θki+10°](i=1,2,…，6)。搜索空間的實(shí)時(shí)動(dòng)態(tài)調(diào)整策略極大地縮小了算法搜索范圍，可在很大程度上提高求解效率，同時(shí)使得最終的適應(yīng)度值更小，可在一定程度上提高求解精度。

考慮對(duì)比試驗(yàn)的公平性，對(duì)于軌跡起點(diǎn)，參考關(guān)節(jié)矢量θk統(tǒng)一指定為θAi+15°及θCi+15°，各關(guān)節(jié)角的搜索空間范圍均設(shè)置為[-180°,180°]。采用三種算法分別對(duì)兩段軌跡的50個(gè)位姿點(diǎn)進(jìn)行求逆運(yùn)算，每種算法獨(dú)立重復(fù)計(jì)算5次，每次記為一組。記錄每組求解結(jié)果中各插值點(diǎn)(除起點(diǎn)外)對(duì)應(yīng)的適應(yīng)度值的最優(yōu)值、最差值，并求取49個(gè)適應(yīng)度值的平均值及標(biāo)準(zhǔn)差。由于軌跡起點(diǎn)在求解時(shí)存在的差異性，故將起點(diǎn)適應(yīng)度值單列。其中最優(yōu)值和最差值反映求解的質(zhì)量，平均值反映算法求解的精度，標(biāo)準(zhǔn)差反映算法的魯棒性和穩(wěn)定性。計(jì)算結(jié)果如表3所示。

通過對(duì)比表3結(jié)果可以看出，對(duì)于連續(xù)軌跡的逆運(yùn)動(dòng)學(xué)求解，利用改進(jìn)的鯨魚算法MSWOA在相同的參數(shù)條件下可以收斂到更小的適應(yīng)度值，且均在1以下，而WOA與W-SA-WOA收斂到的適應(yīng)度值較大，存在遠(yuǎn)大于1的情況；在適應(yīng)度平均值上，MSWOA表現(xiàn)最好，均值相對(duì)最小，每組試驗(yàn)均保持在0.3附近，WOA表現(xiàn)最差，均值較大且波動(dòng)大；由適應(yīng)度值標(biāo)準(zhǔn)差可看出，MSWOA求解結(jié)果的離散程度最小，說明其尋優(yōu)穩(wěn)定性最好，魯棒性更好。

為進(jìn)一步對(duì)比三種算法收斂速度的差異，將更具代表性的軌跡起點(diǎn)作為比較對(duì)象，選取所有試驗(yàn)中每種算法起點(diǎn)適應(yīng)度最大值與最小值對(duì)應(yīng)的收斂曲線并根據(jù)軌跡的不同分別繪制于圖4中。

由圖4可以看出，WOA收斂速度較慢，且會(huì)出現(xiàn)適應(yīng)度值較大的情況，這時(shí)可能已經(jīng)出現(xiàn)了“早熟”收斂現(xiàn)象使得算法較早收斂于局部最優(yōu)解；W-SA-WOA雖可以收斂到較小的適應(yīng)度值，但收斂速度較慢，且跳出局部最優(yōu)時(shí)所需的迭代周期較長，如圖中多段長“臺(tái)階”形曲線所示；MSWOA可以收斂到更小的適應(yīng)度值，當(dāng)陷入局部最優(yōu)時(shí)通過少數(shù)幾次迭代便可跳出，且在前期具有較快的收斂速度，在最大迭代次數(shù)的一半附近即可收斂到較小適應(yīng)度值。

為了更加直觀地評(píng)價(jià)三種算法在求解精度方面的優(yōu)劣，將經(jīng)過求逆運(yùn)算的兩軌跡上各插值點(diǎn)的姿態(tài)精度和位置精度進(jìn)行對(duì)比，分別以姿態(tài)誤差ΔR3×3和位置誤差Δp的歐氏范數(shù)‖ΔR3×3‖及‖Δp‖作為誤差評(píng)價(jià)指標(biāo)。由于每種算法的多組求解具有相似性，為方便統(tǒng)計(jì)，對(duì)5次試驗(yàn)中各插值點(diǎn)的姿態(tài)誤差與位置誤差分別求平均值后繪制于同一坐標(biāo)系下，對(duì)比結(jié)果如圖5所示。

表3 三種鯨魚算法求逆性能對(duì)比

圖4 三種鯨魚算法適應(yīng)度值收斂曲線Fig.4 The convergence curves of fitness values ofthe three algorithms

由圖5可看出，直接將三種算法應(yīng)用于機(jī)器人求逆時(shí)，相比之下，MSWOA求得的解整體上具有更小的姿態(tài)誤差及位置誤差，說明其求解精度更高。

圖5 三種鯨魚算法求逆結(jié)果誤差對(duì)比Fig.5 The error comparison of the results of thethree algorithms

通過以上對(duì)比分析可知， MSWOA不僅解決了基本W(wǎng)OA容易出現(xiàn)的“早熟”及局部收斂等現(xiàn)象，同時(shí)比其余兩種算法具有更高的求解精度及更好的魯棒性，且在收斂速度方面優(yōu)勢顯著。

4.2 試驗(yàn)2

由試驗(yàn)1結(jié)果分析可知MSWOA算法具有較快的收斂速度，在相同的迭代次數(shù)下可以收斂到更優(yōu)解，故將其應(yīng)用于逆運(yùn)動(dòng)學(xué)組合優(yōu)化算法。

本組試驗(yàn)利用組合優(yōu)化算法分別對(duì)兩條連續(xù)軌跡進(jìn)行求逆，與試驗(yàn)1的結(jié)果形成對(duì)比，以說明組合算法求逆時(shí)在計(jì)算成本與精度方面的優(yōu)勢。

經(jīng)大量測試發(fā)現(xiàn)組合算法在求逆解方面具有非常高的性能，為充分節(jié)約計(jì)算成本，將本組試驗(yàn)參數(shù)設(shè)置為：MSWOA種群數(shù)量N=30，最大迭代次數(shù)T=30，N-R算法的最大迭代次數(shù)N0=30，迭代收斂條件為ε≤10-8，其余參數(shù)設(shè)置同前。利用組合算法對(duì)兩條軌跡分別進(jìn)行5次求逆運(yùn)算，記錄每組試驗(yàn)中經(jīng)MSWOA計(jì)算后的各插值點(diǎn)適應(yīng)度值，性能指標(biāo)設(shè)置同前；記錄各點(diǎn)經(jīng)N-R算法迭代的次數(shù)，統(tǒng)計(jì)迭代次數(shù)的起點(diǎn)值、最優(yōu)值、最差值及平均值。試驗(yàn)結(jié)果如表4所示。

表4 組合優(yōu)化算法求逆性能分析

由表4結(jié)果可看出，雖然軌跡起點(diǎn)適應(yīng)度值偏大，但經(jīng)N-R算法少數(shù)幾次迭代后仍可收斂到滿足精度要求的運(yùn)動(dòng)學(xué)逆解；其余各點(diǎn)適應(yīng)度最差值均不超過0.41，每組適應(yīng)度平均值均不超過0.25，標(biāo)準(zhǔn)差均小于0.08，這是由于每次經(jīng)組合算法計(jì)算后的逆解具有較高的精度，以其作為下一點(diǎn)計(jì)算時(shí)的參考矢量及搜索空間中點(diǎn)，不易引起誤差的累積，從而使得除起點(diǎn)外其余各插值點(diǎn)的適應(yīng)度值分布較為均勻，具有較高的求解穩(wěn)定性；N-R算法迭代次數(shù)均不超過5，最優(yōu)值為3，各組平均值均在4次左右，收斂速度極快，求解效率高。

為分析組合算法的求解精度，對(duì)兩軌跡各插值點(diǎn)5次試驗(yàn)的姿態(tài)誤差與位置誤差求平均值，結(jié)果繪制于圖6中。同時(shí)統(tǒng)計(jì)四種算法對(duì)兩軌跡所有試驗(yàn)點(diǎn)在全部試驗(yàn)中的姿態(tài)誤差及位置誤差平均值，對(duì)比結(jié)果記錄于表5中。

對(duì)比圖6和表5結(jié)果可以看出，利用組合算法進(jìn)行機(jī)器人逆運(yùn)動(dòng)學(xué)求解時(shí)，姿態(tài)誤差達(dá)到了10-16，位置誤差達(dá)到了10-13mm，均遠(yuǎn)遠(yuǎn)高于單獨(dú)的鯨魚優(yōu)化算法直接進(jìn)行求逆的精度水平，說明組合算法在求解精度方面有著巨大的優(yōu)勢。

圖6 組合算法求逆結(jié)果誤差分析Fig.6 Error analysis of COA

表5 四種算法逆運(yùn)動(dòng)學(xué)求解精度對(duì)比

經(jīng)組合算法計(jì)算得到的連續(xù)軌跡各位姿點(diǎn)的逆運(yùn)動(dòng)學(xué)解曲線見圖7。從圖中可看出，各關(guān)節(jié)角隨軌跡上插值點(diǎn)位置的變化曲線是連續(xù)且光滑的，可以滿足機(jī)器人運(yùn)動(dòng)控制的平穩(wěn)性要求。

圖7 組合算法求逆的關(guān)節(jié)角度變化曲線Fig.7 The change curves of joint angles of COA

4.3 試驗(yàn)3

為檢驗(yàn)組合算法對(duì)機(jī)器人奇異位姿點(diǎn)求逆的有效性，在工作空間內(nèi)選取兩奇異位姿點(diǎn)進(jìn)行試驗(yàn)。兩奇異點(diǎn)對(duì)應(yīng)的一組逆解分別為

θP=(50°,-45°,-12°,60°,0°,-66°)
θQ=(-15°,24.5°,-81.5°,85°,0°,24°)

將其代入robot.jacob0()函數(shù)計(jì)算機(jī)器人雅可比矩陣，秩為5，行列式為0，均為奇異矩陣，所以該兩位姿點(diǎn)為奇異點(diǎn)。參考關(guān)節(jié)矢量均設(shè)置為θk=(0°,0°,0°,0°,0°,0°)，各關(guān)節(jié)角的搜索空間范圍均為[-180°,180°]，其余試驗(yàn)參數(shù)設(shè)置同試驗(yàn)2，利用組合算法分別對(duì)兩點(diǎn)獨(dú)立求解100次，運(yùn)算結(jié)果統(tǒng)計(jì)如表6所示。

表6 組合算法對(duì)奇異點(diǎn)求逆結(jié)果

由表6可看出，組合算法對(duì)奇異點(diǎn)求逆穩(wěn)定性略差，適應(yīng)度平均值和N-R算法迭代次數(shù)平均值偏大，姿態(tài)精度與位置精度雖較連續(xù)軌跡相比稍差，但求解仍是有效的。分析其原因，在于參考關(guān)節(jié)矢量選取不夠合理，MSWOA尋優(yōu)計(jì)算量不足，如果合理選取參考矢量并調(diào)整搜索空間范圍，適當(dāng)增加MSWOA的計(jì)算成本，即增加種群數(shù)量或迭代次數(shù)，各項(xiàng)性能指標(biāo)值將會(huì)有一定的改善，求解成功率也會(huì)有所提高。

4.4 試驗(yàn)4

為檢驗(yàn)組合算法對(duì)不滿足Pieper準(zhǔn)則的一般6R機(jī)器人求逆的有效性，本文將文獻(xiàn)[22]所討論的一般6R機(jī)器人作為對(duì)象開展仿真試驗(yàn)。該機(jī)器人的D-H參數(shù)如表7所示。經(jīng)先驗(yàn)測試，確定適應(yīng)度函數(shù)3個(gè)權(quán)值系數(shù)取值同前。

表7 一般6R機(jī)器人的D-H參數(shù)

將關(guān)節(jié)轉(zhuǎn)角θ=(-14°,-104°,-116°,2°,8°,-78°)對(duì)應(yīng)的末端位姿作為期望位姿，在其他15組逆解中任取5組作為參照，結(jié)果如表8所示，利用每組解分別進(jìn)行100次獨(dú)立計(jì)算并統(tǒng)計(jì)結(jié)果，算法參數(shù)設(shè)置同試驗(yàn)2。

每次試驗(yàn)時(shí)，將該組解的6個(gè)關(guān)節(jié)角度值經(jīng)四舍五入取整后分別增加一個(gè)[-30°,30°]范圍內(nèi)的隨機(jī)角度值后得到的新的關(guān)節(jié)矢量作為θk，各關(guān)節(jié)角搜索空間范圍均為[-180°,180°]。試驗(yàn)結(jié)果如表9所示，其中成功率S定義為

S=z′/z

式中，z為總的試驗(yàn)次數(shù)；z′為算法求解結(jié)果與參照解有效數(shù)字完全一致的試驗(yàn)次數(shù)。

θk產(chǎn)生示例如下：

θk=(-98°,-6°,-180°,37°,54°,-38°)+r×30°

式中，r=(r1,r2,…,r6),r1～r6為[-1,1]上的隨機(jī)數(shù)。

由表9結(jié)果可知，求解成功率均達(dá)到95%以上，說明使用組合算法對(duì)一般6R機(jī)器人的求逆是有效的；每組試驗(yàn)中N-R算法迭代次數(shù)都未超過最大值30，說明所有求解結(jié)果均達(dá)到了收斂條件所要求的精度，對(duì)于未成功的幾次試驗(yàn)，算法也收斂到了其他15組逆解中的一組，僅與參照解不同。各組適應(yīng)度平均值均小于1.5，N-R算法迭代次數(shù)平均值均小于6；若適當(dāng)增加MSWOA的計(jì)算成本或合理選擇參考關(guān)節(jié)矢量θk并縮小搜索空間范圍，理論上求解成功率可繼續(xù)提高。

表8 試驗(yàn)4所用5組逆運(yùn)動(dòng)學(xué)解

表9 組合算法對(duì)一般6R機(jī)器人求逆結(jié)果

5 結(jié)論

(1)提出了一種用于六自由度關(guān)節(jié)型機(jī)器人逆運(yùn)動(dòng)學(xué)求解的組合優(yōu)化算法。從機(jī)器人正運(yùn)動(dòng)學(xué)出發(fā)，結(jié)合工程應(yīng)用實(shí)際，建立了求逆解問題的目標(biāo)函數(shù)，并設(shè)計(jì)了用于組合算法求解的適應(yīng)度函數(shù)。

(2)對(duì)基本鯨魚算法提出了4種改進(jìn)策略，提高了算法的收斂速度、收斂精度及穩(wěn)定性，改善了其全局尋優(yōu)性能；組合算法將智能優(yōu)化算法搜索到的解作為數(shù)值法的初值進(jìn)行迭代繼續(xù)求解，有效地結(jié)合了兩者的優(yōu)勢。

(3)在MATLAB環(huán)境下進(jìn)行了多組仿真試驗(yàn)，結(jié)果表明：逆運(yùn)動(dòng)學(xué)組合算法能夠保證逆解的唯一性，所求關(guān)節(jié)角變化平穩(wěn)；相比于直接利用鯨魚算法搜索機(jī)器人數(shù)值解，組合算法在滿足較高精度的前提下具有更快的求解速度和穩(wěn)定性，且不受機(jī)械臂構(gòu)型限制，能夠有效解決6R機(jī)器人逆運(yùn)動(dòng)學(xué)求解的問題。下一步將在機(jī)器人實(shí)驗(yàn)平臺(tái)上進(jìn)行該算法的實(shí)驗(yàn)研究，并針對(duì)實(shí)際的機(jī)器人系統(tǒng)開展軌跡規(guī)劃的應(yīng)用研究。