多傳感器信息融合伺服的移動(dòng)機(jī)器人快速跟隨

周 霞，何建忠，王寶磊

（上海理工大學(xué) 光電信息與計(jì)算機(jī)工程學(xué)院，上海200093）

0 引 言

移動(dòng)機(jī)器人擁有豐富的外部感知設(shè)備，可以作為信息融合技術(shù)研究的理想實(shí)驗(yàn)平臺(tái)，數(shù)據(jù)融合技術(shù)能夠?qū)υ夹畔⑦M(jìn)行整合處理，為機(jī)器人快速跟隨提供理想的伺服信息［1］，因此，研究多傳感器信息融合伺服的移動(dòng)機(jī)器人快速跟隨問題重點(diǎn)在于將上述兩方面內(nèi)容的優(yōu)勢(shì)進(jìn)行結(jié)合［2］。實(shí)際研究中，文獻(xiàn) ［3］利用蒙特卡羅粒子濾波方法將多傳感器信息進(jìn)行融合，對(duì)移動(dòng)機(jī)器人快速精確定位進(jìn)行伺服，保證了快速性和精確性；文獻(xiàn) ［4］利用擴(kuò)展卡爾曼濾波方法將里程計(jì)和超聲波信息進(jìn)行融合，應(yīng)用于機(jī)器人定位研究，消除了里程計(jì)的累計(jì)誤差；文獻(xiàn) ［5－6］分別利用自適應(yīng)加權(quán)融合算法和智能推理方法對(duì)紅外傳感器信息和超聲波信息進(jìn)行融合，并且應(yīng)用于移動(dòng)機(jī)器人避障研究，從實(shí)驗(yàn)仿真的角度驗(yàn)證了算法的有效性和實(shí)用性。上述文獻(xiàn)在取得研究進(jìn)展的同時(shí)，也表現(xiàn)出了一定的局限性，主要是信息融合的層次單一，僅進(jìn)行特征級(jí)融合或像素級(jí)融合，當(dāng)傳感器種類、數(shù)量增多時(shí)就會(huì)產(chǎn)生決策上的沖突，不能滿足機(jī)器人復(fù)雜任務(wù)的要求。本論文結(jié)合快速跟隨任務(wù)的需要，從像素級(jí)、特征級(jí)、決策級(jí)3個(gè)層次對(duì)采集信息進(jìn)行融合，涉及的移動(dòng)機(jī)器人外部感知設(shè)備包括雙目攝像機(jī)、單目云臺(tái)攝像機(jī)、激光雷達(dá)、聲納、紅外傳感器、麥克風(fēng)、光電編碼器等。

1 移動(dòng)機(jī)器人傳感器外部世界感知

1.1 雙目攝像機(jī)視覺導(dǎo)航

移動(dòng)機(jī)器人視覺導(dǎo)航包括目標(biāo)檢測(cè)、坐標(biāo)定位和濾波預(yù)測(cè)三部分。

目標(biāo)檢測(cè)模塊的實(shí)現(xiàn)是通過對(duì)目標(biāo)的特征顏色識(shí)別進(jìn)行的，常用色彩空間有RGB、HIS、YUV等色彩空間。RGB空間以紅色R、綠色G、藍(lán)色B等3種基色為坐標(biāo)軸。YUV空間由亮度Y和兩個(gè)色度信號(hào)U、V為坐標(biāo)軸。兩空間的轉(zhuǎn)換關(guān)系如下

如果目標(biāo)非純色，則可以通過實(shí)驗(yàn)的方法對(duì)目標(biāo)特征色進(jìn)行標(biāo)定。本文目標(biāo)特征色為偏淺紅色，采用實(shí)驗(yàn)法，目標(biāo)特征色YUV空間表示如下

通過以上工作即可完成對(duì)目標(biāo)特征色的檢測(cè)，進(jìn)而對(duì)圖像進(jìn)行二值化處理，求取目標(biāo)幾何中心。

目標(biāo)定位模塊的實(shí)現(xiàn)是通過雙目視覺三維測(cè)量原理完成的。

對(duì)圖1假定左目攝像機(jī)坐標(biāo)系O－XYZ為慣性坐標(biāo)系，O1－X1Y1為左目相機(jī)圖像坐標(biāo)系，f1為左目攝像機(jī)焦距，Or－XrYr為右目攝像機(jī)圖像坐標(biāo)系，fr為右目攝像機(jī)焦距，當(dāng)空間中某一點(diǎn)在左右圖像坐標(biāo)系坐標(biāo)分別為 （x1，y1）和（xr，yr）時(shí)，慣性坐標(biāo)系中坐標(biāo)位置可由式 （3）算出［7］

其中，左右目攝像機(jī)相對(duì)位置平移向量為旋轉(zhuǎn)矩陣當(dāng)雙目攝像機(jī)采用圖2所示規(guī)則放置f＝fr＝f1時(shí)，式 （3）可化簡(jiǎn)為式 （4）

式中：d——左右目設(shè)想投影中心連線的距離。

本文利用卡爾曼濾波技術(shù)進(jìn)行預(yù)測(cè)，卡爾曼濾波由狀態(tài)方程、量測(cè)方程以及預(yù)測(cè)遞推公式組成，分別如式（5）式 （7）所示

式中：x（k）、y（k）——機(jī)器人在慣性系下x方向和y方向的位移，——相應(yīng)速度，T ——采樣周期，w（k）——隨機(jī)誤差

式中：x1（k）、y1（k）——信號(hào)測(cè)量值，vx1（k）、vy1（k）——隨機(jī)噪聲

式中：Qk、Rk——w（k）、v（k）的方差，Ak、Ck表示式 （5）、式 （6）狀態(tài)矩陣。

1.2 移動(dòng)機(jī)器人避障設(shè)計(jì)

移動(dòng)機(jī)器人可用于避障設(shè)計(jì)的傳感器包括激光雷達(dá)、超聲波傳感器以及紅外傳感器。鑒于3種傳感器性能，本文避障模塊設(shè)計(jì)中將紅外傳感器屏蔽，主要利用激光雷達(dá)和超聲波傳感器［8］。

激光雷達(dá)適合于近距離避障，超聲波適合遠(yuǎn)距離避障，因此本文對(duì)兩種傳感器返回的數(shù)據(jù)按照自適應(yīng)的方法進(jìn)行融合，即返回?cái)?shù)據(jù)距離遠(yuǎn)則對(duì)超聲波數(shù)據(jù)加以較大的權(quán)重，返回距離近則對(duì)激光雷達(dá)數(shù)據(jù)加以較大的權(quán)重。激光雷達(dá)避障策略采用VHF方法［9］，即以柵格表示環(huán)境進(jìn)行避障。該方法首先通過空間變換將慣性坐標(biāo)系下環(huán)境信息變換到以機(jī)器人為圓心的極坐標(biāo)系中建立柵格地圖，然后根據(jù)需要將柵格地圖進(jìn)行分層，這里分層從內(nèi)到外應(yīng)至少包括三層，即禁區(qū)層、影響層和無關(guān)層，當(dāng)機(jī)器人進(jìn)入禁區(qū)層時(shí)，應(yīng)該立即停止并按原路線返回至安全位置，機(jī)器人進(jìn)入影響層時(shí)應(yīng)按照事先定好的避障軌跡進(jìn)行移動(dòng)，機(jī)器人在無關(guān)層不考慮避障，本文地圖分層如圖3所示。

圖3 激光雷達(dá)柵格地圖

在避障軌跡選擇上，本文借鑒勢(shì)場(chǎng)法［10］的思想，將機(jī)器人的跟隨目標(biāo)視為引力，將柵格地圖中的障礙物視為斥力，其加權(quán)合力方向即為機(jī)器人避障軌跡方向，這里加權(quán)系數(shù)根據(jù)障礙物位于柵格地圖的所在層進(jìn)行自適應(yīng)調(diào)整，越往內(nèi)層斥力加權(quán)系數(shù)越大，越往外層引力系數(shù)越小。超聲波傳感器避障原理較為簡(jiǎn)單，但是由于超聲波在一定情況下會(huì)由于特殊障礙物表面使發(fā)射光不能正常返回相應(yīng)的傳感器，因此需要進(jìn)行合適的建模與處理。本文是與激光雷達(dá)配合使用，且主要用于遠(yuǎn)距離避障，因此采用處理較為快速的中軸線傳感模型及容忍函數(shù)的濾波方法。實(shí)際避障中超聲波傳感器反饋結(jié)果如圖4所示。

圖4 超聲波傳感器反饋

1.3 機(jī)器人自定位模塊設(shè)計(jì)

機(jī)器人自定位模塊是通過位于機(jī)器人輪部的光電編碼器反饋信息處理實(shí)現(xiàn)的，反饋信息包括左輪速度v1和右輪速度vr，在兩輪距離d已知情況下，機(jī)器人旋轉(zhuǎn)角速度，前向速度在一個(gè)采樣周期內(nèi)機(jī)器人導(dǎo)航角變化為Δθ＝w t，則第i次采樣后機(jī)器人導(dǎo)航角為θi＝θi－1＋Δθ。將機(jī)器人前向速度變換到慣性系下因此，一個(gè)周期內(nèi)機(jī)器人在慣性坐標(biāo)系下x方向和y方向位移變化量分別為第i次采樣后機(jī)器人x 方向和y方向坐標(biāo)值為完成初步的機(jī)器人自定位。假設(shè)機(jī)器人速度不會(huì)在短時(shí)間內(nèi)發(fā)生劇烈變化，系統(tǒng)誤差服從均值為0的正態(tài)分布，采用滑動(dòng)平均濾波方案，取得了理想的機(jī)器人自定位結(jié)果。

1.4 人機(jī)交互模塊設(shè)計(jì)

人機(jī)交互模塊包括人機(jī)語音交互和人機(jī)手勢(shì)交互，主要用于機(jī)器人跟隨的開始、停止、以及緊急處理等需要人對(duì)機(jī)器人進(jìn)行干涉的情況。

人機(jī)語音交互采用微軟公司提供的Speech SDK進(jìn)行開發(fā)設(shè)計(jì)，利用命令識(shí)別模式將需要識(shí)別的語音信號(hào)進(jìn)行存儲(chǔ)，加上識(shí)別標(biāo)簽后對(duì)使用者進(jìn)行聲音學(xué)習(xí)訓(xùn)練，訓(xùn)練完成即可以較高精度識(shí)別使用者發(fā)出來的語音命令并產(chǎn)生交互決定。人機(jī)手勢(shì)交互是指噪音較大時(shí)人通過手勢(shì)對(duì)機(jī)器人發(fā)出干涉信號(hào)，本文手勢(shì)識(shí)別采取基于特征的識(shí)別方法，首先利用單目云臺(tái)攝像機(jī)對(duì)手勢(shì)進(jìn)行取樣，然后根據(jù)膚色對(duì)取樣照片進(jìn)行二值化處理，計(jì)算手勢(shì)面積與輪廓周長(zhǎng)比，利用這個(gè)比值的不同完成對(duì)不同手勢(shì)的識(shí)別，進(jìn)而根據(jù)識(shí)別結(jié)果產(chǎn)生相應(yīng)的交互決定。

2 跟隨策略及系統(tǒng)集成

2.1 機(jī)器人跟隨策略

機(jī)器人上電開機(jī)后，使用者首先利用人機(jī)交互模塊相向機(jī)器人發(fā)送跟隨指令，機(jī)器人接到指令后開始跟隨，否則保持原有狀態(tài)繼續(xù)等待。機(jī)器人進(jìn)入跟隨狀態(tài)后，同時(shí)利用雙目攝像導(dǎo)航模塊對(duì)目標(biāo)進(jìn)行定位和避障模塊進(jìn)行避障，如果需要避障則產(chǎn)生避障動(dòng)作，否則正常導(dǎo)航。

需要特別注意的是當(dāng)跟隨目標(biāo)停止運(yùn)動(dòng)并且機(jī)器人已經(jīng)靠近目標(biāo)時(shí)，導(dǎo)航模塊會(huì)給出信息使機(jī)器人繼續(xù)跟隨，而避障模塊則誤認(rèn)為是障礙物采取避障，因此產(chǎn)生動(dòng)作沖突，本文對(duì)這種情況給出的特殊處理是讓機(jī)器人原地等待。另外機(jī)器人在跟隨過程中，機(jī)器人會(huì)實(shí)時(shí)檢測(cè)優(yōu)先級(jí)更高的人機(jī)交互模塊，響應(yīng)使用者的干涉指令。機(jī)器人自定位模塊則通過顯示終端對(duì)機(jī)器人移動(dòng)路線進(jìn)行顯示，機(jī)器人跟隨整體策略如圖5所示。

2.2 系統(tǒng)集成

圖5 機(jī)器人快速跟隨整體策略

由于本文跟隨用的機(jī)器人機(jī)械主體及硬件部分是提前設(shè)計(jì)好的，因此系統(tǒng)集成主要指軟硬件結(jié)合的集成和軟件自身的設(shè)計(jì)優(yōu)化。在前文介紹的傳感器模塊中，都用相應(yīng)的數(shù)據(jù)采集卡與主控機(jī)進(jìn)行通信，而主機(jī)中的信息處理是通過軟件完成的，因此需要設(shè)計(jì)合理的機(jī)器人軟硬件接口，構(gòu)建一個(gè)硬件采集數(shù)據(jù)與軟件信息處理的中介。軟件開發(fā)語言C＋＋的COM中IUnknown接口為此提供了方便，利用它可以方便的構(gòu)造出相應(yīng)的機(jī)器人軟硬件接口，本文機(jī)器人使用設(shè)計(jì)的是可裁剪的IMTObject接口。這個(gè)接口負(fù)責(zé)對(duì)其它接口進(jìn)行統(tǒng)一管理與協(xié)調(diào)，其他接口按照需要在這個(gè)接口基礎(chǔ)上進(jìn)行繼承衍生，主要設(shè)備接口 （IMTDEVICE）包括運(yùn)動(dòng)Motion接口 （包括電機(jī)和編碼器）、視覺Image接口、聲音Voice接口、超聲UltraSonic接口和激光雷達(dá)Laser接口，整體接口關(guān)系示意圖如圖6所示。

圖6 機(jī)器人接口關(guān)系

由于機(jī)器人是一種高度自主的智能設(shè)備，因此在軟件設(shè)計(jì)時(shí)首先要構(gòu)建一個(gè)良好的框架對(duì)軟件進(jìn)行支撐，使其能正確有效的調(diào)節(jié)管理機(jī)器人感知、分析、決策、執(zhí)行等各模塊的關(guān)系，即軟件需要滿足開放性和模塊化的要求。結(jié)合前文介紹的軟硬件接口，本文軟件框架采用分布式智能系統(tǒng)的多智能體結(jié)構(gòu) （MAS）。具體設(shè)計(jì)時(shí)，以 Microsoft Visual Studio 2003為開發(fā)工具。為保證模塊間獨(dú)立運(yùn)行減少耦合影響，使用多線程技術(shù)進(jìn)行并行處理，本文軟件開設(shè)的線程一共有6個(gè)，分別為機(jī)器人視覺導(dǎo)航線程、激光雷達(dá)線程、超聲波線程、運(yùn)動(dòng)線程、人機(jī)交互線程和MFC用戶顯示線程。另外，為加快軟件開發(fā)速度，保證軟件開發(fā)質(zhì)量，本文還是用了開源的OPENCV庫、微軟Speech SDK 5.1語音識(shí)別庫和 XML技術(shù)。

3 實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)環(huán)境選擇普通實(shí)驗(yàn)辦公場(chǎng)所，引導(dǎo)者按照參考跟隨地圖要求引導(dǎo)機(jī)器人完成快速跟隨任務(wù)，實(shí)驗(yàn)環(huán)境參考跟隨地圖如圖7所示。實(shí)驗(yàn)開始時(shí)，機(jī)器人位于起始點(diǎn)A點(diǎn)開機(jī)上電，引導(dǎo)者站在與機(jī)器人一定距離B點(diǎn)向機(jī)器人通過語音發(fā)送跟隨指令，機(jī)器人開始靠近引導(dǎo)者，這個(gè)過程中，AB間會(huì)不斷有干擾者穿插經(jīng)過。機(jī)器人避開干擾，到達(dá)引導(dǎo)者所在的B點(diǎn)時(shí)，干擾者開始發(fā)出聲音干擾引導(dǎo)者的語音指令，引導(dǎo)者向機(jī)器人發(fā)出手勢(shì)指令使機(jī)器人原地等待并且前進(jìn)至遠(yuǎn)距離C點(diǎn)，機(jī)器人開始對(duì)引導(dǎo)者進(jìn)行遠(yuǎn)距離跟隨。遠(yuǎn)距離跟隨的下一任務(wù)為經(jīng)過障礙區(qū)并自動(dòng)避障，完成避障后機(jī)器人進(jìn)入D點(diǎn)開始在引導(dǎo)者引導(dǎo)下巡航至終點(diǎn)E。

圖7 實(shí)驗(yàn)環(huán)境參考跟隨地圖

實(shí)驗(yàn)中，AB段、BC段和DE段對(duì)應(yīng)的傳感器信息融合包括雙目視覺導(dǎo)航模塊的像素級(jí)信息融合、雙目視覺導(dǎo)航模塊與人機(jī)交互模塊以及機(jī)器人自定位模塊的決策級(jí)融合；CD段對(duì)應(yīng)的傳感器信息融合除以上兩種融合外，還包括機(jī)器人避障模塊中激光雷達(dá)與超聲波傳感器返回?cái)?shù)據(jù)的特征級(jí)融合。

實(shí)驗(yàn)效果可以從圖8、圖9、圖10所示的曲線圖加以分析。從圖8可以直觀的看出，機(jī)器人能夠基本在引導(dǎo)者引導(dǎo)下按照參考地圖完成跟隨任務(wù)。綜合分析圖9和圖10中，在圖7參考跟隨地圖的AB段，機(jī)器人從A點(diǎn)前進(jìn)至B點(diǎn)，這個(gè)過程中，機(jī)器人與引導(dǎo)者的距離持續(xù)減小到安全距離；機(jī)器人接到引導(dǎo)者的手勢(shì)指令后，暫停前進(jìn)，與引導(dǎo)者的距離不斷增大；圖7參考跟隨地圖的BC段對(duì)應(yīng)實(shí)際機(jī)器人遠(yuǎn)距離跟隨過程，此時(shí)機(jī)器人速度及與引導(dǎo)者距離的性狀與第AB相類似；圖7CD段對(duì)應(yīng)機(jī)器人的避障過程，這個(gè)過程中機(jī)器人由于障礙物的限制，速度變化很快，與引導(dǎo)者距離相對(duì)適中；圖7DE段為機(jī)器人巡航回到終點(diǎn)，性狀與AB、BC段相類似。另外，整個(gè)機(jī)器人快速跟隨過程用時(shí)132s，途徑距離約20m，能夠快速順利進(jìn)行人機(jī)有交互、巡航、避障，完成跟隨任務(wù)。

4 結(jié)束語

本文討論了移動(dòng)機(jī)器人快速跟隨中傳感器信息融合伺服的應(yīng)用問題，根據(jù)傳感器信息采集處理原理，給出了合適的跟隨策略以及軟硬件集成方案，通過實(shí)驗(yàn)驗(yàn)證此方案能夠很好地解決信息融合單一的問題，在進(jìn)行特征級(jí)融合或像素級(jí)融合時(shí)，能夠很好地解決傳感器種類、數(shù)量增多時(shí)產(chǎn)生的決策上的沖突，移動(dòng)機(jī)器人能夠快速順利的完成復(fù)雜任務(wù)。本文所述方案應(yīng)用于2011年全國(guó)服務(wù)機(jī)器人大賽快速跟隨比賽中也取得了良好的效果。實(shí)驗(yàn)和實(shí)踐結(jié)果都表明了本文方案的有效性和可行性。

：

［1］ZHAO Xiaochuan，LUO Qingsheng.School of aerospace science and engineering Beijing institute of technology ［J］.Proceeding of the 7th World Congress on Intelligent Control and Automation，2008，22 （4）：5019－5023.

［2］ZHOU Fang，HAN Liyan.A survey of multi－sensor information fusion technology ［J］.Journal of Telemetry，Tracking and Command，2006，27 （3）：1－7 （in Chinese）. ［周芳，韓立巖.多傳感器信息融合技術(shù)綜述 ［J］.遙測(cè)遙控，2006，27（3）：1－7.］

［3］ZHANG Xuexi，YANG Yimin.Fast and accurate self－localization of mobile robot based on multi－sensor ［J］.Control Theory＆ Applications，2011，28 （3）：443－448 （in Chinese）.［張學(xué)習(xí)，楊宜民.基于多傳感器信息融合的移動(dòng)機(jī)器人快速精確自定位 ［J］.控制理論與應(yīng)用，2011，28 （3）：443－448.］

［4］CHEN Xiaoning，HUANG Yuqing，YANG Jia.Application of multi－sensor information fusion in localization of mobile robot［J］.Transducer and Microsystem Technology，2008，27（6）：110－113.［陳小寧，黃玉清，楊佳.多傳感器信息融合在移動(dòng)機(jī)器人定位中的應(yīng)用 ［J］.傳感器與微系統(tǒng)，2008，27（6）：110－113.］

［5］WANG Yanping. Application of multi－sensor information fusion technology in obstacles detection of mobile robot ［J］.Electronic Engineer，2009，35 （1）：55－57 （in Chinese）. ［王艷平.多傳感器信息融合技術(shù)在移動(dòng)機(jī)器人障礙物探測(cè)中的應(yīng)用 ［J］.信息化研究，2009，35 （1）：55－57.］

［6］XIAN Baojin，CHEN Songtao.Intelligent autonomous robot multisensor information fusion and application ［J］.Journal of Astronautic Metrology and Measurement，2010，30 （2）：41－44 （in Chinese）.［咸寶金，陳松濤.智能移動(dòng)機(jī)器人多傳感器信息融合及應(yīng)用研究 ［J］.宇航計(jì)測(cè)技術(shù)，2010，30 （2）：41－44.］

［7］ZHANG Guangjun.Machine vision ［M］.Beijing：Science Press，2005 （in Chinese）.［張廣軍.機(jī)器視覺 ［M］.北京：學(xué)出版社，2005.］

［8］YU Jinxia，WANG Lu，CAI Zixing.Self－localization technologies of mobile robot in unknown environment ［M］.Beijing：Publishing House of Electronics Industry，2011 （in Chinese）.［于金霞，王璐，蔡自興.未知環(huán)境中移動(dòng)機(jī)器人自定位技術(shù)［M］.北京：電子工業(yè)出版社，2011.］

［9］CAI Zixing，ZHENG Minjie，ZOU Xiaobing.Real－time obstacle avoidance for mobile robots strategy based on laser radar［J］.Journal of Central South University，2006，37 （2）：324－329（in Chinese）.［蔡自興，鄭敏杰，鄒小兵.基于激光雷達(dá)的移動(dòng)機(jī)器人實(shí)時(shí)避障策略 ［J］.中南大學(xué)報(bào)，2006，37（2）：324－329.］

［10］LI Yunchong，HE Kezhong.A novel obstacle avoidance and navigation method for outdoor mobile robot based on laser radar［J］.Robot，2006，28 （3）：275－278 （in Chinese）.［李云翀，何克忠.基于激光雷達(dá)的室外移動(dòng)機(jī)器人避障與導(dǎo)航新方法 ［J］.機(jī)器人，2006，28 （3）：275－278.］