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

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

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

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

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

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

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

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

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

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

2.2.1三角測(cè)距原理

如果已知兩臺(tái)攝像槍的幾何位置，且同時(shí)知道同一物體在2個(gè)攝像槍中的成像位置，便可以利用三角原理計(jì)算物體在空間的位置，即通過(guò)三角測(cè)距法獲取深度信息。

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

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

圖2 三角測(cè)距示意圖

2.3運(yùn)動(dòng)控制系統(tǒng)

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

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

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

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

圖3 機(jī)械手運(yùn)動(dòng)控制系統(tǒng)框圖

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

2.3.2 逆運(yùn)動(dòng)學(xué)算法的簡(jiǎn)化

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

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

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

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

圖5 局部控制Simulink模型

圖6 現(xiàn)場(chǎng)照片

3.結(jié)束語(yǔ)

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

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

4.參考文獻(xiàn)

1.楊滌，李立濤，楊旭等編著 系統(tǒng)實(shí)時(shí)仿真開(kāi)發(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ùn)動(dòng)控制技術(shù).北京：電子工業(yè)出版社，2006.1;

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

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

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