郭志強(qiáng)，崔天時(shí)，呂信超，張　幀，李格倫，劉斯達(dá)

(東北農(nóng)業(yè)大學(xué)，哈爾濱　150030)

?

六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)求解

郭志強(qiáng)，崔天時(shí)，呂信超，張幀，李格倫，劉斯達(dá)

(東北農(nóng)業(yè)大學(xué)，哈爾濱150030)

摘要：對六自由度機(jī)械臂進(jìn)行了正運(yùn)動(dòng)學(xué)分析與求解，并提出了一套解決六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)問題的算法，同時(shí)使能耗達(dá)到最少。首先從機(jī)械臂的結(jié)構(gòu)特點(diǎn)出發(fā)，建立D-H坐標(biāo)系，得到正運(yùn)動(dòng)學(xué)模型；然后通過對正運(yùn)動(dòng)學(xué)模型的可解性進(jìn)行分析，采用矩陣逆乘的方法來得到機(jī)械臂逆運(yùn)動(dòng)學(xué)的完整析解；再通過求極值的方法來算出機(jī)械臂在運(yùn)動(dòng)過程中哪種運(yùn)動(dòng)軌跡耗能最少；最后用求解實(shí)例的方法驗(yàn)證正運(yùn)動(dòng)學(xué)模型和逆運(yùn)動(dòng)學(xué)求解的正確性。

關(guān)鍵詞：六自由度機(jī)械臂；正運(yùn)動(dòng)學(xué)；逆運(yùn)動(dòng)學(xué)

0引言

我國是農(nóng)業(yè)大國，每年收獲所耗費(fèi)的勞動(dòng)力非常多，機(jī)械臂就成為了替代人類工作的一種很好的工具。機(jī)械臂是模仿人類手臂而設(shè)計(jì)的一種能夠自動(dòng)運(yùn)動(dòng)的裝置，用來實(shí)現(xiàn)抓取、收獲等工作。它能夠在人類無法工作的環(huán)境下代替人類工作，并能夠保證穩(wěn)定性，對提高生產(chǎn)效率和降低勞動(dòng)成本都起到了非常重要的作用，目前已經(jīng)被應(yīng)用在各個(gè)領(lǐng)域當(dāng)中。

六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)求解問題是近幾年來國內(nèi)外主要研究的熱點(diǎn)之一。逆運(yùn)動(dòng)學(xué)求解的困難程度直接與機(jī)械臂的結(jié)構(gòu)有非常大的關(guān)系，許多學(xué)者對六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)求解做出了非常大的貢獻(xiàn)。在求解逆運(yùn)動(dòng)學(xué)問題中，迭代法只能求出一組解；解析法雖然可以得到全部解，但是計(jì)算復(fù)雜；人工神經(jīng)網(wǎng)絡(luò)、遺傳法等只是在理論上進(jìn)行了研究，無法保證解的精度和穩(wěn)定性，很少用于機(jī)械臂的運(yùn)動(dòng)控制當(dāng)中。

為此，本文提出了一種能夠有效地解決六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)問題的實(shí)時(shí)算法，在分析機(jī)械臂運(yùn)動(dòng)特性的基礎(chǔ)上，建立了D-H坐標(biāo)系來研究機(jī)械臂的運(yùn)動(dòng)學(xué)問題。首先建立機(jī)械臂正運(yùn)動(dòng)學(xué)模型，然后采用矩陣逆乘的方法來求解逆運(yùn)動(dòng)學(xué)問題，最后進(jìn)行仿真實(shí)驗(yàn)，驗(yàn)證了機(jī)械臂正運(yùn)動(dòng)學(xué)模型和逆運(yùn)動(dòng)學(xué)求解的正確性。

1正運(yùn)動(dòng)學(xué)模型

機(jī)械臂正運(yùn)動(dòng)學(xué)求解是已知機(jī)械臂各個(gè)關(guān)節(jié)的角度，來求取機(jī)械臂末端操作器的期望位置，主要是利用D-H坐標(biāo)系。一般思路是：首先，在各個(gè)關(guān)節(jié)建立參考坐標(biāo)系；然后確定每兩個(gè)相鄰的坐標(biāo)系之間的關(guān)系；最后，得出機(jī)械臂總變換矩陣。

1.1　D-H坐標(biāo)系的建立

本文以六自由度機(jī)械臂為研究對象，如圖1所示。其中，每個(gè)舵機(jī)都可以單獨(dú)工作。根據(jù)機(jī)械臂的結(jié)構(gòu)特點(diǎn)，使用D-H方法建立基礎(chǔ)坐標(biāo)系，x0-z0表示機(jī)械臂的底座，再依次建立關(guān)節(jié)2-6的坐標(biāo)系，x6-z6表示機(jī)械臂的末端操作器，如圖2所示。

圖1　六自由度機(jī)械臂

1.2　正運(yùn)動(dòng)學(xué)求解

在D-H坐標(biāo)系建立后，根據(jù)相鄰連桿之間的位置關(guān)系，確定D-H參數(shù)。機(jī)械臂各個(gè)關(guān)節(jié)的參數(shù)如表1所示。其中，θ為連桿轉(zhuǎn)角；d為z軸上兩條相鄰公垂線的距離；α為每一條公垂線的長度；?為關(guān)節(jié)軸線之間的夾角。

圖2　六自由度機(jī)械臂坐標(biāo)系的建立

相鄰的兩個(gè)坐標(biāo)系θda?0-1θ10a190°1-2θ20a202-3θ30a303-4θ4-d4a490°4-5θ5d5090°5-6θ6000

機(jī)械臂相鄰相鄰兩個(gè)關(guān)節(jié)坐標(biāo)系的齊次變換矩陣通式為

(1)

其中，Ai+1表示坐標(biāo)系i+1相對于i的關(guān)系，i=1，…，6。

將D-H參數(shù)代入該公式，得到正運(yùn)動(dòng)學(xué)方程。其末端操作器相對于基座的變換方程為

nz=S234C5C6-C234S6

oz=-S234C5S6-C234C6

ax=C1C234S5-S1C5

ay=S1C234S5+C1C5

az=S234S5

px=C1S234d5-S1d4+C1C234a4+C1C23a3+C1C2a2+

C1a1

py=S1S234d5+C1d4+S1C234a4+S1C23a3+S1C2a2+S1a1pz=-C234d5+S234a4+S23a3+S2a2

(2)

其中，正余弦預(yù)算符號在書寫上做了簡化，如S1=sinθ1，C1=cosθ1，S234=sin(θ2+θ3+θ4)。

2逆運(yùn)動(dòng)學(xué)求解及最優(yōu)控制

逆運(yùn)動(dòng)學(xué)求解是根基機(jī)械臂末端操作器預(yù)計(jì)位置，求出每個(gè)關(guān)節(jié)變量平移和旋轉(zhuǎn)地?cái)?shù)值，是正運(yùn)動(dòng)學(xué)的反向求解過程。正運(yùn)動(dòng)學(xué)求解比較簡單而且解唯一；逆運(yùn)動(dòng)學(xué)求解相對復(fù)雜可能存在多解、唯一和無解的情況。本文介紹了一種根據(jù)運(yùn)動(dòng)學(xué)正解，采用矩陣逆乘來求解逆運(yùn)動(dòng)的方法。

2.1　求解關(guān)節(jié)角θ1、θ2和θ3

(3)

讓方程兩邊矩陣的第3行第4列對應(yīng)相等，可得

S1px-C1py=-d4

(4)

(5)

(6)

(7)

最后求得

(8)

讓方程兩端的第1行第3列對應(yīng)相等，第2行第3列對應(yīng)相等，可得到

在式(9)兩邊同時(shí)乘以S234，在式(10)兩邊同時(shí)乘以C234，可以得到

(C1ax+S1ay)S234=C234az

(11)

化簡得

(12)

讓方程第1行第4列與第 2行第4列分 別對應(yīng)相等，可知

移項(xiàng)化簡為

C1px+S1py-a1-S234d5-C234a4=C23a3+C2a2(15)

(16)

令

X1=C1px+S1py-a1-S234d5-C234a4

(17)

X2=pz+C234d5-S234a4

(18)

所以，X1和X2是已知量，對兩邊同時(shí)平方相加得

2a2a3(C23C2+S23S2)

(19)

最后可得

(20)

對式(20)繼續(xù)化簡可得

(21)

2.2　求解關(guān)節(jié)角θ4、θ5和θ6

因?yàn)棣?和θ3已經(jīng)知道，所以得

θ4=θ234-θ2-θ3

對式子RTH兩邊連續(xù)乘以A1-A4的逆矩陣，可得

(22)

讓式(22)中第1行第3列與第2行第3列分別對應(yīng)相等，可知

(23)

(24)

2.3　最優(yōu)控制

機(jī)械臂逆運(yùn)動(dòng)學(xué)的解不是唯一確定的，在計(jì)算出所有解之后，由于關(guān)節(jié)運(yùn)動(dòng)范圍限制要將其中一部分解舍去，在剩下的解當(dāng)中通常選取一個(gè)最理想的接近當(dāng)前機(jī)械臂的解，而選取最理想解的方法通常選用最優(yōu)控制理論。最優(yōu)控制就是在給定條件下對給定的受控系統(tǒng)確定一種控制率，使該系統(tǒng)相應(yīng)于預(yù)先設(shè)定的性能指標(biāo)具有最優(yōu)值?？刂葡到y(tǒng)最優(yōu)化問題包括性能指標(biāo)的合理選擇及最優(yōu)化控制系統(tǒng)的設(shè)計(jì)，而性能指標(biāo)在很大程度上決定了最優(yōu)控制性能和最優(yōu)控制形成。下面先對最優(yōu)控制理論做一個(gè)簡單的介紹。

假定由下面的微分方程構(gòu)成動(dòng)態(tài)系統(tǒng)，則

x(t)=f(t，x(t)，u(t))t∈[0,T]

(25)

其中，x(t)=[x1(t),...,xr(t)]T為t時(shí)刻的狀態(tài)向量；x(t)=dx(t)/dt;u(t)=[u1(t),...,un(t)]T為t時(shí)刻的控制控制向量；f=[f1,...,fr]T為給定的連續(xù)可微的函數(shù)。

本方程表示系統(tǒng)從t=0時(shí)刻開始工作，一直到t=T，T為末端時(shí)間。通過這個(gè)方程，根據(jù)能量關(guān)系，可以得到下面的目標(biāo)函數(shù)，即

(26)

最優(yōu)控制理論其實(shí)就是極值原理和最優(yōu)原理。以機(jī)械臂底座為坐標(biāo)原點(diǎn)，通過正運(yùn)動(dòng)學(xué)分析，可以知道末端操作器相對于原點(diǎn)的位置方程為

(27)

(28)

求出滿足方程式(28)的極小值的各個(gè)角度，即為機(jī)械耗能最少時(shí)每個(gè)舵機(jī)應(yīng)轉(zhuǎn)動(dòng)的角度。

3求解實(shí)例分析

與正運(yùn)動(dòng)學(xué)相比，逆運(yùn)動(dòng)學(xué)求解更難，但是對機(jī)械臂的運(yùn)動(dòng)卻更加重要，它是軌跡規(guī)劃的一個(gè)非常重要的環(huán)節(jié)。所以，必須對其進(jìn)行具體求解實(shí)例分析，具體過程如下：

用MatLab可以繪制出當(dāng)前機(jī)械臂的位姿，如圖3所示。

圖3　已知各個(gè)關(guān)節(jié)角的機(jī)械臂的位姿

假設(shè)機(jī)械臂當(dāng)前各個(gè)舵機(jī)的角度都為0，要使末端操作器到達(dá)滿足上述的終點(diǎn)位置，將終點(diǎn)矩陣方程代入逆運(yùn)動(dòng)學(xué)方程中，可以得到8組解，如表2所示。

表2　逆運(yùn)動(dòng)學(xué)的解

由表2可知：第1、3、5、6組解是存在的，并且其中第3組解和之前已經(jīng)知道的6個(gè)角度非常接近，也滿足式(28)，可以作為最優(yōu)解。這說明了前面所提出的逆運(yùn)動(dòng)學(xué)求解方法與最優(yōu)控制算法的正確性，還說明了從關(guān)節(jié)空間到笛卡爾坐標(biāo)系之間是一種復(fù)映射關(guān)系，即多對一的映射關(guān)系。第2、4、7、8組解無解說明了機(jī)械臂的結(jié)構(gòu)不能滿足條件，適當(dāng)選取齊次矩陣，這幾組解將有可能存在。由于逆運(yùn)動(dòng)學(xué)具有多解的可能性，在日程生產(chǎn)應(yīng)用當(dāng)中，還要考慮機(jī)械臂的工作環(huán)境、空間及初始位姿等條件來選擇合適的解。

4結(jié)論

首先按照機(jī)械臂的結(jié)構(gòu)特點(diǎn)建立了運(yùn)動(dòng)學(xué)模型。然后，通過對正運(yùn)動(dòng)學(xué)方程的分析，提出了矩陣逆乘的方法來求解逆運(yùn)動(dòng)學(xué)方程。在已知末端操作器位姿的情況下求出每個(gè)關(guān)節(jié)的旋轉(zhuǎn)角度，并通過求極值的方法求出機(jī)械臂運(yùn)動(dòng)的最優(yōu)路徑。最后，通過具體的角度計(jì)算證明了這種方法的準(zhǔn)確性，并且充分說明了關(guān)節(jié)空間到笛卡爾坐標(biāo)系之間的多對一關(guān)系。本研究為機(jī)械臂軌跡規(guī)劃算法的研究提供了非常重要的理論依據(jù)。

參考文獻(xiàn)：

[1]姜銘,易紅,李鷺揚(yáng). RGRR-Ι構(gòu)造混聯(lián)6R機(jī)器人[J].機(jī)械工程學(xué)報(bào),2010,46(1):30-36.

[2]錢東海,王新峰,趙偉.基于旋量理論P(yáng)aden-Kaha子問題的6自由度機(jī)器人逆解算法[J].機(jī)械工程學(xué)報(bào),2009,45(9):72-76.

[3]張波濤,劉士榮,史先鵬.多連桿冗余移動(dòng)機(jī)械臂建模與運(yùn)動(dòng)規(guī)劃[J].科技通報(bào),2010,26(2):200-205.

[4]李憲,郭永存,張軍華.模塊化六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)解算與驗(yàn)證[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),2013,(44)4:246-251.

[5]董云,楊濤,李文.基于解析法和遺傳算法的機(jī)械手運(yùn)動(dòng)學(xué)逆解[J].計(jì)算機(jī)仿真,2012,29(3):239-243.

[6]Kalra P，Mahapatra P B，Aggarwal D K. An evolutionary approach for solving the multimodal inverse kinematics problem of industrial robots[J].Mechanism and Machine Theory,2006,41(10):1213-1229..

[7]徐文福,劉宇,強(qiáng)文義.自由漂浮空間機(jī)器人的笛卡爾連續(xù)路徑規(guī)劃[J].控制與決策,2008,23(3):278-282.

[8]Husty M L, Pfurner M, Schrocker H P.A new and efficientalgorithm for the inverse kinematics of a general serial 6R manipula-tor[J].Mechanism and Machine Theory,2007,42(1):66-81.

[9]于艷秋,廖啟征.基于有理數(shù)運(yùn)算的一般6R機(jī)器人位置逆解算法[J].機(jī)械工程學(xué)報(bào),2005,41(3):229-233.

The Solution to Inverse Kinematics About Six Degrees of Freedom Mechanical Arm

Guo Zhiqiang， Cui Tianshi， Lv Xinchao， Zhang Zhen， Li Gelun， Li Sida

(Northeast Agricultural University, Harbin 150030, China)

Abstract：At present, the application of mechanical arm is more and more widely. Forward kinematics of six degrees of freedom mechanical arm is analyzed and solved in this paper, and a set of algorithms to solve the problem of inverse kinematics of six degrees of freedom mechanical arm is proposed, at the same time to achieve a minimum energy consumption. First of all, the d-h coordinate system is established according to the structure characteristics of mechanical arm, then forward kinematics mode is achieved. Second the solvability of forward kinematics model is analyzed, then the complete analysis solution of inverse kinematics of mechanical arm is get based on method of inverse matrix. Again by the method of calculating extremum, which movement locus consumes energy least in the process of movement can be calculated. Finally through the method of solving instances, the correctness of the forward kinematics model and the solution of inverse kinematics is validated.

Key words：six degrees of freedom mechanical arm; forward kinematics; inverse kinematics

中圖分類號：S24；TP24

文獻(xiàn)標(biāo)識碼：A

文章編號：1003-188X(2016)12-0051-05

作者簡介：郭志強(qiáng)(1991-)，男，黑龍江雙鴨山人，碩士研究生，(E-mail)764845828@qq.com。通訊作者：崔天時(shí)(1967-)，男，哈爾濱人，副教授，碩士生導(dǎo)師，博士。

基金項(xiàng)目：國家“863計(jì)劃”項(xiàng)目(810028)

收稿日期：2015-11-03