高精度解耦六自由度機械臂逆運動學(xué)解法_付榮

1、DO I ：K) 16S26/jcki 11 4%2/g Did 07.067設(shè)計與應(yīng)用 1637 計尊機測址打控制.201(). 18(7)Computer Measuivniait & Gintrol文章編號:1671 4598(2010)07 1637 04中圖分類號：TP241文獻標識碼:A71994-2016 China Academic Journal Electronic Publishing House. All rights reserved.DO I ：K) 16S26/jcki 11 4%2/g Did 07.067設(shè)計與應(yīng)用 1637 71994-2016 China

2、Academic Journal Electronic Publishing House. All rights reserved.DO I ：K) 16S26/jcki 11 4%2/g Did 07.067設(shè)計與應(yīng)用 1637 高精度解耦六自由度機械臂逆運動學(xué)解法付榮.居鴿華(北京【】業(yè)人學(xué)電子信息與控制匸程學(xué)院.北京100124)摘要：根牝6門山度機械臂止交解耦的結(jié)構(gòu)待點采用位姿分解方式.將6山度逆運動學(xué)降為3“曲度位置逆運動學(xué).3門山度 方向逆運動學(xué)，利用歐幾甲徳范數(shù)導(dǎo)出機械臂定位.定向的逆運動學(xué)解析解.使機械愕高速.準確運動.在定向控制方面.捉出一種以 單位四元數(shù)為丨1杯綸入的控制形

3、幾只防計妃兩個如度逆解.吒簡化計貳又利實際操作：利川逆運動學(xué)計樣機械臂的匚作空何和奇 片點空間.借助移動機器人乍體口由度彌補因計鋅以及關(guān)節(jié)氏度不夠引起的奇異位阪 極大擴展了機械乎臂的有效運動區(qū)域.關(guān)僦詞：六“山度機逆運動學(xué)，解析解I高精度Real-time and Highaccurate Inverse Kinematic Control Algorithm forDecoupled SixDOF ManipulatorEli Rong Ju 11 ehua(1 College of Elect ronk、Information and Control EngineeringBeijing;

4、 University of Technology* Beiji咋 1()0124. China)Abstract： Thw pa|rr docribcs mi approach of invrrw kinnnalical control for 6 DOF orthogonal and dcrouplrd manipulator.iiill si rue tu n* ciiarad tTistic of this kinl of man pula tors th* k.inrtnati(*ai tii(I4 is smi phfirl to a 3 1)() F orirntat ion(*

5、alcu lation and 3 DOE pos I ii n- cal*ulation Thi t part of kinematic al raln Lit ion arc in(le|H*n11*11)il t liis nianipuL lor is rl basnl un Euclid norm. At th* same tim t this pa|MT look(|uatrniion as the c*onlrol inpu t Io solve th* attitude lirmuiatirol Irm. Thrnr mdhotis can simplify (hr compu

6、talion adiirvethr rrai timeami accuracy rrquiremrnlib and also cany to implement. By analyzing th* inil.iir t L- nrol lh- iikunpuLili. .i n v iiu-iIhmI to -p.inl lining V IroniKey word* 6 DOF rnanipulaton il; analytiral solution; high H(xnirat*71994-2016 China Academic Journal Electronic Publishing

7、House. All rights reserved.DO I ：K) 16S26/jcki 11 4%2/g Did 07.067設(shè)計與應(yīng)用 1637 71994-2016 China Academic Journal Electronic Publishing House. All rights reserved.DO I ：K) 16S26/jcki 11 4%2/g Did 07.067設(shè)計與應(yīng)用 1637 o引言一般6R機器人的實數(shù)逆運動學(xué)解的上限為l譏LEEI 筲利川矢雖乘法找到14個線性無關(guān)的逆運動學(xué)方程.利用分 離變獻將其化為關(guān)節(jié)變戢半角正切的一元16次方程.M A N- FR

8、ED川利用多維兒何和運動學(xué)圖像理論簡化了一元16次方 程的獲取和變戢的求解.但是以上方法仍存在椿度不夠高和計 算址朵的缺點.劉松國Z在這些研究基礎(chǔ)上提出基矩陣分解 的一般6R機器人實時髙楮度逆運動學(xué)兌法.滿足檔度耍求 計算相對簡單.Saeed B. Nilu尸、孫亮利用機械臂運動學(xué) 模熨的矩陣變換.將變雖解弭求得乞關(guān)節(jié)角度解析解.在滿 足高耕度的同時.計算簡單但解耦過程復(fù)雜.對丁解耦6 fl山度機械毬的逆運動學(xué).簡化為3 1*1山度位 置求解、3自由度方向求解.1：述6自由度的算法同樣適用F 3IL1J度位置逆運動學(xué)運算.張敏在3 fl山度機械運動分 析中就借鑒了文獻| 5的畀法.Jorge

9、Angek-s*提出利用歐 兒里徳范數(shù)求解解戟6I山度機械臂的算法.將位W逆運動學(xué) 解簡化為關(guān)節(jié)變厳半和正切的一元四次方程.本文上耍研究位J B2熨月球車身的端.為靈活獲得視覺收稿日期=2010 01 17：修回日期：20100225“基金項目：國家863高科技資助項H(2008AA(X)85).作者簡介：付 榮1986 ). fc.碩1:研究生主要從弔機械竹運動 學(xué)、動力學(xué)控制、機械愕虛擬現(xiàn)實平臺方向的研究.居鶴華(1969 ).男.“動拎制工程師.教授.主要從申牡能控制 “主機器人方向的研九信息驅(qū)動相機盒達到指定位就和朝向的6R機械憶 借鑒文 獻8的方法.利用機械宵正交的特殊構(gòu)醴.將一元四

10、次方 程簡化為一元二次方程.計算蛍更4、II解耦過程簡單.機械皆的奇界位形足宙丁處在特殊位代無法iinai關(guān)節(jié) 角度.導(dǎo)致的運動不町達.木文借用B2些月球乍的白由度彌 補機械度不足.使其避免不町達位擴展機械曹的運 動空間1逆運動學(xué)分析解耦操作F的逆運動學(xué)包括疑心的疋位逆解和定向逆 即C知腕心曲機械臂坐標系位代和方向敢位矢雖及旋轉(zhuǎn)角度. 求各關(guān)節(jié)需要旋轉(zhuǎn)的角度.本文研究的I型月球乍上驅(qū)動相機盒運動的正交解耦機 械臂.整體結(jié)構(gòu)圖如圖I所示機械臂腕心位置即相機盒位 朮 關(guān)節(jié)1-3旌轉(zhuǎn)方向兩阿止交.控制相機盒疋位其解耦 控制和機盒定向的關(guān)節(jié)4-5如圖2所示.圖1整體結(jié)構(gòu)圖71994-2016 Chin

11、a Academic Journal Electronic Publishing House. All rights reserved.DO I ：K) 16S26/jcki 11 4%2/g Did 07.067設(shè)計與應(yīng)用 1637 71994-2016 China Academic Journal Electronic Publishing House. All rights reserved.DO I ：K) 16S26/jcki 11 4%2/g Did 07.067設(shè)計與應(yīng)用 1637 中華測控網(wǎng), ki.ncf71994-2016 China Academic Journal El

12、ectronic Publishing House. All rights reserved. 1638 計算機測航與拎制第IX卷(9)1 1定位逆解(求解Oh Oh 01)運動學(xué)建模采用標準DH坐標系法，如圖2參數(shù)表如 表1所示.表1 I)-H參數(shù)農(nóng)關(guān)節(jié)0ac?asinaad1q010Li02a90000L239000013根據(jù)DH系結(jié)構(gòu)圖Z町以推導(dǎo)出腕心(向雖：)所在 機械宵坐標系的位5：“；+ 0。；+ Q Qi h + 0 Q 0 亦=2(1)式中.入 cosQj； sinQj； c * Yc Z I h = 紐 hj1, 入r: dt =dnO* b) r： Qt =cosO/ Xj

13、sinO, J2/sinO/si nO, X, cosO, 一 M, ；+ Q.氏+ bi“)H 2 = II Qi II2 II &+ Q.反 + 人is II2 (3) 展開紂：I1 =定 + 斥 + a； + b； + 員 + 2 /24 入 2 Xi + 2/4+ 如傷入 2 +(加2“3 2/2加 宀)(丫3 +(2/七卩2Ai)sin0i (4)式(2)右邊収歐幾里徳范數(shù)：/ = |阪乜 II1 = |dA|-26Qic(5)展開得：r2 = &+丫？+2?+岔+歷一(2 的&msO| + 加用 iiA + 2/h 乙)(6)把式(4)和(6)介并同類項.得到下式：JcosO|+

14、BsinOi Cco)i+ DsinO-j- E 0(7)式中.A=2aiX(iB= 2aiYciC= lauiy-D= 2bM + 2人E=屍+涇+尿+尿+厲+ 2b血2b4bx -r如b 入疋一Y Zj a 2b乙由式(2)第3個標雖方程推導(dǎo)出關(guān)丁5、仿的方程.化 簡得到下式：Fcos0| + Gsir/) 1 + /cosOj + /si + 丿=0(8)式中F=m G=lliXc； H =m虹 /=宀6； J = bz + by x2 4- X2X 5/4 X| 乙 + b 入i 根據(jù)式(7)和式(8)導(dǎo)出0,的解析解:osOj = I a Cns0j + DmiA + *)+AG F

15、BB( /?os03 /siiiOj + ./ )|sinOi = 彳g_1F( CmsOg + DsinO； + E)A( Hcnsd4- /sin0+ J)|( 10)2)然后,將式(9)和式(10)兩邊平方再相加得到關(guān)于 石仙、“曲的方程.通過引入三角恒等式推導(dǎo)出的解 析解：/lcos052+ LsinOc 4- A/(vohO 1 si-r NcosOy + PsinOy + 0=0(11)式中.L= 2a?f + M?D2 M= 2(4“：/+斥CD” K 4局/尸+卅芒；N= 2(4iH/ + /4CE)i P= 2(4ail/+rfD)； Q= 4“汀+卩注一也沖語:其中,曠=X

16、l+Y；.引入三角恒等式：cosO JsinO式中三1 卜 r1+ rLin(將其帶入式(ii)得到關(guān)丁。的四次方程：/M+St；+ 7Yj+ u“+ v= 0(12)式中，R= 4a2 )2 + f(E O2 - AailPhS= 4| 4ot KJ- /7)+ HD(E- O| ；T= q2+2八)+円(F C+ID-W2：U= 4| 4/j/( H+ J)+HD(C+E)J ：V= 4ut(/4-J)2 + H(C+ E)2-4aP2；山丁機械的疋交溝型.使式(12)的系數(shù)S、U為翠 式(12)可以看成關(guān)J- r?的一元二次方程.相比一元四次方 程，計算雖大大減小了：/?(云戶+ y=

17、0(13)根抉式(13町以解出關(guān)于心的4個可能的輯 再將得 到的值代入式(9)和式(10)得到解.3)愛后.將式(2)的前購個標呈方程導(dǎo)出關(guān)J- cos02. 山仇的方程.得到。2的解析解：(t02 =、 J /l|j(XccmO| + Yc sinOj 血)-J1J X)XsinO) + yAi cosO| + (Ze hi )M| ( 14)sinOi = A12( Afr cy)sO| 4- YfsinO| a ) +A+ Ah/ill - XjX.sinOi + yX|Cos01 + (Zc- bd ( 15)式中.A + 抓”12 =sinOj +bytl2 + 九入2 宀 conO

18、j + 九”2 入把得到的6和Oj的值代入式(14)和式(15),得到相應(yīng) %的值.逆運動學(xué)定位的4組關(guān)節(jié)角度C全部求出.從這4組 解中選出一組使關(guān)節(jié)轉(zhuǎn)動所需角度和最小的解.L 2定向逆解求解/、0“ 0J傳統(tǒng)定向逆解的輸入是3紐向貳：法線向嵐、指向向最和 接近向顯 這3個向凰町以通過z?py (滾動角.俯仰角.僞 航角)戰(zhàn)轉(zhuǎn)求出.但是這3個角度所確定的方向并不直觀.木 文以單位四元數(shù)作為已知輸入 控制目的宜觀、明礪 且只需 計篦兩個角度逆解計粥簡也 首先依據(jù)小位四元數(shù)的詢3na Academic Journal Electronic Publishing House. All rights

19、reserved. 1638 計算機測航與拎制第IX卷na Academic Journal Electronic Publishing House. All rights reserved. 1638 計算機測航與拎制第IX卷中華測控網(wǎng)na Academic Journal Electronic Publishing House. All rights reserved.第7期付榮 精度解耦六自由度機械骨逆運動學(xué)解法 1639 個元索確定方向逆彌 方向正運動學(xué)公式如下:X*（16）式中.x為相機盒熨轉(zhuǎn)的目標m位向晁 x足相機盤正向的 單位向雖| I o q 0Q0己由位置逆解求出. 變化為：

20、將式（16）0QX= 0心令 Q1- X = | x. Y r, x| r .根據(jù)式（17）得到:cos05 = 乂二cosl）4 = a ； sinO4 = * 力?osv5cosU5o4. o5確定旋轉(zhuǎn)方向.即四元數(shù)的第4個元素.(18)(19)確定旋轉(zhuǎn)角度.這種控制方式仃利J實際操作：在確圧待觀察的物體麻.先確定柑機盒的朝向.再根據(jù)待觀察物的特征確定相機盒 旋轉(zhuǎn)角度.以全向獲得影像數(shù)據(jù).2避奇異位形分析逆運動學(xué)訃外的I的之-就是獲得工作空間以規(guī)劃機械 劈的運動軌跡.扌I摧機械臂町能的最大運動空ML用逆運動學(xué) 解析解il毎關(guān)仍們Q記錄町達點即工作空間和不可達點.利 用mat lab llf

21、liHl圖J是機械臂的丁作空間圖4是機械皆第 .奇異點空間*從中可以看出為確定Mttff的工作空間所掃描 的區(qū)域第一、第二奇異點在木次扌I描中未出現(xiàn).對比圖3和 圖4在匸作空間外惻的奇舁點是由J機械骨長度有限的不可 達點.丁作空間內(nèi)部的奇并點足汁畀廉岡造成的不可達點Q 24n. ,= 0 054n.那么機械骨最人的運動空間就是以b 一匕+b為半徑的半球形區(qū)域當H標位習(xí)超過這個區(qū)域時. 機械宵無法到達.式（9）式（10）,當AG-FB為零時，機 械處第一奇并位膠 此時腕心位F第一個轉(zhuǎn)動軸的軸線 上.式（14）式（15）,當Au2+A為零時.機械處丁第 二奇異位形此時腕心位丁第二個轉(zhuǎn)動軸的軸線上.

22、式（13） 當8的解為負數(shù)時.機械處I：第三奇界位鬆 這是本文所 研究的機械骨位置不能達最多的情況.由圖3和圖4D- II坐標系可以看出.第一轉(zhuǎn)動軸位車 體垂血方向.借助車休fl宙度也不能避免該奇并位形.第二爸 井位形和第二奇井位就可借助乍體改變位腔和朝向變?yōu)榭?達點：U標點在世界坐標系同定.通過乍體坐標系運動使機械 臂坐標系改變.使忖標點在新機械臂唯標系下變?yōu)轭_點.在第3部分仿真中.沒有考慮不町達問題.3逆運動學(xué)捽制仿奪1 I仿真平臺介紹在VC+ + 6 0開發(fā)環(huán)境下，采用Open Inventor搭建f 6 I曲度機械臂虛擬現(xiàn)實仿真系統(tǒng).采用正運動學(xué)驗證逆運動學(xué)解的正確件把逆運動學(xué)所求

23、的角度供入1E運動學(xué)公比 檢驗所得的位置是占打給定值和 同.正運動學(xué)公式如下：卩_ =T.是0的齊次形式：Qii Qii Qu“ Qu Qm QmTi =Qa Qm0 0 0TXT2T,VQ(20)pa表示當前坐標 Pu1圖3機械臂的丁作空間系原點在前一關(guān)節(jié)坐標系的位配 %、兒農(nóng)示I標位代向雖 和坐標系中所需控制的位置向邑.定向控制同樣采取逆解求角 度.正解鯊證結(jié)果的方法.以保證正確性.3 2位苣控制保持相機盒方向始終為00叮給定一條吃線作為腕 心的連續(xù)tl標位汽如圖5所示圖6是正運動學(xué)驗證的實際珮 蹤軌跡.圖7是3個關(guān)廿的角度變化 圖8是實際杭跡和II標 直線的位疋屈 可以看出.謀圣在10

24、*數(shù)雖:級上 這足由于 浮點數(shù)在計算機中的計算造成的誤墾可以忽略.通過珮蹤一條空間1線.驗證機械骨可以耕確地到達工作 空間的任意點.|4第三奇卅位形空何參照表1機械F臂的長度依次為：|= 0 14m. L2 =圖5 |標杲蹤血線中華測控網(wǎng)、 ki.ncf71994-2016 China Academic Journal Electronic Publishing House. All rights reserved. IMO 計算機測賦與拎制第18卷na Academic Journal Electronic Publishing House. All rights reserved. IMO

25、 計算機測賦與拎制第18卷輸入51、0.5 ? 0一 .t/s20 4060 H0 100 120 140 160 1H0 20020 4060 H0 100 120 140 160 1H0 200t/s20圖9單位四尤數(shù)輸入iN2c$?cs3is rtst/s120406080100120bt/s圖7各關(guān)紡角度RvwqIK*?ss. 衛(wèi)na Academic Journal Electronic Publishing House. All rights reserved. IMO 計算機測賦與拎制第18卷na Academic Journal Electronic Publishing Ho

26、use. All rights reserved. IMO 計算機測賦與拎制第18卷2X1002xio方20406080100120140t/s020406080100120140.XIO小020406080100120140t/s圖8逆運動/位置謀差3 3方向控制在相同的腕心位此設(shè)Yf.柑機盒的旋轉(zhuǎn)向足和旋轉(zhuǎn)角度如 圖9所不.圖10所爪是跟蹤口標方向和角度的控制角度變化. 可以看出偏轉(zhuǎn)角度即.與阿元數(shù)輸入第4個元索相您這是因 為本文捉出的四元數(shù)為輸入的逆解只需計算曲個角度值、.山 正運動學(xué)驗證的方向誤簽如圖II所示謀簽數(shù)晁級是10 同樣由浮點數(shù)在計算機中的運算產(chǎn)生.可忽略.通過銀蹤11標方向

27、加0度.驗證機械痔逆運動學(xué)方向控制 的高梢度性.4結(jié)論本文對6K止交解耦機械臂提出了基歐兒里徳范數(shù)的 位、姿分解逆運動學(xué)控制方法.利用正交構(gòu)瞇簡化計宛推圖II角度誤差導(dǎo)出族個關(guān)卩角的解析解.實現(xiàn)機械臂快速、準確運動。提出 以貳位四元數(shù)為輸入的方向控制逆解.簡化計如通過跟蹤H 標位譽和II標方向及機械皆三維實時顯示.證實町行性.本文 另外討論分析了該型機械皆的奇斤位形利用車體|山度避開 奇異位形.這對B2住月球乍的機械-臂逆運動學(xué)實時控制具 冇重要總義.參考文獻：| 1| PK1MKOSE E J F On I hr inpu out pul(*(uation / tlir grnrral 7

28、K in(-4ianisin | J| M Kluinisni ami MiK-liivK* Th wry. 1986. 21 (6)： 509 510.| 刁 LEE H Y IJAXG C G A new vet or theory for thr n Hilly sis of spatial iiirchatiiMns ( J| . M(vliaiiKtn and Machin： 20921.(下轉(zhuǎn)第 124 頁)na Academic Journal Electronic Publishing House. All rights reserved. IMO 計算機測賦與拎制第18卷na

29、 Academic Journal Electronic Publishing House. All rights reserved. IMO 計算機測賦與拎制第18卷中華測控網(wǎng)ki.nct?1刪哪啊ina Academic Journal Electronic Publishing House. All rights reserved. 1M4 計算機測吊與拎制第IX卷(b)(b)(c)(e)計舒所得的移動軌跡的介成圖衣2為第二組實驗中實際移動 軌跡的部分采樣值與定位煤法處理后計算得到的移動軌跡的部 分采樣企的比較小位為cm.從圖中可以看出.算法的定位效果基本町以表示真實 的路於悄況.另外.

30、根據(jù)所紂的相關(guān)數(shù)據(jù).經(jīng)過進步處理. 還可以算出定位II標的移動速度.包括還冇很多英它的后續(xù)應(yīng) 用可以擴展.在此不一贅述.但從圖中我們也町以得知.實 驗結(jié)果還冇不盡如人意的地方.比如沒能完全克眼并種干擾的 彩響并H自一定的系統(tǒng)誤歪和誤匹妃.基序列圖像的室內(nèi) 定位見法礙要那先處理好外憎問題和假設(shè)條件.只冇乞種前提 完全満足后算法的輅準、快速等特點才能凸顯出總 整個牢 內(nèi)定位系統(tǒng)仍需要不斷完善.5結(jié)論木文對基丁 序列圖像塊匹配的肓內(nèi)定位算法進了系統(tǒng)的閘 述.訂先分析了塊匹配脈理.并確圧了雖適合本文算法的搜索 范閑、匹配準則和搜索策略.初始化為前幀和前一幀圖像后. 即確定了搜索范曲 同時進彳J繪佳匹配

31、塊的搜索.完成計算 麻.得到了兩幀圖像幾冇最小絕對怎的片前塊和匹配塊.并返 回坐標進行坐標變撫 從而得到運動矢雖.雖后的實驗結(jié)果 衣明.該算法能夠?qū)崿F(xiàn)快速.粘準地定位效果H仃很好的應(yīng) 用前景.參考文獻:na Academic Journal Electronic Publishing House. All rights reserved. 1M4 計算機測吊與拎制第IX卷實際執(zhí)跡棟法所得軌跡A 1(31.62 一 3.023)1(3162 3()60/12( 36. 72. 0. 0368)82( 36 71 0. 00)A 3(40. U 2. 485)83(40. 8U 4. 00)/4(

32、44 89. 4. 934)34(44. 89. 6. 121)/5(49. 99 7. 994)85(49帕 8 161)/6(53. 05. 9. 831)86(53.05 10. 20)-4 7(51.73. 12. 27)S7(51.7X 14. 28)A 8(68. 3S 19.01)88(68.3, 19.38)/9(74. 47. 22. 68)B9(74 47. 20.40)A 10(81.61. 26. 97)510(81.61. 27.54)11)(2|4|沈樂利I適應(yīng)塊匹配搜索并法研究J計 26 (4)： 797 802.王春艷.兒種塊匹配運動估計棟法的比較|J) 18：

33、 251.彭宇行.等.種有效的匹配準則及塊匹配鋅圖9第二組實輪衣2第二殂實驗化也國安飾文.趙銀冷.肚于郴位郴關(guān)法的全局運動估計權(quán)法|J1計算機測蚩與控制.2(X)8. 16 (4)： 532 534 張培珍.江華俊.樣機應(yīng)川.2006.張丿也上張媛媛.科教論叢2007.陶陽.劉志明.法計算機工程與科 2007. 29 (2)： 69 72.(5)朱長征.沈振康一種改進的完全拽索塊匹配笄法| J)紅外與 激比工稚 2004 33(4)： 389 391.| 6j Xu Jit* Biih P Lai Maiu Cheung (Jiok Kh un. Adaptive Motion Trucking Block Mil telling Algorithms ft)r V idro Ceiling ( J| IEEE T runsm* lions On Circuits ani S y si rm s For ilco T Scirch Algorilhm ( Bk:ck M 4 ion Estimation