基于實(shí)時(shí)逆運(yùn)動(dòng)學(xué)算法的6R 機(jī)器人三維仿真*

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

(東北大學(xué) 機(jī)械工程與自動(dòng)化學(xué)院，沈陽(yáng) 110819)

0 引言

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

1 6R 機(jī)器人運(yùn)動(dòng)學(xué)模型和運(yùn)動(dòng)學(xué)逆解

1.1 6R 機(jī)器人運(yùn)動(dòng)學(xué)模型

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

圖1 六自由度工業(yè)機(jī)器人的三維模型

圖2 機(jī)器人關(guān)節(jié)坐標(biāo)系

相鄰關(guān)節(jié)坐標(biāo)系的四步變換都是相對(duì)于當(dāng)前坐標(biāo)系的，因此所有的矩陣都是右乘，從而可得到兩個(gè)相鄰關(guān)節(jié)坐標(biāo)系的變換矩陣式(1)。為了簡(jiǎn)化表達(dá)，本文將sin(θi)記為Si，將cos(θi)記為Ci。根據(jù)表1 中的參數(shù)和式(1)可得到機(jī)器人的總變換矩陣，即機(jī)器人正運(yùn)動(dòng)學(xué)模型，式(2)。

表1 機(jī)器人的D-H 參數(shù)表

續(xù)表

1.2 機(jī)器人運(yùn)動(dòng)學(xué)逆解

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

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

引入矩陣P

對(duì)式(16)作如下構(gòu)造，可對(duì)角化變換矩陣Ri

式(3)通過(guò)上面的變換，左右兩邊為4 ×4 階的復(fù)數(shù)矩陣，利用左右兩邊矩陣的元素相等可求解得到運(yùn)動(dòng)學(xué)逆解如下。

式中，k為整數(shù)，六個(gè)式中k的取值可不同，但必須使θ1，θ2…θ6的值在關(guān)節(jié)角的轉(zhuǎn)動(dòng)范圍內(nèi)，其中:

1.3 運(yùn)動(dòng)學(xué)模型和運(yùn)動(dòng)學(xué)逆解的驗(yàn)證

對(duì)于機(jī)器人運(yùn)動(dòng)學(xué)模型的驗(yàn)證可借助于三維建模軟件。在工業(yè)機(jī)器人三維模型環(huán)境下為其各個(gè)關(guān)節(jié)設(shè)定好初始角度(如表2 所示)，然后選擇空間任意一點(diǎn)P，分別測(cè)量得到該點(diǎn)在X0Y0Z0坐標(biāo)系和X6Y6Z6坐標(biāo)系下的坐標(biāo)如圖3 所示。將設(shè)定的關(guān)節(jié)角度代入機(jī)械人正運(yùn)動(dòng)學(xué)模型(式(2))，可將P點(diǎn)在X6Y6Z6坐標(biāo)系下的坐標(biāo)變換為X0Y0Z0坐標(biāo)系下的坐標(biāo)，如式(4)所示。通過(guò)正運(yùn)動(dòng)學(xué)模型計(jì)算得到的坐標(biāo)與測(cè)量得到的坐標(biāo)基本相等，由此可驗(yàn)證機(jī)器人正運(yùn)動(dòng)學(xué)模型的正確性。

表2 機(jī)器人各關(guān)節(jié)的初始化角度

圖3 空間點(diǎn)坐標(biāo)測(cè)量圖

對(duì)于機(jī)器人逆運(yùn)動(dòng)學(xué)的驗(yàn)證，同樣取表2 中的關(guān)節(jié)角，通過(guò)機(jī)器人運(yùn)動(dòng)學(xué)模型得到機(jī)器人期望姿態(tài)即A，代入逆運(yùn)動(dòng)學(xué)解析解中可求得所有該期望位姿所對(duì)應(yīng)的關(guān)節(jié)角如表3 所示。由此可驗(yàn)證運(yùn)動(dòng)學(xué)逆解的正確性。

表3 期望位姿的全部逆解

2 多項(xiàng)式插值軌跡規(guī)劃算法

由于本文研究的6R 機(jī)器人其主要工作任務(wù)是為機(jī)床自動(dòng)裝卸工件，只需控制機(jī)器人末端執(zhí)行器起點(diǎn)、終點(diǎn)以及中間幾個(gè)點(diǎn)的位置和姿態(tài)，因此關(guān)節(jié)空間的軌跡規(guī)劃即可滿足要求。本文采用應(yīng)用最多的多項(xiàng)式插值規(guī)劃算法來(lái)保證關(guān)節(jié)位置、速度和加速度在時(shí)域的連續(xù)性和穩(wěn)定性。

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

將起點(diǎn)和終點(diǎn)位置的約束條件帶入式(6)中可求得a0，a1…a5再帶回式(6)中就可得到機(jī)器人關(guān)節(jié)在任意時(shí)間的位置，速度和加速度，即運(yùn)動(dòng)規(guī)律。

3 機(jī)器人三維仿真平臺(tái)構(gòu)建

基于前面的運(yùn)動(dòng)學(xué)模型，運(yùn)動(dòng)學(xué)逆解和五次多項(xiàng)式的軌跡規(guī)劃算法，利用MATLAB 的Simulink 3D Animation 構(gòu)建集成機(jī)器人運(yùn)動(dòng)學(xué)和軌跡規(guī)劃模型的實(shí)時(shí)三維仿真平臺(tái)，其效果如圖4 所示。該平臺(tái)由機(jī)器人控制面板和三維可視化窗口組成，通過(guò)控制面板可實(shí)現(xiàn)與機(jī)器人的實(shí)時(shí)交互仿真，三維可視化窗口則可實(shí)時(shí)顯示機(jī)器人的仿真狀態(tài)。

圖4 機(jī)器人實(shí)時(shí)三維仿真平臺(tái)

4 應(yīng)用實(shí)例

以機(jī)器人抓取和安裝工件為實(shí)例在構(gòu)建的三維仿真平臺(tái)上進(jìn)行仿真。機(jī)器人工作過(guò)程中需要控制的關(guān)鍵位置如圖5 所示。根據(jù)圖5 的機(jī)器人期望位姿通過(guò)三維仿真平臺(tái)可得到相應(yīng)的關(guān)節(jié)角如表4 所示(由于逆解存在多解，本文根據(jù)圖5 的位姿選擇相應(yīng)的逆解)。

圖5 機(jī)器人的期望位姿

表4 期望位姿的逆解

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

圖6 末端執(zhí)行器軌跡

圖7 第二關(guān)節(jié)的關(guān)節(jié)軌跡、角速度和角加速度曲線

5 結(jié)論

(1)在機(jī)器人逆運(yùn)動(dòng)學(xué)求解過(guò)程中，引入復(fù)指數(shù)，從而簡(jiǎn)化了逆運(yùn)動(dòng)學(xué)的求解過(guò)程，減少了計(jì)算量，求解過(guò)程既沒(méi)有增根也沒(méi)有漏根，得到的逆運(yùn)動(dòng)學(xué)解析解可保證機(jī)器人三維仿真平臺(tái)的實(shí)時(shí)性。

(2)構(gòu)建的具有較強(qiáng)交互性的機(jī)器人實(shí)時(shí)三維仿真平臺(tái)，使機(jī)器人的仿真更為直觀有效。而且可實(shí)時(shí)計(jì)算得到機(jī)器人的運(yùn)動(dòng)學(xué)、逆運(yùn)動(dòng)學(xué)和軌跡規(guī)劃結(jié)果，對(duì)機(jī)器人的進(jìn)一步研究具有重要意義。

(3)由應(yīng)用實(shí)例可得出構(gòu)建的三維仿真平臺(tái)可靠有效，五次多項(xiàng)式的軌跡規(guī)劃算法也可使機(jī)器人在自動(dòng)抓取和安裝工件過(guò)程中運(yùn)行穩(wěn)定，可避免機(jī)器人振動(dòng)，有利于機(jī)器人精確到達(dá)預(yù)定位置。

［1］劉松國(guó)，朱世強(qiáng)，王宣銀.基于矩陣分解的一般機(jī)器人實(shí)時(shí)高精度逆運(yùn)動(dòng)學(xué)算法［J］. 機(jī)械工程學(xué)報(bào)，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］劉達(dá)，王田苗. 一種解析和數(shù)值相結(jié)合的機(jī)器人逆解算法［J］. 北京航空航天大學(xué)學(xué)報(bào)，2007，33(6):727 -730.

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

［5］王其軍，杜建軍. MOTOMAN 機(jī)器人逆運(yùn)動(dòng)學(xué)新分析［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ī)器人仿真設(shè)計(jì)［J］. 科學(xué)技術(shù)與工程，2012，12(3):543 -550.

［9］甘亞輝，戴先中. 一種高效的開(kāi)放式關(guān)節(jié)型機(jī)器人3D 仿真環(huán)境構(gòu)建方法［J］. 機(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］周剛. 垂直關(guān)節(jié)型6 自由度機(jī)器人笛卡爾空間軌跡規(guī)劃的研究［D］.廣州:華南理工大學(xué)，2011.