六自由度機械臂逆運動學求解

郭志強，崔天時，呂信超，張　幀，李格倫，劉斯達

(東北農業大學，哈爾濱　150030)

?

六自由度機械臂逆運動學求解

郭志強，崔天時，呂信超，張幀，李格倫，劉斯達

(東北農業大學，哈爾濱150030)

摘要：對六自由度機械臂進行了正運動學分析與求解，并提出了一套解決六自由度機械臂逆運動學問題的算法，同時使能耗達到最少。首先從機械臂的結構特點出發，建立D-H坐標系，得到正運動學模型；然后通過對正運動學模型的可解性進行分析，采用矩陣逆乘的方法來得到機械臂逆運動學的完整析解；再通過求極值的方法來算出機械臂在運動過程中哪種運動軌跡耗能最少；最后用求解實例的方法驗證正運動學模型和逆運動學求解的正確性。

關鍵詞：六自由度機械臂；正運動學；逆運動學

0引言

我國是農業大國，每年收獲所耗費的勞動力非常多，機械臂就成為了替代人類工作的一種很好的工具。機械臂是模仿人類手臂而設計的一種能夠自動運動的裝置，用來實現抓取、收獲等工作。它能夠在人類無法工作的環境下代替人類工作，并能夠保證穩定性，對提高生產效率和降低勞動成本都起到了非常重要的作用，目前已經被應用在各個領域當中。

六自由度機械臂逆運動學求解問題是近幾年來國內外主要研究的熱點之一。逆運動學求解的困難程度直接與機械臂的結構有非常大的關系，許多學者對六自由度機械臂逆運動學求解做出了非常大的貢獻。在求解逆運動學問題中，迭代法只能求出一組解；解析法雖然可以得到全部解，但是計算復雜；人工神經網絡、遺傳法等只是在理論上進行了研究，無法保證解的精度和穩定性，很少用于機械臂的運動控制當中。

為此，本文提出了一種能夠有效地解決六自由度機械臂逆運動學問題的實時算法，在分析機械臂運動特性的基礎上，建立了D-H坐標系來研究機械臂的運動學問題。首先建立機械臂正運動學模型，然后采用矩陣逆乘的方法來求解逆運動學問題，最后進行仿真實驗，驗證了機械臂正運動學模型和逆運動學求解的正確性。

1正運動學模型

機械臂正運動學求解是已知機械臂各個關節的角度，來求取機械臂末端操作器的期望位置，主要是利用D-H坐標系。一般思路是：首先，在各個關節建立參考坐標系；然后確定每兩個相鄰的坐標系之間的關系；最后，得出機械臂總變換矩陣。

1.1　D-H坐標系的建立

本文以六自由度機械臂為研究對象，如圖1所示。其中，每個舵機都可以單獨工作。根據機械臂的結構特點，使用D-H方法建立基礎坐標系，x0-z0表示機械臂的底座，再依次建立關節2-6的坐標系，x6-z6表示機械臂的末端操作器，如圖2所示。

圖1　六自由度機械臂

1.2　正運動學求解

在D-H坐標系建立后，根據相鄰連桿之間的位置關系，確定D-H參數。機械臂各個關節的參數如表1所示。其中，θ為連桿轉角；d為z軸上兩條相鄰公垂線的距離；α為每一條公垂線的長度；?為關節軸線之間的夾角。

圖2　六自由度機械臂坐標系的建立

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

機械臂相鄰相鄰兩個關節坐標系的齊次變換矩陣通式為

(1)

其中，Ai+1表示坐標系i+1相對于i的關系，i=1，…，6。

將D-H參數代入該公式，得到正運動學方程。其末端操作器相對于基座的變換方程為

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)

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

2逆運動學求解及最優控制

逆運動學求解是根基機械臂末端操作器預計位置，求出每個關節變量平移和旋轉地數值，是正運動學的反向求解過程。正運動學求解比較簡單而且解唯一；逆運動學求解相對復雜可能存在多解、唯一和無解的情況。本文介紹了一種根據運動學正解，采用矩陣逆乘來求解逆運動的方法。

2.1　求解關節角θ1、θ2和θ3

(3)

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

S1px-C1py=-d4

(4)

(5)

(6)

(7)

最后求得

(8)

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

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

(C1ax+S1ay)S234=C234az

(11)

化簡得

(12)

讓方程第1行第4列與第 2行第4列分 別對應相等，可知

移項化簡為

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

(16)

令

X1=C1px+S1py-a1-S234d5-C234a4

(17)

X2=pz+C234d5-S234a4

(18)

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

2a2a3(C23C2+S23S2)

(19)

最后可得

(20)

對式(20)繼續化簡可得

(21)

2.2　求解關節角θ4、θ5和θ6

因為θ2和θ3已經知道，所以得

θ4=θ234-θ2-θ3

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

(22)

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

(23)

(24)

2.3　最優控制

機械臂逆運動學的解不是唯一確定的，在計算出所有解之后，由于關節運動范圍限制要將其中一部分解舍去，在剩下的解當中通常選取一個最理想的接近當前機械臂的解，而選取最理想解的方法通常選用最優控制理論。最優控制就是在給定條件下對給定的受控系統確定一種控制率，使該系統相應于預先設定的性能指標具有最優值。控制系統最優化問題包括性能指標的合理選擇及最優化控制系統的設計，而性能指標在很大程度上決定了最優控制性能和最優控制形成。下面先對最優控制理論做一個簡單的介紹。

假定由下面的微分方程構成動態系統，則

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

(25)

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

本方程表示系統從t=0時刻開始工作，一直到t=T，T為末端時間。通過這個方程，根據能量關系，可以得到下面的目標函數，即

(26)

最優控制理論其實就是極值原理和最優原理。以機械臂底座為坐標原點，通過正運動學分析，可以知道末端操作器相對于原點的位置方程為

(27)

(28)

求出滿足方程式(28)的極小值的各個角度，即為機械耗能最少時每個舵機應轉動的角度。

3求解實例分析

與正運動學相比，逆運動學求解更難，但是對機械臂的運動卻更加重要，它是軌跡規劃的一個非常重要的環節。所以，必須對其進行具體求解實例分析，具體過程如下：

用MatLab可以繪制出當前機械臂的位姿，如圖3所示。

圖3　已知各個關節角的機械臂的位姿

假設機械臂當前各個舵機的角度都為0，要使末端操作器到達滿足上述的終點位置，將終點矩陣方程代入逆運動學方程中，可以得到8組解，如表2所示。

表2　逆運動學的解

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

4結論

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

參考文獻：

[1]姜銘,易紅,李鷺揚. RGRR-Ι構造混聯6R機器人[J].機械工程學報,2010,46(1):30-36.

[2]錢東海,王新峰,趙偉.基于旋量理論Paden-Kaha子問題的6自由度機器人逆解算法[J].機械工程學報,2009,45(9):72-76.

[3]張波濤,劉士榮,史先鵬.多連桿冗余移動機械臂建模與運動規劃[J].科技通報,2010,26(2):200-205.

[4]李憲,郭永存,張軍華.模塊化六自由度機械臂逆運動學解算與驗證[J].農業機械學報,2013,(44)4:246-251.

[5]董云,楊濤,李文.基于解析法和遺傳算法的機械手運動學逆解[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]徐文福,劉宇,強文義.自由漂浮空間機器人的笛卡爾連續路徑規劃[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]于艷秋,廖啟征.基于有理數運算的一般6R機器人位置逆解算法[J].機械工程學報,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

文獻標識碼：A

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

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

基金項目：國家“863計劃”項目(810028)

收稿日期：2015-11-03