基于智能優(yōu)化箱粒子濾波的移動機器人FastSLAM

羅景文，秦世引

（1.云南師范大學 信息學院，昆明 650500；2.北京航空航天大學 自動化科學與電氣工程學院，北京 100083）

隨著人工智能和機器人技術的高速發(fā)展及其在各行各業(yè)的推廣應用，人們對于擁有自主作業(yè)能力的智能機器人的需求日益增加，而同步定位與地圖構建（simultaneous localization and mapping，SLAM）是實現機器人真正智能化的關鍵支撐技術。目前，針對SLAM 的求解途徑大致可歸納為基于最優(yōu)濾波的方法和基于圖優(yōu)化的方法［1］。其中，基于最優(yōu)濾波的方法主要依據遞歸貝葉斯狀態(tài)估計理論，主要包括基于擴展卡爾曼濾波（extended Kalman filter，EKF）的方法和基于粒子濾波（particle filter，PF）的方法。對于在EKF架構下解算的SLAM 算法，環(huán)境特征的感知信息會隨著機器人的運動持續(xù)地加入到聯合狀態(tài)特征向量及其協方差矩陣中，其主要問題在于對特征點深度和協方差矩陣的初始值依賴較大，如果設置欠妥，則會使得估計結果不一致，甚至導致算法 不 收 斂［2-3］。為 此，Doucet等［4］根 據Rao-Blackwellized粒子濾波（Rao-Blackwellized particle filter，RBPF）的分解思想提出了RBPF-SLAM，該方法可以有效解決地圖構建過程中的數據關聯問題，并且可以根據實際應用需要構建出不同類型的 地 圖。隨 后，Montemerlo 等［5］又 提 出 了FastSLAM算法，并通過進一步改進和優(yōu)化提出了更加完善的版本FastSLAM 2.0［6］，但該算法需要采樣足夠數量的粒子才能正確完成地圖構建，由于每個粒子都存儲了一張周圍環(huán)境的完整地圖，同時又包含了機器人軌跡估計的結果，需要占用大量的計算資源和存儲空間。因此，FastSLAM 類算法面臨的主要挑戰(zhàn)是如何盡可能減少采樣所需的粒子數。為了解決算法存儲量過大的問題，Eliazar和Parr［7］研究并提出了一種基于分布式粒子的SLAM算法，大幅度降低了算法占據的存儲空間。為了減少地圖構建過程中需要采樣的粒子數，通常采取將當前時刻的觀測與已構建的地圖進行掃描匹配，再將匹配結果替代誤差較大的里程計讀數，進而改善粒子采樣的提議分布［8-10］。

然而，這類基于概率的SLAM 算法中都存在一個共同的缺陷，即濾波一致性問題。Bailey等［11］研究發(fā)現，RBPF-SLAM 在足夠數量粒子條件下可以構建出較為準確的地圖，但其濾波結果的一致性僅在較短時間內滿足要求。而此問題如果采用區(qū)間分析（interval analysis，IA）技術則可以得到有效地解決［12］。這是由于區(qū)間分析所針對的運算對象是區(qū)間量，當浮點數受到四舍五入運算的影響產生誤差時，利用區(qū)間運算卻能得到包含精確解的嚴格區(qū)間界限，因此可以提供有界的結果，保證了濾波的一致性。Gning等［13］將區(qū)間運算的特點與傳統(tǒng)PF不受系統(tǒng)模型約束的性能優(yōu)勢相結合，提出了一種新穎的非線性濾波算法，稱為箱粒子濾波（box particle filter，BPF），其核心思想是：將傳統(tǒng)的點粒子和系統(tǒng)誤差統(tǒng)計模型分別取代為區(qū)間箱粒子和系統(tǒng)誤差界限模型，再進行貝葉斯濾波。相比于傳統(tǒng)PF，其最大的性能優(yōu)勢在于所需采樣的箱粒子數少且運行速度快，因而算法的運算復雜度低、實時性好。在目標跟蹤領域的一些應用中［14-15］，BPF只需采樣數十個箱粒子就可以達到與傳統(tǒng)PF采樣數千粒子所獲得的濾波精度和可靠性。

基于以上分析，本文在FastSLAM 算法架構基礎上，充分利用BPF所需粒子數少、運行速度快的性能優(yōu)勢，針對BPF中的箱粒子退化現象，綜合運用螢火蟲算法（firefly algorithm，FA）［16］的尋優(yōu)機制和IA技術，創(chuàng)立高性能的智能優(yōu)化BPF算法，使得箱粒子集朝著機器人真實軌跡靠近，進而提高移動機器人位姿估計和同步定位的精度。結合擴展區(qū)間卡爾曼濾波（extended interval Kalman filter，EIKF）［17］完成環(huán)境地圖的構建和更新，進而增加路標協方差矩陣預測和更新的精度和魯棒性。

1 區(qū)間分析

在區(qū)間分析［18］中，一維閉合區(qū)間定義了實數域中一個連續(xù)、封閉的子集：

如果將上述區(qū)間的定義推廣至Rn維實數空間，則n維區(qū)間向量，或稱n維箱粒子［X］定義了n個一維閉合區(qū)間的笛卡兒積。

圖1展示了一個二維實數域中的箱粒子，其基本數學定義如表1所示。

圖1 二維空間中的箱粒子及其數學定義Fig.1 Box particle and its mathematical definition in two-dimensional space

表1 二維箱粒子的基本數學定義Table 1 Basic mathematical definition of two-dimensional box particle

一般情況下，Rn空間中的箱粒子［X］經過非線性函數f映射后所得到的f（［X］）為非規(guī)則的箱體形狀。為了便于計算和分析，使得［X］在非線性函數的映射作用后得到規(guī)則的箱體形狀，區(qū)間分析定義了包含函數（inclusion functions），即已知函數f：Rn→Rm，則其對應的區(qū)間函數［f］：IRn→IRm是包含函數，如果滿足：

式中：IRn為n維實區(qū)間的集合。

對于各種類型的函數f，區(qū)間分析的一個主要目的就是提供合理有效的包含函數［f］，使得［f］（［X］）在包含所有可能解的同時其區(qū)間所含范圍不太大，這樣就可以很大程度上減少算法的計算量，提高收斂速度。為此，區(qū)間分析引入了區(qū)間約束（interval constraint）的概念，即利用某種特定算法對原區(qū)間進行約束，也就是解決約束滿足問題（constraint satisfaction problem，CSP），其定義為：在n維實數域上，實值約束函數g（x）=g（x1，…，xn）=0的約束集為

式中：g（x）=（g1（x），g2（x），…，gm（x））T，x=（x1，x2，…，xn）。則CSP的解集為

在式（4）中，H的本質就是求解出一個包含［X］中所有x，并滿足約束函數g（x）的具有最小冗余的箱粒子［X］′，進而利用［X］′代替［X］，即S?［X］′?［X］。在BPF中，箱粒子的區(qū)間收縮一般采用前向后向傳播算法，也稱約束傳播（constraints propagation，CP）算法或CP收縮子，該算法的特點是實現起來簡單，而且在高冗余數據和約束情況下，具有較高的執(zhí)行效率［13］。

2 智能優(yōu)化箱粒子濾波

2.1 箱粒子濾波算法

BPF充分結合了序列蒙特卡羅（sequential Monte Carlo，SMC）方法和IA技術，其核心思想是：利用在狀態(tài)空間中隨機采樣的具有一定體積的箱粒子代替常規(guī)的點粒子，再利用系統(tǒng)誤差界限模型代替系統(tǒng)誤差統(tǒng)計模型對加權箱粒子集進行序貫迭代。在區(qū)間分析的框架中，k時刻系統(tǒng)狀態(tài)向量xk和觀測向量zk分別表示為區(qū)間狀態(tài)向量［Xk］∈IRnx和區(qū)間觀測向量［Zk］∈IRny。同理，狀態(tài)轉移函數f和觀測函數h分別表示為包含函數［f］和［h］，則系統(tǒng)狀態(tài)方程為

式中：［Vk］和［Wk+1］分別為狀態(tài)轉移噪聲和觀測噪聲箱粒子；［Uk］為控制輸入箱粒子。

Gning等［19］在貝葉斯濾波框架下將BPF視為采用混合均勻概率密度函數（probability density function，PDF）的濾波算法，即將每個箱粒子內部都擬合為一個以該箱粒子為支撐集的均勻PDF，每個PDF很好地描述了對應箱粒子的狀態(tài)特性。因此，若令U［X］表示以箱粒子［X］為支撐集的PDF，則隨機變量x可表示為一組均勻PDF的加權和。

式中：N為混合均勻PDF的數目；［Xi］為第i個箱粒子；=1，?i，ωi≥0。

基于上述分析，根據貝葉斯濾波估計原理，BPF的迭代步驟如下：

1）預測。在k時刻，假設狀態(tài)xk的PDF可表示為

則時間更新步驟如下：

3）重采樣。針對BPF中的箱粒子退化現象，通常采取計算有效樣本Neff=的大小，并選擇一個閾值Nth，如果Neff＜Nth，則執(zhí)行隨機子劃分重采樣，即根據需要重采樣的次數，將當前時刻得到的箱粒子隨機地選取其某一維狀態(tài)子區(qū)間進行均勻的劃分，使得箱粒子能夠保持一個合適的尺寸。該方法不但保證了當前時刻的箱粒子集能夠順利進入下一時刻的迭代，而且也有效去除了箱粒子內部的冗余區(qū)域。然而，該方法對箱粒子的子區(qū)間進行選取是隨機的，這會導致每次實驗的結果都不同，因此反復執(zhí)行隨機子劃分重采樣對濾波精度影響較大。

2.2 螢火蟲算法智能優(yōu)化箱粒子濾波

盡管BPF在許多實際的應用中被證明是有效的，但對于高非線性系統(tǒng)和觀測誤差較大的情況下，其濾波精度不高［20］。作為一種新的非線性濾波算法，BPF的發(fā)展時間較短，理論體系還需進一步完善，算法的性能還有進一步提高的空間，其應用方面則主要集中在目標跟蹤領域。針對箱粒子的退化現象，考慮結合FA的尋優(yōu)機制對箱粒子集的空間分布進行智能優(yōu)化，以改善BPF的濾波性能。

FA［16］是受自然界中的螢火蟲通過熒光進行信息交流的群體行為啟發(fā)而演變得來的。螢火蟲的熒光亮度和螢火蟲間的吸引度是FA的2個主要因素。其中，熒光亮度用于表示螢火蟲所處當前位置的優(yōu)劣程度，并決定了其下一時刻的運動方向；吸引度表示螢火蟲之間的相互作用，并決定了螢火蟲移動的距離。FA就是通過對熒光亮度和吸引度的不斷迭代，使得螢火蟲群體智能地向狀態(tài)空間內更好的區(qū)域運動，進而實現目標的優(yōu)化。

FA作為一種新型仿生群智能優(yōu)化算法，其最大的優(yōu)勢在于在工程上容易實現，需要調整的參數少，具有較好的收斂速度和收斂精度。文獻［21］將FA的優(yōu)化機制應用于PF，提高了PF的估計精度，降低了狀態(tài)估計所需的粒子數。因此，本文考慮結合BPF的運行機制和FA的尋優(yōu)機制，根據最新觀測利用區(qū)間分析定義箱粒子的熒光亮度計算公式及空間位置更新公式，進而模擬螢火蟲進行覓食或交流的個體行為，使得箱粒子智能地向全局范圍內更優(yōu)的位置移動，這樣就可以提高箱粒子集的整體質量，克服箱粒子的退化問題，避免了反復執(zhí)行隨機子劃分重采樣。

1）箱粒子的熒光亮度更新公式。在標準FA中，當前時刻的每個螢火蟲都需要和其他位置的螢火蟲對比熒光亮度值大小，螢光亮度值低的螢火蟲會被螢光亮度值高的螢火蟲吸引，并朝著螢光亮度值高的螢火蟲所處的空間位置移動。因此，螢火蟲的相對熒光亮度更新公式定義為

式中：Im和γ分別為螢火蟲的最大螢光亮度和光強吸收系數；dij為螢火蟲i與j的空間歐氏距離。

如果直接將式（20）的更新機制應用于BPF，則將會進一步增加算法的運算復雜度。為了確保濾波精度，螢火蟲在自適應迭代尋優(yōu)過程中需要考慮最新的觀測。而在區(qū)間分析的框架下，其關鍵思想就是求出當前時刻的預測觀測和實際觀測在多維狀態(tài)空間中的“交”。因此，構造箱粒子熒光亮度的計算公式為

根據式（21），由于每個時刻的實際觀測值僅有一個，利用實際觀測值和每個箱粒子的預測觀測值進行對比，代替每個箱粒子之間熒光亮度值的對比，就可有效減小算法的運算復雜度。

2）箱粒子的吸引度計算公式。在標準FA中，螢火蟲的吸引度公式為

式中：βm為最大吸引度，一般設置為［0.8，1］內的常數。

如果螢火蟲的吸引度越大，則其越容易吸引附近的螢火蟲向自身所處的位置運動，這樣一來，就會使得所有箱粒子都集中在狀態(tài)的真實值附近，造成箱粒子的多樣性喪失。基于上述分析，根據區(qū)間分析中2個中心區(qū)間距離的定義，給出k+1時刻箱粒子與全局最優(yōu)箱粒子［gk+1］之間的空間歐氏距離為

式中：·2為2-范數。

在箱粒子熒光亮度更新公式基礎上，進一步考慮在吸引度公式中加入一個隨機權值項，這樣就能改變箱粒子的移動信息，既能讓箱粒子集在高似然區(qū)域合理的分布，又可以在一定程度上增強箱粒子的多樣性，確保了濾波過程的持續(xù)運行。因此，構造箱粒子間的吸引度為

式中：N（0，1）為均值為0、方差為1的高斯分布隨機向量；（1-）·N（0，1）為隨機權值項。

當完成位置更新之后，需要計算并對比箱粒子的熒光亮度值，更新全局最優(yōu)箱粒子：

3）箱粒子的位置更新公式。在標準FA中，螢火蟲i被螢火蟲j吸引后移動的位置更新公式為

式中：xi和xj分別為螢火蟲i和j的空間位置；rand表示某個隨機數，且服從均勻分布U（0，1）；μ∈［0，1］為步長因子。

如果將式（26）的更新策略直接應用于BPF，則需要用箱粒子i和其余的箱粒子j進行交互運算，這將對算法的實時性帶來不利影響。為此，考慮利用每個箱粒子與全局最優(yōu)箱粒子間的對比來替代箱粒子間的相互對比，利用每個箱粒子中心位置的更新使每個箱粒子的位置更新，這會提高FA的全局尋優(yōu)能力，且該階段的運算復雜度將由O（N2）減少至O（N）?；谏鲜龇治?，構造箱粒子的位置更新公式為

由式（27）可知，箱粒子的隨機步長隨di變化，且2di（rand-1/2）∈［-di，di］。采用上述位置更新策略后，當箱粒子間的空間位置相距較遠時，箱粒子位置更新過程中吸引度所發(fā)揮的引導作用相對較弱，而箱粒子自身可以在區(qū)間［-di，di］內隨機自主的運動，這樣就使得算法能夠搜索較大的狀態(tài)空間；當箱粒子之間的空間歐氏距離較近時，箱粒子在狀態(tài)空間中的自主探索能力和移動范圍隨著空間歐氏距離di的減小而減弱，而此時箱粒子在位置更新過程中，吸引度起到的主導作用將逐漸增大，進而智能地引導箱粒子朝著全局最優(yōu)的箱粒子所在位置移動。

通過以上優(yōu)化策略，利用FA智能優(yōu)化引導箱粒子集朝著高似然區(qū)域移動，可以有效克服箱粒子的退化現象，并且沒有增加額外的操作和參數，保持了算法的簡單性。另外，考慮到FA本身具有較強的收斂能力，如果迭代次數過多，則會造成算法的計算復雜度增高，而且利用FA智能優(yōu)化的目的是為了使得箱粒子集朝著似然度高的區(qū)域移動，不需要收斂到最優(yōu)值，否則會造成箱粒子的多樣性喪失。因此，通過設定最大迭代次數，保證算法的實時性和箱粒子多樣性，即當熒光亮度函數值大于設置的迭代終止閾值時，算法停止迭代，否則繼續(xù)執(zhí)行迭代，直至到達最大迭代次數。當算法迭代至設定閾值時，表明箱粒子已經較好地分布在狀態(tài)的真實值周圍。

3 移動機器人FastSLAM 方案設計

FastSLAM 算法的核心在于運用RBPF思想對SLAM問題后驗概率進行分解，其實現了地圖規(guī)模和狀態(tài)估計之間的有效解耦。具體說來，就是將SLAM問題分解成為機器人路徑x1：k估計和基于該路徑估計的地圖M 創(chuàng)建2個子問題，即

式中：z1：k=｛z1，z2，…，zk｝和u1：k=｛u1，u2，…，uk｝分別為觀測序列和控制輸入序列；mi為第i個環(huán)境路標的位置估計；n1：k=｛n1，n2，…，nk｝為數據關聯的相關一致性。

根據SLAM 的動態(tài)貝葉斯圖模型，當已知機器人的運動軌跡x1：k時，對其周圍環(huán)境地圖M 中存在的N個特征進行估計是相互獨立的。因此，FastSLAM采用PF估算機器人所有可能的軌跡后驗，其中的每個粒子都包含一份機器人完整的潛在地圖。然而，為了提高估計精度，就需要增加描述后驗概率分布所需的粒子數，這對于FastSLAM算法來說代價巨大，會增加算法的復雜度。此外，反復的重采樣會造成粒子有效性和多樣性的損失，導致出現粒子退化現象，使得算法的性能下降，甚至失效；另一方面，在所得路徑估計基礎上，利用常規(guī)EKF對地圖特征估計存在計算量過大、精度不高甚至發(fā)散等問題。

因此，考慮將改進的智能優(yōu)化箱粒子濾波（IOBPF）用于實現移動機器人位姿估計，并在此基礎上利用EIKF完成地圖構建與更新，為了表示方便，簡記為IOBPF-SLAM 算法。其中，k時刻第i個箱粒子的結構形式如下：

3.1 移動機器人同步定位

在區(qū)間分析框架下，移動機器人系統(tǒng)的運動模型表示為

式中：k時刻的輸入區(qū)間向量［Uk］由機器人的基本位移［Δdk］和基本旋轉角［Δθk］組成，即［Uk］=（［Δdk］，［Δθk］）T，（［x］×［y］）T為機器人在全局坐標系下的位置，［θ］為機器人運動方向。

機器人系統(tǒng)的距離-角度觀測模型為

式中：觀測區(qū)間向量［Zk］由距離觀測區(qū)間量［rk］和角度觀測區(qū)間量［φk］組成，即［Zk］=（［rk］，［φk］）T。

因此，k+1時刻的狀態(tài)估計值為

在具體應用中，可將權值最大的箱粒子的中心近似為狀態(tài)估計值。另外，針對估計值，其對應的狀態(tài)估計協方差為

3.2 環(huán)境地圖構建

Chen等［17］將區(qū)間分析引入卡爾曼濾波，將噪聲模型和系統(tǒng)模型的誤差用區(qū)間范圍來表示，提出了一種新的區(qū)間卡爾曼濾波（interval Kalman filter，IKF）算法，其迭代公式與標準卡爾曼濾波的結構形式完全相同，只是運算變量為區(qū)間向量或區(qū)間矩陣，估計結果為2條邊界曲線，最終結果將2個邊界值取平均或加權處理獲得。由于區(qū)間運算保證了結果的完備性，在一些實際應用中［22-23］，IKF相比于卡爾曼濾波展示出了更好的魯棒性和精度。

對于區(qū)間非線性系統(tǒng)，結合IKF和EKF便可進一步得到EIKF的迭代公式。為了下文表示方便，利用上標“I”表示區(qū)間量，即［Xk］表示為，狀態(tài)轉移包含函數［f］和觀測包含函數［h］分別表示為fI和hI，為狀態(tài)估計的區(qū)間協方差矩陣，和分別為系統(tǒng)噪聲區(qū)間協方差矩陣和觀測噪聲區(qū)間協方差矩陣，則EIKF算法的遞歸過程如下。

1）狀態(tài)初始值。

2）計算區(qū)間雅可比矩陣。

4）計算新息和區(qū)間卡爾曼增益。

5）更新。

常規(guī)FastSLAM 算法利用EKF更新地圖，但需要對非線性的模型進行線性化。由于機器人系統(tǒng)具有強非線性，線性化只能精確到泰勒級數的一階，造成估計精度下降甚至導致濾波發(fā)散。因此，本文利用EIKF取代EKF完成環(huán)境特征估計。根據FastSLAM 的解算過程，如果機器人的運動軌跡已知，則對各個特征進行估計彼此是相互獨立的，而k+1時刻是否對第j個環(huán)境特征進行更新取決于k時刻第j個特征是否被傳感器觀測到。從而，根據傳感器獲得的最新觀測完成數據關聯后，需考慮以下情況：

3）如果機器人最新的觀測沒有包含環(huán)境地圖中的第j個路標，則其均值和協方差不變。

4）如果機器人再也沒有觀測到周圍環(huán)境的任何特征，則將前一時刻的地圖信息作為當前時刻的地圖信息。

在上述EIKF的遞推式中，增益矩陣的計算涉及區(qū)間矩陣的求逆，而這類求逆問題往往采用迭代的方式求解，算法比較復雜，且耗時長，不利于實時性。因此，為了減少計算量，進一步提高算法的實時性，本文采用結合Hansen算法［25］和上邊界矩陣的區(qū)間矩陣求逆方法，具體見算法1。

算法1區(qū)間矩陣的求逆算法。

綜合上述，IOBPF-SLAM 算法的整體結構如圖2所示。

圖2 IOBPF-SLAM算法的整體結構Fig.2 Overall structure of IOBPF-SLAM algorithm

4 仿真實驗與結果分析

4.1 仿真實驗對比分析

為了驗證本文算法的性能，根據Bailey［26］開發(fā)的SLAM算法通用模擬器設計了尺寸為180 m×80 m，并具有60個路標特征的環(huán)境地圖及機器人運行參考 軌 跡，分 別 對 FastSLAM 2.0、UFastSLAM［27］、STSRCDKF-SLAM［28］、基于標準BPF算法的BPFSLAM及IOBPF-SLAM開展一系列仿真實驗并對相關性能進行對比分析。仿真實驗在MATLAB 2014a環(huán)境下運行，區(qū)間運算利用INTLAB 9.0工具箱。計算機的配置型號為：CPU Intel? CoreTMi5-5200U，主頻2.20 GHz，內存8 GB。

仿真中，Np和Nb分別表示粒子數和箱粒子數。對于FA群智能優(yōu)化步驟，光強吸收系數γ=1，最大吸引度βm=0.85，迭代終止閾值設置為0.02。采用獨立兼容最近鄰（individual compatibility nearest neighbor，ICNN）算法完成地圖構建過程中的數據關聯。機器人的初始位置設定為全局坐標系的（0，0）點，并以1.5 m/s的速度運行，其余仿真參數設置如表2所示。根據區(qū)間觀測的不確定性，傳感器觀測表示為

表2 仿真參數設置Table 2 Simulation parameter setting

式中：區(qū)間長度Δ=［2.5，2.5］T。

圖3展示了5種SLAM算法在模擬環(huán)境中的估計結果?？梢钥闯?，UFastSLAM 利用無跡粒子濾波（unscented particle filter，UPF）計算機器人狀態(tài)的不確定性，獲得了更加精確的方差，從而改善了估計精度。但隨著機器人的持續(xù)運動，新的環(huán)境特征將不斷加入狀態(tài)向量中，由此引起尺度無跡變換（scaled unscented transformation，SUT）的維數逐漸增加，導致機器人在運動一段距離后，其軌跡估計便開始出現了明顯的誤差。而STSRCDKF-SLAM利用強跟蹤平方根容積卡爾曼濾波，一方面融合了平方根策略，使得其協方差矩陣具備對稱性，有效減小了由于系統(tǒng)發(fā)生突變導致濾波發(fā)散而帶來的誤差；另一方面，在濾波增益矩陣中加入漸消因子對其進行實時調整，動態(tài)自適應地調節(jié)數據的權值，進而改善了濾波精度。BPF-SLAM 由于采用IA技術，相比于FastSLAM 2.0在一定程度上提高了估計精度，但由于常規(guī)BPF面對箱粒子退化現象需要執(zhí)行反復地進行隨機子劃分重采樣，也出現了一定的誤差，而IOBPF-SLAM估計路徑與參考軌跡吻合度更高，相對應的路標估計也更為準確。

圖3 模擬環(huán)境下的不同SLAM算法運行結果比較Fig.3 Comparison of running results among different SLAM algorithms in simulation environment

為了驗證算法在增長觀測噪聲條件下的誤差性能，設置環(huán)境特征的角度觀測噪聲為2°，距離觀測噪聲分別為0.1，0.3，0.4，0.55，0.7，0.8，1.0，1.2，1.35，1.5 m進行仿真實驗。圖4展示了上述5種SLAM 算法在開展了30次仿真實驗后的均方根誤差（RMSE）均值及標準差的比較情況?？梢钥闯?，5種SLAM 算法的軌跡估計誤差及標準差隨著觀測噪聲的增長逐漸增加。然而，IOBPF-SLAM算法由于采用FA智能優(yōu)化，提高了箱粒子集的整體估計效能，并提供了全局一致的結果，因此其RMSE均值和標準差增長速度要低于其他算法。這意味著IOBPF-SLAM 的估計精度相較于其他幾種算法更好，并具有較好的運行穩(wěn)定性。

圖4 不同SLAM算法的RMSE比較Fig.4 Comparison of RMSE among different SLAM algorithms

為了在上述增長觀測噪聲條件下驗證IOBPF-SLAM能有效避免粒子退化，對于N次仿真實驗，利用有效粒子百分比進行評估。

在開展了30次仿真實驗后，從圖5可以看出，IOBPF-SLAM的有效箱粒子百分比均高于82%，這意味著在算法執(zhí)行過程中，最多僅有18%的箱粒子沒有發(fā)揮作用，然而這部分箱粒子主要是為了保持箱粒子的多樣性，這說明了FA智能優(yōu)化的有效性。

圖5 不同SLAM算法的有效粒子數百分比Fig.5 Percentage of effective particle number for different SLAM algorithms

Gning等［19］指出，如果BPF的濾波結果為狀態(tài)的最優(yōu)估計，則需要滿足：①真實狀態(tài)向量x必須包含于后驗PDF的所有支撐集區(qū)域內；②狀態(tài)后驗PDF的支撐集區(qū)域體積應該為最小。針對條件①，Gning等［15］進一步指出了包含準則ρk，并說明了當不滿足此條件時，該濾波器不收斂。為了解釋ρk，需要利用置信集的概念。在BPF中，一般采用所有支撐集箱粒子的“并集”來表示置信集Ck（1），即

因此，包含準則ρk可由如下計算得到：

如果滿足ρk=1，則表明在所有以［］為支撐集的后驗PDF內已經包含了狀態(tài)的真實值。從圖6可以看出，相比于標準的BPF，IOBPF運用FA優(yōu)化機制引導箱粒子集智能地向高似然區(qū)域移動，使該箱粒子集更好地滿足了ρk=1，因此提高了濾波性能。

圖6 不同箱粒子的包含值Fig.6 Inclusion values of different box particles

對于一個濾波算法而言，如果其估計誤差滿足：①均值等于零；②小于或等于其計算所得的協方差，則稱該濾波算法是一致的。如果一個濾波算法不一致，則其濾波精度就具有不確定性，這會導致算法估計結果不可靠。Bailey等［11］分析并闡述了粒子退化現象是造成FastSLAM 算法不一致的主要原因，并進一步提出了利用歸一化估計方差（normalized estimation error squared，NEES）準則來驗證算法是否滿足一致性。具體而言，假設為k時刻機器人的真實狀態(tài)，且該狀態(tài)的維數為d，并假設為其估計值和方差，則NEES計算如下：

NEES本質上描述的是一種加權距離，一般需要開展N 次蒙特卡羅實驗，然后采用平均NEES進行檢驗，即

式中：給定顯著性水平α=0.05，并將雙邊概率為95%的區(qū)域作為一致性區(qū)域時的界定區(qū)間為［2.19，3.93］。

因此，為了評估上述5種SLAM 算法的一致性，分別開展30次仿真實驗，結果如圖7所示，其中紅色的實線和紅色的虛線分別表示一致性測試的下限和上限?？梢钥闯?，UFastSLAM 采用無跡變換，避免了由于線性化帶來的誤差，其一致性優(yōu)于FastSLAM 2.0；而STSRCDKF-SLAM 改進了提議分布，并采用自適應重采樣有效保持了粒子的多樣性，因此其一致性保持更為持久。相比起來，IOBPF-SLAM在區(qū)間分析基礎上結合FA的尋優(yōu)機制，構造了箱粒子集的智能優(yōu)化策略，因此提供了有界的、更為一致性的結果。

圖7 不同SLAM算法的一致性比較Fig.7 Comparison of consistency among different SLAM algorithms

通過對不同SLAM 算法的計算機CPU 運行時間來對比不同粒子數條件下的SLAM算法計算復雜度。對于每組設定的粒子數，分別開展30次仿真實驗，其平均計算復雜度如圖8所示?？梢钥闯?，上述5種SLAM 算法的時間復雜度隨著粒子數的增長逐步增加，FastSLAM 2.0的運算復雜度與粒子數呈近似線性關系。在使用相同粒子數的條件下，由于涉及區(qū)間運算和群智能優(yōu)化過程，提出算法的復雜度要高于其他算法，但由上述圖3的運行結果可知，IOBPF-SLAM 利用10個箱粒子所獲得的估計精度就已經超過前3種基于PF的SLAM 算法。因此，為獲得較高的估計精度，IOBPF-SLAM的計算代價相對最低，算法執(zhí)行所需時間更短。

4.2 移動機器人測試結果與性能評價

為測試IOBPF-SLAM 的實際應用效果，利用裝備Raspberrypi-3b控制主板和SLAMTEC激光雷達的差分驅動輪式移動機器人（RPLIDAR-A1）分別針對Grid SLAM［8］、GMapping［9］、BPF-SLAM和IOBPF-SLAM開展對比實驗。實驗利用一臺上位機筆記本電腦在Ubuntu14.04下安裝機器人操作系統(tǒng)（ROS），利用teleop_twist_keyboard軟件包控制機器人運動，并結合占用概率地圖構建方法［29］，采用柵格地圖來建立環(huán)境模型，通過加載圖形可視化工具Rviz顯示地圖的構建過程。實驗場景如圖9所示。

圖9 差分驅動輪式移動機器人及其實驗場景Fig.9 Differential drive wheeled mobile robot and experimental scene

實驗中，移動機器人運動速度為0.4 m/s，最大轉向速度為0.3 rad/s，控制頻率為40 Hz，激光雷達最大測距范圍為6 m，并具有360°全方位掃描能力。環(huán)境中設置了30個路標點及2個紙箱作為障礙物，生成柵格地圖分辨率為25 cm2/cell，粒子數目設置為Np=10，Nb=10。由圖10的地圖構建過程可以看出，當采用10個粒子時，Grid-SLAM無法正常完成地圖構建，而其余3種算法均能完成二維柵格地圖構建。

圖10 地圖構建過程Fig.10 Map building process

為了定量分析實驗結果，表3比較了在正確構建出環(huán)境地圖條件下4種算法的性能參數?？梢钥闯?，Grid SLAM需要100個粒子才能正確完成地圖構建，而GMapping在RBPF的基礎上改進了采樣提議分布并采用選擇性重采樣策略，有效減少了所需粒子數，同時防止了粒子退化現象，是目前最有效的柵格地圖構建方法，但仍然需要30個粒子才能完成地圖構建。相比起來，BPF-SLAM和IOBPF-SLAM使用更少的粒子數就能完成地圖構建，從而提高了SLAM系統(tǒng)的實時性。

表3 不同SLAM 算法的性能比較Table 3 Comparison of per formance among different SLAM algorithms

另外，2種基于區(qū)間箱粒子的SLAM 算法主要的時間消耗在于區(qū)間運算，且每個箱粒子都存儲并保持自己的柵格地圖，對每個箱粒子進行FA智能優(yōu)化就意味著涉及整個地圖的處理，因此，IOBPF-SLAM的運行時間相對于BPF-SLAM 要長一些，但其所需粒子數更少，精度更好。圖11進一步展示了IOBPF-SLAM的軌跡估計結果和設定路標的估計結果。移動機器人從“起點”位置（0，0）開始沿著黑色參考路徑以藍色箭頭方向順時針運動一個回環(huán)，基于改進的FA智能優(yōu)化箱粒子所估計得到的運動軌跡能正確跟隨由參考點定義的軌跡，并且所有的參考點都包含在定位箱粒子中，因此提供了一致的結果。

圖12比較了實驗中不同SLAM 算法對于設定路標的位置估計誤差。可以看出，移動機器人的整個運動過程中，在改進的智能優(yōu)化BPFSLAM估計所得的潛在路徑基礎上，IOBPF-SLAM采用EIKF對環(huán)境特征進行估計，雖然有少數幾個路標點的位置估計誤差大于GMapping，但IOBPFSLAM對整個環(huán)境中設定路標的估計精度更高。

圖12 不同SLAM算法的路標位置估計誤差比較Fig.12 Comparison of estimation errors of landmark position among different SLAM algorithms

圖13的地圖構建結果進一步展示了IOBPFSLAM生成的二維柵格地圖與真實環(huán)境一致，其邊緣清晰，沒有重疊或扭曲，并且在整個過程中程序運行平穩(wěn)，因此是可行有效的，可為工程應用提供參考依據。

圖13 IOBPF-SLAM生成的二維柵格地圖Fig.13 Two-dimensional grid map generated by IOBPF-SLAM

5 結論

1）本文算法將區(qū)間分析引入移動機器人FastSLAM算法架構中，利用BPF取代傳統(tǒng)PF，并通過利用FA的動態(tài)尋優(yōu)機制智能地引導箱粒子集朝著移動機器人的真實軌跡靠近，該策略沒有增加額外的操作和參數，保持了算法的簡單性。

2）IOBPF-SLAM 可有效解決傳統(tǒng)FastSLAM算法需要大量粒子構建地圖的問題，同時克服了箱粒子的退化，避免了反復執(zhí)行隨機子劃分重采樣，因此在粒子數較少的情況下，能夠獲得良好的定位精度和地圖構建精度，且計算量適中，滿足實時性的要求。

3）仿真和實驗結果表明，IOBPF-SLAM 相較于其他幾種SLAM 算法，為移動機器人的同步定位與位姿估計提供了更為精確、可靠的結果。此外，通過對設定路標的位置估計發(fā)現，EIKF在計算量方面與傳統(tǒng)EKF算法相當，但其濾波效果和魯棒性相對于EKF有明顯的優(yōu)勢。

考慮到BPF具有的性能優(yōu)勢，在后續(xù)工作中，將在區(qū)間分析基礎上進一步結合其他智能優(yōu)化與學習進化方法改善BPF的性能，并將其應用于復雜環(huán)境或其他類型傳感器的移動機器人SLAM問題。