基于實時逆運動學算法的6R 機器人三維仿真*

黎柏春，楊建宇，耿 磊，于天彪，王宛山

(東北大學 機械工程與自動化學院，沈陽 110819)

0 引言

近年來機器人逆運動學一直是人們研究的重點。然而機器人運動學的逆解具有一定的難度，特別是針對6R 機器人的逆解，許多學者在這方面已做了大量的工作［1，5］。目前機器人運動學逆解的方法主要有兩種:解析法和數值法。解析法運算速度快，實時性好，但只有機器人結構滿足一定的幾何條件才可求得相應的逆運動學解析解。數值法適用于6R 機器人的大多數情況，但無法得到全部解，而且有迭代不收斂的危險。因此工業上廣泛應用的6R 機器人其幾何結構均滿足求得逆運動學解析解的條件［4］。由于機器人運動學逆解存在多解的情況，因此在求得其逆解后需根據實際情況，以無碰撞、軌跡連續和行程最小等原則選取最優解，并進行后續的軌跡規劃等研究。為了直觀地選取機器人的逆解，分析機器人的工作過程，機器人三維仿真已成為機器人研究的重要手段。例如文獻［6］開發了機器人仿真工具箱;文獻［7］對五自由度的機械臂進行了建模和仿真研究;文獻［8］基于OpenGL 對機器人進行了三維仿真研究;文獻［9］介紹了一種新穎的構建機器人3D 仿真環境的方法。但在這些研究文獻中，機器人的逆運動學求解基本采用數值解法，計算效率較低，難以求解得到逆解的所有解［10-11］，而且未結合軌跡規劃和實際應用進行仿真。

1 6R 機器人運動學模型和運動學逆解

1.1 6R 機器人運動學模型

本文研究的六自由度工業機器人，其結構如圖1所示。根據機器人結構和D-H 參數法為機器人建立坐標系如圖2 所示，第n個關節的坐標系為Xn-1Yn-1Zn-1，X6Y6Z6為機器人末端執行器坐標系。為機器人建立好關節坐標系后，根據機器人的幾何參數(如圖2 所示)可得到D-H 參數表，如表1 所示。由于關節坐標系是在圖1 所示的機器人姿態，為保證θ1，θ2…θ6的實際意義與機器人控制系統中的一致，因此參數表中θn有初始角和正負情況。

圖1 六自由度工業機器人的三維模型

圖2 機器人關節坐標系

相鄰關節坐標系的四步變換都是相對于當前坐標系的，因此所有的矩陣都是右乘，從而可得到兩個相鄰關節坐標系的變換矩陣式(1)。為了簡化表達，本文將sin(θi)記為Si，將cos(θi)記為Ci。根據表1 中的參數和式(1)可得到機器人的總變換矩陣，即機器人正運動學模型，式(2)。

表1 機器人的D-H 參數表

續表

1.2 機器人運動學逆解

機器人逆運動學是進行機器人軌跡規劃的基礎，更是進行機器人仿真和控制的前提。機器人逆運動學求解是根據機器人末端執行器的期望位姿，求出機器人各個關節的角度。根據機器人末端執行器的期望位置可求得等式(2)中右邊的矩陣，由式(2)兩邊的矩陣元素相等可得到12 個方程。由于等式(2)中右邊矩陣中的旋轉矩陣(同時在前三行和前三列的部分)的各個分量線性相關，即九個方程實際為三個方程。因此實際為六個方程求解六個未知數，理論上可求得各關節變量。對于本文研究的6R 機器人而言，屬于末端三個腕關節的軸線相交于一點的垂直關節型6R 機器人(如圖1 所示)，根據文獻［12］可知，滿足可求得解析解的條件，最多存在8 組逆解。雖然由上述的12個方程通過逐步消元法可求解得到逆解，但求解過程復雜，容易出現增根和漏根的情況。因此本文引入復指數，通過矩陣變換求解運動學逆解。

用矩陣Mi表示Ti(di)Ti(ai)Ri(αi)，Ri表示Ri(θi)，由式(1)和(2)可得

引入矩陣P

對式(16)作如下構造，可對角化變換矩陣Ri

式(3)通過上面的變換，左右兩邊為4 ×4 階的復數矩陣，利用左右兩邊矩陣的元素相等可求解得到運動學逆解如下。

式中，k為整數，六個式中k的取值可不同，但必須使θ1，θ2…θ6的值在關節角的轉動范圍內，其中:

1.3 運動學模型和運動學逆解的驗證

對于機器人運動學模型的驗證可借助于三維建模軟件。在工業機器人三維模型環境下為其各個關節設定好初始角度(如表2 所示)，然后選擇空間任意一點P，分別測量得到該點在X0Y0Z0坐標系和X6Y6Z6坐標系下的坐標如圖3 所示。將設定的關節角度代入機械人正運動學模型(式(2))，可將P點在X6Y6Z6坐標系下的坐標變換為X0Y0Z0坐標系下的坐標，如式(4)所示。通過正運動學模型計算得到的坐標與測量得到的坐標基本相等，由此可驗證機器人正運動學模型的正確性。

表2 機器人各關節的初始化角度

圖3 空間點坐標測量圖

對于機器人逆運動學的驗證，同樣取表2 中的關節角，通過機器人運動學模型得到機器人期望姿態即A，代入逆運動學解析解中可求得所有該期望位姿所對應的關節角如表3 所示。由此可驗證運動學逆解的正確性。

表3 期望位姿的全部逆解

2 多項式插值軌跡規劃算法

由于本文研究的6R 機器人其主要工作任務是為機床自動裝卸工件，只需控制機器人末端執行器起點、終點以及中間幾個點的位置和姿態，因此關節空間的軌跡規劃即可滿足要求。本文采用應用最多的多項式插值規劃算法來保證關節位置、速度和加速度在時域的連續性和穩定性。

式(5)即為用于插值的多項式，t為機器人工作過程中的時間。式中i值(i次多項式)由約束條件決定。分別設機器人起點和終點的關節位置為θn(t0)和θn(tf)，為防止機器人在啟動和停止時由于速度和加速度突變產生振動，保證機器人精確抓取和安放工件，因此起點和終點位置速度和加速度均為零，即˙θn(t0)=0 ，˙θn(tf)=0 ，¨θn(t0)=0 和¨θn(tf)=0 。六個約束，即采用五次多項式插值。

將起點和終點位置的約束條件帶入式(6)中可求得a0，a1…a5再帶回式(6)中就可得到機器人關節在任意時間的位置，速度和加速度，即運動規律。

3 機器人三維仿真平臺構建

基于前面的運動學模型，運動學逆解和五次多項式的軌跡規劃算法，利用MATLAB 的Simulink 3D Animation 構建集成機器人運動學和軌跡規劃模型的實時三維仿真平臺，其效果如圖4 所示。該平臺由機器人控制面板和三維可視化窗口組成，通過控制面板可實現與機器人的實時交互仿真，三維可視化窗口則可實時顯示機器人的仿真狀態。

圖4 機器人實時三維仿真平臺

4 應用實例

以機器人抓取和安裝工件為實例在構建的三維仿真平臺上進行仿真。機器人工作過程中需要控制的關鍵位置如圖5 所示。根據圖5 的機器人期望位姿通過三維仿真平臺可得到相應的關節角如表4 所示(由于逆解存在多解，本文根據圖5 的位姿選擇相應的逆解)。

圖5 機器人的期望位姿

表4 期望位姿的逆解

機器人抓取和安裝工件過程可分為三段路徑，設三段路徑所花時間均為5s，由此通過仿真平臺可得到機器人末端執行器的軌跡、關節軌跡曲線、角速度曲線和角加速度曲線如圖6 和圖7 所示(由于篇幅所限，本文只列出第二關節的相關曲線)。由圖7 的曲線可以看出關節軌跡曲線平滑，角速度曲線和角加速度曲線連續，從而避免機器人的振動，保證機器人在工作中的穩定性，有利于機器人精確到達預定位置。

圖6 末端執行器軌跡

圖7 第二關節的關節軌跡、角速度和角加速度曲線

5 結論

(1)在機器人逆運動學求解過程中，引入復指數，從而簡化了逆運動學的求解過程，減少了計算量，求解過程既沒有增根也沒有漏根，得到的逆運動學解析解可保證機器人三維仿真平臺的實時性。

(2)構建的具有較強交互性的機器人實時三維仿真平臺，使機器人的仿真更為直觀有效。而且可實時計算得到機器人的運動學、逆運動學和軌跡規劃結果，對機器人的進一步研究具有重要意義。

(3)由應用實例可得出構建的三維仿真平臺可靠有效，五次多項式的軌跡規劃算法也可使機器人在自動抓取和安裝工件過程中運行穩定，可避免機器人振動，有利于機器人精確到達預定位置。

［1］劉松國，朱世強，王宣銀.基于矩陣分解的一般機器人實時高精度逆運動學算法［J］. 機械工程學報，2008，44(11):304 -309

［2］Ali T，Hasan，A.M.S. Hamouda etc. An adaptive -learning algorithm to solve the inverse kinematics problem of a 6 D.O.F serial robot manipulator［J］. Advances in Engineering Software，2006(37):432 –438.

［3］劉達，王田苗. 一種解析和數值相結合的機器人逆解算法［J］. 北京航空航天大學學報，2007，33(6):727 -730.

［4］倪振松，廖啟征，魏世民，等. 空間6R 機器人位置反解的對偶四元數法［J］. 機械工程學報，2009，45(11):25 -29.

［5］王其軍，杜建軍. MOTOMAN 機器人逆運動學新分析［J］. 2010，42(3):451 -454.

［6］Kucuk S，Bingul Z. An off-line robot simulation toolbox［J］. Computer Applications in Engineering Education，2010，18(1):41 -52.

［7］Mohammed Abu Q，Iyad A，Hatem E. Modeling and simulation of 5 DOF education robot arm［C］// Advanced Computer Control (ICACC ). Shenyang: IEEE， 2010:569 -574.

［8］陳素麗，任福深. 基于OpenGL 的弧焊機器人仿真設計［J］. 科學技術與工程，2012，12(3):543 -550.

［9］甘亞輝，戴先中. 一種高效的開放式關節型機器人3D 仿真環境構建方法［J］. 機器人，2012，34(5):628 -633.

［10］Zhang Pei-yan，Lu Tian-sheng，Song Li-bo. RBF networksbased inverse kinematics of 6R manipulator［J］. Advanced Manufacture Technology，2005(26):144 -147.

［11］Ali T，Hasan，A.M.S. Hamouda etc. An adaptive learning algorithm to solve the inverse kinematics problem of a 6 D.O.F serial robot manipulator［J］. Advances in Engineering Software，2006(37):432 -438.

［12］周剛. 垂直關節型6 自由度機器人笛卡爾空間軌跡規劃的研究［D］.廣州:華南理工大學，2011.