機(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)系，則退化為目前國(guó)內(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)分別求解。基于Rao-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)定與建圖問題可以用概率密度

　





















?

　　同時(shí)定位、標(biāo)定與建圖問題的本質(zhì)是多傳感器

　　信息融合意義下的狀態(tài)估計(jì)問題。本文采用序貫方式融合兩類觀測(cè)信息進(jìn)行狀態(tài)的觀測(cè)更新，針對(duì)每一類觀測(cè)，當(dāng)同時(shí)存在多個(gè)該類觀測(cè)時(shí)，仍然采用序貫方式加以融合，如對(duì)于步驟2中2）的觀測(cè)更新，當(dāng)存在多個(gè)傳感器節(jié)點(diǎn)對(duì)機(jī)器人的觀測(cè)時(shí)，采用序貫方式融合多傳感器節(jié)點(diǎn)的觀測(cè)信息，該策略同樣應(yīng)用于步驟2中3）、步驟3中2）和步驟3中3）中存在多個(gè)同類觀測(cè)的情形。假定普適機(jī)器人系統(tǒng)中包含一個(gè)移動(dòng)機(jī)器人和一個(gè)由M個(gè)節(jié)點(diǎn)構(gòu)成的傳感器網(wǎng)絡(luò)，且環(huán)境中包含N個(gè)特征點(diǎn)，粒子個(gè)數(shù)選為K.在極端情形下，即各傳感器節(jié)點(diǎn)始終可以觀測(cè)到機(jī)器人和所有環(huán)境特征點(diǎn)，且機(jī)器人在任意時(shí)刻也都可以觀測(cè)到所有環(huán)境特征點(diǎn)時(shí)，本文算法所要融合的數(shù)據(jù)量最大，通過算法分析可知此情況下算法循環(huán)次數(shù)為K（M（N+1）+N（M+1）），或者說，本算法在最壞情況下的時(shí)間復(fù)雜度為O（KMN）

　3實(shí)驗(yàn)與分析

　　由于實(shí)際的普適機(jī)器人系統(tǒng)尚未搭建完成，僅進(jìn)行了仿真實(shí)驗(yàn)，驗(yàn)證本文方法的可行性和有效性.

　　由于充分考慮機(jī)器人觀測(cè)信息，定位精度較高，但由于傳感器隨機(jī)器人運(yùn)動(dòng)，導(dǎo)致這兩種傳統(tǒng)SLAM方法都存在定位誤差隨運(yùn)動(dòng)時(shí)間明顯增大的缺陷。而本文U-SLAM中，由于傳感器網(wǎng)絡(luò)各節(jié)點(diǎn)獨(dú)立于機(jī)器人，解除了觀測(cè)與機(jī)器人運(yùn)動(dòng)之間的數(shù)據(jù)耦合，在明顯提高機(jī)器人位姿估計(jì)精度的同時(shí)，估計(jì)的穩(wěn)定性也有大幅改善.

　　第二組仿真實(shí)驗(yàn)進(jìn)行傳感器網(wǎng)絡(luò)標(biāo)定精度對(duì)比，分別對(duì)僅依據(jù)運(yùn)動(dòng)方程的機(jī)器人位姿、依據(jù)FastSLAM2.0的機(jī)器人位姿，以及本文的普適機(jī)器人系統(tǒng)U-SLAM中聯(lián)合機(jī)器人位姿與環(huán)境特征這三類傳感器網(wǎng)絡(luò)標(biāo)定方法進(jìn)行對(duì)比，三類方法各運(yùn)行50次，得到的傳感器網(wǎng)絡(luò)標(biāo)定誤差的數(shù)學(xué)期望和方差如圖6所示。可以看出，僅依據(jù)運(yùn)動(dòng)方程的方法，機(jī)器人位姿估計(jì)誤差隨運(yùn)動(dòng)距離迅速增加，得到的標(biāo)定結(jié)果誤差很大，難以滿足要求;依據(jù)FastSLAM2.0算法能夠有效減少機(jī)器人位姿估計(jì)誤差，從而提高了節(jié)點(diǎn)標(biāo)定的精度，但由于僅考慮了節(jié)點(diǎn)對(duì)機(jī)器人的觀測(cè)，產(chǎn)生的標(biāo)定誤差仍然較高。而本文U-SLAM中，傳感器網(wǎng)絡(luò)各節(jié)點(diǎn)標(biāo)定、機(jī)器人位姿估計(jì)相對(duì)獨(dú)立，解除了觀測(cè)與機(jī)器人運(yùn)動(dòng)之間的數(shù)據(jù)耦合，在明顯提高機(jī)器人位姿估計(jì)精度的同時(shí)，傳感器節(jié)點(diǎn)標(biāo)定的精度和穩(wěn)定性也得到了大幅改善。
?

?

　　中融合了機(jī)器人的觀測(cè)信息，但機(jī)器人感知范圍有限而未察覺目標(biāo)變動(dòng)，仍以先前獲取Lm的位置和當(dāng)前觀測(cè)來推算當(dāng)前位姿，從而導(dǎo)致定位錯(cuò)誤。對(duì)于FastSLAM1.0算法，機(jī)器人定位只根據(jù)自身控制信息完成，未融合觀測(cè)信息從而避免受動(dòng)態(tài)環(huán)境的影響。而本文U-SLAM中，通過傳感器網(wǎng)絡(luò)監(jiān)測(cè)動(dòng)態(tài)環(huán)境特征的變化，并將其告知移動(dòng)機(jī)器人，避免了動(dòng)態(tài)路標(biāo)對(duì)機(jī)器人定位的影響，并獲得了比FastSLAM1.0更高的定位精度。

　　

　　上述實(shí)驗(yàn)均在WindowsXP系統(tǒng)下進(jìn)行，計(jì)算主機(jī)的CPU采用Pentium42.4GHz，內(nèi)存為1G.在該配置下運(yùn)行EKF-SLAM的平均耗時(shí)為12.3s，UKF-SLAM的平均耗時(shí)為15.4s，F(xiàn)astSLAM1.0平均耗時(shí)為80.7s，F(xiàn)astSLAM2.0的平均耗時(shí)為135.4s.本文U-SLAM方法運(yùn)行的平均耗時(shí)為280.8s，雖然實(shí)時(shí)性較差，但仍能滿足實(shí)際系統(tǒng)的需要，并且重要的是，能夠在定位和建圖過程中在線完成傳感器網(wǎng)絡(luò)標(biāo)定。實(shí)驗(yàn)中沒有進(jìn)行本文方法與傳統(tǒng)先離線標(biāo)定后在線定位建圖的對(duì)比，主要原因是定位和建圖的精度很大程度上取決于標(biāo)定的精度，而在傳統(tǒng)方法中離線標(biāo)定的精度本身隨標(biāo)定方法和策略差異很大。

　　

　　4結(jié)論

　　本文針對(duì)普適機(jī)器人系統(tǒng)提供服務(wù)的基礎(chǔ)工作，即機(jī)器人定位、傳感器網(wǎng)絡(luò)標(biāo)定和環(huán)境建圖三個(gè)互相耦合的關(guān)鍵問題展開研究，提出了普適機(jī)器人系統(tǒng)同時(shí)定位、標(biāo)定與建圖的概念，給出了同時(shí)機(jī)器人定位、傳感器網(wǎng)絡(luò)標(biāo)定與環(huán)境建圖問題的聯(lián)合條件概率表示，基于貝葉斯公式和馬爾科夫特性將其分解為若干可解項(xiàng)，并由Rao-Blackwellized粒子濾波迭代求解，設(shè)計(jì)了位姿粒子的采樣提議分布和權(quán)值更新公式，并給出了機(jī)器人定位、傳感器網(wǎng)絡(luò)標(biāo)定和環(huán)境建圖的遞推公式。

　　作為人們?nèi)粘Ｉ詈凸ぷ鞯闹匾獔?chǎng)所，機(jī)器人的服務(wù)環(huán)境往往較為復(fù)雜，其中目標(biāo)往往具有不同程度的動(dòng)態(tài)性。在這種動(dòng)態(tài)服務(wù)環(huán)境下普適機(jī)器人系統(tǒng)具有獨(dú)特的優(yōu)勢(shì)，將會(huì)發(fā)揮更顯著的作用。在完成傳感器網(wǎng)絡(luò)標(biāo)定和環(huán)境地圖的初步構(gòu)建之后，如何通過機(jī)器人和傳感器網(wǎng)絡(luò)的交互共同維護(hù)隨環(huán)境動(dòng)態(tài)更新的特征地圖是下一步的研究?jī)?nèi)容.