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

關鍵詞：排爆機器人 運動控制 雙目視覺定位 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. 前言

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

2. 系統(tǒng)設計

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

2.1機械結構

機器人整體結構分為車體和機械手兩部分，車體兩側各安裝一個電機，采用鏈傳動實現四輪驅動，通過控制兩側電機的轉速和轉向可使小車實現任意半徑的轉彎。

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

圖1 機械手機構示意圖

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

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

2.2.1三角測距原理

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

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

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

圖2 三角測距示意圖

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

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

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

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

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

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

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

2.3.2 逆運動學算法的簡化

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

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

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

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

圖5 局部控制Simulink模型

圖6 現場照片

3.結束語

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

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

4.參考文獻

1.楊滌，李立濤，楊旭等編著 系統(tǒng)實時仿真開發(fā)環(huán)境與應用.北京：清華大出版社，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.叢爽，李澤湘 編著 實用運動控制技術.北京：電子工業(yè)出版社，2006.1;

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

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

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