李慶1，謝一首1，鄭力新1，周凱汀2,張裕坤1

　?。?.華僑大學(xué) 工學(xué)院，福建 泉州 362021；2.華僑大學(xué) 信息科學(xué)與工程學(xué)院，福建 廈門 361021）

　　摘要：根據(jù)STEPSA1400型工業(yè)機器人的具體結(jié)構(gòu)特點，建立了機器人的運動學(xué)方程，使用只需一次矩陣逆乘的逆解方法，求出逆解。與常規(guī)求解方法相比，此方法減少了多次矩陣逆乘帶來的計算量。在解的表達式中，采用雙變量正切函數(shù)以避免解的丟失。針對多重解問題，采用“最短行程”原則，選取與當(dāng)前關(guān)節(jié)角度值的歐氏距離較小的解作為逆解結(jié)果。最后，使用MATLAB編寫程序，對文中推導(dǎo)出的方程進行驗證與仿真，實驗結(jié)果證明了解的準(zhǔn)確性和可行性。對該型機器人的運動學(xué)分析與仿真為其后的離線編程、軌跡規(guī)劃等打下了基礎(chǔ)，同時，文中的方法與思想也適用于其他關(guān)節(jié)型機器人。

　　關(guān)鍵詞：工業(yè)機器人；SA1400機器人；六自由度；逆運動學(xué)；最短行程；MATLAB仿真

0引言

　　華僑大學(xué)研究生科研創(chuàng)新能力培育計劃資助項目（1400422004）近年來，隨著經(jīng)濟和社會的發(fā)展，我國出現(xiàn)人力成本上漲、勞動力供給減少以及制造業(yè)就業(yè)意愿下降的現(xiàn)象，這些現(xiàn)象嚴(yán)重制約了我國制造業(yè)的國際競爭力。于是一些企業(yè)開始把目光投向“機器換人”，利用自動化技術(shù)來建設(shè)無人化工廠以解決當(dāng)前困局，制造業(yè)的轉(zhuǎn)型升級已是大勢所趨。面對德國提出的“工業(yè)4.0”，我國出臺的“中國制造2025”將重點發(fā)展工業(yè)機器人與新一代信息技術(shù)等領(lǐng)域，“智能制造”成為了中國制造的主攻方向，而機器人也成為這一主題下最受關(guān)注的領(lǐng)域之一。實現(xiàn)“中國制造2025”，最重要的智能部件就是網(wǎng)絡(luò)化的機器人，機器人產(chǎn)業(yè)將成為未來幾十年內(nèi)全球制造業(yè)的角力場。2013年，中國工業(yè)機器人的總銷量為3.7萬臺，成為世界第一的機器人大國，也是全球增長速度最快的機器人市場。2014年，全球工業(yè)機器人的銷量為22.9萬臺，中國內(nèi)地售出5.7萬臺，占全球銷量的四分之一［1］。

　　目前，機器人正解的求法已比較統(tǒng)一，而逆解的求解方法有多種，主要分為封閉解法和數(shù)值解法。封閉解法又分為代數(shù)解法和幾何解法。封閉解法計算速度快、效率高、便于實時控制，而數(shù)值解法因其迭代性質(zhì)，使其求解速度較慢，所以大多數(shù)情況下都是使用封閉解法［23］。逆解過程中，一般在關(guān)節(jié)角度范圍內(nèi)計算機器人關(guān)節(jié)角度，文獻［4］在解關(guān)節(jié)角時采用單變量反正切函數(shù)，可能造成一個解的丟失。機器人逆解存在多解，如文獻［5］中就有8組解，但控制機器人只能有一組解，而文中沒有給出選取最優(yōu)解的方法。

1運動學(xué)模型的建立

　　本文根據(jù)上海新時達機器人有限公司SA系列工業(yè)機器人中的1400型機器人的特點進行研究。SA1400型機器人有6個自由度，而且6個關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)。為了描述機器人各連桿之間的相對位置和方向關(guān)系，需要根據(jù)關(guān)節(jié)結(jié)構(gòu)在每個連桿上建立一個連桿坐標(biāo)系。常用的方法是D-H (Denavit-Hartenberg)參數(shù)法，即使用矩陣方法來描述運動學(xué)問題。只要已知各關(guān)節(jié)的D-H參數(shù)，就可根據(jù)正運動學(xué)公式A1A2A3A4A5A6=0T6得到機器人末端的位置和姿態(tài)［2］。

　　SA1400機器人各連桿坐標(biāo)系如圖1所示，相鄰兩連桿n-1與n之間的相對關(guān)系能夠按照兩次旋轉(zhuǎn)和兩次平移的四次齊次變換來建立，并把齊次變換矩陣記為An。此關(guān)系式為：

　　式中：θ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軸線之間的公法線距離，即連桿長度，n=1,2,3…6［5］。D-H參數(shù)表如表1。

　　根據(jù)表1可得各變換矩陣如下：

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

　　0T6=A1A2A3A4A5A6

　　2逆運動學(xué)方程的推導(dǎo)及求解

　　一般具有6個自由度的機器人沒有逆運動學(xué)封閉解，但某些特殊結(jié)構(gòu)的機器人還是可以得到多組封閉解的，大多數(shù)工業(yè)機器人都可用Pieper提出的方法來求解，這種方法是針對6個關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)且后3個關(guān)節(jié)軸線相交的操作臂。此方法也可應(yīng)用于包括移動關(guān)節(jié)的其他形式的操作臂。觀察圖1中機器人，其3、4、5關(guān)節(jié)的軸線Z3、Z4、Z5交于一點，因此，這3個關(guān)節(jié)的運動不能產(chǎn)生沿Z2軸線方向的運動，所以這3個關(guān)節(jié)的變換矩陣乘積A3A4A5的第3行第4列上的元素為零。具有此特點的機器人，其運動學(xué)逆解存在以下簡便求解方法［69］：

　　A3A4A5=A－12A－11TA－16(1)

　　等式左邊為

　　(1)求θ1

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

　　S1(px－d6ax)+C1(py－d6ay)=0

　　則

　　(2)求θ2

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

　　a3C3-d4S3=-a2-C2v-S2u…①

　　a3S3+d4C3=-C2u+S2v……②(2)

　　式中：u=a1+C1(axd6－px)+S1(py－ayd6)，v=azd6－pz+d1，①②兩邊平方相加，且令w=(d24+a23－a22－v2－u2)/(2a2)，得:

　　C2v+S2u=w

　　(6)求θ6

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

　　JC6－KS6=－C4

　　從以上各角度表達式可知，逆解存在多解，而控制機器人各關(guān)節(jié)的角度是唯一的。若忽略避障要求和軌跡優(yōu)化問題，可按照以下步驟得到唯一解。首先，如有必要，將所得解加減360°，以補出關(guān)節(jié)角度表達式值域沒有包含的其他可能解；其次，由于關(guān)節(jié)運動范圍的限制，應(yīng)舍去其中一些解（甚至全部）；最后，根據(jù)“最短行程”原則，選取一個最近解，使得每一個運動關(guān)節(jié)的移動量最小，以保證運動的連續(xù)、快速和低能耗，同時可用加權(quán)法使得解側(cè)重于移動小連桿而不是移動大連桿［8，10］。

　　3計算實例

　　已知空間中的A,B兩點，其位姿矩陣分別為：

　　逆解得到A點各關(guān)節(jié)角度θ1~θ6依次為：0(180),0(83.12),0,0(90),0(0),0(0)（單位：度）；B點6個關(guān)節(jié)角度依次為：25(-155),-15(89.60),20,-30(-150),15,-35(-93.29)（單位：度）。括號中為該關(guān)節(jié)的第二個解，本計算實例中假定各關(guān)節(jié)上一時刻的角度都為0度，則選取與0度的歐式距離較小的解為最優(yōu)解。正解得到末端位姿分別為XA=903 mm,YA=0,ZA=1 120 mm,αA=0°,βA=-90°,γA=180°；XB=687.2 mm,YB=-306.5 mm,ZB=1 014.9 mm,αB=-99.6°, βB=-22.2°,γB=-109.4°（α,β,γ為位姿坐標(biāo)系相對于機器人底座坐標(biāo)系或基坐標(biāo)系的RPY角）。計算結(jié)果表明，本文中的正逆解方程是正確的。

4運動學(xué)仿真

　　為驗證本文正逆解方程的準(zhǔn)確性和可行性，使用MATLAB軟件對機器人走曲線軌跡的運動過程進行仿真［11］。仿真過程三維動畫截圖與末端軌跡曲線如圖2，圖中的理論軌跡與實際軌跡重合，說明本文正逆解方程是正確的。運動過程中各關(guān)節(jié)角度值如圖3，從圖中可知，運動過程中各關(guān)節(jié)角度值變化連續(xù)，且都在表1所列的關(guān)節(jié)角度范圍內(nèi)，說明本文所解方程是可行的，具有實用性［12］。

5結(jié)束語

　　本文采用DH參數(shù)法建立了STEPSA1400型機器人的連桿坐標(biāo)系，確定了該型機器人的DH參數(shù)及連桿間的位姿變換矩陣，求出了正運動學(xué)方程。針對3個相鄰軸相交于一點的6自由度操作臂，在研究總結(jié)了三軸相交的Pieper解法后，使用了一種避免大量矩陣逆乘運算的逆解方法?？紤]到三角函數(shù)的取值范圍和機器人各關(guān)節(jié)角之間的影響，角度值方程采用了雙變量反正切函數(shù)，通過自變量的符號確定關(guān)節(jié)角度所在的象限，進而取得合理解。針對逆解過程中出現(xiàn)的多解問題，采用基于歐氏距離的“最短行程”原則選取最優(yōu)解。為了驗證所求解方程的準(zhǔn)確性和可行性，使用MATLAB進行了運動學(xué)仿真，仿真過程較真實地模擬了實際機器人的運動情況，仿真結(jié)果達到預(yù)期目標(biāo)。本文為該型機器人的應(yīng)用及其動力學(xué)與控制器的研究打下基礎(chǔ)，同時，也為其他六自由度機器人運動學(xué)分析提供參考。

參考文獻

　?。?］ 中國測控網(wǎng).工業(yè)4.0時代:工業(yè)機器人行業(yè)發(fā)展機遇分析［EB/OL］.(20151202)［20151204］ http://www.ck365. cn/wap/index.php?mid=21&itemid=39875.

　　［2］ 韓建海.工業(yè)機器人（第三版）［M］.武漢:華中科技大學(xué)出版社,2015.

　?。?］ 王其軍,杜建軍.MOTOMAN機器人逆運動學(xué)新分析［J］.哈爾濱工業(yè)大學(xué)學(xué)報,2010,42(3):451454．

　　［4］ 王雪松,許世范,郝繼飛.MOTOMAN機械手逆運動方程新的推導(dǎo)方法及求解［J］.中國礦業(yè)大學(xué)學(xué)報,2001,30(1):7376.

　?。?］ 蔡自興.機器人學(xué)（第二版）［M］.北京:清華大學(xué)出版社,2009.

　?。?］ PAUL R, SHIMANO B, MAYER G. Differential kinematic control equations for simple manipulators［J］. IEEE Trans SMC,1981,11(6):449455.

　?。?］ PIEPER D, ROTH B. The kinematics of manipulaor under computer control［C］. Proceeding of the Second International Congress on Theory of Machines and Mechanisms,1969(2):159169.

　　［8］ CRAIG J J.機器人學(xué)導(dǎo)論(第三版)［M］.贠超，譯.北京:機械工業(yè)出版社,2014.

　?。?］ 崔建國,宋愛國,黃惟一.遙操作系統(tǒng)中MOTOMANSV3機器人的運動建模研究［J］.東南大學(xué)學(xué)報:自然科學(xué)版,2003,33(4):424429.

　?。?0］ 葉上高,劉電霆.機器人運動學(xué)逆解及奇異和多解的處理［J］.機床與液壓,2014,42(3):2729.

　　［11］ 林海峰,王姮.多關(guān)節(jié)機器人工作空間仿真方法［J］.微型機與應(yīng)用,2014,33(2):7274.

　?。?2］ 陳發(fā)堂,牛勇清,韓娜娜,等.協(xié)議一致性測試平臺的搭建及仿真實現(xiàn)［J］.電子技術(shù)應(yīng)用,2014,40(4):137140.