李慶, 謝一首， 鄭力新， 張?jiān)＠ぃ?莊禮鴻

(華僑大學(xué) 工業(yè)智能化技術(shù)與系統(tǒng)福建省高校工程研究中心， 福建 泉州 362021)

?

采用Matlab的六自由度機(jī)器人三維運(yùn)動(dòng)學(xué)仿真

李慶, 謝一首， 鄭力新， 張?jiān)＠ぃ?莊禮鴻

(華僑大學(xué) 工業(yè)智能化技術(shù)與系統(tǒng)福建省高校工程研究中心， 福建 泉州 362021)

摘要：對(duì)SA1400型六自由度工業(yè)機(jī)器人進(jìn)行三維的運(yùn)動(dòng)學(xué)建模與仿真，建立機(jī)器人的正逆運(yùn)動(dòng)學(xué)方程，并得到正逆解.為驗(yàn)證方程及所求解的正確性，首先，使用Solidworks建立機(jī)器人各個(gè)部件三維模型；然后，用Matlab將所求得的正逆解編寫為程序，導(dǎo)入機(jī)器人各部件，利用Matlab的三維繪圖功能，以顯示機(jī)器人的三維模型，并對(duì)機(jī)器人按軌跡運(yùn)動(dòng)的過程進(jìn)行動(dòng)態(tài)仿真，記錄運(yùn)動(dòng)過程中機(jī)器人各關(guān)節(jié)角度值的變化，提示求解得到的不合理角度值和奇異點(diǎn).結(jié)果表明：仿真過程較真實(shí)地模擬了實(shí)際機(jī)器人的運(yùn)動(dòng)情況，仿真結(jié)果達(dá)到預(yù)期目標(biāo)，證明根據(jù)正逆運(yùn)動(dòng)學(xué)方程求得的解的準(zhǔn)確性和可行性.

關(guān)鍵詞：三維仿真； 工業(yè)機(jī)器人； 六自由度； 運(yùn)動(dòng)學(xué)； Matlab

機(jī)器人是“智能制造”這一主題下最受關(guān)注的領(lǐng)域之一.雖然中國(guó)已成為世界第一的機(jī)器人大國(guó)，但國(guó)產(chǎn)機(jī)器人的水平與國(guó)外相差較大，關(guān)鍵技術(shù)和零部件仍依賴進(jìn)口，精度、穩(wěn)定性和壽命不如國(guó)外生產(chǎn)的機(jī)器人，應(yīng)用也主要集中于沿海地區(qū)的汽車和電子制造業(yè).此外，我國(guó)機(jī)器人產(chǎn)業(yè)還存在“產(chǎn)、學(xué)、研”脫節(jié)的問題，機(jī)器人制造企業(yè)很難通過有效渠道獲得與掌握研究機(jī)構(gòu)及高校的最新成果.在機(jī)器人研究中，仿真是非常重要的環(huán)節(jié)，正確、合理的仿真可以減少實(shí)體機(jī)器人運(yùn)行過程中的各種問題.本文采用Matlab對(duì)六自由度機(jī)器人進(jìn)行三維運(yùn)動(dòng)學(xué)仿真.

(a) 結(jié)構(gòu)圖 (b) D-H坐標(biāo)系　　圖1　SA1400機(jī)器人Fig.1　SA1400 robot

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

機(jī)器人運(yùn)動(dòng)學(xué)實(shí)現(xiàn)由機(jī)器人關(guān)節(jié)變量組成的關(guān)節(jié)空間與笛卡爾空間之間的相互轉(zhuǎn)換.運(yùn)動(dòng)學(xué)的研究包含2個(gè)方面：一是正運(yùn)動(dòng)學(xué)，由機(jī)器人各關(guān)節(jié)角度計(jì)算機(jī)器人末端的位置與姿態(tài)；二是逆運(yùn)動(dòng)學(xué)，由末端的位置與姿態(tài)求解機(jī)器人在此位姿的各關(guān)節(jié)角度[1].SA1400型機(jī)器人的結(jié)構(gòu)圖與D-H坐標(biāo)系，如圖1所示.相鄰連桿n-1與n之間的D-H坐標(biāo)系變換矩陣[2-3]為

各連桿的D-H參數(shù)，如表1所示.表1中：θn為關(guān)節(jié)n的旋轉(zhuǎn)角度，即兩連桿夾角，符合右手定則為正；dn為關(guān)節(jié)n的偏距，即兩連桿距離；αn為關(guān)節(jié)n和n-1軸線之間的夾角，即連桿扭角， 符合右手定則為正；an為關(guān)節(jié)n和n-1軸線之間的公法線距離，即連桿長(zhǎng)度，n=1,2,3,…,6[4]；θ為關(guān)節(jié)角度范圍；vmax為最大關(guān)節(jié)速度.

表1　D-H參數(shù)表

由表1可得各變換矩陣為

式中：Sn=sinθn,Cn=cosθn，下同.因此，末端執(zhí)行器的位姿方程為

機(jī)器人運(yùn)動(dòng)學(xué)逆解的方法主要分為封閉解法和數(shù)值解法，封閉解法又分為代數(shù)解法和幾何解法.封閉解法計(jì)算速度快、效率高，便于實(shí)時(shí)控制，而數(shù)值解法求解速度較慢，所以常用封閉解法求解.大多數(shù)工業(yè)機(jī)器人都可以應(yīng)用Pieper準(zhǔn)則，以簡(jiǎn)化逆解的運(yùn)算[5-7].逆解方程為

(1)

1) 令式(1)左右兩邊矩陣的(3,4)元素(表示矩陣的第3行第4列，下同)相等，可得

S1(px-d6ax)+C1(py-d6ay)=0.

2) 令式(1)左右兩邊(2,4)和(1,4)元素分別相等，并化簡(jiǎn)為

(2a)

(2b)

3) 式(2)中，式(2a)×d4-式(2b)×a3,式(2a)×a3+式(2b)×d4,可得

式中:m=-a2-vC2-uC2,n=-uC2+vS2，則有θ3=ATAN2 (S3,C3).

4) 令式(1)左右兩邊(2,3)與(1,3)元素分別相等，可得

(3a)

(3b)

由式(3a)×S3-式(3b)×C3，可得

則有θ5=ATAN2 (S5,C5).

5) 令式(1)左右兩邊(3,3)元素相等，可得

則有θ4=ATAN2 (S4,C4).

6) 令式(1)左右兩邊(3,2)相等元素，可得

JC6-KS6=-C4.

式中：J=oxS1+oyC1,K=nxS1+nyC1，則

對(duì)于多解問題，首先，應(yīng)根據(jù)關(guān)節(jié)運(yùn)動(dòng)范圍對(duì)所求得的解進(jìn)行取舍，然后，按照以下2種方法得到各關(guān)節(jié)的解.一是先求得多組關(guān)節(jié)角，再用“最短行程”得到一組最優(yōu)解[2,8]；二是先求得θ1的多個(gè)解，再計(jì)算所求解與θ1當(dāng)前角度值的歐氏距離，取其小者作為θ1的最優(yōu)解，用此解求θ2，再利用歐氏距離求取θ2的最優(yōu)解，其余關(guān)節(jié)角依此類推[9].最終可獲得控制機(jī)器人實(shí)現(xiàn)目標(biāo)位姿的1組最優(yōu)關(guān)節(jié)角，2種最優(yōu)解的取值方法，如圖2所示.

2運(yùn)動(dòng)學(xué)仿真

為了驗(yàn)證所求解方程的準(zhǔn)確性和可行性，使用Solidworks建立機(jī)器人的三維模型，并在Matlab中進(jìn)行運(yùn)動(dòng)學(xué)仿真[10-11].仿真流程，如圖3所示.

1) 螺旋線軌跡.螺旋線的軸心線垂直于基座標(biāo)系(底座坐標(biāo)系)的xy平面，在xy平面內(nèi)的坐標(biāo)為(1 200,0)，z坐標(biāo)范圍為0～720mm，螺旋線半徑為100mm.機(jī)器人末端在按軌跡運(yùn)動(dòng)的過程中，其軸線z6始終豎直向下，從下往上運(yùn)動(dòng).在仿真中，為整段螺旋線軌跡選定了720個(gè)關(guān)鍵點(diǎn)，即高度每增加1mm，機(jī)器人末端到達(dá)1個(gè)新的位姿.

螺旋線軌跡仿真過程三維動(dòng)畫截圖與末端軌跡曲線，如圖4所示.由圖4可知：理論軌跡和實(shí)際軌跡重合，說明文中正逆解方程是正確的.

圖3　仿真流程 圖4　螺旋線軌跡　Fig.3　Simulation process Fig.4　Helical path

運(yùn)動(dòng)過程中各關(guān)節(jié)的角度值(θn)，如圖5所示.由圖5可知：在運(yùn)動(dòng)過程中，各關(guān)節(jié)角度值連續(xù)變化，且都在表1所列的關(guān)節(jié)角度范圍內(nèi)，說明文中所解方程是可行的，具有實(shí)用性.

(a) 關(guān)節(jié)1 (b) 關(guān)節(jié)2 (c) 關(guān)節(jié)3

(d) 關(guān)節(jié)4 (e) 關(guān)節(jié)5 (f) 關(guān)節(jié)6圖5　螺旋線軌跡各關(guān)節(jié)角度值Fig.5　Each joint angle value of helical path

2) 球面軌跡.球心坐標(biāo)為(900,-200,900)，半徑為100 mm，球面軌跡是由半徑不同的螺旋線組成.機(jī)器人末端在按軌跡運(yùn)動(dòng)的過程中，其軸線z6始終指向球心，從上往下運(yùn)動(dòng).球面軌跡的關(guān)鍵點(diǎn)數(shù)為1 088個(gè).

圖6　球面軌跡Fig.6　Sphere path

球面軌跡仿真過程三維動(dòng)畫截圖與末端軌跡曲線,如圖6所示.圖6中：機(jī)器人正在走球面的底部軌跡，點(diǎn)云為機(jī)器人的實(shí)際軌跡.由圖6可知：箭頭所指處的理論軌跡和實(shí)際軌跡中不重合，這是因?yàn)闄C(jī)器人在此處走球面軌跡和其末端軸線z6始終指向球心，不能同時(shí)完成，即運(yùn)動(dòng)奇異點(diǎn)，方程解得的關(guān)節(jié)5的角度值超出表1中的角度范圍(-115°～115°)，所以程序采用關(guān)節(jié)5上一時(shí)刻的角度值，導(dǎo)致實(shí)際軌跡出現(xiàn)偏差.

運(yùn)動(dòng)過程中各關(guān)節(jié)的角度值，如圖7所示.由圖7可知：運(yùn)動(dòng)過程中各關(guān)節(jié)角度值連續(xù)變化，且都在表1的關(guān)節(jié)角度范圍內(nèi).

(a) 關(guān)節(jié)1 (b) 關(guān)節(jié)2 (c) 關(guān)節(jié)3

(d) 關(guān)節(jié)4 (e) 關(guān)節(jié)5 (f) 關(guān)節(jié)6圖7　球面軌跡各關(guān)節(jié)角度值Fig.7　Each joint angle value of sphere path

3結(jié)束語

對(duì)SA1400型機(jī)器人進(jìn)行仿真，使機(jī)器人分別按螺旋線軌跡和球面軌跡運(yùn)動(dòng)，并對(duì)運(yùn)動(dòng)過程進(jìn)行分析.實(shí)時(shí)的三維動(dòng)態(tài)顯示使仿真過程更加直觀，關(guān)節(jié)角度值變化曲線圖有利于對(duì)仿真過程進(jìn)行分析.仿真結(jié)果證明文中所求得的解是正確、可行的，且文中的方法與思想也適用于其他關(guān)節(jié)型機(jī)器人.

參考文獻(xiàn)：

[1]韓建海.工業(yè)機(jī)器人[M].3版.武漢:華中科技大學(xué)出版社,2015:60-86.

[2]王其軍,杜建軍.MOTOMAN機(jī)器人逆運(yùn)動(dòng)學(xué)新分析[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2010,42(3):451-454.

[3]王雪松,許世范,郝繼飛.MOTOMAN機(jī)械手逆運(yùn)動(dòng)方程新的推導(dǎo)方法及求解[J].中國(guó)礦業(yè)大學(xué)學(xué)報(bào),2001,30(1):73-76.

[4]蔡自興.機(jī)器人學(xué)[M].2版.北京:清華大學(xué)出版社,2009：34-52.

[5]PAUL R,SHIMANO B,MAYER G.Differential kinematic control equations for simple manipulators[J].IEEE Trans SMC,1981,11(6):449-455.

[6]PIEPER D,ROTH B.The kinematics of manipulaor under computer control[J].Proceeding of the Second International Congress on Theory of Machines and Mechanisms,1969,2:159-169.

[7]JOHN J C.機(jī)器人學(xué)導(dǎo)論[M].3版.贠超,譯.北京:機(jī)械工業(yè)出版社,2014：48-105.

[8]崔建國(guó),宋愛國(guó),黃惟一.遙操作系統(tǒng)中MOTOMAN-SV3機(jī)器人的運(yùn)動(dòng)建模研究[J].東南大學(xué)學(xué)報(bào)(自然科學(xué)版),2003,33(4):424-429.

[9]葉上高,劉電霆.機(jī)器人運(yùn)動(dòng)學(xué)逆解及奇異和多解的處理[J].機(jī)床與液壓,2014,42(3):27-29.

[10]羅家佳,胡國(guó)清.基于Matlab的機(jī)器人運(yùn)動(dòng)仿真研究[J].廈門大學(xué)學(xué)報(bào)(自然科學(xué)版),2005,44(5):640-644.

[11]臧慶凱,李春貴,閆向磊.基于MATLAB的PUMA560機(jī)器人運(yùn)動(dòng)仿真研究[J].廣西科學(xué)院學(xué)報(bào),2010,26(4):397-400.

(責(zé)任編輯： 錢筠英文審校： 吳逢鐵)

3D Kinematics Simulation of Six DOF Robot Using Matlab

LI Qing, XIE Yishou, ZHENG Lixin,ZHANG Yukun, ZHUANG Lihong

(Universities Engineering Research Center of Fujian Province Industrial Intelligent Technology and Systems,Huaqiao University, Quanzhou 362021, China)

Abstract：For the 3D kinematics modeling and simulation of SA1400 six degrees of freedom industrial robot, the robot kinematics equations are established and forward and inverse solutions are got. In order to verify the correctness of the equations and solutions. Firstly, solidworks is used to build 3D model of robot components. Then, forward and inverse solutions are programmed using Matlab, and models of robot components are imported. The 3D graphics functions of Matlab is used to display the 3D model of the robot, and do dynamic simulation of robot trajectory movement, while recording the change in the angle of each joint of robot during the simulation and suggesting irrational angle values and singular points. The results show that the process factually simulates the actual movement of the robot. The simulation results achieve the expected goal, and prove the accuracy and feasibility of solutions obtained from forward and inverse kinematics equations.

Keywords：3D simulation; industrial robot; six degrees of freedom; kinematics; Matlab

中圖分類號(hào)：TP 391.9

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

基金項(xiàng)目：福建省科技廳科研計(jì)劃資助項(xiàng)目(2013H2002)； 華僑大學(xué)研究生科研創(chuàng)新能力培育計(jì)劃資助項(xiàng)目(1400422004)

通信作者：鄭力新(1967-)，男，教授，博士，主要從事工業(yè)自動(dòng)化技術(shù)、人工智能的研究.E-mail:zlxgxy@hqu.edu.cn.

收稿日期：2015-12-29

doi:10.11830/ISSN.1000-5013.2016.03.0299

文章編號(hào)：1000-5013(2016)03-0299-05