基于優化Gmapping 算法的巷道噴漿機器人建圖研究

韓彥峰，李君君，肖科

（重慶大學 機械與運載工程學院，重慶 400030）

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

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

噴漿機器人在煤礦巷道環境中精確定位與地圖構建是完成噴漿作業的關鍵技術之一［13］.Gmapping算法優化了RBPF算法存在的一些問題，在室內小場景中獲得了較好的建圖效果［14］，但在煤礦巷道等大場景中長期運行時，后期粒子退化現象嚴重，粒子多樣性大幅下降，建圖精度降低，地圖不能形成閉環.

針對粒子濾波后期出現粒子有效性和多樣性缺失，經過多次迭代后丟失正確狀態的粒子導致估計失敗的問題，本文提出了分類回收重采樣算法（Classification Recovery Resampling，CRR），該方法在抑制粒子退化時通過對低權重粒子進行修正回收，在最大程度上利用現有信息，保護粒子多樣性.

1 Gmapping算法簡介

Gmapping 算法是以RBPF 算法為核心的即時定位與地圖構建算法［15］，其運行步驟主要為狀態預測、掃描匹配、采樣、計算權重、自適應重采樣、地圖更新.其主要特點為：

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

圖1 位姿分布Fig.1 Pose distribution

2）通過自適應重采樣來平衡權值退化與多樣性貧化問題.在重要性采樣結束后當權重差異程度參數Neff小于閾值時才重采樣［17］，即通過減少不必要的重采樣來保護粒子多樣性，只在權重退化嚴重時才進行重采樣.

式中：N為粒子總數；為粒子歸一化權重；i為粒子序號.Gmapping 算法優化了RBPF 算法存在的一些問題，在室內小場景中獲得了較好的建圖效果，但在大場景中長期運行時，累計重采樣次數增加，后期粒子貧化現象依然嚴重.

2 分類回收重采樣算法

2.1 算法的原理

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

2.1.1 粒子分類方案

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

2.1.2 大權重粒子復制方案

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

首先統計大權重粒子總數D，計算權重值總和ns以及平均權重ar.

然后依次計算排序后的粒子復制次數ni，ni為大于或等于wi/ar的最小整數.根據各粒子的復制次數復制產生新的粒子集，若大權重粒子類都復制完畢還未達到Nb個，則再從最大權重粒子開始重新復制直到滿足規定粒子數.

2.1.3 小權重粒子回收方案

在Gmapping 中每個粒子都攜帶了其特有的軌跡與地圖，只因為每次估計的權重較低就直接刪除并不合理，這樣不僅損害了粒子多樣性也浪費了之前步驟的信息.因此，本文提出將部分低權重粒子進行修正后回收，最大限度地保護粒子多樣性與已有信息.

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

小權重粒子在排序后的粒子集中編號為D+1～N-1，小權重粒子隨機在大權重粒子集中選擇模板，計算自身與模板之間的差距：

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

修正后的粒子：

對回收的粒子再次進行掃描匹配，更新粒子軌跡權重Ws.從小權重粒子集中回收N-Nb個粒子放入新粒子中，維持粒子集總數不變，若小權重粒子數小于N-Nb，則循環回收直到滿足N-Nb個.

2.2 算法的步驟

Gmapping算法的步驟為：

1）粒子根據權重降序排序并分類；

2）計算大權重粒子類的權重均值及復制次數；

3）復制Nb個大權重粒子；

4）修正回收N-Nb個小權重粒子，更新軌跡權重；

5）均分位姿權重.

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

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

3 仿真試驗

3.1 基礎性能測試

根據文獻［20］研究結果，基于高斯分布新粒子的改進通用粒子濾波重采樣算法（Particle Filter with the Improved Resampling，PF-IR）與現有算法相比更有優勢.在相同條件下，分別采用Gmapping 中使用的重要性重采樣（IR）算法、PF-IR 算法與本文提出的CRR 算法進行對比分析.分析過程中采用的系統模型狀態方程為：

觀測方程為：

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

仿真參數設置：x0=0.1，Qk=5，Rk=1，采樣周期dt=1，回收粒子比例b=20%，仿真粒子數N=10，仿真時間T=10 000.用均方根誤差和狀態估計誤差標準差來評價2 種算法對模型跟蹤定位的精度［21］.均方根誤差、狀態估計誤差標準差分別為：

式中：xk為狀態實際值；為估計值；為誤差均值；T為仿真時間.

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

表2 誤差統計及運行時間分析Tab.2 Error statistics and running time analysis

圖2 CRR、PF-IR與IR算法狀態估計結果Fig.2 State estimation results of CRR，PF-IR，and IR algorithms

圖3 CRR、PF-IR與IR算法狀態估計誤差對比Fig.3 Comparison of state estimation errors between CRR，PF-IR，and IR algorithms

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

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

圖4 CRR與CR算法狀態估計結果Fig.4 CRR and CR algorithm state estimation results

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

3.2 地圖構建測試

采用slam benchmarking*上的典型數據集ACES building（以下簡稱ACES）和MIT Killian Court（以下簡稱MIT）對算法進行測試，并利用其提供的評估工具Metric Evaluator［22］對測試結果進行定量評估.

ACES 和MIT 數據集都有典型的巷道特征.其中ACES 場景較小，整體大小為40 m × 40 m，結構簡單.MIT 數據集規模較大，達到了250 m × 215 m 且結構復雜，包含多個互相形成閉環回路的長走廊，建圖難度高［23］.試驗參數設置如表3所示.

表3 試驗參數設置Tab.3 Experimental parameter setting

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

圖6 ACES數據集10個粒子的建圖結果Fig.6 Mapping results for 10 particles in ACES dataset

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

圖7 ACES數據集定位誤差Fig.7 Positioning error of ACES dataset

MIT 數據集60 個粒子的建圖結果如圖8 所示.機器人從標記點A開始，穿過標記為B的第一個回路.然后，它穿過標記為C和D的回路，完成回路E后移回標記為A和B的地方，再經過標記為F和G的兩個大回路.圖中矩形部分是顯示地圖容易出現錯誤的放大圖.

圖8 MIT數據集60個粒子的建圖結果Fig.8 Mapping results for 60 particles in MIT dataset

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

在此次試驗中分別統計了各算法計算出的4 677 個定位誤差，如圖9 所示.由圖9 可知，使用IR和CR 的Gmapping 算法定位平移誤差和旋轉誤差都遠遠超過正常水平，最大誤差分別達到了40 m、12°左右.使用PF-IR算法2種誤差都有所降低但幅度不及CRR 算法.在此次試驗中，本文算法定位誤差與PF-IR 算法相比降低了一個數量級，平移誤差和旋轉誤差始終處在較低水平.

圖9 MIT數據集定位誤差Fig.9 Positioning error of MIT dataset

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

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

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

表4 建圖采樣周期及算法運行時間統計Tab.4 Mapping sampling period and algorithm running time statistics

4 結論

為解決Gmapping 算法在巷道大場景中權值退化和粒子多樣性貧化問題，本文提出了分類回收重采樣算法.在粒子濾波的重采樣時，針對低權重粒子并不直接刪除，而是修正后按總粒子數的一定比例進行回收利用，利用現有信息，在抑制權值退化時盡量保護粒子多樣性.通過試驗得出了以下結論：

1）在一維系統跟蹤試驗中，CRR 算法相比于PF-IR、IR 算法精度分別提高了20.0%、32.3%.利用兩個具有典型巷道特征的數據集ACES building、MIT Killian Court 對改進后的Gmapping 算法進行了驗證，結果表明，無論在小場景還是在大場景中，相比于原始算法，都在使用相同粒子數的情況下，獲得更準確、清晰的環境地圖.

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

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