王 翰，蔣 林,*，周 亮

(1.武漢科技大學(xué) 機(jī)械傳動(dòng)與制造工程湖北省重點(diǎn)實(shí)驗(yàn)室, 湖北 武漢 430081；2.武漢科技大學(xué) 機(jī)器人與智能系統(tǒng)研究院, 湖北 武漢 430081)

0 引言

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

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

1 柵格地圖拓?fù)浠?/h2>

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

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

1.2 大環(huán)境的拓?fù)浠?jié)點(diǎn)

日常所涉及的建筑物內(nèi)部空間會(huì)按照房間的功能性進(jìn)行區(qū)分。柵格地圖中空白區(qū)域用白色表示，障礙物用黑色表示，所以房間的空間和邊界大小會(huì)有所不同，根據(jù)房間的大小不同將其拓?fù)浠癁閱蝹€(gè)的節(jié)點(diǎn)。提出的拓?fù)浠牧鞒倘缦?

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é)點(diǎn)，內(nèi)核的相關(guān)距離小于一定的距離D，默認(rèn)為同一節(jié)點(diǎn)。

X1-X2

(2)

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

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

(3)

4)節(jié)點(diǎn)的分布滿足相應(yīng)的要求停止劃分，得到相應(yīng)的節(jié)點(diǎn)。

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

圖1 拓?fù)浠?jié)點(diǎn)圖

Fig.1 Graph of topological node

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

Fig.2 Graph of node partition

圖3 節(jié)點(diǎn)樣本集

Fig.3 Sample set of node

2 基于拓?fù)涔?jié)點(diǎn)室內(nèi)定位算法

2.1 節(jié)點(diǎn)的預(yù)匹配定位

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

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

Tab.1 Result of node matching

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

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

2.2 多峰粒子濾波定位

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

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

(4)

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

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

(5)

重要性重采樣根據(jù)上一步驟得到的置信度，多峰粒子濾波定位同樣基于粒子的順序排序同時(shí)按照順序抽樣的法則進(jìn)行重采樣，順序采樣保證重采樣產(chǎn)生的粒子向高權(quán)值粒子區(qū)域移動(dòng)，以趨近于目標(biāo)分布即機(jī)器人的位置。重采樣粒子的概率與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é)果利用粒子集中每個(gè)粒子的位姿及最新的觀測(cè)量來更新維護(hù)地圖，得到地圖的后驗(yàn)概率p(M|zt,ut)同時(shí)確定機(jī)器人在t時(shí)刻的位置。多峰定位根據(jù)樣本集較高的權(quán)值的合理的選擇建議分布函數(shù)可以的得到，通過提取匹配得到機(jī)器人可能存在的節(jié)點(diǎn)的概率，

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時(shí)刻機(jī)器人的位置，Mi表示第i個(gè)節(jié)點(diǎn),z1:t、u1:t-1分別表示機(jī)器人從開始到t時(shí)刻機(jī)器人的運(yùn)動(dòng)模型參數(shù)和觀測(cè)模型概率。通過對(duì)每個(gè)節(jié)點(diǎn)中的粒子的權(quán)重求和歸一化得到所有粒子的重心及機(jī)器人的真實(shí)位置，

(8)

3 定位實(shí)驗(yàn)數(shù)據(jù)對(duì)分析

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

3.1 多峰粒子濾波定位實(shí)驗(yàn)

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

3.2 單峰隨機(jī)粒子濾波定位實(shí)驗(yàn)

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

圖4 多峰分布定位點(diǎn)云圖

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

圖5 單峰隨機(jī)分布定位點(diǎn)云圖

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

3.3 實(shí)驗(yàn)數(shù)據(jù)對(duì)比分析

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

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

粒子均權(quán)重w：每一個(gè)粒子通過運(yùn)動(dòng)模型和觀測(cè)模型計(jì)算得到可能表示機(jī)器人的真實(shí)位置的概率。

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

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

Fig.6 Particle weight change comparison chart

4 結(jié)論

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