Matlab Robotics Toolbox逆運動學數值解分析

李慧敏

（大同煤礦集團機電裝備公司中央機廠， 山西 大同 037001）

引言

機器人逆運動的求解一般有解析法與數值法。PIEPER[1]給出了六自由度機器人存在解析解的情況，然而對于不存在解析解以及解析解很難得到的情況下，只能利用數值迭代的方法求解機器人運動方程。

數值迭代法的求解速度雖然比解析解要慢得多，但其具有更加廣泛的適用性。常用的數值迭代法有牛頓迭代法[2]、共軛梯度法[3]等。這些求解方法的缺點在于收斂速度較慢，同時，合適的迭代初始值很難獲得。文獻[4]中提出基于雅可比逆矩陣求解方法[4]，通過不斷修正機器人各個關節位置，最終使得機器人位姿與目標位姿之間的誤差在預定的范圍內。Matlab Robotics Toolbox[5]包含了豐富的機器人運動學庫函數，其逆運動學求解函數正是基于雅可比偽逆矩陣的原理，同時在雅可比矩陣奇異的情況下，工具箱又轉而采用雅可比轉置的方法。本文即是以Puma560六軸機器人為例，對其求解的準確性予以驗證。

1 雅可比偽逆矩陣數值迭代原理

機器人各個關節實際運動曲線與經過線性化處理后，其關節矢量增量與機器人末端位姿變化關系可以表示為：

機器人末端執行器在基坐標下的位移Δx可以通過關節矢量Δq來得到。Δx越小，其線性關系越準確。通過數值迭代計算的過程越接近真實運動曲線。同時在線性逼近的過程中，機器人末端位姿與目標位姿之間的誤差要在預定的目標之內。

假設機器人末端執行器目標姿態分別表示為[Rf，Pf]，當前的位姿[Rc，Pc]，因此位姿誤差可以表示為：

誤差 Δx=[ΔP，ΔW]，||Δx||=||ΔP||+||ΔW||，其中 ΔP，ΔW分別為：

修正后的機器人關節適量增量為dq，因此迭代后得到的新的關節矢量q"=q+dq，當計算得到的誤差Δx在設定的誤差范圍之內時，q"即為逆運動學的解，當誤差大于設定值時，則返回重新計算。

遇到雅克比矩陣為奇異矩陣時，直接計算雅克比的逆往往行不通，將式（1）展開，得到線性方程組的形式，并給出了其近似解：

式中：Jij為雅克比矩陣的第i行第j列元素。

2 雅可比轉置矩陣數值迭代原理

針對雅克比矩陣奇異的情況，Matlab Robotics Toolbox也給出了其解決辦法，使用雅克比轉置來代替雅克比偽逆矩陣。

雅克比轉置的原理是給機器人每個關節上一組驅動力矩，從而驅動機械臂各關節運動向目標靠近，最終機器人末端執行器能夠收斂到目標點的位置。同時為加快迭代速度，將求解步長增大α倍的處理方式，于是式（1）與式（7）變更為：

雅克比偽逆與雅克比轉置法都存在一定的弊端。偽逆雅克比法雖然迭代速度相對較快，但是無法解決矩陣奇異問題。雅克比轉置迭代法每次都需要沿迭代點的負梯度方向進行搜索，故其收斂速度相較雅克比偽逆法而言較為緩慢。同時應當注意的是，使用數值方法求機器人運動學逆解最后只能限制計算結果的誤差大小，得到一組解。對于機器人運動學中存在的多重解問題，數值解法存在著一定的局限性。

3 Matlab Robotics Toolbox機器人運動學逆解實例

Matlab Robotics Toolbox中包含了Puma560機器人模型，庫文件中對Puma560的運動與動力參數都予以定義，工具箱的機器人逆運動學求解函數ikine：式中

：q為求解得到的機器人關節角矢量，R為用戶定義的機器人模型，T為機器人基座標系到末端坐標系的齊次轉換矩陣。這里任意選取Puma560機器人的一個狀態來進行驗證，如圖1所示。

圖1 Puma560機器人

圖1中所示的Puma560末端在基座標系下的位置矢量為[0.294，0.014，0.659]，對應的 RPY 姿態角為[34.283，-50.523，-39.809]，機器人關節角矢量為[33.3，33.8，-16.9，-10.8，41.7，-77.6]，因此末端在基座標下的齊次變換矩陣為：

利用ikine函數，反解得到的關節角度矢量為：[33.3766，33.7471，-16.8847，-162.0685，-31.332 7，59.359 9]，如圖2所示，繪制了其數值迭代狀態與時間的關系。橫軸表示的是迭代時間，為1 000 ms，q表示關節角度矢量，e表示迭代誤差，α表示求解步長放大倍數。

圖2 迭代狀態與時間的關系

由圖2可以看出ikine在求解過程中，關節角θ4，θ6值會出現較大的波動，但最終會收斂到某個值，ikine函數求解步長沒有做放大處理。

在驗證ikine求解效果時我們發現，機器人末端的位置矢量是符合初始位置要求的，但姿態角并不能匹配機器人初始設定狀態，ikine不能很好的處理6軸機器人逆運動學解問題，針對這種情況，Matlab Robotic Toolbox提供新的處理六自由度以上的機器人逆運動學方程求解函數ikine6s，利用ikine6s重新求解后我們得到一組新的關節角度矢量：

利用機器人正運動學可以推導其末端在在基座下的位姿變換矩陣：

式（13）與式（11）是完全吻合的，可以看出ikine6s的求解結果具有較高的精度，但是只得到一組解，ikine6s無法處理機器人運動學中存在的多重解問題。

4 結論

1）機器人運動學方程的數值解法有廣泛的適用性，雅克比偽逆與雅克比轉置的求解方法都可以在線性逼近的過程中，限定機器人末端位姿與目標位姿之間的誤差。雅克比偽逆法雖然迭代速度相對較快，但是無法解決矩陣奇異問題。

2）Matlab Robotic Toolbox中的機器人逆運動學求解是基于雅克比偽逆與雅克比轉置法的，其庫函數ikine的迭代結果僅能滿足末端位置需求，不能很好的解決復雜自由度機器人逆運動學方程問題，而專門針對六軸或六軸以上機器人的求解函數ikine6s求解結果具有較高的精度，因此利用Matlab Robotic Toolbox可以快速計算出機器人目標軌跡的離散點角度矢量信息，無需得到其解析解，其缺點在于只能得到一組解，無法解決多重解的問題。