一種普適機(jī)器人系統(tǒng)同時(shí)定位、標(biāo)定與建圖方法

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

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

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

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

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

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

　　1系統(tǒng)描述

　　1.1系統(tǒng)構(gòu)成

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

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

　　2普適機(jī)器人系統(tǒng)同時(shí)定位、標(biāo)定與建圖

　　2.1基本思想

　　從概率的觀點(diǎn)看，普適機(jī)器人系統(tǒng)同時(shí)定位、標(biāo)定與建圖問題可以用概率密度

.3粒子權(quán)值計(jì)算及重采樣