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

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

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

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

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

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

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

　　1系統描述

　　1.1系統構成

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

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

　　2普適機器人系統同時(shí)定位、標定與建圖

　　2.1基本思想

　　從概率的觀(guān)點(diǎn)看，普適機器人系統同時(shí)定位、標定與建圖問(wèn)題可以用概率密度

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