摘 要：本文提出了一種基于雙目視覺定位導(dǎo)航的自動抓取排爆機(jī)器人控制系統(tǒng)方案，介紹了雙目視覺定位的原理和運動控制系統(tǒng)的設(shè)計，并利用MATLAB/RTW工具生成基于xPC目標(biāo)的實時控制系統(tǒng)，運行于配置PC/104的試驗樣機(jī)上，取得了較好的結(jié)果。

關(guān)鍵詞：排爆機(jī)器人 運動控制 雙目視覺定位 MATLAB/RTW

Abstract：A control system scheme of a Bomb-disposal Robot based on the binocular stereo vision location and navigation is proposed in this paper. The principle of the binocular vision location and the design of a motion control system are described. Realtime control system of the robot based on xPC target through the MATLAB/RTW run successfully in test prototype furnished with PC/104, and satisfactory result is obtained.

Keywords：Bomb-disposal Robot Motion Control Binocular Vision Location MatLab/RTW

1. 前言

排爆機(jī)器人作為一種特殊環(huán)境下使用的機(jī)器人在很多國家都已開展了研制，有的已有產(chǎn)品面世，但大都是為一種遙控機(jī)器人，抓起目標(biāo)物的過程中需要人的干預(yù)，由于有多個關(guān)節(jié)需要協(xié)調(diào)控制，且抓起目標(biāo)物時的定位精度通常在5mm以下，故操作者沒有長期的訓(xùn)練是難于勝任的。本文提出了一種基于雙目視覺定位導(dǎo)航的手眼協(xié)調(diào)系統(tǒng)來完成自動控制抓起目標(biāo)物的過程，該控制方案在實驗樣機(jī)上得到了驗證，取得了較滿意的結(jié)果。

2. 系統(tǒng)設(shè)計

排爆機(jī)器人由機(jī)械系統(tǒng)、雙目視覺定位系統(tǒng)和運動控制系統(tǒng)三部分組成，可對目標(biāo)物進(jìn)行準(zhǔn)確測距定位并自動完成抓取功能。

2.1機(jī)械結(jié)構(gòu)

機(jī)器人整體結(jié)構(gòu)分為車體和機(jī)械手兩部分，車體兩側(cè)各安裝一個電機(jī)，采用鏈傳動實現(xiàn)四輪驅(qū)動，通過控制兩側(cè)電機(jī)的轉(zhuǎn)速和轉(zhuǎn)向可使小車實現(xiàn)任意半徑的轉(zhuǎn)彎。

本機(jī)器人機(jī)械手具有5個自由度，分別對應(yīng)為腰、肩、肘、腕關(guān)節(jié)和手爪轉(zhuǎn)位關(guān)節(jié)，其結(jié)構(gòu)示意圖見圖1。各關(guān)節(jié)由直流電機(jī)驅(qū)動，均采用位置閉環(huán)伺服控制。兩攝像頭安裝在小臂上，為機(jī)器人雙目視覺。

圖1 機(jī)械手機(jī)構(gòu)示意圖

2.2雙目視覺定位和手眼協(xié)調(diào)

雙目視覺定位[2]系統(tǒng)是由安裝在小臂上的兩個攝像頭攝取同一目標(biāo)物兩幅圖像，利用三角測距原理，經(jīng)圖像匹配和計算得出目標(biāo)物與機(jī)械手坐標(biāo)系的精確相對位置。通過手爪運動軌跡規(guī)劃使得手爪在接近目標(biāo)物的過程中，攝像頭一直對準(zhǔn)目標(biāo)物，利用這種手眼協(xié)調(diào)方式以使目標(biāo)物始終處于圖像監(jiān)控之下;然后在手爪離目標(biāo)物較近的位置（手爪路徑規(guī)劃時確定）時再進(jìn)行一次更為精確的測距，最后準(zhǔn)確抓取目標(biāo)。整個抓取過程由機(jī)器人控制系統(tǒng)自動完成。

2.2.1三角測距原理

如果已知兩臺攝像槍的幾何位置，且同時知道同一物體在2個攝像槍中的成像位置，便可以利用三角原理計算物體在空間的位置，即通過三角測距法獲取深度信息。

如圖2所示?？紤]最簡單的情形，L和R分別為2個參數(shù)相同的針孔攝像槍，f為2個攝像槍的焦距，L和R光軸平行，與X軸重合，Y軸垂直于紙面。以L的攝像槍坐標(biāo)系為世界坐標(biāo)系，R的原點或投影中心的偏移量為b，b成為立體視覺系統(tǒng)的基線。目標(biāo)點為P，P在左右攝像槍中投影的x坐標(biāo)分別為x1,x2。根據(jù)空間幾何的原理，有如下公式：

已知（x1,y1）,（x2,y2），焦距f和基線長度b即可以得到P點的三維坐標(biāo)（X，Y，Z）

圖2 三角測距示意圖

2.3運動控制系統(tǒng)

機(jī)器人運動控制系統(tǒng)采用全數(shù)字式伺服方式，即除電機(jī)驅(qū)動及以后的部分為硬件外，其余的如插補(bǔ)算法、比較單元、控制器等均數(shù)字化、軟件化后由計算機(jī)來完成。其突出優(yōu)點是可靈活運用各種復(fù)雜的控制規(guī)律，并降低硬件成本。

2.3.1手爪路徑規(guī)劃和插補(bǔ)

要完成手眼協(xié)調(diào)控制，即在自動抓取目標(biāo)物的前期（手爪接近目標(biāo)物）攝像頭一直對準(zhǔn)目標(biāo)物，則從手爪起始點到精確測距點的軌跡應(yīng)為一直線。為實現(xiàn)手爪近似水平伸向目標(biāo)物，從精確測距點到最終抓取目標(biāo)點應(yīng)為另一直線，故手爪路徑由兩段直線組成。

手爪路徑規(guī)劃在直角坐標(biāo)空間[3]進(jìn)行，手爪設(shè)計移動速度為50mm/s，由于機(jī)器人手爪運動軌跡精度要求并不高，故采用ΔL=5mm作定距插補(bǔ)得到密集化的數(shù)據(jù)系列，相當(dāng)于粗插補(bǔ)周期100ms。得到一系列目標(biāo)點后利用逆運動學(xué)算法算出各關(guān)節(jié)角度系列，各關(guān)節(jié)采用位置閉環(huán)伺服控制達(dá)到所要求的角度，功能框圖如圖3所示。由于機(jī)械手為開鏈懸臂結(jié)構(gòu)，為了使各手臂運動平穩(wěn)，必須對手臂運動進(jìn)行速度控制，故在關(guān)節(jié)空間進(jìn)一步插補(bǔ)作速度規(guī)劃，插補(bǔ)速度曲線采用梯形速度曲線模式，插補(bǔ)周期為10ms，控制規(guī)律采用數(shù)字PID算法，電機(jī)由PWM波調(diào)速，PWM波由軟件產(chǎn)生，周期為0.5ms，即PWM頻率為2kHz。另外，若反饋環(huán)節(jié)開路或出現(xiàn)故障會造成偏差過大甚至越來越大，這就使得手臂過速及到達(dá)目標(biāo)位置而不能停止，故為安全起見，增加了偏差檢測保護(hù)模塊。各關(guān)節(jié)閉環(huán)位置控制系統(tǒng)框圖見圖4。

圖3 機(jī)械手運動控制系統(tǒng)框圖

圖4 各關(guān)節(jié)位置閉環(huán)伺服控制系統(tǒng)框圖

2.3.2 逆運動學(xué)算法的簡化

五個自由度以上的逆運動學(xué)算法[4]復(fù)雜，要求計算機(jī)的計算時間長。從工程實現(xiàn)的角度考慮完全可以適當(dāng)簡化，按最通常的情況，手爪是水平抓取目標(biāo)物，而手爪的轉(zhuǎn)動方位取決于目標(biāo)物抓起時的著力位置，抓起目標(biāo)時可先讓腰關(guān)節(jié)轉(zhuǎn)動使手爪對準(zhǔn)目標(biāo)物，這樣處理實際上就在逆運動算法時減少了三個自由度，只剩下兩個自由度即兩個關(guān)節(jié)角度需要計算，大大簡化了計算過程，減少了計算機(jī)的處理時間。另外，由于位置反饋使用的是增量式編碼器，所以開機(jī)后機(jī)械手各關(guān)節(jié)必須確定一個位置（置位位置），該位置時寫入可逆計數(shù)器一個確定值，各關(guān)節(jié)角度示意如圖1。

2.3.3 xPC目標(biāo)實時控制系統(tǒng)

本機(jī)器人運動控制系統(tǒng)是基于MATLAB/RTW[1]（Real-Time Workshop）的xPC目標(biāo)[5]實時控制系統(tǒng)。xPC目標(biāo)采用了宿主機(jī)-目標(biāo)機(jī)的技術(shù)途徑，宿主機(jī)用于運行Simulink[6]，建立控制模型，生成目標(biāo)代碼后下載至目標(biāo)機(jī)。宿主機(jī)還運行控制界面程序和發(fā)送操作控制命令。目標(biāo)PC機(jī)則用于執(zhí)行所生成的代碼。xPC提供了一個高度減縮型的實時操作核，運行于目標(biāo)PC機(jī)上。本機(jī)器人采用了PC/104作為實時目標(biāo)機(jī)，配合一塊I/O擴(kuò)展卡作編碼器信號輸入和電機(jī)控制信號輸出。

圖5為控制系統(tǒng)局部Simulink模型，由視覺系統(tǒng)傳送來目標(biāo)點先經(jīng)驗算是否可及，不可及則報錯，可及則計算分配腰關(guān)節(jié)應(yīng)轉(zhuǎn)過的角度和其他關(guān)節(jié)經(jīng)手爪軌跡規(guī)劃（粗插補(bǔ)）得到應(yīng)轉(zhuǎn)過的角度，角度均對應(yīng)為編碼器的脈沖數(shù)，目標(biāo)脈沖數(shù)與編碼器反饋脈沖數(shù)之差經(jīng)數(shù)字PID算法輸出至PWM波產(chǎn)生模塊，PWM波輸出至電機(jī)驅(qū)動器驅(qū)動直流電機(jī)。圖6為機(jī)器人現(xiàn)場抓取一水瓶照片。

圖5 局部控制Simulink模型

圖6 現(xiàn)場照片

3.結(jié)束語

本設(shè)計創(chuàng)新點在于把雙目視覺定位系統(tǒng)應(yīng)用于機(jī)器人目標(biāo)導(dǎo)航抓取，其定位精度高，抓取時無需人的干預(yù)，抓取目標(biāo)精度達(dá)到5mm以內(nèi)?？刂葡到y(tǒng)是基于MATLAB/RTW生成的xPC目標(biāo)實時系統(tǒng)，該方法開發(fā)周期短，易于調(diào)試，是控制系統(tǒng)產(chǎn)品開發(fā)和驗證的一個有力工具。

另外，由于負(fù)載的變化，使得機(jī)械手臂在運動時某些位置會產(chǎn)生抖顫，速度變化不圓滑，這都有待于從控制規(guī)律（如增加前饋控制或采用自適用控制）和改善硬件（如采用高脈沖數(shù)的編碼器、減小機(jī)械傳動間隙等）等方面來解決。

4.參考文獻(xiàn)

1.楊滌，李立濤，楊旭等編著 系統(tǒng)實時仿真開發(fā)環(huán)境與應(yīng)用.北京：清華大出版社，2002;

2.Tsai R. Y An efficient and accurate camera calibration technique for 3D machine vision. In Proc. IEEE Conf, on Computer Vision and Pattern Recognition, Miami Beach. FL, 1986, 364374;

3.叢爽，李澤湘 編著 實用運動控制技術(shù).北京：電子工業(yè)出版社，2006.1;

4.殷際英，何廣平 編著 關(guān)節(jié)型機(jī)器人.北京：化學(xué)工業(yè)出版社，工業(yè)裝備與信息工程出版中心，2003;

5.徐國政，陳勇 基于Matlab/xPC Target的數(shù)據(jù)采集系統(tǒng).微計算機(jī)信息[J]，20051033，63-64;

6.李穎，朱伯立，張威 編 Simulink動態(tài)系統(tǒng)建模與仿真基礎(chǔ).西安：西安電子科技大學(xué)出版社，2004.7。