大環(huán)境下拓撲化柵格地圖的多峰定位研究

王 翰，蔣 林,*，周 亮

(1.武漢科技大學 機械傳動與制造工程湖北省重點實驗室, 湖北 武漢 430081；2.武漢科技大學 機器人與智能系統(tǒng)研究院, 湖北 武漢 430081)

0 引言

機器人實現(xiàn)自主執(zhí)行任務需要對環(huán)境進行地圖構(gòu)建和自定位[1]。為了實現(xiàn)室內(nèi)大環(huán)境地圖構(gòu)建和機器人定位，國內(nèi)外的研究人員開發(fā)了各種不同的地圖構(gòu)建和機器人定位的算法?；赗OS開源的SLAM框架Gmapping可以很好實現(xiàn)柵格地圖創(chuàng)建和機器人定位[2]。

地圖可以分兩種，柵格地圖和拓撲地圖。柵格地圖可以將環(huán)境中的障礙物信息通過二值化的柵格完整地表示在地圖上，機器人可以通過激光雷達等傳感器獲得機器人在真實環(huán)境中的位置實現(xiàn)自定位[3]。另一方面，拓撲地圖可以將環(huán)境用節(jié)點和邊表示，可以大量簡化地圖的信息?；谝阎貓D的機器人定位需要建立環(huán)境的完整地圖，EKFSLAM、EIFSLAM等基于KF的定位建圖算法，可以很好地實現(xiàn)小場景的定位建圖，算法會因為一致性問題[4]無法消除模型誤差，最終導致機器人無法正常地運行。RBPF、FastSLAM等基于PF的算法可以處理非高斯非線性的系統(tǒng)，但是通過粒子估計后驗概率，計算量過大，容易產(chǎn)生粒子耗盡等問題。為了解決這個問題，Murphy 和Doucet等人提出Rao-Blcakwellised一種新的濾波方式來解決SLAM問題。Montemerlo等在這個基礎(chǔ)上提出新一代的SLAM定位算法FastSLAM 2.0，F(xiàn)astSLAM 2.0的優(yōu)勢源于粒子濾波能處理非線性機器人運動模型[5]。每一個粒子不僅表示瞬時的機器人位置，還關(guān)聯(lián)機器人運動過程中的路徑和環(huán)境的相關(guān)特征[6]。FastSLAM 2.0算法中提出一種求取重要性函數(shù)的方法，可以在一定的程度緩解粒子耗盡的問題。針對粒子耗盡問題，布倫瑞克技術(shù)大學的學者提出一種融合地標測量的似然與柵格地圖信息來更新提議分布[7]，以近似后驗分布的方式實現(xiàn)機器人的初始位姿未知下的快速定位。將機器人傳感器的觀測數(shù)據(jù)和環(huán)境的特征相關(guān)聯(lián)，是提高定位精度和魯棒性的關(guān)鍵。紐約城市大學的學者則通過將交互式模型和聯(lián)合數(shù)據(jù)混合關(guān)聯(lián)的方式提高定位的魯棒性[8]。將環(huán)境進行拓撲化的目的是為了有效地提取環(huán)境中的特征數(shù)據(jù)。特征數(shù)據(jù)可以很大程度提高機器人的傳感器數(shù)據(jù)和環(huán)境特征數(shù)據(jù)關(guān)聯(lián)性。斯坦福大學智能機器人研究團隊于2013年首先提出將柵格地圖和拓撲地圖相結(jié)合的混合地圖實現(xiàn)算法，并在PM-3平臺上進行了實驗驗證，但是實現(xiàn)的效果并不理想。雖然提取了環(huán)境拓撲信息，但是關(guān)聯(lián)性不夠。2015年谷歌公司開源SLAM框架Cartographer，該框架可以很好地實現(xiàn)大環(huán)境建圖,該框架是采用基于優(yōu)化的方法來實現(xiàn)機器人的定位和建圖，并且實現(xiàn)了回環(huán)檢測，可以很好地修正機器的運動過程中的累計誤差[9]。Cartographer中回環(huán)檢測的判定，采用當前幀的激光雷達數(shù)據(jù)匹配已經(jīng)生成的子地圖。廣東工業(yè)大學學者在Cartographer回環(huán)檢測的基礎(chǔ)上，提出基于連續(xù)多幀的激光數(shù)據(jù)檢測回環(huán)的方法，在很大程度上定位精度。只有在柵格地圖保存了完整的環(huán)境的條件下，提取拓撲信息，才能使得拓撲化的地圖被充分的利用。在研究室內(nèi)已知地圖的機器人定位發(fā)現(xiàn)，粒子的數(shù)目對粒子濾波定位算法的效果影響很大，正常的粒子數(shù)量可能沒有辦法收斂到機器人準確的位置，粒子過多會導致機器人無法正常運行。通過實驗證明了基于拓撲化柵格地圖的多峰粒子濾波方法可以快速準確地實現(xiàn)機器人在大環(huán)境中的定位。

1 柵格地圖拓撲化

1.1 大環(huán)境下粒子分布問題

實驗所選取的環(huán)境為標準的辦公樓層，該辦公樓短邊長18 m，長邊長76 m，總面積約為1 368 m2。在該辦公樓中，對可進入的8個房間、電梯口、衛(wèi)生間走廊進行地圖構(gòu)建，機器人在該環(huán)境中的可運行區(qū)域約為800 平方米。在已經(jīng)建好的柵格地圖中，通過單峰高斯分布與固定比率隨機散布粒子對算法進行初始化，并采用粒子濾波算法進行定位測試實驗。粒子初始化方式相同，僅將初始的粒子數(shù)分別設(shè)定為500、5 000與50 000個，500-5 000-50 000三種初始粒子數(shù)的分布點云圖表明粒子散布密度隨著粒子數(shù)目增加。三組初始粒子數(shù)量不同的定位實驗中，500與5 000個粒子的實驗由于初始粒子在真實位姿附近粒子分布過少，實驗中得到粒子最終不收斂和錯誤收斂的結(jié)果；當初始粒子為5000，機器人的工控機因為內(nèi)存不足而無法運行。實驗證明三種粒子數(shù)值在該場景都難以很好的實現(xiàn)機器人的定位。實際大場景下機器人定位，必須對粒子的分布進行規(guī)劃以平衡算法實時性與算法收斂成功率的關(guān)系。為了解決基于粒子濾波定位在大環(huán)境下的不足，提出一種基于拓撲化的柵格地圖多峰粒子濾波定位算法，通過預處理過程中的節(jié)點預匹配，再通過多峰粒子濾波定位算法實現(xiàn)機器人的定位。

1.2 大環(huán)境的拓撲化節(jié)點

日常所涉及的建筑物內(nèi)部空間會按照房間的功能性進行區(qū)分。柵格地圖中空白區(qū)域用白色表示，障礙物用黑色表示，所以房間的空間和邊界大小會有所不同，根據(jù)房間的大小不同將其拓撲化為單個的節(jié)點。提出的拓撲化的流程如下:

1)根據(jù)地圖的大小設(shè)定相關(guān)區(qū)域的內(nèi)核(即圖1中圓形的區(qū)域)，內(nèi)核只能存在于環(huán)境中的空白區(qū)域。

總體的區(qū)域O，空白區(qū)域的內(nèi)核Xi：

O={X1,X2,…，Xn}。

(1)

2)遍歷地圖中的房間區(qū)域，根據(jù)可以容下內(nèi)核的區(qū)域劃分節(jié)點，內(nèi)核的相關(guān)距離小于一定的距離D，默認為同一節(jié)點。

X1-X2

(2)

3)內(nèi)核的大小根據(jù)環(huán)境的大小可以自行調(diào)節(jié)，重復步驟2)，得到新的節(jié)點信息，與已經(jīng)劃分的區(qū)域信息進行比較，若存在變化則更新節(jié)點信息，若沒有更新就不改變原有的節(jié)點信息，得到最后的k個節(jié)點為機器人定位所用，

O′={X1,X2,…，Xk}

(3)

4)節(jié)點的分布滿足相應的要求停止劃分，得到相應的節(jié)點。

樓層節(jié)點劃分如圖1，依據(jù)圖1可以發(fā)現(xiàn)，房間區(qū)域大小不同，所能容納的內(nèi)核數(shù)目不一樣。同時地圖中的房間都有很明顯的邊界，所以樣本節(jié)點的邊界也相對較明顯，按照一定的剔除規(guī)則將節(jié)點分布到各個區(qū)域。本次節(jié)點的劃分過程將電梯口以及衛(wèi)生間走廊排除在外，選取到8個房間為樣本節(jié)點。最終的節(jié)點的劃分如圖2所示，本次實驗提取房間作為機器人可能存在的位置作為樣本集，分成數(shù)字1～8如圖3。

圖1 拓撲化節(jié)點圖

Fig.1 Graph of topological node

圖2 節(jié)點劃分表示圖

Fig.2 Graph of node partition

圖3 節(jié)點樣本集

Fig.3 Sample set of node

2 基于拓撲節(jié)點室內(nèi)定位算法

2.1 節(jié)點的預匹配定位

節(jié)點的預匹配定位是通過觀測模型匹配和先驗知識計算得到。柵格地圖的匹配即通過激光雷達的點云數(shù)據(jù)和機器人所處環(huán)境的進行匹配。先驗概率通過貝葉斯遞推和機器人運動模型得到。為了消除相似的節(jié)點造成導致機器人定位失敗，采用預匹配的定位方式提高定位的精度。通過機器人在不同的節(jié)點位置的激光點云生成的局部地圖與節(jié)點樣本集如圖3所示進行匹配，實驗數(shù)據(jù)如表1所示。

表1 節(jié)點匹配結(jié)果

Tab.1 Result of node matching

匹配率節(jié)點1節(jié)點2節(jié)點3節(jié)點4節(jié)點5節(jié)點6節(jié)點7節(jié)點8節(jié)點10.9960.8510.8940.4720.5760.3770.4120.471節(jié)點20.8510.9450.9670.4620.5570.3800.4140.462節(jié)點30.8940.9670.9850.4620.5570.3800.4140.462節(jié)點40.4720.4620.4620.9850.5670.5740.5850.641節(jié)點50.5760.5570.5570.5670.9120.4600.4940.554節(jié)點60.3770.3800.3800.5740.4600.9850.5880.574節(jié)點70.4120.4140.4140.5850.4940.5880.9550.584節(jié)點80.4710.4620.4620.6410.5540.5740.5840.955

分析可以發(fā)現(xiàn)只有當機器人所在節(jié)點的位置是機器人真實的位置，才會得到該節(jié)點比較高的匹配率，這也就很好地避免機器人定位失敗的情形。但是實驗可以發(fā)現(xiàn)，在節(jié)點1、2、3，還是發(fā)生了相互耦合情況，因為環(huán)境比較相似，單幀激光雷達的數(shù)據(jù)不足以很快的區(qū)分。機器人準確定位則需要聯(lián)合運動模型的數(shù)據(jù)來匹配得到準確機器人的位置。

2.2 多峰粒子濾波定位

多峰粒子濾波定位算法是一種基于遞歸的Rao-Blcakwellised濾波器實現(xiàn)的在線SLAM算法。多峰粒子濾波定位算法是標準的粒子濾波定位算法的改進版本。通過將原有的單峰高斯分布的建議分布改進為多峰分布，結(jié)合機器人可能存在的多個位置進行概率預測，合理分布粒子的數(shù)目，通過預匹配可以得到機器人可能存在的位置，將粒子合理的分布在機器人可能存在的位置附近，從而加快機器人的定位。算法步驟如下：

采樣該步驟中，機器人在t時刻，利用t-1時刻的狀態(tài)估計xt-1,以及t-1時刻至t時刻的控制量ut，通過運動模型得到狀態(tài)轉(zhuǎn)移分布p(xt|ut,xt-1),并從狀態(tài)轉(zhuǎn)移分布中隨機采樣獲取t時刻的假定狀態(tài)xt[k]。

(4)

權(quán)重計算通過t時刻的觀測模型函數(shù)，返回每一個粒子包含的激光信息與環(huán)境的匹配程度即粒子的置信度wt[k],

wt[k]=p(zt|xt[k])。

(5)

重要性重采樣根據(jù)上一步驟得到的置信度，多峰粒子濾波定位同樣基于粒子的順序排序同時按照順序抽樣的法則進行重采樣，順序采樣保證重采樣產(chǎn)生的粒子向高權(quán)值粒子區(qū)域移動，以趨近于目標分布即機器人的位置。重采樣粒子的概率與wt[k]成比例，產(chǎn)生的粒子按照建議分布和權(quán)重wt[k]的乘積分布：

bel(x0:t)=wt[k]p(xt|xt-1,ut)·p(x0:t-1|z1:t-1,u1:t-1)。

(6)

定位結(jié)果利用粒子集中每個粒子的位姿及最新的觀測量來更新維護地圖，得到地圖的后驗概率p(M|zt,ut)同時確定機器人在t時刻的位置。多峰定位根據(jù)樣本集較高的權(quán)值的合理的選擇建議分布函數(shù)可以的得到，通過提取匹配得到機器人可能存在的節(jié)點的概率，

p(Mt=Mi|z1:t,u1:t)=ηp(zt|Mt=Mi,z1:t-1,u1:t)p(Mt=Mi|z1:t-1,u1:t)=ηp(zt|Mt=Mi)p(Mt=Mi|z1:t-1,u1∶t)，

(7)

η表示歸一化參數(shù)，Mt表示t時刻機器人的位置，Mi表示第i個節(jié)點,z1:t、u1:t-1分別表示機器人從開始到t時刻機器人的運動模型參數(shù)和觀測模型概率。通過對每個節(jié)點中的粒子的權(quán)重求和歸一化得到所有粒子的重心及機器人的真實位置，

(8)

3 定位實驗數(shù)據(jù)對分析

實驗選取圖1的辦公樓環(huán)境，本次實驗設(shè)定房間1為機器人的初始位置。粒子的初始粒子數(shù)目為5 000，分別進行初始分布為多峰和單峰的對比定位實驗。

3.1 多峰粒子濾波定位實驗

多峰定位實驗，機器人旋轉(zhuǎn)提取多幀的激光數(shù)據(jù)融合得到房間1的全方位激光點云圖。以該點云圖生成的局部地圖與節(jié)點樣本預匹配，得到機器人的初始位置屬于節(jié)點樣本集1、2、3。如表1中的數(shù)據(jù)，以樣本集1、2、3的房間中心點為高斯分布均值建立3個獨立高斯分布粒子云作為該次機器人定位中的初始粒子點云，地圖中的多峰高斯粒子點云圖如圖4所示。

3.2 單峰隨機粒子濾波定位實驗

全局隨機單峰分布初始化下初定位實驗為多峰高斯分布實驗的對照試驗。在本次實驗中，粒子云只包含隨機點云而未加入高斯分布點云以增大估計粒子搜尋到最佳位姿的可能性。全局隨機分布初始化粒子點云圖如圖5(a)所示。算法將機器人的初始最優(yōu)位姿設(shè)定為地圖的中心，圖5(b)為機器人定位完成的點云圖，該圖表明粒子隨著機器人的運動不斷向激光雷達數(shù)據(jù)匹配度較高的方向運行，此刻機器人的位置仍在房間1中，但由于粒子的重采樣過程根據(jù)激光的點云數(shù)據(jù)匹配得到，由于存在相似的環(huán)境以至于重采樣過程中大量的粒子收斂到房間2中導致定位失敗。

圖4 多峰分布定位點云圖

Fig.4 Point cloud map of multi-peak distribution localization

圖5 單峰隨機分布定位點云圖

Fig.5 Point cloud map of single-peak random distribution localization

3.3 實驗數(shù)據(jù)對比分析

對比分析圖4(a)、圖5(a),定位初始化時，地圖中都包含大量的粒子，每一個即為一個位姿估計粒子。初始化時每一個粒子被分配相同的權(quán)重，機器人的最優(yōu)位置由所有的粒子云的重心位置決定。根據(jù)馬爾科夫貝葉斯遞推，下一刻的粒子權(quán)重由上一時刻的粒子重采樣得到。通過節(jié)點預匹配將機器人可能的位置限制在節(jié)點樣本集1，2，3內(nèi)，故粒子的初始點云有3個，但隨著機器人的運動、粒子的重采樣，樣本集中權(quán)重較小的粒子數(shù)會逐漸減少、逐漸向正確的粒子點云峰趨近。粒子最后都會收斂機器人的真實位置。但是通過對比可以發(fā)現(xiàn)，多峰分布實驗中粒子收斂到機器人的真實位置，即放置機器人的房間1，但是單峰隨機分布實驗中，粒子也收斂但是定位在房間2中，定位失敗。

機器人運行過程中的粒子均權(quán)重的變化曲線如圖6所示，粒子的均權(quán)重總體呈上升趨勢，最終會收斂到一個穩(wěn)定的狀態(tài)附近。在收斂的過程中粒子的均權(quán)重會出現(xiàn)較大的波動，分析可知，由于相似環(huán)境的存在，所以在匹配的過程中，粒子的權(quán)重變化會比較劇烈。后續(xù)的過程中均權(quán)重也會有起伏的變化，這是因為機器人在運行的過程中，由于里程計累計誤差造成定位不準。總體分析可以發(fā)現(xiàn)，多峰高斯分布的收斂速度總體是要優(yōu)于單峰隨機分布的。

粒子均權(quán)重w：每一個粒子通過運動模型和觀測模型計算得到可能表示機器人的真實位置的概率。

迭代次數(shù)n：機器人運行過程中粒子更新的次數(shù)。

圖6 粒子均權(quán)重變化對比圖

Fig.6 Particle weight change comparison chart

4 結(jié)論

本文針對室內(nèi)大場景下基于粒子濾波的定位算法難以有效接近機器人的真實位姿，無法定位、定位錯誤的問題，提出了一種基于拓撲化柵格地圖多峰高斯分布的估計粒子初始化方法。該方法相較于單峰隨機分布粒子濾波定位算法，加入局部地圖節(jié)點預匹配的過程，通過預匹配的過程改進定位的流程，該方法在根據(jù)房間的大小將不同的房間分化為不同的節(jié)點。在機器人開始定位前，機器人先通過采集四周環(huán)境信息，生成局部地圖導入節(jié)點樣本集模型中進行匹配，求解出機器人所處的最大概率節(jié)點。機器人的初始估計粒子將以樣本集內(nèi)的樣本中心位置為均值構(gòu)造的高斯分布組成，樣本集內(nèi)所含樣本數(shù)決定該次初始化中高斯分布峰值個數(shù)。實驗表明，初始化粒子在相同的條件下，定位更快更準確。