基于布谷鳥算法和牛頓法組合算法的6R機器人運動學(xué)逆解

趙 瑜

(江蘇信息職業(yè)技術(shù)學(xué)院 基礎(chǔ)課部，江蘇 無錫 214153)

0 引言

精確求解6R工業(yè)機器人逆運動學(xué)一直是機器人領(lǐng)域的難題[1]。目前，主要有三種求解6R機器人逆運動學(xué)的方法：幾何解法、解析法和數(shù)值法。幾何解法具有表達(dá)形象、直觀的優(yōu)點，但對于三維空間內(nèi)機器人的逆運動學(xué)問題，該方法求解會變得更加復(fù)雜，因此想要得到精確解便非常困難，缺乏通用性[2]。對于機器人幾何結(jié)構(gòu)不滿足Pieper準(zhǔn)則時，采用解析法求解將會異常復(fù)雜，無法求解得到封閉形式的逆解。此時，只能采用數(shù)值解法求得近似解[3]。

數(shù)值方法主要分為兩類：直接數(shù)值法求解和間接數(shù)值法求解。直接數(shù)值法求解是根據(jù)機器人運動學(xué)關(guān)系構(gòu)建迭代方程式，結(jié)合數(shù)值計算方法，可迭代求解出近似解。這一類方法中，牛頓迭代法[2，4]及其改進(jìn)算法[5]運用廣泛。而間接數(shù)值法主要通過將機器人運動學(xué)關(guān)系進(jìn)行轉(zhuǎn)化，構(gòu)造成適當(dāng)?shù)哪繕?biāo)函數(shù)，采用智能優(yōu)化算法搜索求解[6-7]。此外，基于神經(jīng)網(wǎng)絡(luò)和訓(xùn)練近似模型的間接方法也有應(yīng)用[8-10]。然而，直接數(shù)值法的迭代求解結(jié)果受初始值的設(shè)定比較敏感，很容易出現(xiàn)無法收斂的情況。而間接數(shù)值法中智能優(yōu)化算法的迭代后期收斂性較差，求解較高精度的解所需迭代次數(shù)較長，神經(jīng)網(wǎng)絡(luò)和近似模型算法訓(xùn)練庫的生成計算量非常大，且模型建立過程復(fù)雜。

由于智能優(yōu)化算法在全局上具有較強的尋優(yōu)能力，牛頓迭代法具有收斂速度快、精度自校正的優(yōu)點。本文提出一種基于智能優(yōu)化算法和改進(jìn)牛頓迭代法的組合算法，用于求解一般6R機器人逆解。組合算法通過對目標(biāo)函數(shù)設(shè)置閾值，采用布谷鳥算法經(jīng)少量迭代求得質(zhì)量較好的初始解，再將該初始解作為初值，使用改進(jìn)牛頓迭代法求取高精度運動學(xué)逆解。最后通過仿真試驗和幾個智能算法求解對比，驗證了該組合算法的有效性。

1 6R機器人運動學(xué)建模

根據(jù)6R機器人連桿和關(guān)節(jié)的位置關(guān)系，從機器人的底座開始，到末端執(zhí)行器為止，將各連桿從小到大依次編號，并將基座定義為0號連桿。對于每個關(guān)節(jié)的編號，規(guī)定對于任意連桿i，其靠近基座方向的那一關(guān)節(jié)編號為i，而遠(yuǎn)離基座方向的那一端為關(guān)節(jié)i+1。據(jù)此，可通過后置D-H法可依次得到6R機器人的連桿參數(shù)，如表1所示。

表1 6R機器人連桿參數(shù)

根據(jù)上表1中的連桿參數(shù)，可以獲得末端執(zhí)行器在基坐標(biāo)系下的表達(dá)式為：

Tend=T1·T2·T3·T4·T5·T6

(1)

上式中：Ti為4×4變換矩陣，表示第i個連桿坐標(biāo)系與其前一個連桿坐標(biāo)系的變換關(guān)系。變換矩陣是基于齊次坐標(biāo)變換法得到，其具體表達(dá)式如下：

Ti=Rz(θi)·Tz(di)·Tx(ai)·Rx(αi)

(2)

其中各坐標(biāo)變換矩陣分別如下所示：

求解機器人逆運動學(xué)就是在已知機器人末端位置Tend的條件下，求解各個關(guān)節(jié)轉(zhuǎn)角的過程。定義6R機器人的廣義速度V，它是一個6維向量，由角速度ω和線速度v共同組成。則有：

(3)

其中：J(θ)是Jacobian矩陣，該矩陣是一個6×6的矩陣。Jacobian矩陣中的行數(shù)表示機器人的自由度數(shù)量，列數(shù)代表機器人的關(guān)節(jié)數(shù)量。定義微分運動矢量D為

(4)

將上式(4)中對θ的微分改為差分的形式有：

D=J(θ)Δθ

(5)

進(jìn)而有：

θn+1=θn+D·J-1(θn)

(6)

其中J-1(θ)為Jacobian矩陣的逆，為了避免J(θ)會出現(xiàn)不滿秩的情況，采用廣義逆矩陣代替。則求解6R機器人逆運動學(xué)可以轉(zhuǎn)化求解以下的優(yōu)化問題：

minf(θ)=‖D·J-1(θ)‖2

(7)

2 組合算法求解

傳統(tǒng)的數(shù)值迭代算法具有收斂速度快的優(yōu)勢，但在求解機器人逆運動學(xué)問題時，此類方法受設(shè)定初值的影響較大，容易出現(xiàn)迭代發(fā)散的情況。而采用群體智能算法求解機器人逆運動學(xué)的主要問題在于迭代后期收斂速度較慢，不能有效滿足機器人設(shè)定精度要求。但是群體智能算法通過設(shè)定一定數(shù)量的種群來求解目標(biāo)函數(shù)，其每次迭代都會產(chǎn)生大量新的解，這樣便容易獲得適合傳統(tǒng)的迭代算法的初始解。

結(jié)合上述兩類算法的優(yōu)點，在求解6R機器人逆運動學(xué)問題時，可以使用群智能算法求解滿足一定精度的關(guān)節(jié)角作為迭代算法的初始值，然后轉(zhuǎn)用牛頓迭代算法迭代求解?；谏鲜鏊枷?，本文采用通過動態(tài)調(diào)整學(xué)習(xí)率的牛頓法結(jié)合布谷鳥算法形成一種組合算法，用于求解6R機器人逆運動學(xué)。組合算法通過設(shè)置閾值，先用布谷鳥算法尋得滿足閾值條件的低精度初始解，然后通具有動態(tài)調(diào)整學(xué)習(xí)率的牛頓法迭代，獲得高精度的逆解。

2.1布谷鳥算法

布谷鳥[11](CS)算法是通過對布谷鳥尋窩產(chǎn)卵這一行為模擬提出的一種群體智能優(yōu)化算法。該算法的理論主要是源于布谷鳥的獨特繁殖方式——寄宿式繁殖，以及萊維隨機游走策略。上述策略可以抽象成為以下規(guī)則：

(1)每個布谷鳥每次只產(chǎn)一個蛋，并將其放入隨機選擇的鳥巢中孵化

(2)隨機選擇一組鳥巢，并將最佳的鳥巢保留至下一代

因此，基于上述規(guī)則的CS算法其數(shù)學(xué)表達(dá)式如下：

(8)

(9)

(10)

(11)

其中：

(12)

σv=1

(13)

在CS算法中，式(8)用于執(zhí)行全局搜索，式(9)用于執(zhí)行局部搜索。

2.2改進(jìn)牛頓法

牛頓法是具有二階收斂速度的尋優(yōu)算法，常用于求解非線性方程組[12]。對于非線性方程F(x)=0，將其近似解記作x0，然后用泰勒公式將F(x)展開，取展開式的線性部分，并令展開式為零，便可得初步求得近似解x1，再在x1處進(jìn)行泰勒展開，如此往復(fù)展開，便可求得滿足設(shè)定精度的方程解xn+1。這一過程的數(shù)學(xué)表達(dá)式為：

(14)

學(xué)習(xí)率(Learningrate)是深度學(xué)習(xí)中重要的超參數(shù)，它決定著目標(biāo)函數(shù)能否收斂到局部最小值并控制著收斂的速率[13]。合適的學(xué)習(xí)率能使代價函數(shù)收斂更快，一般地，動態(tài)調(diào)整學(xué)習(xí)率能使代價函數(shù)更快收斂。常用的學(xué)習(xí)率衰減方法有分段常衰減、指數(shù)衰減、多項式衰減和余弦衰減。

引入上述學(xué)習(xí)率的思想，結(jié)合式(7)，將式(6)改寫為以下的迭代形式：

θn+1=θn+Learnn·f(θn)

(15)

其中Learnn表示第n次迭代時的學(xué)習(xí)率，為了保證逆運動學(xué)的求解的精度，設(shè)計了一種指數(shù)形式學(xué)習(xí)率衰減策略，其表達(dá)如下：

(16)

其中：η0為初始學(xué)習(xí)率，其值為1；ηt為動態(tài)調(diào)整參數(shù)，其值設(shè)為0.001；N為使得迭代至設(shè)定精度所設(shè)定的總步數(shù)。

因此，上述CS和具有動態(tài)學(xué)習(xí)率的牛頓迭代法結(jié)合的6R機器人逆運動學(xué)組合算法的流程圖如圖1：

圖1 組合算法流程圖 圖2 IRB4600機器人

為了更好的地評價組合算法在求解機器人逆運動學(xué)上的性能，設(shè)置布谷鳥算法、鯨魚算法[14](WOA)和量子粒子群算法[15](QPSO)作為對比。

3 逆運動學(xué)求解實例

根據(jù)表1所示連桿參數(shù)，在MATLAB環(huán)境中利用Robotics Toolbox創(chuàng)建IRB4600機器人的仿真模型，如圖2所示。

在IRB4600機器人的工作空間內(nèi)隨機生成一條路徑，每次在該路徑隨機取1個點分別用不同的智能算法計算適應(yīng)度函數(shù)(7)。定義末端位置誤差為

(17)

設(shè)置CS、WOA、QPSO和組合算法的迭代總數(shù)均為3000，種群大小均為50，其中組合算法中閾值ε=0.2。在路徑上隨機選取30個點求解并求其平均值，結(jié)果如表2：

表2 不同算法的迭代結(jié)果

上表的結(jié)果說明，使用CS、WOA和QPSO分別對適應(yīng)度函數(shù)(7)迭代3000步，能使適應(yīng)度的數(shù)值收斂至0.3以下。其中CS和QPSO的收斂結(jié)果相差不大，WOA較他們略好。但這三種算法對應(yīng)末端執(zhí)行器的誤差均在100mm以上，這完全無法滿足機器人使用時的精度要求，這表明僅僅使用智能優(yōu)化算法來求解，存在求解精度低，結(jié)果不可靠的問題。而采用本文所提出的組合算法，在迭代3000步后能使適應(yīng)度函數(shù)的值迭代至10-10數(shù)量級，同時末端執(zhí)行器的精度保證在10-9數(shù)量級，較布谷鳥、鯨魚算法和量子粒子群算法提升明顯。夠滿足工業(yè)應(yīng)用對位置的高精度要求。

現(xiàn)以機器人某一時刻在生成路徑上的位姿為例，其在基坐標(biāo)系下的矩陣為：

分別采用布谷鳥算法、鯨魚算法、量子粒子群算法和組合算法對上述位置所對應(yīng)的適應(yīng)度函數(shù)進(jìn)行求解，圖3至圖6分別為CS、WOA、QPSO和本文所述算法的收斂曲線。

圖3 CS迭代求解適應(yīng)度曲線 圖4 WOA迭代求解適應(yīng)度曲線

圖5 QPSO迭代求解適應(yīng)度曲線 圖6 組合算法迭代求解適應(yīng)度曲線

從上述收斂曲線可以看出，使用各種智能算法迭代，適應(yīng)度曲線在前期快速衰減。其中鯨魚算法和布谷鳥算法在前期的收縮過程中尋優(yōu)能力較強，使目標(biāo)函數(shù)值下降較快。但是，不同的智能算法的收斂精度差異明顯，鯨魚算法能將目標(biāo)函數(shù)(7)的值收斂至0.43左右，量子粒子群算法能使其收斂至0.2左右，由表2的結(jié)果可知兩種算法的效果并不理想。對于布谷鳥算法，其收斂精度較鯨魚算法和量子粒子群算法較好，收斂時的適應(yīng)度函數(shù)值為0.149，根據(jù)表2所示的結(jié)果可以得出，這一數(shù)量級下的適應(yīng)度函數(shù)值還無法滿足末端執(zhí)行器對位置精度的要求，因此單純地采用智能算法求解，效果并不理想。

圖6為組合算法迭代過程中適應(yīng)度函數(shù)曲線?？闯鲈谇捌诓脊萨B迭代過程中會出現(xiàn)適應(yīng)度函數(shù)值較大的情況，但是當(dāng)適應(yīng)度函數(shù)值達(dá)到閾值后，轉(zhuǎn)用牛頓迭代法求解時，其收斂速度較布谷鳥、鯨魚算法和量子粒子群算法相比更快，而且收斂精度更高。為了更加清晰地體現(xiàn)組合算法求解過程中的適應(yīng)度的變化，表3顯示了不同迭代解精度與迭代所用次數(shù)及末端執(zhí)行器的誤差大小。

表3 組合算法求解過程的精度與誤差

根據(jù)表3的結(jié)果可以看出，使用布谷鳥迭代至21次時便已經(jīng)達(dá)到設(shè)置的閾值，此時的末端執(zhí)行器的位置誤差仍然較大，但是當(dāng)組合算法轉(zhuǎn)為改進(jìn)牛頓法迭代后，能夠在70步左右將末端位置誤差減小至1mm一下，此后的迭代過程中，適應(yīng)度函數(shù)值及對應(yīng)末端位置誤差快速減小，最終獲得了高精度逆解。這說明組合算法中采用布谷鳥算法求解能獲得質(zhì)量較好的初值。指數(shù)函數(shù)形式的學(xué)習(xí)率衰減策略能有效地加快牛頓法的迭代精度，從而獲得較高的質(zhì)量的逆解，執(zhí)行器的末端精度可以控制在10-9(mm)數(shù)量級。

4 結(jié)論

為解決6R機器人逆運動學(xué)高精度求解，結(jié)合直接數(shù)值法和間接數(shù)值法各自的優(yōu)點，提出了一種組合算法。組合算法通過設(shè)置一個合適閾值，采用布谷鳥算法求解獲得一個低精度解，并將該低精度解作為牛頓迭代法的初始解，然后采用一種指數(shù)形式的學(xué)習(xí)率衰減策略動態(tài)調(diào)整牛頓法迭代，從而快速求得高精度的機器人逆解。在MATLAB環(huán)境下進(jìn)行了仿真實驗，結(jié)果表明:組合算法求解得到的平均誤差在10-9數(shù)量級，具有較高精度，能夠滿足工業(yè)應(yīng)用的要求。