一種普適機器人系統(tǒng)同時定位、標定與建圖方法

　　機器人定位、傳感器網(wǎng)絡標定與環(huán)境建圖是普適機器人系統(tǒng)中三個相互耦合的基本問題，其有效解決是普適機器人系統(tǒng)提供高效智能服務的前提。本文提出了普適機器人系統(tǒng)同時機器人定位、傳感器網(wǎng)絡標定與環(huán)境建圖的概念，通過分析三者之間的耦合關系，給出同時定位、標定與建圖問題的聯(lián)合條件概率表示，基于貝葉斯公式和馬爾科夫特性將其分解為若干可解項，并借鑒Rao-Blackwellized粒子濾波的思想分別求解。首先，聯(lián)合傳感器網(wǎng)絡對機器人的觀測、機器人對已定位環(huán)境特征的觀測以及機器人自身控制量，設計了位姿粒子的采樣提議分布和權值更新公式;其次，聯(lián)合傳感器網(wǎng)絡對機器人運動軌跡及已定位環(huán)境特征的觀測，設計了傳感器網(wǎng)絡標定的遞推公式;然后，聯(lián)合傳感器網(wǎng)絡和機器人對（已定位或新發(fā)現(xiàn)）環(huán)境特征的觀測，設計了環(huán)境建圖的遞推公式。給出了完整的同時定位、標定與建圖算法，并通過仿真實驗驗證了該算法的有效性。

　　在機器人研究領域中，普適機器人系統(tǒng)是傳感器網(wǎng)絡技術與服務機器人技術的交叉領域［1−3］，通過將這兩種技術有效結合實現(xiàn)了兩者的優(yōu)勢互補：一方面，傳感器網(wǎng)絡可視為機器人感知能力的延伸，遍布整個環(huán)境的傳感器網(wǎng)絡能夠為機器人提供全局感知能力，彌補機器人對全局環(huán)境感知能力的缺陷;另一方面，機器人可視為傳感器網(wǎng)絡的執(zhí)行機構，從而使傳感器網(wǎng)絡具備了主動服務能力。因此，普適機器人系統(tǒng)在面積較大、動態(tài)性較強的服務環(huán)境，如家庭、醫(yī)院、展覽館等具有重要應用前景。

　　眾所周知，自身定位和環(huán)境地圖構建是移動機器人進行環(huán)境認知及路徑規(guī)劃，并最終提供高效智能服務的基礎［4］。對普適機器人系統(tǒng)而言，傳感器網(wǎng)絡標定同樣是保證其高效工作的重要一環(huán)，已標定傳感器網(wǎng)絡能夠實時定位機器人和環(huán)境動態(tài)目標，并據(jù)此動態(tài)更新環(huán)境地圖。事實上，不知道傳感器節(jié)點位置而采集的數(shù)據(jù)在實際應用中并沒有太大意義［2］。在將普適機器人系統(tǒng)引入某一服務環(huán)境伊始，機器人定位、傳感器網(wǎng)絡標定和環(huán)境建圖就成為其面臨的三個基礎問題［3］。其中定位貫穿于機器人工作的始終，而傳感器網(wǎng)絡標定和環(huán)境地圖構建則在普適機器人系統(tǒng)正常工作之初完成，并在工作過程中隨網(wǎng)絡節(jié)點或環(huán)境變化而實時更新。此外，分析可知，普適機器人系統(tǒng)中機器人定位、傳感器網(wǎng)絡標定和環(huán)境建圖三個問題相對獨立而又互相耦合：一方面，傳感器網(wǎng)絡提供全局觀測，能輔助機器人完成動態(tài)環(huán)境下的定位;另一方面，傳感器網(wǎng)絡的精確標定是其輔助機器人定位的前提。更進一步地，通過對已建地圖的觀測，將有助于提高傳感器網(wǎng)絡標定精度和機器人定位精度。若僅探討機器人定位、環(huán)境建圖二者之間耦合關系，則退化為目前國內外學者廣泛研究的同時機器人定位與環(huán)境建圖（Simultaneouslocalizationandmapping，SLAM）問題。

　　在同時機器人定位與環(huán)境建圖方面，利用概率方法解決SLAM問題是目前的研究方向和熱點［4］，其中，基于擴展卡爾曼濾波（ExtendedKalman?l-ter，EKF）的SLAM方法不適于解決存在非高斯噪聲情況的估計問題［5］;粒子濾波方法適用于非線性和非高斯情形，但這種方法在問題維數(shù)較高時計算量過大，難以滿足系統(tǒng)實時性要求;Rao-Blackwellized粒子濾波用擴展卡爾曼濾波處理非線性部分，用粒子濾波處理非高斯部分，因此同時具備擴展卡爾曼濾波和粒子濾波的優(yōu)勢，并已被成功應用于SLAM中［6−8］。然而，由于傳感器機載，機器人觀測誤差與運動誤差相耦合，定位和建圖誤差會隨機器人運動距離發(fā)生不可避免的擴散［9］。文獻［10］提出了基于粒子濾波的無線傳感器網(wǎng)絡輔助SLAM方法，用于解決求解問題空間維數(shù)高和多數(shù)據(jù)關聯(lián)問題;文獻［11］提出了基于傳感器網(wǎng)絡分布式感知的移動機器人SLAM方法，來創(chuàng)建大規(guī)模環(huán)境的精確地圖;文獻［12］提出了無線傳感器網(wǎng)絡環(huán)境下基于粒子濾波的移動機器人SLAM算法。上述方法雖然都體現(xiàn)了利用傳感器網(wǎng)絡的全局觀測來輔助SLAM的思想，但都尚未考慮傳感器網(wǎng)絡節(jié)點的標定問題，而該問題的有效解決是傳感器網(wǎng)絡輔助機器人定位和環(huán)境特征建圖的基礎。

　　在同時傳感器網(wǎng)絡標定與機器人定位方面，文獻［13］提出了一種基于機器人的攝像機網(wǎng)絡在線自標定方法，以此為基礎，文獻［14］提出了一種分布式感知協(xié)作的移動機器人MonteCarlo定位方法，利用已完成標定傳感器網(wǎng)絡的觀測來輔助機器人定位。事實上，類似于SLAM問題，傳感器網(wǎng)絡標定與機器人定位可以同時進行（即同時傳感器網(wǎng)絡標定與機器人定位），針對該問題，文獻［15］提出了一種基于Rao-Blackwellized粒子濾波的同時傳感器網(wǎng)絡標定與機器人定位方法，但對于聯(lián)合機器人定位、傳感器網(wǎng)絡標定和環(huán)境建圖的研究，目前尚未展開。

　　考慮到普適機器人系統(tǒng)同時機器人定位、傳感器網(wǎng)絡標定與環(huán)境建圖問題的耦合關系，為充分融合定位、標定和建圖過程中涉及的多類信息來源，并避免繁瑣的傳感器網(wǎng)絡離線標定環(huán)節(jié)，本文提出了普適機器人系統(tǒng)同時定位、標定與建圖的概念。在進行三者的聯(lián)合求解時，從概率的角度進行理論分析，將聯(lián)合條件概率分解為若干可解項分別求解。基于Rao-Blackwellized粒子濾波思想，聯(lián)合機器人控制信息、傳感器網(wǎng)絡對機器人的觀測，以及機器人對已建環(huán)境地圖的觀測估計機器人位姿粒子及其權值分布，進而根據(jù)傳感器網(wǎng)絡對機器人和已建環(huán)境地圖的觀測來標定傳感器網(wǎng)絡的參數(shù)，最后聯(lián)合機器人和傳感器網(wǎng)絡對環(huán)境的觀測構建家庭環(huán)境的特征地圖。

　　1系統(tǒng)描述

　　1.1系統(tǒng)構成

　　本文討論的對象為普適機器人系統(tǒng)，該系統(tǒng)主要由三個部分構成：具備普適感知和處理能力的傳感器網(wǎng)絡及處理主機、與普適處理主機交互的移動式服務機器人，以及環(huán)境中的各種目標（包括服務對象、操作物品及環(huán)境路標等，這里統(tǒng)稱為目標）。圖1給出了一個典型的家庭普適機器人系統(tǒng)的示意圖，其實現(xiàn)方案如下：由RGB-D攝像頭作為節(jié)點構建傳感器網(wǎng)絡，該攝像頭能夠同時獲取視域范圍內目標的顏色和距離信息，各RGB-D攝像頭通過數(shù)據(jù)線連接到一臺處理主機的圖像采集卡上，該主機負責分析處理各攝像頭所捕獲的圖像，并通過無線網(wǎng)絡實現(xiàn)與服務機器人的通訊;家庭服務機器人平臺選用配備手眼系統(tǒng)的Pioneer3DX型移動機器人，并為該機器人設計標識色塊以便傳感器節(jié)點觀測定位;選取家庭環(huán)境和目標的尺度不變特征變換（Scaleinvariantfeaturetransform，SIFT）［16］進行特征檢測、匹配和識別。此外，為家庭常見目標粘貼標識其名稱、功能及用法等信息的QRcode標簽，通過閱讀標簽機器人能實現(xiàn)對物品的深層次認知。

　　傳感器網(wǎng)絡的節(jié)點部署需要綜合考慮節(jié)點觀測范圍、能耗、障礙分布等因素，該問題已有相關文獻論述［17−19］，在此不做過多討論，假定傳感器節(jié)點已被較為合理地部署。

　　2普適機器人系統(tǒng)同時定位、標定與建圖

　　2.1基本思想

　　從概率的觀點看，普適機器人系統(tǒng)同時定位、標定與建圖問題可以用概率密度

.3粒子權值計算及重采樣