柳　賀，劉　蕾，曾　輝，李　勛，萬　君

(安徽埃夫特智能裝備有限公司 蔡鶴皋院士工作站，安徽 蕪湖　241009)

?

6R機器人運動學分析及奇異處理研究

柳賀，劉蕾，曾輝，李勛，萬君

(安徽埃夫特智能裝備有限公司 蔡鶴皋院士工作站，安徽 蕪湖241009)

摘要：針對機器人逆運動學過程中存在運算復雜、多組解的選取問題，提出了一種6R機器人逆運動學的幾何方法.在求解的推導過程中，采用幾何法與雙變量正切函數(shù)相結(jié)合避免了失解，針對有多組逆解的情況，采用“插補連續(xù)性”的最短路徑與最優(yōu)標志設置準則相結(jié)合選取一組最優(yōu)解，同時考慮奇異位的避讓，滿足實際工程中的需要，在公司生產(chǎn)的多款機器人中獲得了成熟應用.

關鍵詞：工業(yè)機器人；逆運動學；奇異；插補連續(xù)

機器人運動學分析，包括正逆運動學分析，正運動學為由機器人各關節(jié)角度來確定機器人的TCP位姿，而逆運動學根據(jù)工具坐標系相對于基坐標系的目標位姿，求解機器人各關節(jié)角[1].逆運動學在機器人學中占有非常重要的地位，是機器人軌跡規(guī)劃和運動控制的基礎，直接影響著控制的快速性與準確性.一般機器人運動學逆解算法可分為以下幾種：解析法[2，3]( 又稱反變換法)和數(shù)值解法，其中反變換法求解過程直觀，因而被廣泛采納，但其求解過程中需多次進行矩陣變換的逆運算和4*4維矩陣的乘積運算，導致求解過程復雜耗時[4].

目前大多數(shù)6R工業(yè)機器人的幾何結(jié)構(gòu)都滿足Pieper準則，即3個相鄰關節(jié)軸線交于一點或相互平行，其運動學逆解可以得到數(shù)量一定的若干組封閉解，一般為8組或16組，對于后3個關節(jié)為旋轉(zhuǎn)關節(jié)而且軸線相交于一點的6R工業(yè)機器人，其前3個關節(jié)決定末端執(zhí)行器的位置，后3個關節(jié)決定末端執(zhí)行器的姿態(tài)[5-7].

圖1　機器人坐標系的建立Fig.1　robot coordinate system

本文以自己公司生產(chǎn)的ER165C機器人為研究對象，提出基于幾何法的運動學分析方法，計算量小，便于實時控制，同時對多解進行有效選取，并對奇異位置進行分析與避讓處理.

1建立坐標系

一般方式的建立坐標系方法，如圖1所示，建立相應的各個關節(jié)的坐標系，規(guī)定與軸滿足右手法則(右手系)轉(zhuǎn)角為正方向.其中全局坐標系為O0- X0 Y0 Z0，簡記為O系.各個關節(jié)為1，2，3，4，5，6系，工具坐標系為T系.在初始位姿下與T系重合的全局坐標系為GT系.

2機器人運動學正解

已知各個關節(jié)的轉(zhuǎn)角，求得機器人工具端OT的姿態(tài)和位置，即為機器人的正解.用坐標變換來描述為從O到T的變換[8].從圖1可知，從0到1的變換矩陣為1T0.依次類推2T1、3T2、4T3、5T4和6T5.從而可以得到從0到6的坐標變換6T0.從0到T的變換為TT0=6T0TT6.

其中：z0=710;x1=400,x2=1100,x3=225,y3=1110,zT=280用齊次坐標變換矩陣來表示機器人的正解運算如下式(1)所示：

TT0=1T02T13T24T35T46T5TTT6

(1)

3機器人運動學逆解

已知末端的位姿，求得各個關節(jié)的轉(zhuǎn)角就是機器人的逆運動學求解.采用幾何法進行求解，過程如下：

通過T系和變換TT6，可以求得腕部關節(jié)處6T0的變換矩陣.6T0=TT0(TT6)-1.利用這個關系可以進行逆解求解[9].

3.1第一關節(jié)角θ1求解

參見圖2，從幾何關系中可以看到腰部旋轉(zhuǎn)角度求解為兩解，分別為前腰(Front)與后腰(Back)：

θ1=arctan2(yp,xp)

(2)

(3)

3.2第二關節(jié)角θ2求解

考慮θ1解的情況，參見圖3，則有：

圖2　第一關節(jié)角θ1計算　　　　　　　　　　　　圖3　第二關節(jié)角θ2計算Fig.2　solution of joint 1　　　　　　　　　　　　Fig.3　solution of joint 2

從圖3可以看出：

(4)

另一個角度θb為：

θb=arctan2(2yp,2xp)，

(5)

于是第二個關節(jié)角θ2為：

θ2=θb-θa

(6)

顯然這個值有兩個解，另一個解見圖4，分別為高臂(Convex)與低臂(Concave).

3.3第三個關節(jié)角θ3求解

見圖5，先求初始角度：

從而得到角度：

θ3=-(θd-θc)

(7)

另一個解θ3為：

(8)

圖4　第二、三關節(jié)角的另解　　　　　　　　　　　圖5　第三關節(jié)角計算Fig.4　another solution of joint 2 and 3　　　　　　　　Fig.5　solution of joint 3

3.4第四、五和六個關節(jié)角θ4、θ5、θ6求解

關于旋轉(zhuǎn)的解，則按式:

6T0=1T02T13T24T35T46T5和6T3=2T31T20T16T0=4T35T46T5

于是有：θ5=±arccos(6T3(3,3))的兩個解，為高手腕(Positive)與低手腕(Negative ).

3.5機器人的多解選取

逆解存在多解，而機器人的運動控制需要唯一的確切解.首先，若關節(jié)角關節(jié)運動范圍超過上述的值域范圍，則在必要情況下，需將求得的關節(jié)角可能解加減360度，補出關節(jié)運動范圍內(nèi)的其他可能解，特別在第四與第六關節(jié)[10]；其次，選取關節(jié)運動范圍內(nèi)的可能解；最后，由運動插補連續(xù)性，選取與該關節(jié)角上一插補周期的值距離最近的可能解為唯一最優(yōu)解.各關節(jié)角的求解順序為[11]：先確定的唯一最優(yōu)解，再利用θ1的解按照上述方法和多解處理步驟求取其余關節(jié)角的解.最后所得的這組唯一的最優(yōu)關節(jié)角即可作為實現(xiàn)機器人目標位姿的關節(jié)控制輸入[12-13].

但在實際應用中，往往需要直接調(diào)用機器人的運動學庫，這時無法按照插補的連續(xù)性進行選解，需要對運動學確定選解最優(yōu)設置：比如FIAG=Front+Convex+Positive等模式進行選解.

4機器人奇異解的判斷與避讓處理

若目標P點的yP=0且xP=0時，θ1自由度丟失，其變化不影響腕部的位置，此時θ1為無窮多組解，該種情況在機器人空間內(nèi)出現(xiàn)的機會比較少，一般做θ1取上個插補周期的值.

若6T3(3,3)=1，也就是θ5=0，關節(jié)4和6的旋轉(zhuǎn)軸線共線，此時θ4+θ6有唯一解，而θ4與θ6有無窮多種組合，實際工程中一般進行在θ5接近0度時，進行報警停止與奇異位提示，或者進行自動轉(zhuǎn)換到關節(jié)空間，θ5過零后回到插補空間，但這樣會損失部分線性精度.

5ER165C機器人的運動控制驗證

按照上述的算法封裝成運動學庫，分為：TRF_FOR正解與TRF_INV逆解，在每個插補周期，路徑規(guī)劃控制器調(diào)用運動庫函數(shù)進行軸運動控制.

采用實際工程中的控制器離線仿真軟件進行實際軌跡控制，軌跡為一圓弧，圓弧上四點的位姿為：

cp0:CARTPOS:=(x:=1880.77,y:=218.5,z:=1460.3,a:=0,b:=59.3865,c:=180,mode:=0)

cp1:CARTPOS:=(x:=1880.77,y:=-181.5,z:=1060.3,a:=0,b:=59.3865,c:=-180,mode:=0)

cp2:CARTPOS:=(x:=1880.77,y:=-581.5,z:=1460.3,a:=0,b:=80.53,c:=-172.5,mode:=0)

cp3:CARTPOS:=(x:=1880.77,y:=-181.5,z:=1860.3,a:=0,b:=51.68,c:=-180,mode:=0)

測試TCP與各軸位移如下圖所示：

圖6　3D實際運行監(jiān)視　　　　　　　　圖7　各關節(jié)運動位移圖　　　　　　　　　　圖8　TCP的位姿圖　　　Fig.6　3D model view　　　　　　　　　Fig.7　positions of 6 joints　　　　　　　　　　Fig.8　positions of TCP

由圖7(縱軸從上至下為θ1-θ6的值，單位：° ；橫軸為時間軸，單位：s)與圖8(縱軸從上至下為TCP的X、Y、Z、A、B、C的值，單位：mm與°；橫軸為時間軸，單位：s )可以看出，機器人在復雜的圓弧運動中，其位姿均大幅運動，能夠平穩(wěn)的實現(xiàn)圓弧插補運動，表明了該六自由度機器人的運動學算法完善，可以達到期望的目標.

6結(jié)論

在機器人逆解求關節(jié)角度過程中，采用幾何法進行逆解運算，比一般的解析算法更加簡單高效，便于實時控制；本文給出了六自由度機器人對應某一位姿的多組解，采用“插補連續(xù)性”的最短路徑與最優(yōu)標志設置準則相結(jié)合選取一組最優(yōu)解，滿足實際工程中的需要.

參考文獻：

[1]劉鵬,宋濤,鵭超,等.焊接機器人運動學分析及軌跡規(guī)劃研究[J].機電工程,2013,(4):390-394.

[2]SAEED B， NIKU. Introduction to Robotics[M]. BEIJING: Publishing House of Electronics Industry,2013.54-57.

[3]扶宇陽,葛阿萍.基于MATLAB的工業(yè)機器人運動學仿真研究[J].機械工程與自動化,2013,(3):40-42.

[4]Hudyjaya SiswoyoJo, Nazim MirNasiri. Dynamic modeling and walk simulation for a new four-degree of-freedom parallelogram bipedal robot with sideways stability control[J]. Mathematical and Computer Modelling, 2013,(1):25-28.

[5]蘇學滿,孫麗麗,楊明,等.基于Matlab的六自由度機器人運動特性分析[J].機械設計與制造,2013,(1):78-80.

[6]謝斌,蔡自興.基于MATLAB Robotics Toolbox 的機器人學仿真實驗教學[J].計算機教育,2010,(19):140-143.

[7]張剛,黃海波.HP20機器人的運動學逆解計算及模擬仿真[J].輕工機械,2011,29(6):40-45.

[8]孫浩,趙玉剛,姜文革,等.碼垛機器人結(jié)構(gòu)設計與運動分析[J].制造業(yè)自化,2013,(14):66-68.

[9]劉萍,陳瑩.五自由度關節(jié)式機械手運動學分析及仿真[J].制造業(yè)自動化,2011,(19):9-11.

[10]張普行,嚴軍輝,賈秋玲.六自由度機械手的運動學分析[J].制造業(yè)自動化,2011,(20):68-71.

[11]MAYROGA R V，SANONGBOOM . A radial basis function network approach for geometrically bounded manipulator inverse kinematics computation[J]/ /2003 IEEE/RSJ International Conference on Intelligent robots and system，2003，4(12):3564-3569.

[12]DENAVIT J，HARTENBERG R S. A kinematic notation for lower-pair mechanisms based on matrices[J]. ASM Journal of Applied Mechanics，1955，22(2):215-221.

[13]TAROKH M，KIM M. Inverse Kinematics of 7-DOF Robots and Limbs by Decomposition and Approximation[J]. IEEE Transaction on Robotics，2007，23(3):6-11.

Solution for Inverse Kinematics and Research for the Singularity of the 6R Robot

LIU He，LIU Lei，ZENG Hui，LI Xun，WAN Jun

(Anhui Efort Intelligent Equipment Co., Ltd， Academician Station Of Cai He-gao, Wuhu 241009, China)

Abstract:Aiming to solving the problem of computing complex and a lot of solutions in the solution process of inverse kinematics of robot,a geometric algorithm method for inverse kin-ematics of 6R manipulators is presented. The geometric algorithm method and dual variable tangent function are used to avoid losing solutions. The shortest distance rule of consecutive interpolation and the optimization flag are presented to choose a solution closest to the current manipulator for a number of inverse kinematics solutions. Additionally, singularity is avoided. The research results have been applied successfully in 6R robots of the company.

Key words:industry robot; inverse kinematics; singularity; consecutive interpolation

中圖分類號：TP24

文獻標志碼：A

文章編號：1001-2443(2016)02-0132-05

作者簡介：柳賀(1981-)，男，高級工程師，碩士，研究領域：機器人運動控制，工業(yè)控制總線等.

基金項目：安徽省工業(yè)機器人成套技術(shù)開發(fā)與應用項目(11Z0101010)和國家科技部863計劃(2014AA041601).

收稿日期：2014-09-27

DOI：10.14182/J.cnki.1001-2443.2016.02.007

引用格式：柳賀，劉蕾，曾輝，等.6R機器人運動學分析及奇異處理研究[J].安徽師范大學學報：自然科學版，2016，39(2)：132-136.