多傳感器信息融合伺服的移動機器人快速跟隨

周 霞，何建忠，王寶磊

（上海理工大學 光電信息與計算機工程學院，上海200093）

0 引 言

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

1 移動機器人傳感器外部世界感知

1.1 雙目攝像機視覺導航

移動機器人視覺導航包括目標檢測、坐標定位和濾波預測三部分。

目標檢測模塊的實現是通過對目標的特征顏色識別進行的，常用色彩空間有RGB、HIS、YUV等色彩空間。RGB空間以紅色R、綠色G、藍色B等3種基色為坐標軸。YUV空間由亮度Y和兩個色度信號U、V為坐標軸。兩空間的轉換關系如下

如果目標非純色，則可以通過實驗的方法對目標特征色進行標定。本文目標特征色為偏淺紅色，采用實驗法，目標特征色YUV空間表示如下

通過以上工作即可完成對目標特征色的檢測，進而對圖像進行二值化處理，求取目標幾何中心。

目標定位模塊的實現是通過雙目視覺三維測量原理完成的。

對圖1假定左目攝像機坐標系O－XYZ為慣性坐標系，O1－X1Y1為左目相機圖像坐標系，f1為左目攝像機焦距，Or－XrYr為右目攝像機圖像坐標系，fr為右目攝像機焦距，當空間中某一點在左右圖像坐標系坐標分別為 （x1，y1）和（xr，yr）時，慣性坐標系中坐標位置可由式 （3）算出［7］

其中，左右目攝像機相對位置平移向量為旋轉矩陣當雙目攝像機采用圖2所示規則放置f＝fr＝f1時，式 （3）可化簡為式 （4）

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

本文利用卡爾曼濾波技術進行預測，卡爾曼濾波由狀態方程、量測方程以及預測遞推公式組成，分別如式（5）式 （7）所示

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

式中：x1（k）、y1（k）——信號測量值，vx1（k）、vy1（k）——隨機噪聲

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

1.2 移動機器人避障設計

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

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

圖3 激光雷達柵格地圖

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

圖4 超聲波傳感器反饋

1.3 機器人自定位模塊設計

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

1.4 人機交互模塊設計

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

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

2 跟隨策略及系統集成

2.1 機器人跟隨策略

機器人上電開機后，使用者首先利用人機交互模塊相向機器人發送跟隨指令，機器人接到指令后開始跟隨，否則保持原有狀態繼續等待。機器人進入跟隨狀態后，同時利用雙目攝像導航模塊對目標進行定位和避障模塊進行避障，如果需要避障則產生避障動作，否則正常導航。

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

2.2 系統集成

圖5 機器人快速跟隨整體策略

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

圖6 機器人接口關系

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

3 實驗驗證

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

圖7 實驗環境參考跟隨地圖

實驗中，AB段、BC段和DE段對應的傳感器信息融合包括雙目視覺導航模塊的像素級信息融合、雙目視覺導航模塊與人機交互模塊以及機器人自定位模塊的決策級融合；CD段對應的傳感器信息融合除以上兩種融合外，還包括機器人避障模塊中激光雷達與超聲波傳感器返回數據的特征級融合。

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

4 結束語

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

：

［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）. ［周芳，韓立巖.多傳感器信息融合技術綜述 ［J］.遙測遙控，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）.［張學習，楊宜民.基于多傳感器信息融合的移動機器人快速精確自定位 ［J］.控制理論與應用，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.［陳小寧，黃玉清，楊佳.多傳感器信息融合在移動機器人定位中的應用 ［J］.傳感器與微系統，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）. ［王艷平.多傳感器信息融合技術在移動機器人障礙物探測中的應用 ［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）.［咸寶金，陳松濤.智能移動機器人多傳感器信息融合及應用研究 ［J］.宇航計測技術，2010，30 （2）：41－44.］

［7］ZHANG Guangjun.Machine vision ［M］.Beijing：Science Press，2005 （in Chinese）.［張廣軍.機器視覺 ［M］.北京：學出版社，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）.［于金霞，王璐，蔡自興.未知環境中移動機器人自定位技術［M］.北京：電子工業出版社，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）.［蔡自興，鄭敏杰，鄒小兵.基于激光雷達的移動機器人實時避障策略 ［J］.中南大學報，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）.［李云翀，何克忠.基于激光雷達的室外移動機器人避障與導航新方法 ［J］.機器人，2006，28 （3）：275－278.］