韓彥峰，李君君，肖科

（重慶大學(xué) 機(jī)械與運(yùn)載工程學(xué)院，重慶 400030）

煤礦的開采需要挖掘大量的巷道，這些巷道 為煤礦挖掘和煤炭運(yùn)輸提供了通道，巷道噴漿支護(hù)是保障巷道正常使用和安全的最有效的措施之一.隨著國(guó)家煤礦工業(yè)智能化進(jìn)程的推進(jìn)，煤礦巷道噴漿機(jī)器人將是實(shí)現(xiàn)無(wú)人自主噴漿作業(yè)最好的方法之一［1］.

由于環(huán)境因素的限制，礦山車輛的定位無(wú)法利用全球衛(wèi)星定位系統(tǒng)來(lái)實(shí)現(xiàn).早期煤礦多采用鋪設(shè)軌道來(lái)實(shí)現(xiàn)車輛的移動(dòng)與定位，但這種方式成本高且在巷道施工早期無(wú)法實(shí)現(xiàn)［2］，這就要求噴漿機(jī)器人要在未知的環(huán)境中，通過(guò)自身的移動(dòng)和傳感器的數(shù)據(jù)來(lái)確定自己的位置同時(shí)構(gòu)建環(huán)境的地圖，即實(shí)現(xiàn)同步定位與地圖構(gòu)建技術(shù)［3］.傳統(tǒng)的定位與建圖問(wèn)題是通過(guò)機(jī)器人的運(yùn)動(dòng)數(shù)學(xué)模型來(lái)進(jìn)行推導(dǎo)的，由于無(wú)法考慮傳感器的測(cè)量誤差、電機(jī)控制精度以及計(jì)算過(guò)程中的近似計(jì)算誤差等，定位效果很差［4］.考慮到機(jī)器人中的不確定性因素，Gustafsson 等［5］在概率論與數(shù)理統(tǒng)計(jì)的框架下對(duì)機(jī)器人定位與建圖問(wèn)題進(jìn)行了研究，將同時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）問(wèn)題轉(zhuǎn)換成狀態(tài)估計(jì)問(wèn)題［6］.隨后許多學(xué)者利用卡爾曼濾波、擴(kuò)展卡爾曼濾波等方法來(lái)進(jìn)行求解，但基于擴(kuò)展卡爾曼濾波的SLAM 算法計(jì)算復(fù)雜度大、精度不高［7］.Kelly等［8］提出粒子濾波器算法（Rao-Blackwellised Particle Filter，RBPF），分別用粒子濾波器和擴(kuò)展卡爾曼濾波器實(shí)現(xiàn)對(duì)機(jī)器人的位姿估計(jì)和環(huán)境路標(biāo)點(diǎn)的位置估計(jì)［9］.李帥鑫等［10］采用RBPF 算法構(gòu)建機(jī)器人特征地圖并命名為FastSLAM 算法.RBPF 采用運(yùn)動(dòng)模型作為建議分布，但由于激光雷達(dá)比里程計(jì)精度高，通過(guò)運(yùn)動(dòng)模型采樣的粒子落在觀測(cè)分布的區(qū)間較少［11］，使得觀測(cè)更新過(guò)程精度低.Li等［12］通過(guò)掃描匹配的方法直接從觀測(cè)分布的區(qū)間進(jìn)行采樣，使得利用較少的粒子數(shù)獲得了較高的精度，將改進(jìn)后的算法用于構(gòu)建柵格地圖并應(yīng)用到實(shí)體機(jī)器人中，形成經(jīng)典的Gmapping算法.

噴漿機(jī)器人在煤礦巷道環(huán)境中精確定位與地圖構(gòu)建是完成噴漿作業(yè)的關(guān)鍵技術(shù)之一［13］.Gmapping算法優(yōu)化了RBPF算法存在的一些問(wèn)題，在室內(nèi)小場(chǎng)景中獲得了較好的建圖效果［14］，但在煤礦巷道等大場(chǎng)景中長(zhǎng)期運(yùn)行時(shí)，后期粒子退化現(xiàn)象嚴(yán)重，粒子多樣性大幅下降，建圖精度降低，地圖不能形成閉環(huán).

針對(duì)粒子濾波后期出現(xiàn)粒子有效性和多樣性缺失，經(jīng)過(guò)多次迭代后丟失正確狀態(tài)的粒子導(dǎo)致估計(jì)失敗的問(wèn)題，本文提出了分類回收重采樣算法（Classification Recovery Resampling，CRR），該方法在抑制粒子退化時(shí)通過(guò)對(duì)低權(quán)重粒子進(jìn)行修正回收，在最大程度上利用現(xiàn)有信息，保護(hù)粒子多樣性.

1 Gmapping算法簡(jiǎn)介

Gmapping 算法是以RBPF 算法為核心的即時(shí)定位與地圖構(gòu)建算法［15］，其運(yùn)行步驟主要為狀態(tài)預(yù)測(cè)、掃描匹配、采樣、計(jì)算權(quán)重、自適應(yīng)重采樣、地圖更新.其主要特點(diǎn)為：

1）通過(guò)掃描匹配采樣過(guò)程，將激光信息引入建議分布，由于激光雷達(dá)一般比里程計(jì)精度更高，觀測(cè)模型中位姿分布更集中，如圖1 所示，其中p(X|Z)和p(X|X′，U)分別表示單獨(dú)從里程計(jì)模型、里程計(jì)激光雷達(dá)聯(lián)合模型中進(jìn)行采樣的位姿分布概率曲線，L(i)表示位姿分布區(qū)間.可以使用更少的粒子來(lái)表示位姿分布［16］.

圖1 位姿分布Fig.1 Pose distribution

2）通過(guò)自適應(yīng)重采樣來(lái)平衡權(quán)值退化與多樣性貧化問(wèn)題.在重要性采樣結(jié)束后當(dāng)權(quán)重差異程度參數(shù)Neff小于閾值時(shí)才重采樣［17］，即通過(guò)減少不必要的重采樣來(lái)保護(hù)粒子多樣性，只在權(quán)重退化嚴(yán)重時(shí)才進(jìn)行重采樣.

式中：N為粒子總數(shù)；為粒子歸一化權(quán)重；i為粒子序號(hào).Gmapping 算法優(yōu)化了RBPF 算法存在的一些問(wèn)題，在室內(nèi)小場(chǎng)景中獲得了較好的建圖效果，但在大場(chǎng)景中長(zhǎng)期運(yùn)行時(shí)，累計(jì)重采樣次數(shù)增加，后期粒子貧化現(xiàn)象依然嚴(yán)重.

2 分類回收重采樣算法

2.1 算法的原理

粒子濾波器經(jīng)過(guò)多次迭代后粒子集的重要性權(quán)值的方差會(huì)越來(lái)越大，最后權(quán)值只集中在少數(shù)粒子上，從而導(dǎo)致粒子集不能近似表示后驗(yàn)概率分布［18］.在Gmapping 中采用重要性重采樣（Importance Resampling，IR）來(lái)抑制粒子退化，但其是大量繁殖權(quán)值高的粒子，刪除權(quán)值低的粒子，這又會(huì)導(dǎo)致粒子多樣性缺失問(wèn)題［19］.為了能夠更好地同時(shí)緩解這兩個(gè)問(wèn)題，本文提出了分類回收重采樣算法，在改善粒子退化問(wèn)題時(shí)盡可能保護(hù)粒子多樣性，使算法在長(zhǎng)期運(yùn)行過(guò)程中能有效地表示后驗(yàn)概率分布.

2.1.1 粒子分類方案

重采樣的本質(zhì)就是多次復(fù)制權(quán)重大的粒子，淘汰權(quán)重小的粒子.在粒子集權(quán)重方差大于閾值時(shí)直接對(duì)粒子權(quán)重進(jìn)行降序排序，利用平均權(quán)值=1/N將粒子集分為兩類，復(fù)制權(quán)重值wit≥的粒子，對(duì)于權(quán)重值的粒子并不直接舍棄，在對(duì)低權(quán)重粒子進(jìn)行修正后按總粒子數(shù)的一定比例進(jìn)行回收.

2.1.2 大權(quán)重粒子復(fù)制方案

分類后的大權(quán)重粒子權(quán)重值都高于平均權(quán)值，屬于較好的粒子，從該類粒子中直接復(fù)制產(chǎn)生Nb個(gè)新粒子，其中Nb=(1 -b)N，N為粒子總數(shù)，b為回收粒子占比.

首先統(tǒng)計(jì)大權(quán)重粒子總數(shù)D，計(jì)算權(quán)重值總和ns以及平均權(quán)重ar.

然后依次計(jì)算排序后的粒子復(fù)制次數(shù)ni，ni為大于或等于wi/ar的最小整數(shù).根據(jù)各粒子的復(fù)制次數(shù)復(fù)制產(chǎn)生新的粒子集，若大權(quán)重粒子類都復(fù)制完畢還未達(dá)到Nb個(gè)，則再?gòu)淖畲髾?quán)重粒子開始重新復(fù)制直到滿足規(guī)定粒子數(shù).

2.1.3 小權(quán)重粒子回收方案

在Gmapping 中每個(gè)粒子都攜帶了其特有的軌跡與地圖，只因?yàn)槊看喂烙?jì)的權(quán)重較低就直接刪除并不合理，這樣不僅損害了粒子多樣性也浪費(fèi)了之前步驟的信息.因此，本文提出將部分低權(quán)重粒子進(jìn)行修正后回收，最大限度地保護(hù)粒子多樣性與已有信息.

對(duì)于小權(quán)重粒子類的粒子，它的上一代粒子本身就是好粒子，誤差影響使現(xiàn)在時(shí)刻的位姿估計(jì)產(chǎn)生了較大的偏差，權(quán)重較低，所以選取現(xiàn)在時(shí)刻大權(quán)重粒子為模板對(duì)其進(jìn)行修正，將修正后的粒子回收進(jìn)新粒子集.

小權(quán)重粒子在排序后的粒子集中編號(hào)為D+1～N-1，小權(quán)重粒子隨機(jī)在大權(quán)重粒子集中選擇模板，計(jì)算自身與模板之間的差距：

式中：X代表一個(gè)粒子，該粒子的位姿為x、y、θ；t為時(shí)間；i、j為粒子序號(hào).再加入服從正態(tài)分布N(0，σ2)的擾動(dòng)值ni-j，其中方差σ2根據(jù)模板的權(quán)值和自身與模板間差距計(jì)算.

修正后的粒子：

對(duì)回收的粒子再次進(jìn)行掃描匹配，更新粒子軌跡權(quán)重Ws.從小權(quán)重粒子集中回收N-Nb個(gè)粒子放入新粒子中，維持粒子集總數(shù)不變，若小權(quán)重粒子數(shù)小于N-Nb，則循環(huán)回收直到滿足N-Nb個(gè).

2.2 算法的步驟

Gmapping算法的步驟為：

1）粒子根據(jù)權(quán)重降序排序并分類；

2）計(jì)算大權(quán)重粒子類的權(quán)重均值及復(fù)制次數(shù)；

3）復(fù)制Nb個(gè)大權(quán)重粒子；

4）修正回收N-Nb個(gè)小權(quán)重粒子，更新軌跡權(quán)重；

5）均分位姿權(quán)重.

分類回收重采樣算法偽代碼如表1所示.

表1 分類回收重采樣算法偽代碼Tab.1 Pseudo code of classification recovery resampling algorithm

3 仿真試驗(yàn)

3.1 基礎(chǔ)性能測(cè)試

根據(jù)文獻(xiàn)［20］研究結(jié)果，基于高斯分布新粒子的改進(jìn)通用粒子濾波重采樣算法（Particle Filter with the Improved Resampling，PF-IR）與現(xiàn)有算法相比更有優(yōu)勢(shì).在相同條件下，分別采用Gmapping 中使用的重要性重采樣（IR）算法、PF-IR 算法與本文提出的CRR 算法進(jìn)行對(duì)比分析.分析過(guò)程中采用的系統(tǒng)模型狀態(tài)方程為：

觀測(cè)方程為：

式中：k為時(shí)間，以1 為周期進(jìn)行采樣取值；zk表示k時(shí)刻的系統(tǒng)觀測(cè)值；wk-1和vk是均值為0，方差分別為Qk和Rk的高斯噪聲，分別服從正態(tài)分布N(0，Qk)和N(0，Rk).

仿真參數(shù)設(shè)置：x0=0.1，Qk=5，Rk=1，采樣周期dt=1，回收粒子比例b=20%，仿真粒子數(shù)N=10，仿真時(shí)間T=10 000.用均方根誤差和狀態(tài)估計(jì)誤差標(biāo)準(zhǔn)差來(lái)評(píng)價(jià)2 種算法對(duì)模型跟蹤定位的精度［21］.均方根誤差、狀態(tài)估計(jì)誤差標(biāo)準(zhǔn)差分別為：

式中：xk為狀態(tài)實(shí)際值；為估計(jì)值；為誤差均值；T為仿真時(shí)間.

CRR、PF-IR 與IR 算法對(duì)式（6）、式（7）組成的系統(tǒng)模型的狀態(tài)估計(jì)結(jié)果，如圖2 所示.為了清晰起見，圖2 中只展示了后50 個(gè)時(shí)間點(diǎn)，由圖2 可以看出，CRR 算法比PF-IR 算法、IR 算法更接近真實(shí)值.圖3 和表2 對(duì)3 種算法的誤差和運(yùn)行時(shí)間進(jìn)行了統(tǒng)計(jì)分析，結(jié)果表明，與PF-IR 算法和IR 算法相比，CRR 算法在一定程度上提高了跟蹤精度.CRR 算法的均方根誤差和狀態(tài)估計(jì)誤差標(biāo)準(zhǔn)差相比PF-IR 算法和IR 算法分別降低了20.2%、30.0%和32.3%、49.1%.CRR 運(yùn)行時(shí)間相比PF-IR 算法和IR 算法分別增加了30.4%和47.6%.但單次總體重采樣運(yùn)行時(shí)間在1/10 000 s 數(shù)量級(jí)，機(jī)器人運(yùn)動(dòng)一段時(shí)間后才會(huì)進(jìn)行一次重采樣計(jì)算，這個(gè)時(shí)間遠(yuǎn)大于該數(shù)量級(jí).因此，CRR 算法在提高跟蹤定位精度的同時(shí)依然能保證算法的實(shí)時(shí)性.

表2 誤差統(tǒng)計(jì)及運(yùn)行時(shí)間分析Tab.2 Error statistics and running time analysis

圖2 CRR、PF-IR與IR算法狀態(tài)估計(jì)結(jié)果Fig.2 State estimation results of CRR，PF-IR，and IR algorithms

圖3 CRR、PF-IR與IR算法狀態(tài)估計(jì)誤差對(duì)比Fig.3 Comparison of state estimation errors between CRR，PF-IR，and IR algorithms

為了驗(yàn)證粒子回收的必要性，去除CRR 算法中粒子回收環(huán)節(jié)進(jìn)行消融試驗(yàn).為了便于描述，將去除粒子回收環(huán)節(jié)后的CRR 算法稱為分類重采樣算法（Classified Resampling，CR）.對(duì)CR 算法和CRR 算法利用上述系統(tǒng)的狀態(tài)方程進(jìn)行了跟蹤定位精度對(duì)比試驗(yàn)，并選取后期某一時(shí)刻粒子分布狀態(tài)進(jìn)行了多樣性對(duì)比.

CRR 與CR 算法狀態(tài)估計(jì)結(jié)果如圖4 所示，圖5為k=9 986時(shí)粒子分布情況.由圖4和圖5可知，CRR算法跟蹤定位精度要高于CR 算法；CR 算法的粒子大多數(shù)集中于少數(shù)狀態(tài)，從而失去了粒子多樣性，不利于濾波的狀態(tài)估計(jì).而CRR 在高似然區(qū)和低似然區(qū)都有一定數(shù)量的粒子分布，在一定程度上保證粒子多樣性，從而提高狀態(tài)估計(jì)的精度.

圖4 CRR與CR算法狀態(tài)估計(jì)結(jié)果Fig.4 CRR and CR algorithm state estimation results

圖5 k=9 986時(shí)粒子分布情況Fig.5 Particle distribution when k=9 986

3.2 地圖構(gòu)建測(cè)試

采用slam benchmarking*上的典型數(shù)據(jù)集ACES building（以下簡(jiǎn)稱ACES）和MIT Killian Court（以下簡(jiǎn)稱MIT）對(duì)算法進(jìn)行測(cè)試，并利用其提供的評(píng)估工具M(jìn)etric Evaluator［22］對(duì)測(cè)試結(jié)果進(jìn)行定量評(píng)估.

ACES 和MIT 數(shù)據(jù)集都有典型的巷道特征.其中ACES 場(chǎng)景較小，整體大小為40 m × 40 m，結(jié)構(gòu)簡(jiǎn)單.MIT 數(shù)據(jù)集規(guī)模較大，達(dá)到了250 m × 215 m 且結(jié)構(gòu)復(fù)雜，包含多個(gè)互相形成閉環(huán)回路的長(zhǎng)走廊，建圖難度高［23］.試驗(yàn)參數(shù)設(shè)置如表3所示.

表3 試驗(yàn)參數(shù)設(shè)置Tab.3 Experimental parameter setting

ACES 數(shù)據(jù)集10 個(gè)粒子的建圖結(jié)果如圖6 所示.機(jī)器人從A點(diǎn)開始，依次經(jīng)過(guò)B、C、D點(diǎn)，再回到A點(diǎn)形成閉環(huán)，然后才從A點(diǎn)出發(fā)到達(dá)E點(diǎn)，再分別到達(dá)B、C、D點(diǎn)完成建圖.可以看出使用IR 算法的Gmapping得到的地圖整體形狀上存在畸變，在標(biāo)記1、2處存在地圖重疊、分層現(xiàn)象，由于閉環(huán)誤差較大，在3處形成了錯(cuò)誤結(jié)構(gòu)；使用PF-IR 算法的地圖在整體上無(wú)明顯錯(cuò)誤，但在圖中標(biāo)記處閉環(huán)效果不佳，地圖出現(xiàn)了重疊；使用CR 算法的地圖一致性得不到保障，在圖中標(biāo)記處出現(xiàn)了較大程度的錯(cuò)位；而使用CRR 算法的地圖結(jié)構(gòu)正確無(wú)畸變，邊界清晰、無(wú)重疊分層，很好地反映了環(huán)境真實(shí)情況.

圖6 ACES數(shù)據(jù)集10個(gè)粒子的建圖結(jié)果Fig.6 Mapping results for 10 particles in ACES dataset

精確的定位是構(gòu)建準(zhǔn)確地圖的前提，所以定位精度的高低也能從側(cè)面反映建圖的優(yōu)劣.根據(jù)數(shù)據(jù)集提供的1 279 對(duì)按時(shí)間先后排列的真實(shí)相對(duì)位姿關(guān)系，對(duì)算法的定位誤差進(jìn)行定量分析，如圖7 所示.由圖7 可知，IR 算法在序號(hào)300 處存在較大的平移誤差，CR 平移誤差與IR 相近；PF-IR 和CRR 算法的平移誤差都有所降低，但CRR效果更好.在此次試驗(yàn)中旋轉(zhuǎn)誤差差別不大.

圖7 ACES數(shù)據(jù)集定位誤差Fig.7 Positioning error of ACES dataset

MIT 數(shù)據(jù)集60 個(gè)粒子的建圖結(jié)果如圖8 所示.機(jī)器人從標(biāo)記點(diǎn)A開始，穿過(guò)標(biāo)記為B的第一個(gè)回路.然后，它穿過(guò)標(biāo)記為C和D的回路，完成回路E后移回標(biāo)記為A和B的地方，再經(jīng)過(guò)標(biāo)記為F和G的兩個(gè)大回路.圖中矩形部分是顯示地圖容易出現(xiàn)錯(cuò)誤的放大圖.

圖8 MIT數(shù)據(jù)集60個(gè)粒子的建圖結(jié)果Fig.8 Mapping results for 60 particles in MIT dataset

由圖8 可知，使用IR 算法進(jìn)行重采樣的Gmapping算法的構(gòu)圖一致性很差，在標(biāo)記點(diǎn)1、2處地圖出現(xiàn)較大程度的重疊分層，在標(biāo)記3 處地圖結(jié)構(gòu)錯(cuò)亂.CR 算法的效果與IR 相似，在標(biāo)記1 處存在畸變，標(biāo)記2、3 處形成了兩處結(jié)構(gòu)相同但位置不同的地圖，建圖未能形成閉環(huán).PF-IR 算法較原算法效果有一定的提升，但在標(biāo)記1、2 處依然存在重疊和結(jié)構(gòu)等現(xiàn)象.使用本文算法能夠在大場(chǎng)景獲得清楚、結(jié)構(gòu)正確的地圖，從圖8（d）放大圖中能看出在易錯(cuò)局部也未出現(xiàn)分層等錯(cuò)誤現(xiàn)象.

在此次試驗(yàn)中分別統(tǒng)計(jì)了各算法計(jì)算出的4 677 個(gè)定位誤差，如圖9 所示.由圖9 可知，使用IR和CR 的Gmapping 算法定位平移誤差和旋轉(zhuǎn)誤差都遠(yuǎn)遠(yuǎn)超過(guò)正常水平，最大誤差分別達(dá)到了40 m、12°左右.使用PF-IR算法2種誤差都有所降低但幅度不及CRR 算法.在此次試驗(yàn)中，本文算法定位誤差與PF-IR 算法相比降低了一個(gè)數(shù)量級(jí)，平移誤差和旋轉(zhuǎn)誤差始終處在較低水平.

圖9 MIT數(shù)據(jù)集定位誤差Fig.9 Positioning error of MIT dataset

在建圖后期，以位姿的x值為參考，統(tǒng)計(jì)了4 種算法在2 個(gè)數(shù)據(jù)集運(yùn)行時(shí)的粒子分布情況，結(jié)果如圖10 所示.由圖10 可知，4 種算法在各自估計(jì)值處出現(xiàn)了不同的分布情況，由于使用IR 和CR 算法的Gmapping 區(qū)大量復(fù)制粒子，導(dǎo)致后期每個(gè)粒子的估計(jì)值高度相似，出現(xiàn)了粒子貧化現(xiàn)象；PF-IR 的粒子與前2 種方法相比多樣性有所增加但不及CRR 算法.本文算法在復(fù)制高權(quán)重粒子時(shí)也對(duì)低權(quán)重粒子進(jìn)行了修正回收，可以看出本文算法的大部分粒子也位于高似然區(qū)，但同時(shí)在低似然區(qū)也保留了一定數(shù)量的粒子，更符合粒子濾波的基本思想.

圖10 粒子分布圖Fig.10 Particle distribution diagram

由一維系統(tǒng)仿真結(jié)果可知，CRR 算法的運(yùn)行時(shí)間比原算法運(yùn)行時(shí)間長(zhǎng)，所以對(duì)SLAM 的實(shí)時(shí)性進(jìn)行了研究.只有當(dāng)機(jī)器人走過(guò)一定的距離、旋轉(zhuǎn)一定的角度或者經(jīng)過(guò)一定的時(shí)間才會(huì)采樣進(jìn)行地圖增量計(jì)算，所以存在最短采樣周期.2 個(gè)數(shù)據(jù)集的建圖采樣周期及算法運(yùn)行時(shí)間統(tǒng)計(jì)如表4 所示，統(tǒng)計(jì)了采用不同重采樣算法的一次地圖增量計(jì)算的最長(zhǎng)運(yùn)行時(shí)間.由表4 可知，IR 與PF-IR 算法運(yùn)行時(shí)間相近，CRR 算法與IR、PF-IR 相比有所提升，但依然低于最短采樣周期，在2 個(gè)數(shù)據(jù)集中CRR 運(yùn)行時(shí)間分別為最短采樣周期的20.36%和62.27%，能夠滿足建圖的實(shí)時(shí)性.

表4 建圖采樣周期及算法運(yùn)行時(shí)間統(tǒng)計(jì)Tab.4 Mapping sampling period and algorithm running time statistics

4 結(jié)論

為解決Gmapping 算法在巷道大場(chǎng)景中權(quán)值退化和粒子多樣性貧化問(wèn)題，本文提出了分類回收重采樣算法.在粒子濾波的重采樣時(shí)，針對(duì)低權(quán)重粒子并不直接刪除，而是修正后按總粒子數(shù)的一定比例進(jìn)行回收利用，利用現(xiàn)有信息，在抑制權(quán)值退化時(shí)盡量保護(hù)粒子多樣性.通過(guò)試驗(yàn)得出了以下結(jié)論：

1）在一維系統(tǒng)跟蹤試驗(yàn)中，CRR 算法相比于PF-IR、IR 算法精度分別提高了20.0%、32.3%.利用兩個(gè)具有典型巷道特征的數(shù)據(jù)集ACES building、MIT Killian Court 對(duì)改進(jìn)后的Gmapping 算法進(jìn)行了驗(yàn)證，結(jié)果表明，無(wú)論在小場(chǎng)景還是在大場(chǎng)景中，相比于原始算法，都在使用相同粒子數(shù)的情況下，獲得更準(zhǔn)確、清晰的環(huán)境地圖.

2）比較了3 種算法建圖的定位精度和粒子分布情況.在此次試驗(yàn)中，本文算法的定位誤差最大值比對(duì)照算法降低了一個(gè)數(shù)量級(jí)且分布更均勻；對(duì)照算法粒子分布后期高度集中在估計(jì)值附近，出現(xiàn)了粒子貧化現(xiàn)象；本文算法的粒子分布主要集中在高似然區(qū)域，同時(shí)在低似然區(qū)域保留了少量粒子，保持了較好的粒子多樣性.

3）在實(shí)際建圖過(guò)程中，對(duì)比了IR、PF-IR、CRR算法一次地圖增量計(jì)算最大運(yùn)行時(shí)間.IR 與PF-IR算法運(yùn)行時(shí)間相近，CRR 算法與IR、PF-IR 相比有所提升，但依然低于最短采樣周期，在2 個(gè)數(shù)據(jù)集中CRR 運(yùn)行時(shí)間分別為最短采樣周期的20.36%和62.27%，能夠滿足SLAM的實(shí)時(shí)性要求.