基于粒子濾波的未知環(huán)境通信的多機(jī)器人SLAM算法研究

曾凌烽，高易年，王 欣

(1.深圳供電局有限公司，廣東 深圳 518000)(2.華南理工大學(xué)計算機(jī)科學(xué)與技術(shù)學(xué)院，廣東 廣州 510640)

電力設(shè)備人工流動巡檢排查隱患已逐漸被巡檢的機(jī)器人所取代。機(jī)器人巡檢可以識別更加細(xì)微且人工五感分辨困難的隱患及缺陷，因此如何讓多臺機(jī)器人在未知環(huán)境中移動時結(jié)合其各自位姿和地圖模型協(xié)同控制成為了人工智能領(lǐng)域的研究熱點[1]。多機(jī)器人協(xié)同工作可大幅提高整個系統(tǒng)的工作效率、準(zhǔn)確性和穩(wěn)定性，但多機(jī)器人協(xié)作場景需設(shè)計比較復(fù)雜的概率濾波器，通過融合機(jī)器人編隊中每個成員獲得的原始信息和感知信息來估計不同機(jī)器人的即時定位與地圖構(gòu)建(simultaneous localization and mapping, SLAM)[2]。如果機(jī)器人位姿的初始對應(yīng)關(guān)系已知，那么很容易獲得不同機(jī)器人的SLAM，但機(jī)器人位姿的初始對應(yīng)關(guān)系往往是不清楚的，這導(dǎo)致機(jī)器人協(xié)同工作時信息的一致性很難保障。此外，由于無線信道對故障的敏感性且收集到的大量信息可以迅速使成員之間的通信達(dá)到飽和，因此需要采用分布式方法降低編隊中所有成員的通信壓力，且編隊成員必須保持在中心節(jié)點的傳感范圍內(nèi)。任務(wù)分配由一個中央單元完成，該單元將任務(wù)分配給個人或以分布的方式進(jìn)行管理，但這兩種情況都需要進(jìn)行數(shù)據(jù)的共享。用于解決SLAM的技術(shù)需要具備可擴(kuò)展性，并對動態(tài)環(huán)境具有魯棒性，因為機(jī)器人編隊在相同的場景中運動時，即使面對移動障礙的隊友，每個機(jī)器人構(gòu)建的地圖也應(yīng)當(dāng)保持一致[3]。

為此，本文提出一種基于Rao-Blackwellized粒子濾波(Rao-Blackwellized particle filters,RBPF)的多機(jī)器人SLAM算法來解決移動機(jī)器人在未知工作環(huán)境的定位和地圖構(gòu)建問題。

1 粒子濾波器和SLAM

1.1 單機(jī)器人SLAM算法

將后驗概率p分解為兩個因子[4]：

p(x1:t,m|z1:t,u0:t-1,x0)=p(m|x1:tz1:t,u0:t-1,x0)p(x1:t|z1:t,u0:t-1,x0)

(1)

式中：t為時間；x1:t為機(jī)器人位姿序列；z1:t為觀察序列；u0:t-1為由機(jī)器人執(zhí)行動作序列；m為機(jī)器人工作區(qū)域地圖；x0為初始位姿。

給定機(jī)器人的初始位姿x0，計算機(jī)器人位姿序列x1:t和地圖m上的后驗概率p(x1:t,m|z1:t,u0:t-1,x0)。

(2)

由圖1可知，通過觀察序列z1:t和執(zhí)行動作序列u0:t-1，在機(jī)器人位姿序列x1:t的觀察引導(dǎo)下獲得機(jī)器人軌跡。由于濾波器的狀態(tài)空間非常稀疏，無法確保采樣的收斂性，為此本文將里程計與激光數(shù)據(jù)相結(jié)合，從而產(chǎn)生“穩(wěn)定的”測距位姿。

圖1 單機(jī)器人SLAM貝葉斯網(wǎng)絡(luò)

1.2 多機(jī)器人SLAM算法

單機(jī)器人SLAM算法易擴(kuò)展到多個機(jī)器人上，前提是機(jī)器人初始位姿已知?？紤]一對機(jī)器人，將后驗概率p分解為3個因子[6]：

(3)

圖2 初始位姿的多機(jī)器人SLAM貝葉斯網(wǎng)絡(luò)

2 多機(jī)器人遭遇過程分析

Rao-Blackwellized粒子濾波器的目的是從噪聲測量中有效估計SLAM后驗[8]。在處理多機(jī)器人協(xié)作場景時，應(yīng)以適當(dāng)?shù)姆绞绞褂脼V波器，以便融合遭遇過程中每個機(jī)器人攜帶的信息。

由圖3可知，多機(jī)器人在遭遇后，在對外部數(shù)據(jù)估計期間，當(dāng)機(jī)器人i沿著機(jī)器人j的軌跡繼續(xù)行進(jìn)時，機(jī)器人i處理另一個機(jī)器人的信息，設(shè)d1:tji,1為剩余信息。由于機(jī)器人i獲取的環(huán)境、機(jī)器人j的里程以及位姿信息不是由自身物理傳感器獲得，故稱為虛擬運動。遭遇后，機(jī)器人i的后驗概率p(x1:tij,1,mi|d1:tij,1,d1:tij,1)融合了機(jī)器人j的數(shù)據(jù)且地圖和軌跡都進(jìn)行了相應(yīng)的更新。

3 實驗

本實驗在有人穿越走廊和包含許多反射表面的辦公室環(huán)境下進(jìn)行，通過兩個機(jī)器人來建造地圖，驗證本文所提算法的魯棒性。實驗所用機(jī)器人為Activ Media Pioneers P3-DX移動機(jī)器人，具體如圖4所示。

圖3 遭遇過程

圖4 實驗所用機(jī)器人

該機(jī)器人配備激光測距傳感器SICK LMS200、云臺攝像頭和里程計。此外，機(jī)器人編隊的每個成員都附有一個用于區(qū)分機(jī)器人的視覺標(biāo)識。機(jī)器人1和機(jī)器人2執(zhí)行RBPF-SLAM(Rao-Blackwellized粒子濾波方法的即時定位和建圖)，同時結(jié)合機(jī)器人2接收的數(shù)據(jù)信息，獲得由機(jī)器人1估計的地圖和相應(yīng)的軌跡，具體如圖5所示。

圖5 實驗建構(gòu)的地圖

將機(jī)器人1和機(jī)器人2分別放置在非重疊的位置S1和S2，執(zhí)行RBPF-SLAM，直到兩個機(jī)器人第一次遭遇。一旦機(jī)器人相遇，就會使用基于客戶端/服務(wù)器架構(gòu)無線通信來測量相對位姿并交換數(shù)據(jù)。機(jī)器人1在其后驗中融合與路徑S2-I2相關(guān)的外部信息，繼續(xù)自身路線，在路線(A)中行進(jìn)并應(yīng)用SLAM進(jìn)行估計。機(jī)器人2訪問實驗室(B)直到到達(dá)位置F2。機(jī)器人1訪問房間(C)直到到達(dá)位置F1，由于位置F1和位置F2可以等同起來，即機(jī)器人1第二次遇到機(jī)器人2。從機(jī)器人2接收的數(shù)據(jù)允許機(jī)器人1完成其地圖，減少探索所需的時間并增強(qiáng)地圖閉環(huán)。同樣的過程應(yīng)用于機(jī)器人2，產(chǎn)生類似的地圖。

該方法采用機(jī)器人之間環(huán)境的非重疊表示。盡管每個隊友建立的地圖并不完全相同，但由于粒子濾波器的隨機(jī)性，它們只有少數(shù)單元不同，保留了場景的結(jié)構(gòu)。因此，該方法可以構(gòu)建可用于編隊協(xié)調(diào)的共享地圖。

圖6展示了在每一步處理的傳感器數(shù)據(jù)長度。盡管步數(shù)與物理時間不對應(yīng)，但它與處理數(shù)據(jù)的數(shù)量相關(guān)聯(lián)，數(shù)據(jù)長度的下降趨勢確保了程序的可持續(xù)性。在該實驗中，外部信息在幾秒鐘內(nèi)得到處理，驗證了RBPF-SLAM高效性。

圖6 每一時刻需要處理的傳感器數(shù)據(jù)長度

4 結(jié)束語

本文對位置環(huán)境通信的多機(jī)器人SLAM算法進(jìn)行了研究，提出了RBPF-SLAM算法，并應(yīng)用于有人穿越走廊且具有許多反射表面的環(huán)境下的地圖建造實驗中。結(jié)果表明，所提出的方法相對于傳統(tǒng)的SLAM算法能夠有效地確保在初始位置關(guān)系不明確時多機(jī)器人協(xié)同工作時信息的一致性，同時算法的運行效率也比較高。本文的研究對多機(jī)器人協(xié)同工作的開展具有一定的工程實踐價值。