高精度解耦六自由度機械臂逆運動學解法

1、1638計算機測量與控制第18卷計算機測量與控制.2010. 18(7)Computer Measurement & Control:TP241:1671-4598(2010)07-1637-04付榮，居鶴華(北京工業(yè)大學電亍信息與控削工程學院.北京100124):W6自由度機械腎正交解獵的結構待點采用位姿分解方夭.將6自由度逆運動學降為3自由度位豎逆運功半、3自由度 方向逆運功學：利用歐幾星戀范歎導出機械障定位定佝的逆運功學解析幫養(yǎng)機械轉高速、準輪迖功.左定向控制方面.提出一種以 單位四尤歎為目林輸入的控制形式.只Wi+W個角度逆存既簡化計霧.又十廳實際操作：利用逆運功學計霧機械臂的工作空閭

2、和奇 異點空間皆助移動機臭人車體自由度彌補因計算以及關節(jié)長度不輪引起的奇卑位形.極大擴展的有效運功區(qū)域.:六自由度機械研：逆運功學：康析聲：高犒度Real - time and Ffigh - accurate Inverse Kinematic Control Algorithm forDecoupled Six - DOF ManipulatorFu Ro ng . J u Hehua(1. College of Eectronic Information and Control Engineering Beijing University of Technology Beijing 10

3、0124 China)Abstrad : This paper describes an approach of inverse kinenutical control for 6 DOF orthogonal and decoupled manipulator. Based on the structure charactenstic of this kind of manipulators lhe kinenutical nodel is simplified to a 3 DOF onentation calculation and 3 DOF posture calculation.

4、This tun part of knematical calculation are independeni The p3$i(ion and attitude determination problem of this manipulator is solved based on Euclid norm Al lhe same lime Ihisprpcr took quaternion as the control input to solve the attitude deternunation problem. These methodscan simplify the co inp

5、ut aton . achieve the real time and accuracy requirements. and also easy to implement. By analyzing the singularity of the motion region of the manipulation a new method to expand the imtion repon is proposed by using extra DOF from rowr.Key w)rds : 6 DOF manipulator: inverse kinematic control: anal

6、ytical solution: high - accurate中華測控網(wǎng) 1994-2011 Ciina Academic Journal Electronic Publishing House All rights reserved 1638計算機測量與控制第18卷中華測控網(wǎng) 1994-2011 Ciina Academic Journal Electronic Publishing House All rights reserved 1638計算機測量與控制第18卷0一般6RU器人的實數(shù)逆運動學解的上限為161LEE121 等利用矢量乘法找到14個線性無關的逆運動學方程.利用分 離變量將

7、其化為關節(jié)變量半角正切的一元16次方穆 MAN- FRED利用多維幾何和運動學圖像理論簡化了一尤16次方 程的茯取和變量的求解但是以上方法仍存在精度不第高和計 算復親的缺點.劉松國山在這些硏究基砧上提出基于矩陣分解 的一般6R機器人實時高特度逆運動學算法.滿足特度要求. 計算相對簡單.Saeed B. Niku、孫菇利用機枝臂運動學 模型的矩陣變換.將變量解耦.求得各關節(jié)角度解析解.在滿 足高精度的同時.計算簡單，但解耦過程復奈對于解耦b自由度機械臂的逆運動學.簡化為3自由度位 呈求解 3自由度方向求解上述6自由度的算法同樣適用于 3自由度位蚩逆運動學運算張念在3自由度機械手運動分 析中就借鑒

8、了文獻15的算法.Jorge Angeles111提出利用歐 幾里德范皴求解解耦6自由度機械管的算法.將位聖逆運動學 解簡化為關節(jié)變量半角正切的一元四次方程本文主要硏究位于B2型月球車身前端.為靈活獲得視覺:201001-17;:2010-02-25.國家863高科技資助項目(2OO8AAOO85).:付 *(1986 .女.審士研兜生主硬從亭機械If運功 學、幼力學控制、機械臂虛擬現(xiàn)實平臺方向的研憲昌鶴華H96知男自功揑制工穆師軟授.主喪從亭旨卷揑制. 人方商的研兜.信息駆動相機盒達利指定位置和朝向的6R機減臂借鑒文 獻181的方法.利用機彼臀正交的持殊構型.將一尤四次方 稈筒化為一元二次方

9、程.計算量更小.且稱稱過穆簡單機械臂的奇異位形是由于處在特殊位豎無法計算出關節(jié) 角度.導致的運動不口丿達本文借用B2坦月球車的自由醫(yī)殊 補機械臂自由度不足.便其避免不可達位豎.擴展機械臂的運 動空間1解藕襟作手的逆運動學包牯阮心的定位逆解和定向逆解. 即巳知阮心在機械管坐標系位旻和方向單位矢量及旋錢角度. 求各關節(jié)需要旋轉的甬度本文硏究的B2型月球車上軀動相機盒運動的正交解耦機 械臂整體結構圖如圖1所示機械臂骯心位置即相機盒位 豎.關節(jié)13旋轉方向兩兩正交.控制相機盒定位.其解耦 于控制相機盒定向的關節(jié)46.如圖2所示.SI整體結構圖根據(jù)式(7)和式(8)導出丨的解析解:COS |-4G-5如

10、B( /cos 1 亠 /sin j + J) /sin1 = A J J 卜 1 Ctos 3 + /9Sin 3 ag mA( /cos 3 + /sin 5 + J) /(10)運動學建樓采用標準DH坐標系法如圖2秦數(shù)表如 衰所示表1 0- H聲歎表關節(jié)cossind1010LI02、90*000L23390*000L3根DH系結構圖2.可以推導出腕心(向量c)所在 機械管坐標系的位蛋：a 4- Qim 70cn 4- Qi Q： Qi 14 = c(1)式中.1 CDS i ；1 = sin；C = Xr Yc Z2 7； fe =L hii bi 2 T ：f5=L aicos 1

11、at sin iT; Qi =COS 1 isin r/sin 1sin 1iCOS i /COS i是相鄰坐標系間的靛轅關系.-0ir/分別是機械臂的關節(jié)角、偏超長度和扭角歩 裁.乂、*、乙是腕心的期筑位置根據(jù)文獻(81對錠轉坐 標系的定義:；=護,年式爭換成:01(1 Q 之 Q1Q： 7 = Q| J 7r 1)首先.將或(2)兩邊分別取歐幾里徳范數(shù).推導出 I的解析解氏(2)左邊取歐幾里應吃裁：r |Q (fe + Qi h + /a i) |2 |Q |2 1 Q 懇 hm | 展開得：P =rc + i$ 4-+M+Zfi + 2/4 2 3 + 2hl h 3 + 2fe fe

12、2 + (2(n ay 2/c hi 2 3Kos i “ (2fc 2 a)4- 2hi ie 3)sin 3 (4； ft(2)右彷取歐n.srr#：/ = iiQan】|l = iin：丄H 細Q# (5)展開得：- x; + r? + z + / -(2ai X把式(4)和(6)合并同類項，得到下式：4cos I + Bsin I + CVos 3 + Dsin 3 + = 0(7)式中人=2ai Xf; H = 2ai Kr;C =- 2hb 2、； D = 2m y4- 2/ua： s; = ij + 4- uj + M + tt +2 3 2lnby 3 +2fcfa 2X?S乞

13、./M 2加Z 由式(2)第3個標量方程推導出關于|、j的力穆.化 簡得列下式：ACos 1 Gsin 1 4 /cos 5 4- /sin j + y 0(8丿式中F= 1 V: / = i X(； H = 2 j仙；/ = iw : J = fe 4-2 4- 2+ 加 l2) 然后.將式(9)和式(10)兩邊平方再相加得到關于 sin 3. cos ,的方程通過引入三甬恒等式推導出的解 析解：Kcos j2 4- Lsin 52 4- Mcos 3sin 3 4- Arcos j 4- Psin j 4 0(ID 式中 : Q 4aT72 4 TE - 4aT T 3其中.2 - X；

14、4- 7 .引入三角恒等式：cos 三匕4 . sin 三匕；式中. =tan() 將其帶入式(II)得封關于3的四次方程： /C54.S3+rj4. /;V - 42 4aj j 2；由于機彼手的正交構型.便式(12)的系數(shù)s、u為零. 式(12)可以看成關于的一元二次方穆.相比一元四次方 穆.計算量大大儼小了：/?( b2 + r 5 + v = o(13)根據(jù)式(13)可以解出關于3的4個可能的解.再將得 到的值代入式(9)和式(10)得到，解3) 最后.將式(2)的前兩個標量方穆導出關于cos 2. sin 2的方程.得秀:的解析解：COS 2 =2 Au ( X.cos 1 亠 Yt

15、sm 1 A11 + Ah5)-412/-1 Xrsin 1 * r令 QiSXm. = ( X ,.X y.X ：JT .根據(jù)式(17)得到：cos s = X ：f 18；cos 4 = 4；sin 4 =丄4(19/8S $COS 54、，隔定旋錢方向.6即四兀敵的第4個元素.確定族 轉角度這種控制方歡有*懺實際操作：在確走恃觀寮的物體 后先購定相機盒的朝向.再根播侍觀奈物的持征晞龍相機盒 就轉甬度.以全面獲得比像數(shù)據(jù)2逆運動學計算的目的之一就是獲得工作空間以規(guī)劃機械 臂的運動軌謹掃描機械臂可能的最大運動空間.用逆運動學 解析解計算關節(jié)角度.記錄可達點即工作空間和不可達點.利 用mall

16、ab畫出圖3是機械臂的工作空間圖4長機械臂第 三奇異點空問從中可以香出為確定機械臂的工作空間所掃描 的區(qū)域.第一、第二奇異點在本次掃描中未出現(xiàn).對比圖3和 圖4.在工作空間外側的奇異點長由于機誡臂長度有限的不可 達點.工作空間內部的奇異點是計算原因造成的不可達點.03機械骨的工作空間機績”二需異點如04第三奇幷位形空間參照表1.機械手臂的長度依次為：Lx =Q 14m Li =ft 24m. M=a 054m,那么機械管最大的運動空間就是以Li +山+山為半徑的半球形區(qū)域.當目標位蚤超過這個區(qū)域時. 機械宵無法對達.式(9)式(10).當AGFB為公時機 械手處于第一奇異位形.比時骯心位于第一

17、個轉動軸的軸線 上式(14)式(15).當4n2 + Ai22為零時.機枝手處于第 二奇異位形，此時腕心位于第二個轉動軸的軸線上式(13). 當5的解為負殺時.機械手處于第三奇異位形.這是本文所 硏究的機械臂位要不能達最多的情況由圖3和圖4DH坐標系可以看出.第一轉動軸位于車 體垂直方向儈助車體自由度也不能避免依奇異位形.第二奇 異位形和第三奇異位形.就可借助車體改變位置和朝向變?yōu)榭?達點：目標點在世界坐標系BI定通過車體坐標系運動使機械 臂坐標系改變.使目標點在新機械臂坐標系下變?yōu)榭蛇_點在第3部分仿真中.沒有芳慮不可達問創(chuàng)在VC+&0開發(fā)環(huán)危下.采用Open Inventor搭逮了 6 自由

18、度機械譬虎擬現(xiàn)實仿宴系統(tǒng).釆用正運動半驗證逆運動學解的正確性把逆運動學所求 的角度蒂入正運動學公式.檢驗所得的位要是否與給定值相 同.正運動學公式如下：V*T,是Q,的齊次形式：QiiQinQiyf?814|劉松國朱世孫.王宜館.持干矩陣分F的一股6R機臭人實時高 悄度逆運動學算i* )b機械工程學1R. 2008. 44 ( 11): 304-309.151 Saeed B. Niku.機品人半導論一析、系統(tǒng)及應用M孫畜春朱紀洪.劉國棟等詳.北京：電子工業(yè)出版社.2004.【6孫 亮馬 江阮曉剛.基干OpenCL 六自由度機械If三淮 仿莫工具的諒計|J |.計算機測與控制2009. 17 (5): 982-985.7】張 敏石秀華社向覚等 三