人臉追蹤機(jī)械臂的運(yùn)動(dòng)學(xué)研究

崔 晨，李麗宏

(太原理工大學(xué) 電氣與動(dòng)力工程學(xué)院， 太原 030600)

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

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

圖1 人臉追蹤機(jī)械臂仿真原理示意圖

圖2 人臉追蹤機(jī)械臂工作流程框圖

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

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

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

圖3 機(jī)械臂后三軸軸線示意圖

圖4 六自由度協(xié)作機(jī)械臂機(jī)械結(jié)構(gòu)設(shè)計(jì)模型示意圖

1 六自由度機(jī)械臂的正運(yùn)動(dòng)學(xué)分析

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

采用D-H法建立連桿坐標(biāo)系，分別給六個(gè)關(guān)節(jié)建立一個(gè)參考坐標(biāo)系，實(shí)現(xiàn)任意相鄰兩個(gè)坐標(biāo)系間的轉(zhuǎn)換，最后得出正運(yùn)動(dòng)學(xué)總變換矩陣。

如圖5所示即為建立的六自由度協(xié)作機(jī)械臂的笛卡爾直角坐標(biāo)系，表1為所建模型對(duì)應(yīng)的參數(shù)值。

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

(1)

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

(2)

(3)

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

2 六自由度機(jī)械臂的逆運(yùn)動(dòng)學(xué)分析

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

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有兩個(gè)解。

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

(10)

因?yàn)棣?有兩個(gè)解，所以θ5有4個(gè)解。

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

(11)

當(dāng)s5=0時(shí)，即θ5=nπ(n=0,1,2…)，此時(shí)機(jī)構(gòu)發(fā)生奇異，無法求出θ6。

當(dāng)θ5≠0時(shí)，

θ6=atan2(s6,c6)

(12)

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

式(1)等號(hào)兩端矩陣的(1，3)，(2，3)元素對(duì)應(yīng)相等，可得：

-c234s5=axc1+ays1

-s234s5=az

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

(13)

2.2 求解θ2、θ3和θ4

(14)

令式(14)的(1，4)，(2，4)元素對(duì)應(yīng)相等，得：

-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)

進(jìn)而求得：

θ3=θ23-θ2

(19)

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

θ4=θ234-θ23

(20)

因?yàn)閙1、m2分別有4個(gè)值，所以θ2有8個(gè)解，繼而θ3和θ4也分別有8個(gè)解。

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

3 仿真驗(yàn)證

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

3.1 六自由度機(jī)械臂工作空間分析

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

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

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

3.2 六自由度協(xié)作機(jī)械臂正逆解驗(yàn)證

選取的位姿值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°。根據(jù)前文所述的逆解計(jì)算方法，運(yùn)用Matlab編寫矩陣的逆解程序，將W代入編寫的Matlab逆解函數(shù)中得到8組逆解[16]，如表2。

表2 8組逆解

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

圖7 六自由度協(xié)作機(jī)械臂實(shí)際運(yùn)行的最終狀態(tài)

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

(21)

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

(22)

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

對(duì)比Matlab機(jī)器人工具箱仿真得出的正解矩陣(22)與前文正運(yùn)動(dòng)學(xué)建模得出的正解矩陣(21)T1。計(jì)算兩個(gè)矩陣之間的相對(duì)誤差，其余7組對(duì)比方法相同。

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

(23)

(24)

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

表3 誤差 ‰

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

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

綜上，仿真結(jié)果均和正解矩陣得出的結(jié)果一致，證明運(yùn)動(dòng)學(xué)正解模型正確。

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

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

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

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

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

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

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

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

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

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

5 結(jié)論

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

運(yùn)用蒙特卡洛法分析了機(jī)械臂工作空間，選取工作空間的位姿值，通過Matlab中的機(jī)器人工具箱，驗(yàn)證了機(jī)械臂運(yùn)動(dòng)學(xué)正逆解的正確性，從而得到了該機(jī)械結(jié)構(gòu)類型的六軸機(jī)械臂的一般性運(yùn)動(dòng)學(xué)逆解方法。