一種改進(jìn)的串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)問題數(shù)值解法

馬建偉，閆惠騰，沈亞彬，張紅園，呂 琦，高 松

(大連理工大學(xué) 精密與特種加工教育部重點(diǎn)實(shí)驗(yàn)室，遼寧 大連 116000)

0 引言

串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)問題指根據(jù)給定的末端位姿求解對應(yīng)的各關(guān)節(jié)角度(位移)值，是串聯(lián)機(jī)械臂運(yùn)動(dòng)控制的基礎(chǔ)[1-2]。自從串聯(lián)機(jī)械臂問世以來，其逆運(yùn)動(dòng)學(xué)問題受到廣泛關(guān)注，求解方法主要分為解析法和數(shù)值法兩類。解析法借助串聯(lián)機(jī)械臂的幾何結(jié)構(gòu)和代數(shù)方法，推導(dǎo)逆運(yùn)動(dòng)學(xué)問題解析解，計(jì)算速度快，計(jì)算結(jié)果準(zhǔn)確，但需要串聯(lián)機(jī)械臂幾何構(gòu)型滿足Pieper準(zhǔn)則，即3個(gè)相鄰關(guān)節(jié)軸相交于一點(diǎn)或3個(gè)相鄰關(guān)節(jié)軸相互平行[3]。數(shù)值法通常將機(jī)械臂逆運(yùn)動(dòng)學(xué)問題看作非線性方程組，并基于迭代方法求解，無需串聯(lián)機(jī)械臂幾何構(gòu)型滿足Pieper準(zhǔn)則，收斂結(jié)果由迭代初始條件決定。近年來，這兩類逆運(yùn)動(dòng)學(xué)求解方法的研究也隨著機(jī)械臂的普及而不斷涌現(xiàn)。

對于自由度小于6的串聯(lián)機(jī)械臂和滿足Pieper準(zhǔn)則的6自由度串聯(lián)機(jī)械臂的逆運(yùn)動(dòng)學(xué)問題，求解逆運(yùn)動(dòng)學(xué)問題的過程比較簡單[3]。Calderon等[4]使用解析法在5自由度機(jī)械臂保持末端執(zhí)行器坐標(biāo)系中單軸方向不變的情況下，求解給定位姿對應(yīng)的逆運(yùn)動(dòng)學(xué)解。應(yīng)文杰等[5]基于牛頓下山法建立了3自由度機(jī)械臂的逆運(yùn)動(dòng)數(shù)值求解方法。

對于自由度冗余的串聯(lián)機(jī)械臂和不滿足Pieper準(zhǔn)則的6自由度串聯(lián)機(jī)械臂的逆運(yùn)動(dòng)學(xué)問題，求解過程則相對比較復(fù)雜。在國內(nèi)學(xué)者的研究中，石建平等[6]以機(jī)械臂末端位姿誤差最小為目標(biāo)，將冗余串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)問題轉(zhuǎn)化為一個(gè)等效的最優(yōu)化問題，并提出一種改進(jìn)的粒子群優(yōu)化算法求解，算法具有較高的收斂精度。林陽等[7]將多種群遺傳算法應(yīng)用于不滿足Pieper準(zhǔn)則的一般串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)問題中，提出一種高精度并行計(jì)算的逆運(yùn)動(dòng)學(xué)求解算法，該算法采用動(dòng)態(tài)變異率的變異算子以提高收斂速度，并將位姿分別設(shè)為目標(biāo)函數(shù)以保證二者收斂精度一致。董云等[8]針對平面3自由度冗余串聯(lián)機(jī)械臂的逆運(yùn)動(dòng)學(xué)問題，提出一種解析法和遺傳算法結(jié)合的求解方法，該算法相比單獨(dú)的遺傳算法具有計(jì)算量小和適應(yīng)性強(qiáng)的優(yōu)點(diǎn)。劉松國等[9]提出一種優(yōu)化的一般6R串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)解析解法，將逆運(yùn)動(dòng)學(xué)問題中6個(gè)基礎(chǔ)方程變形處理，并基于符號(hào)運(yùn)算將目標(biāo)矩陣從24階降低到16階，提高了算法的計(jì)算速度和穩(wěn)定性。林明等[10]采用自適應(yīng)的交叉、變異算子，改進(jìn)了排序方法與遺傳算法，以求解PUMA560機(jī)械臂的逆運(yùn)動(dòng)學(xué)問題。在國外學(xué)者的研究中，Husty等[11]利用經(jīng)典多維幾何構(gòu)造方法，將6自由度串聯(lián)機(jī)械臂運(yùn)動(dòng)鏈從中間斷為2個(gè)3自由度運(yùn)動(dòng)鏈，通過分別求解2個(gè)3自由度運(yùn)動(dòng)鏈以及2個(gè)2自由度運(yùn)動(dòng)鏈的逆運(yùn)動(dòng)學(xué)問題，獲得16組能夠使末端執(zhí)行器達(dá)到相同位姿的逆運(yùn)動(dòng)學(xué)解，其中包含復(fù)數(shù)解。Erleben等[12]使用精確的Hessian矩陣代替Levenberg-Marquardt(LM)迭代法中近似的Hessian矩陣，迭代求解機(jī)械臂逆運(yùn)動(dòng)學(xué)問題，通過與擬牛頓法和LM迭代法對比，證明了方法具有較快的收斂速度和較強(qiáng)的收斂能力。Sugihara等[13]基于LM迭代法利用迭代方程殘差的平方模量作為參數(shù)因子，提出了一種具有高數(shù)值穩(wěn)定性和收斂性能的串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)問題數(shù)值解法。綜上，對于求解串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)問題，已有許多學(xué)者提出了解決方法，其中不乏一些優(yōu)秀的數(shù)值解法。然而，由于基于智能算法的數(shù)值解法計(jì)算過程復(fù)雜，計(jì)算量大，迭代法仍是最常見的數(shù)值解法。但是，常用數(shù)值迭代法如牛頓迭代法、共軛梯度法等，收斂速度較慢，且合適的迭代初始值難獲取，致使常規(guī)基于迭代法的數(shù)值解法在實(shí)際計(jì)算過程中，仍存在收斂速度慢、易出現(xiàn)不收斂的問題，影響算法魯棒性。

為解決上述問題，提出了一種改進(jìn)的串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)問題數(shù)值解法。創(chuàng)新將LM迭代法中每一迭代步的參數(shù)因子μ由當(dāng)前迭代步對應(yīng)的微分運(yùn)動(dòng)向量的二范數(shù)確定，并額外設(shè)置步長因子用于以提高每一迭代步的迭代步長。提出的方法顯著提高了算法的收斂速度和收斂能力，有效彌補(bǔ)了常規(guī)的數(shù)值法魯棒性不足的缺點(diǎn)。

1 常規(guī)的數(shù)值解法

以圖1所示的6自由度串聯(lián)機(jī)械臂為例，首先介紹常規(guī)的數(shù)值法求解逆運(yùn)動(dòng)學(xué)問題的計(jì)算過程。按照式(1)計(jì)算6自由度串聯(lián)機(jī)械臂正運(yùn)動(dòng)學(xué)問題。

圖1 串聯(lián)機(jī)械臂

(1)

式中: 關(guān)節(jié)0為基坐標(biāo)系，n為末端關(guān)節(jié)坐標(biāo)系；ηx、ηy、ηz分別為末端關(guān)節(jié)坐標(biāo)系n的xn軸與基坐標(biāo)系的x0、y0、z0軸的夾角余弦值；ox、oy、oz分別為末端關(guān)節(jié)坐標(biāo)系n的yn軸與基坐標(biāo)系的x0、y0、z0軸的夾角余弦值；ax、ay、az分別為末端關(guān)節(jié)坐標(biāo)系n的zn軸與基坐標(biāo)系的x0、y0、z0軸的夾角余弦值；px、py、pz為末端關(guān)節(jié)坐標(biāo)系n原點(diǎn)在基坐標(biāo)系中的笛卡爾坐標(biāo)。

6自由度串聯(lián)機(jī)械臂正運(yùn)動(dòng)學(xué)問題表達(dá)式還可由旋轉(zhuǎn)和平移組合變換表示，即：

0T6=Trans(px,py,pz)Rot(z,rx)Rot(y,ry)Rot(x,rz)=

(2)

式中:crx、cry、crz、srx、sry、srz分別是cosrx、cosry、cosrz、sinrx、sinry、sinrz的簡寫，Trans(px,py,pz)表示末端關(guān)節(jié)坐標(biāo)系沿基坐標(biāo)系的x0、y0、z0軸平移px、py、pz距離，Rot(z,rx)Rot(y,ry)Rot(x,rz)表示末端關(guān)節(jié)坐標(biāo)系先繞著基坐標(biāo)系z軸旋轉(zhuǎn)rz角，再繞基坐標(biāo)系y軸旋轉(zhuǎn)ry角，再繞著基坐標(biāo)系x軸旋轉(zhuǎn)rx角。

聯(lián)立式(1)和(2)，得到各關(guān)節(jié)轉(zhuǎn)角θi(i=1,2,3,4,5,6)與旋轉(zhuǎn)角度rx、ry、rz和平移距離px、py、pz之間的關(guān)系式，即：

(3)

(4)

在迭代計(jì)算過程中，待求解逆運(yùn)動(dòng)學(xué)問題的機(jī)械臂末端位姿記為Tend，末端位姿對應(yīng)的逆運(yùn)動(dòng)學(xué)解記為θend，當(dāng)前迭代步對應(yīng)的逆運(yùn)動(dòng)學(xué)解記為θcur，迭代步逆運(yùn)動(dòng)學(xué)解對應(yīng)的機(jī)械臂末端位姿記為Tcur。上述定義確定后，則按照如下步驟計(jì)算給定末端位姿的逆運(yùn)動(dòng)學(xué)問題。

① 由Tend和Tcur按照式(5)計(jì)算當(dāng)前迭代步微分算子Δcur。

(5)

② 計(jì)算當(dāng)前迭代步逆運(yùn)動(dòng)學(xué)解θcur對應(yīng)的雅可比矩陣J，并按照式(6)計(jì)算當(dāng)前迭代步搜索方向。

(6)

式中：μ為參數(shù)因子，其數(shù)值需預(yù)先設(shè)定，且數(shù)值大小在步驟③中按照迭代結(jié)果自適應(yīng)地改變。引入?yún)?shù)因子μ的目的，為了防止當(dāng)JTJ接近奇異時(shí)dθ過大，從而影響迭代算法的收斂能力。隨后，將θcur與dθ相加得到新的逆運(yùn)動(dòng)學(xué)解θnew，再利用正運(yùn)動(dòng)學(xué)計(jì)算新的逆運(yùn)動(dòng)學(xué)解對應(yīng)的末端位姿Tnew。

③ 由Tend和Tnew按照式(5)計(jì)算新的微分算子Δnew，并從中確定新的微分運(yùn)動(dòng)向量Dnew，并比較當(dāng)前迭代步微分運(yùn)動(dòng)向量Dcur與新的微分運(yùn)動(dòng)向量Dnew的二范數(shù)大小。若||Dnew||2<||Dcur||2，則接受新的逆運(yùn)動(dòng)學(xué)解θnew，令μ=μ/2，θcur=θnew，Dcur=Dnew，返回步驟①進(jìn)行下一次迭代；若||Dnew||2≥||Dcur||2，則拒絕新的逆運(yùn)動(dòng)學(xué)解θnew，令μ=2μ，返回步驟①進(jìn)行下一次迭代。

重復(fù)步驟①～③，直到當(dāng)前迭代步微分運(yùn)動(dòng)向量Dcur的二范數(shù)小于迭代精度e，算法收斂，得到機(jī)械臂末端位姿Tend對應(yīng)的逆運(yùn)動(dòng)學(xué)問題的一個(gè)解θend?；蛘叩螖?shù)超出設(shè)定上限，算法未能在規(guī)定迭代次數(shù)內(nèi)收斂，未能得到滿足迭代精度的機(jī)械臂逆運(yùn)動(dòng)學(xué)解。

2 改進(jìn)的數(shù)值解法

同樣以圖1所示的6自由度串聯(lián)機(jī)械臂為例，提出一種改進(jìn)的串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)問題數(shù)值解法，改進(jìn)的數(shù)值法流程圖見圖2。

圖2 改進(jìn)的數(shù)值法流程框圖

具體計(jì)算過程如下：

① 由Tend和Tcur按照式(5)計(jì)算當(dāng)前迭代步微分算子Δcur，并由微分算子Δcur確定當(dāng)前迭代步微分運(yùn)動(dòng)向量Dcur。判斷當(dāng)前迭代步微分運(yùn)動(dòng)向量Dcur的二范數(shù)是否小于迭代精度e。若||Dcur||2

② 創(chuàng)新將LM迭代法中每一迭代步的參數(shù)因子由當(dāng)前迭代步對應(yīng)的微分運(yùn)動(dòng)向量的二范數(shù)確定，并額外設(shè)置步長因子以提高每一代步的迭代步長，即按照式(7)計(jì)算參數(shù)因子μ。

μ=λ||Dcur||2

(7)

其中，λ為步長因子，是小于1的正參數(shù)，其數(shù)值越小，當(dāng)前迭代步在搜索方向上步長越大，算法收斂速度越快。但λ不能太小，過小的λ反而會(huì)減慢算法的收斂速度。從經(jīng)驗(yàn)角度，λ需根據(jù)具體問題在1×10-3～1×10-2靈活設(shè)定。然后，計(jì)算當(dāng)前迭代步逆運(yùn)動(dòng)學(xué)解θcur對應(yīng)的雅可比矩陣J，并按式(6)計(jì)算當(dāng)前迭代步搜索方向dθ。

③ 將θcur與dθ相加得到新的逆運(yùn)動(dòng)學(xué)解θnew，再利用正運(yùn)動(dòng)學(xué)計(jì)算新的逆運(yùn)動(dòng)學(xué)解對應(yīng)的機(jī)械臂末端位姿Tnew。在改進(jìn)的數(shù)值法中無需通過比較自適應(yīng)地調(diào)整參數(shù)因子μ的值，直接令Tcur=Tnew，θcur=θnew，返回步驟①進(jìn)行下一次迭代。

重復(fù)步驟①～③，直到當(dāng)前迭代步微分運(yùn)動(dòng)向量Dcur的二范數(shù)小于迭代精度e，算法收斂，得到串聯(lián)機(jī)械臂末端位姿Tend對應(yīng)的逆運(yùn)動(dòng)學(xué)問題的一個(gè)解θend。或者迭代次數(shù)超出設(shè)定上限，算法未能在規(guī)定迭代次數(shù)內(nèi)收斂，未能得到滿足迭代精度的逆運(yùn)動(dòng)學(xué)解。需要說明的是，改進(jìn)的串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)問題數(shù)值解法理論上是全局收斂的，但迭代收斂的過程與初始條件有關(guān)，為避免較長的迭代過程，需要設(shè)置迭代次數(shù)上限。

通過對比可以看出，常規(guī)的數(shù)值法和改進(jìn)的數(shù)值法均使用了LM迭代法。不同的是，常規(guī)的數(shù)值法中參數(shù)因子μ按照每一次迭代結(jié)果自適應(yīng)地改變，而改進(jìn)的數(shù)值法基于當(dāng)前迭代步對應(yīng)的微分運(yùn)動(dòng)向量的二范數(shù)確定參數(shù)因子μ，這使得迭代算法的收斂速度和收斂能力顯著提高[14-15]。

3 實(shí)驗(yàn)研究

通過實(shí)驗(yàn)驗(yàn)證改進(jìn)的數(shù)值法的有效性。實(shí)驗(yàn)對象為圖1所示的6自由度串聯(lián)機(jī)械臂，其幾何參數(shù)和關(guān)節(jié)旋轉(zhuǎn)范圍如表1、2所示。實(shí)驗(yàn)是在Intel i5-10400F 2.9 GHz CPU和16Gb RAM的Windows系統(tǒng)下Matlab中進(jìn)行的。

表1 6自由度串聯(lián)機(jī)械臂改進(jìn)DH參數(shù)

表2 6自由度串聯(lián)機(jī)械臂各關(guān)節(jié)旋轉(zhuǎn)范圍

Matlab Robotics Toolbox包含了豐富的機(jī)器人運(yùn)動(dòng)學(xué)庫函數(shù)，其逆運(yùn)動(dòng)學(xué)求解函數(shù)正是基于雅可比偽逆矩陣的原理，使用LM迭代法進(jìn)行求解[16]。按照改進(jìn)的DH參數(shù)法確定的各關(guān)節(jié)連桿幾何參數(shù)，在Matlab中借助robotics工具箱建立6自由度串聯(lián)機(jī)械臂數(shù)學(xué)模型，并由工具箱提供的函數(shù)(fkine函數(shù))計(jì)算機(jī)械臂正運(yùn)動(dòng)學(xué)問題。其中，θ為連桿夾角，d為連桿距離，a為連桿長度，α為連桿扭角。都是機(jī)械臂各關(guān)節(jié)連桿幾何參數(shù)。偏置為建模輔助參數(shù)，代表了改進(jìn)DH參數(shù)法定義的關(guān)節(jié)零位(各關(guān)節(jié)連桿夾角θi均為0)與SR20A機(jī)械臂關(guān)節(jié)零位(各關(guān)節(jié)轉(zhuǎn)角均為0)之間的偏差。

在驗(yàn)證過程中，首先在6自由度串聯(lián)機(jī)械臂工作空間范圍內(nèi)隨機(jī)生成10個(gè)末端位姿，如表3所示。形式滿足式(2)中的旋轉(zhuǎn)和平移組合變換。

表3 隨機(jī)生成的末端位姿

從表4中可以看出，在使用Matlab Robotics Toolbox中ikine函數(shù)的數(shù)值解法計(jì)算10個(gè)機(jī)械臂末端位姿逆運(yùn)動(dòng)學(xué)問題的過程中，在其中5個(gè)機(jī)械臂末端位姿處算法未能在迭代次數(shù)上限內(nèi)，收斂到一個(gè)滿足迭代精度的機(jī)械臂逆運(yùn)動(dòng)學(xué)解，則收斂能力為50%。而在使用改進(jìn)的數(shù)值法計(jì)算10個(gè)機(jī)械臂末端位姿逆運(yùn)動(dòng)學(xué)問題的過程中，在9個(gè)末端位姿處算法均能在迭代次數(shù)上限內(nèi)收斂到一個(gè)滿足迭代精度的逆運(yùn)動(dòng)學(xué)解，則改進(jìn)的數(shù)值法收斂能力為90%。因此，改進(jìn)的數(shù)值法相比于ikine函數(shù)的數(shù)值法，算法的收斂能力提升了1.8倍。

表4 2種數(shù)值法的收斂能力

在2種數(shù)值法均能夠收斂的末端位姿處，對2種數(shù)值法的收斂速度進(jìn)行了比較，結(jié)果如表5所示。在這些末端位姿處，2種算法收斂過程如圖3所示。

表5 2種數(shù)值法的收斂速度

圖3 2種數(shù)值法微分運(yùn)動(dòng)向量二范數(shù)下降速度

從表5中可以看出，在2種數(shù)值法均能夠收斂的6自由度串聯(lián)機(jī)械臂末端位姿處，ikine函數(shù)的數(shù)值法的平均收斂時(shí)間為0.337 3 s，而改進(jìn)的數(shù)值法的平均收斂時(shí)間為0.042 5 s。因此，改進(jìn)的數(shù)值法相比于ikine函數(shù)的數(shù)值法，算法的收斂速度提升了7.9倍。此外，從圖3中也可以直觀地看出，改進(jìn)的數(shù)值法在計(jì)算過程中作為迭代結(jié)束依據(jù)的微分運(yùn)動(dòng)向量二范數(shù)||Dcur||2下降更快，迭代次數(shù)更少，收斂速度提升顯著。

4 結(jié)論

針對常規(guī)的多軸串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)數(shù)值解法存在的收斂速度慢、易出現(xiàn)不收斂等問題，基于LM迭代法提出了一種改進(jìn)的多軸串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)數(shù)值解法。本方法在常規(guī)數(shù)值解法的基礎(chǔ)上，創(chuàng)新將LM迭代法中每一迭代步的參數(shù)因子由當(dāng)前迭代步對應(yīng)的微分運(yùn)動(dòng)向量二范數(shù)確定，進(jìn)一步提升迭代步搜索方向的準(zhǔn)確性，并額外設(shè)置步長因子以提高每一迭代步的迭代步長。

實(shí)驗(yàn)結(jié)果表明，提出的基于當(dāng)前迭代步微分運(yùn)動(dòng)向量二范數(shù)計(jì)算參數(shù)因子的改進(jìn)的數(shù)值法，相比與Matlab Robotics Toolbox中ikine函數(shù)的數(shù)值解法，無論在收斂能力還是收斂速度上，都有顯著提升，有效解決了數(shù)值法魯棒性不足的問題，能夠在解析法無法應(yīng)用的場合中快速穩(wěn)定求解多軸串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)問題。