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

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

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

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

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

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

　　1系統(tǒng)描述

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

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

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

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

　　2.1基本思想

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

　





















?

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

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

　3實驗與分析

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

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

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

?

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

　　

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

　　

　　4結(jié)論

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

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