崔 晨，李麗宏

(太原理工大學 電氣與動力工程學院， 太原 030600)

人機協(xié)作是工業(yè)機器人領域最熱門的研究方向之一，協(xié)作機器人已經成為一個新的研究熱點[1]。在協(xié)作機器人領域，鮮有人將人臉追蹤和機械臂結合。因此本研究設計了人臉追蹤機械臂，六軸機械臂由六個高集成度科爾摩根關節(jié)模組構成，末端連接顯示屏和Kinect視覺檢測裝置，用于將顯示屏實時跟隨人臉，協(xié)助人的日常工作，其原理如圖1所示。

人臉追蹤機械臂的工作流程如下(見圖2)，首先是通過設計的上位機界面開啟Kinect視覺傳感器并使能機械臂，Kinect將實時捕捉的人臉位姿返回機械臂控制模塊。同時控制模塊通過關節(jié)模組編碼器采集電機當前位置。兩者在控制模塊中經過運動學解析得出各關節(jié)轉動量，控制機械臂運動到指定位姿。

圖1 人臉追蹤機械臂仿真原理示意圖

圖2 人臉追蹤機械臂工作流程框圖

而人臉追蹤機械臂的控制主要是對其末端執(zhí)行器的位置和姿態(tài)進行控制，以使機械臂完成相應的動作。因此，對機械臂進行運動學分析是機械臂實現人臉追蹤必不可少的一個環(huán)節(jié)。

當前已有不少學者對六自由機械臂運動學進行了分析研究，例如：姜宏超等[2]對符合Pieper準則[3]的六自由度模塊化機械臂MT-ARM進行逆運動學分析；張化平等[4]針對MOTOMAN-MA1400安川弧焊工業(yè)機器人進行運動學分析，并運用Pro-E建立了機械臂運動仿真平臺；王立權等[5]針對6R關節(jié)型機器人推導了代數逆解結果，并研究了逆解的漏根、增根和多根問題；莫毅[6]對六自由度工業(yè)機器人建立D-H模型，通過對機器人本體的標定測量，驗證了D-H模型的正確性。

上述研究雖然都是針對六自由度的機械臂，但是機械臂機械結構均是末端三軸軸線相交于一點，符合Pieper法則之一，即3個相鄰關節(jié)交于一點；而本文后三軸軸線不相交于一點，如圖3，但是2、3、4軸軸線方向平行，如圖4，同樣滿足Pieper法則，即3個相鄰關節(jié)軸相互平行。雖然都是六自由度機械臂存在封閉解[7]的充分條件。但是因為現實中大多數機械臂符合前者相鄰三軸交于一點的條件，相應的其運動學研究也很豐富。鮮有滿足后者條件的機械臂，對應的其運動學研究較少。因此針對本機械臂，通過D-H法得到機械臂正解，而求逆解方法采用解析法[8]，可以得到全部解。通過Matlab機器人工具箱檢驗正逆運動學，運用膠囊碰撞檢測算法設定約束條件選取最優(yōu)解。本機械臂設定允許位置誤差范圍為10 mm，姿態(tài)角誤差范圍為0.02。

圖3 機械臂后三軸軸線示意圖

圖4 六自由度協(xié)作機械臂機械結構設計模型示意圖

1 六自由度機械臂的正運動學分析

機械臂機械結構仿照優(yōu)傲公司UR系列，由六個科爾摩根RGM關節(jié)模組構成，六個關節(jié)均為轉動關節(jié)。分別為基座關節(jié)1，大臂關節(jié)2，小臂關節(jié)3，手腕關節(jié)4、5、6，分別控制俯仰角、偏航角、滾動角[9]。

采用D-H法建立連桿坐標系，分別給六個關節(jié)建立一個參考坐標系，實現任意相鄰兩個坐標系間的轉換，最后得出正運動學總變換矩陣。

如圖5所示即為建立的六自由度協(xié)作機械臂的笛卡爾直角坐標系，表1為所建模型對應的參數值。

圖5 六自由度協(xié)作機械臂D-H坐標系

(1)

將D-H參數代入式(1)，得到相鄰關節(jié)間的變換矩陣,最終得到足端坐標系相對于基座標的總變換矩陣為:

(2)

(3)

即式(3)為式(2)左上角元素構成的3維方陣。

2 六自由度機械臂的逆運動學分析

運動學逆解是已知末端位姿求解各關節(jié)角度，其解可能存在多重解，也可能無解。下面采用矩陣逆乘法[12]求解六自由度協(xié)作機械臂逆解。

2.1 首先求解θ1、θ5和θ6

(4)

由式(3)左右兩邊矩陣第三行元素相等得：

nxs1-nyc1=c6s5

(5)

oxs1-oyc1=-s5s6

(6)

axs1-ayc1=c5

(7)

pxs1-pyc1=70c5+155.3

(8)

由式(7)、式(8)得：

θ1=atan2(py-70ay,px-70ax)+

(9)

為使θ1有解，必須有(px-70ax)2+(py-70ay)2-155.32≥0，θ1有兩個解。

由等式(4)、式(5)得：

(10)

因為θ1有兩個解，所以θ5有4個解。

由式(7)、式(10)得：

(11)

當s5=0時，即θ5=nπ(n=0,1,2…)，此時機構發(fā)生奇異，無法求出θ6。

當θ5≠0時，

θ6=atan2(s6,c6)

(12)

將θ5代入式(12)，θ6也有4個解。

式(1)等號兩端矩陣的(1，3)，(2，3)元素對應相等，可得：

-c234s5=axc1+ays1

-s234s5=az

聯(lián)合求解可得：

(13)

2.2 求解θ2、θ3和θ4

(14)

令式(14)的(1，4)，(2，4)元素對應相等，得：

-350c23-400c2=m1

(15)

-350s23-400s2=m2

(16)

其中，

m1=102.3s6(nxc1+nys1)-70axc1+pxc1-

70ays1+pys1+102.3c6(oxc1+oys1)

m2=pz-70az+102.3ozc6(oxc1+oys1)

(17)

θ23=atan2(400s2+m2,400c2+m1)

(18)

進而求得：

θ3=θ23-θ2

(19)

由式(13)、式(18)得：

θ4=θ234-θ23

(20)

因為m1、m2分別有4個值，所以θ2有8個解，繼而θ3和θ4也分別有8個解。

綜上所述，本研究所描述的六自由度協(xié)作機械臂最終可以得到8組逆解。

3 仿真驗證

為了驗證六自由度協(xié)作機械臂的正逆運動學的正確性,首先應用蒙特卡洛法分析了機械臂工作空間，在可達工作空間范圍內隨機選取姿態(tài)點，結合機械臂實例并利用Matlab中的機器人工具箱[13]進行驗證。下文選取一組工作空間內的位姿值闡述運動學正逆解相互驗證分析[14]的具體方法，統(tǒng)計出8組逆解計算精度，并進行理論分析和解釋。

3.1 六自由度機械臂工作空間分析

使用蒙特卡洛法[15]計算機械臂工作空間，基于運動學模型，通過計算機偽隨機算法，在各關節(jié)物理約束下生成關節(jié)變量，代入正解矩陣計算機械臂末端執(zhí)行器的位置。

由圖6所示的三維工作空間示意圖得出，六自由度協(xié)作機械臂在X軸方向為：-882 mm到882 mm，Y軸方向：-882 mm到882 mm,Z軸方向：-363 mm到967 mm。

圖6 六自由度協(xié)作機械臂三維工作空間示意圖

3.2 六自由度協(xié)作機械臂正逆解驗證

選取的位姿值W=[-387.300 0、-204.600 0、487.300 0、-54°、30°、125°]，位置值px、py、pz分別為：-387.300 0、-204.600 0、487.300 0和姿態(tài)角R、P、Y分別為：-54°、30°、125°。根據前文所述的逆解計算方法，運用Matlab編寫矩陣的逆解程序，將W代入編寫的Matlab逆解函數中得到8組逆解[16]，如表2。

表2 8組逆解

將所得的8組逆解分別輸入機械臂控制器中，機械臂運行的最終狀態(tài)如圖7所示。

圖7 六自由度協(xié)作機械臂實際運行的最終狀態(tài)

將8組逆解依次代入正解齊次變換矩陣[17]式(2)得到8組正解矩陣。其中，T1為第一組逆解代入得到的正解矩陣。

(21)

隨后采用機器人工具箱Robotics Toolbox[17]中提供的函數對六自由度機器人建模，用表二中8組逆解中的第一組代入仿真程序得到如圖8所示的仿真結果，并得到仿真正解矩陣式(22)。

(22)

圖8 六自由度協(xié)作機械臂Matlab仿真結果

對比Matlab機器人工具箱仿真得出的正解矩陣(22)與前文正運動學建模得出的正解矩陣(21)T1。計算兩個矩陣之間的相對誤差，其余7組對比方法相同。

誤差分析分為姿態(tài)誤差和位置誤差，姿態(tài)誤差，由式(3)所示的姿態(tài)矩陣，即矩陣的左上角三維矩陣求得。位置誤差則有矩陣最后一列的位置值求得。

(23)

(24)

根據式(23)和式(24)可得全部8組誤差，如表3所示。

表3 誤差 ‰

由表3可得姿態(tài)誤差在2‰與3‰之間，位置誤差則保持在萬分位上，完全能夠滿足人臉追蹤機械臂得定位要求。

在機械臂可達工作空間范圍內另外選取5組位姿值，重復同樣的實驗步驟，得到的姿態(tài)誤差和位置精度與表3相符。

綜上，仿真結果均和正解矩陣得出的結果一致，證明運動學正解模型正確。

4 最優(yōu)解的選取

針對六自由度協(xié)作機械臂最優(yōu)解的選取經過三輪篩選，以表2的8組逆解為例進行說明。

第一輪篩選：基于機械臂物理關節(jié)約束條件，防止機械臂在運行過程中關節(jié)軸與連接桿間自身產生物理碰撞，根據機械臂機械結構得出本文六自由度協(xié)作機械臂物理關節(jié)約束如下：

-180°<θ1<180°, -180°<θ2<0

-130°<θ3<130°, -180°<θ4<180°

-160°<θ5<160°, -180°<θ6<180°

第二輪篩選：如圖7所示，機械臂安裝在工作臺上，為防止機械臂運行中觸碰工作臺，根基幾何位形，大臂和小臂的位姿形態(tài)應符合：兩者的垂直距離之和必須高于工作桌面。

因此θ2和θ3需滿足如下關系式：a3sin(-θ2-θ3)+a2sin(-θ2)<0。

經過前二輪篩選后,表2前四組解均滿足條件。

第三輪篩選：基于第二輪篩選，計算符合物理約束的每組逆解的關節(jié)轉角絕對值總和，計算結果越小，表示轉動角度和越小，轉動消耗的能量越小，第一組解計算結果最小，因此第一組解選為最優(yōu)解。

5 結論

針對本研究的人臉追蹤機械臂，根據相鄰三個關節(jié)軸軸線方向平行存在封閉解這一充分條件，運用D-H法建立機械臂的關節(jié)坐標系，求得機械臂的運動學正解，采用解析法，通過矩陣逆乘求解各關節(jié)角度值。

運用蒙特卡洛法分析了機械臂工作空間，選取工作空間的位姿值，通過Matlab中的機器人工具箱，驗證了機械臂運動學正逆解的正確性，從而得到了該機械結構類型的六軸機械臂的一般性運動學逆解方法。