排爆機(jī)器人機(jī)械臂運(yùn)動(dòng)控制研究

張 潔，孟 浩，王 杰，許家紫

全球范圍內(nèi)爆炸恐怖襲擊事件頻發(fā)，無論是“911”恐怖襲擊，還是我國以“東突”為代表的“三股勢(shì)力”多次采取爆炸手段制造出來的一系列惡性恐怖事件，無疑對(duì)社會(huì)穩(wěn)定、國家安全和人民生命財(cái)產(chǎn)都帶來嚴(yán)重危害［1］。隨著國際上反恐形勢(shì)的日趨嚴(yán)峻和反恐斗爭(zhēng)的深入，國際上的一些大公司、大學(xué)及研究所，如美國的Remotec、Robot，英國的P.W.Allen、ABP，德國的Telerob，加拿大的Pedsco，北京航空航天大學(xué)，上海交通大學(xué)，華南理工大學(xué)，中科院沈陽自動(dòng)化研究所等，都先后從事相關(guān)系統(tǒng)和技術(shù)的研究并取得了進(jìn)步。

本文對(duì)機(jī)械臂系統(tǒng)穩(wěn)定的協(xié)調(diào)控制問題進(jìn)行了研究，基于真實(shí)的工程背景（考慮排爆實(shí)際參數(shù)要求和主要桿件的材質(zhì)特性），從工程實(shí)現(xiàn)的角度解決了對(duì)機(jī)械臂的協(xié)調(diào)控制。與現(xiàn)有的排爆機(jī)器人機(jī)械臂控制技術(shù)相比，該機(jī)械臂具有大力矩、成本低、操作簡(jiǎn)單、控制可靠的顯著特點(diǎn)，可為產(chǎn)品化或批量生產(chǎn)提供參考。

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

1.1 機(jī)械臂運(yùn)動(dòng)學(xué)正解

本文研制的機(jī)器人是操作簡(jiǎn)單、適應(yīng)性強(qiáng)、能夠勝任多種排爆任務(wù)的四自由度遠(yuǎn)程遙控排爆機(jī)器人，如圖1（a）所示。其機(jī)械臂屬于關(guān)節(jié)式機(jī)器臂，4個(gè)關(guān)節(jié)都是轉(zhuǎn)動(dòng)關(guān)節(jié)。前2個(gè)關(guān)節(jié)確定手腕參考點(diǎn)的位置，后2個(gè)關(guān)節(jié)確定手爪的方位。關(guān)節(jié)1、2和3的軸線水平，且平行，距離分別為a1，a2。關(guān)節(jié)3和4的軸線垂直相交于一點(diǎn)，該點(diǎn)作為手爪的參考點(diǎn)，也作為連桿坐標(biāo)系｛3｝和｛4｝的原點(diǎn)。各連桿坐標(biāo)系如圖1（b）所示。

圖1 排爆機(jī)器人

表1 機(jī)械臂D－H參數(shù)表

由以上圖表可看出，坐標(biāo)系i－1繞Zi－1軸旋轉(zhuǎn)θi角，并沿Zi－1軸平移距離di，再沿Xi軸平移距離ai，最后繞Xi軸旋轉(zhuǎn)αi角便可得到坐標(biāo)系i。這種一個(gè)連桿到下一個(gè)連桿間相對(duì)關(guān)系可用相對(duì)位置的四個(gè)齊次變換來描述，并稱為Ai矩陣。關(guān)系式為：

將表1中參數(shù)代入式（2）求得機(jī)械臂正運(yùn)動(dòng)學(xué)方程：

其中：0T4表示機(jī)械臂的末端裝置即連桿4的坐標(biāo)系相對(duì)于基坐標(biāo)系0的總變換矩陣；n，o，a，p分別描述夾手x，y，z三個(gè)方向的接近矢量、方向矢量、法線矢量和夾手中心原點(diǎn)矢量；s1，c1，s12，c12分 別 表 示 sinθ1，cosθ1，sin（θ1＋θ2），cos（θ1＋θ2），其余類推。

1.2 機(jī)械臂運(yùn)動(dòng)學(xué)反解

在機(jī)械臂正運(yùn)動(dòng)學(xué)方程的基礎(chǔ)上，若末端連桿的位姿已經(jīng)給定，即n，o，a和p為已知，則求關(guān)節(jié)變量θ1，θ2，θ3，θ4的值稱為運(yùn)動(dòng)反解［3－5］。用未知的連桿逆變換左乘方程（3）兩邊，把關(guān)節(jié)變量分離出來，從而求解。具體步驟如下：

1.2.1θ4求解

用逆變換0T－11左乘方程（3）兩邊，則

令矩陣 方程（4）兩端第三行元素對(duì)應(yīng)相等，可得：az＝0，pz＝0，nz＝s4，oz＝c4，所以有

1.2.2θ2、θ1、θ3求解

令矩陣方程（6）兩邊的元素（1，4）和（2，4）分別

聯(lián)立方程組（7）求解得s12和c12：

據(jù)矩陣（10）兩邊元素（3，1）、（1，4）和（3，4）分別對(duì)應(yīng)相等，可得

式中s123和c123通過式（12）求得。再結(jié)合式（9）和式（12）可得θ1和θ3。

1.3 其他說明

至此機(jī)械臂運(yùn)動(dòng)學(xué)方程的正反解已求完。由于本設(shè)計(jì)機(jī)械臂關(guān)節(jié)的旋轉(zhuǎn)是通過電動(dòng)推桿的伸縮實(shí)現(xiàn)，所以角度值需要轉(zhuǎn)化為長度值，可利用三角形余弦定理轉(zhuǎn)換。

2 技術(shù)重點(diǎn)

2.1 機(jī)械臂控制系統(tǒng)設(shè)計(jì)

機(jī)械臂控制系統(tǒng)［6，7］分為上下位機(jī)兩個(gè)部分，其中上位機(jī)控制系統(tǒng)主要包括圖像接收、圖像采集、中央控制、信息反饋、遙控等模塊，硬件模塊實(shí)物圖如圖2（a）所示。下位機(jī)控制系統(tǒng)主要有數(shù)據(jù)采集、推桿電機(jī)驅(qū)動(dòng)、載體大功率電機(jī)驅(qū)動(dòng)、圖像發(fā)射等模塊，硬件模塊實(shí)物圖如圖2（b）所示。

上位機(jī)和下位機(jī)的數(shù)據(jù)傳輸采用多線程技術(shù)以滿足機(jī)器人的圖像傳輸及控制信號(hào)傳輸?shù)膶?shí)時(shí)性要求，通過上下位機(jī)間的雙向數(shù)據(jù)無線通信，能實(shí)現(xiàn)遠(yuǎn)距離操作控制臺(tái)實(shí)現(xiàn)行走底座的加減速運(yùn)動(dòng)、機(jī)械臂的協(xié)調(diào)運(yùn)動(dòng)及手爪的張合旋轉(zhuǎn)最終完成排爆任務(wù)，工作過程如圖3所示，首先機(jī)器人車體運(yùn)動(dòng)到目標(biāo)物能被機(jī)械臂抓取的范圍內(nèi)，操作人員確定目標(biāo)點(diǎn)，雙目視覺系統(tǒng)便會(huì)求得目標(biāo)點(diǎn)三維坐標(biāo)，通過逆運(yùn)動(dòng)學(xué)方程實(shí)時(shí)解算出各關(guān)節(jié)角度結(jié)合位于機(jī)械臂上的角度傳感器會(huì)反饋回來的機(jī)械臂當(dāng)前角度規(guī)劃運(yùn)動(dòng)軌跡，通過驅(qū)動(dòng)電機(jī)實(shí)現(xiàn)。

圖2 上位機(jī)硬件模塊實(shí)物

圖3 控制系統(tǒng)作業(yè)過程

2.2 上下位機(jī)控制流程

上下位機(jī)流程圖如圖4所示。其中，上位機(jī)綜合信息平臺(tái)可顯示4路視頻信號(hào)和兩路傳感器反饋信號(hào)，控制指令通過上位機(jī)進(jìn)行編碼發(fā)送給下位機(jī)。下位機(jī)軟件系統(tǒng)主要負(fù)責(zé)對(duì)上位機(jī)發(fā)來的指令進(jìn)行解碼及反饋外圍電路的響應(yīng)信號(hào)給上位機(jī)軟件系統(tǒng)。

圖4 上下位機(jī)控制系統(tǒng)流程圖

3 實(shí)驗(yàn)結(jié)果及分析

為了驗(yàn)證排爆機(jī)器人抓取目標(biāo)物的精度和機(jī)械臂正逆運(yùn)動(dòng)學(xué)模型的正確性，在樣機(jī)上進(jìn)行了多次抓取實(shí)驗(yàn)如圖5，并得出多組實(shí)驗(yàn)數(shù)據(jù)如表2。

圖5 排爆機(jī)器人抓取實(shí)驗(yàn)

表2 機(jī)器人抓取實(shí)驗(yàn)部分?jǐn)?shù)據(jù)

表2中θ1－θ4為關(guān)節(jié)旋轉(zhuǎn)角，L1－L3為由旋轉(zhuǎn)角轉(zhuǎn)化而來的電動(dòng)推桿伸出長度，x，y，z為機(jī)械臂末端相對(duì)連桿基坐標(biāo)系O－X0Y0Z0的坐標(biāo)值。由機(jī)械臂抓取實(shí)際結(jié)果和所得數(shù)據(jù)可知此機(jī)械臂通過控制電動(dòng)推桿可以實(shí)現(xiàn)末端手爪抓取目標(biāo)物，完成排爆任務(wù)，由此驗(yàn)證了系統(tǒng)運(yùn)動(dòng)控制算法的準(zhǔn)確性。

4 結(jié)論

本文針對(duì)排爆機(jī)器人的特殊作業(yè)要求，設(shè)計(jì)了一款符合實(shí)際運(yùn)作的實(shí)用型機(jī)械臂；采用D－H方法建立機(jī)械臂的關(guān)節(jié)坐標(biāo)，計(jì)算以關(guān)節(jié)角為變量的變換矩陣進(jìn)而獲得正運(yùn)動(dòng)學(xué)方程，并使用矩陣逆乘的解析法求解逆運(yùn)動(dòng)學(xué)，導(dǎo)出各關(guān)節(jié)角的解析解，實(shí)現(xiàn)機(jī)械臂快速、準(zhǔn)確運(yùn)動(dòng)；最后通過機(jī)器人實(shí)際運(yùn)作，驗(yàn)證了各關(guān)節(jié)空間組合和末端手爪位姿對(duì)應(yīng)關(guān)系的準(zhǔn)確性。本文為具有相似結(jié)構(gòu)的機(jī)械臂提供了正、逆運(yùn)動(dòng)學(xué)求解方法，為機(jī)械臂控制的實(shí)現(xiàn)提供實(shí)際依據(jù)。

［1］方 向，高振儒，周守強(qiáng)，等.反爆炸恐怖襲擊防排爆技術(shù)綜述［J］.中國工程科學(xué)，2013（05）：80－83.

［2］趙亮亮，馬振書，穆希輝，等.危險(xiǎn)作業(yè)機(jī)器人機(jī)械臂設(shè)計(jì)及其運(yùn)動(dòng)學(xué)分析［J］.機(jī)械設(shè)計(jì)與制造，2011（08）：155－157.

［3］蔡自興.機(jī)器人學(xué)［M］.北京：清華大學(xué)出版社，2009：39－52.

［4］吳 磊，史儀凱，王 萑.四自由度機(jī)械手臂運(yùn)動(dòng)學(xué)分析及雅可比矩陣求解［J］.機(jī)械科學(xué)與技術(shù)，2009（06）：764－767.

［5］彭圣明，裴海龍，王清陽.基于機(jī)械臂的運(yùn)動(dòng)學(xué)研究與應(yīng)用［J］.機(jī)械設(shè)計(jì)與制造，2011（07）：136－138.

［6］吳 繁，張偉軍，袁建軍，等.排爆機(jī)器人的遠(yuǎn)程控制系統(tǒng)［J］.機(jī)械與電子，2010（05）：55－58.

［7］劉 軍.遠(yuǎn)程無線反恐機(jī)器人功能設(shè)計(jì)與實(shí)現(xiàn)［J］.西安郵電大學(xué)學(xué)報(bào)，2013（02）：116－120.