采用Matlab的六自由度機器人三維運動學仿真

李慶, 謝一首， 鄭力新， 張裕坤， 莊禮鴻

(華僑大學 工業智能化技術與系統福建省高校工程研究中心， 福建 泉州 362021)

?

采用Matlab的六自由度機器人三維運動學仿真

李慶, 謝一首， 鄭力新， 張裕坤， 莊禮鴻

(華僑大學 工業智能化技術與系統福建省高校工程研究中心， 福建 泉州 362021)

摘要：對SA1400型六自由度工業機器人進行三維的運動學建模與仿真，建立機器人的正逆運動學方程，并得到正逆解.為驗證方程及所求解的正確性，首先，使用Solidworks建立機器人各個部件三維模型；然后，用Matlab將所求得的正逆解編寫為程序，導入機器人各部件，利用Matlab的三維繪圖功能，以顯示機器人的三維模型，并對機器人按軌跡運動的過程進行動態仿真，記錄運動過程中機器人各關節角度值的變化，提示求解得到的不合理角度值和奇異點.結果表明：仿真過程較真實地模擬了實際機器人的運動情況，仿真結果達到預期目標，證明根據正逆運動學方程求得的解的準確性和可行性.

關鍵詞：三維仿真； 工業機器人； 六自由度； 運動學； Matlab

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

(a) 結構圖 (b) D-H坐標系　　圖1　SA1400機器人Fig.1　SA1400 robot

1運動學建模

機器人運動學實現由機器人關節變量組成的關節空間與笛卡爾空間之間的相互轉換.運動學的研究包含2個方面：一是正運動學，由機器人各關節角度計算機器人末端的位置與姿態；二是逆運動學，由末端的位置與姿態求解機器人在此位姿的各關節角度[1].SA1400型機器人的結構圖與D-H坐標系，如圖1所示.相鄰連桿n-1與n之間的D-H坐標系變換矩陣[2-3]為

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

表1　D-H參數表

由表1可得各變換矩陣為

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

機器人運動學逆解的方法主要分為封閉解法和數值解法，封閉解法又分為代數解法和幾何解法.封閉解法計算速度快、效率高，便于實時控制，而數值解法求解速度較慢，所以常用封閉解法求解.大多數工業機器人都可以應用Pieper準則，以簡化逆解的運算[5-7].逆解方程為

(1)

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

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

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

(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，則

對于多解問題，首先，應根據關節運動范圍對所求得的解進行取舍，然后，按照以下2種方法得到各關節的解.一是先求得多組關節角，再用“最短行程”得到一組最優解[2,8]；二是先求得θ1的多個解，再計算所求解與θ1當前角度值的歐氏距離，取其小者作為θ1的最優解，用此解求θ2，再利用歐氏距離求取θ2的最優解，其余關節角依此類推[9].最終可獲得控制機器人實現目標位姿的1組最優關節角，2種最優解的取值方法，如圖2所示.

2運動學仿真

為了驗證所求解方程的準確性和可行性，使用Solidworks建立機器人的三維模型，并在Matlab中進行運動學仿真[10-11].仿真流程，如圖3所示.

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

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

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

運動過程中各關節的角度值(θn)，如圖5所示.由圖5可知：在運動過程中，各關節角度值連續變化，且都在表1所列的關節角度范圍內，說明文中所解方程是可行的，具有實用性.

(a) 關節1 (b) 關節2 (c) 關節3

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

2) 球面軌跡.球心坐標為(900,-200,900)，半徑為100 mm，球面軌跡是由半徑不同的螺旋線組成.機器人末端在按軌跡運動的過程中，其軸線z6始終指向球心，從上往下運動.球面軌跡的關鍵點數為1 088個.

圖6　球面軌跡Fig.6　Sphere path

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

運動過程中各關節的角度值，如圖7所示.由圖7可知：運動過程中各關節角度值連續變化，且都在表1的關節角度范圍內.

(a) 關節1 (b) 關節2 (c) 關節3

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

3結束語

對SA1400型機器人進行仿真，使機器人分別按螺旋線軌跡和球面軌跡運動，并對運動過程進行分析.實時的三維動態顯示使仿真過程更加直觀，關節角度值變化曲線圖有利于對仿真過程進行分析.仿真結果證明文中所求得的解是正確、可行的，且文中的方法與思想也適用于其他關節型機器人.

參考文獻：

[1]韓建海.工業機器人[M].3版.武漢:華中科技大學出版社,2015:60-86.

[2]王其軍,杜建軍.MOTOMAN機器人逆運動學新分析[J].哈爾濱工業大學學報,2010,42(3):451-454.

[3]王雪松,許世范,郝繼飛.MOTOMAN機械手逆運動方程新的推導方法及求解[J].中國礦業大學學報,2001,30(1):73-76.

[4]蔡自興.機器人學[M].2版.北京:清華大學出版社,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.機器人學導論[M].3版.贠超,譯.北京:機械工業出版社,2014：48-105.

[8]崔建國,宋愛國,黃惟一.遙操作系統中MOTOMAN-SV3機器人的運動建模研究[J].東南大學學報(自然科學版),2003,33(4):424-429.

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

[10]羅家佳,胡國清.基于Matlab的機器人運動仿真研究[J].廈門大學學報(自然科學版),2005,44(5):640-644.

[11]臧慶凱,李春貴,閆向磊.基于MATLAB的PUMA560機器人運動仿真研究[J].廣西科學院學報,2010,26(4):397-400.

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

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

中圖分類號：TP 391.9

文獻標志碼：A

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

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

收稿日期：2015-12-29

doi:10.11830/ISSN.1000-5013.2016.03.0299

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