FastSLAM算法的仿生優(yōu)化改進(jìn)研究

張鎧翔，姜文剛

(江蘇科技大學(xué) 電子信息學(xué)院，江蘇 鎮(zhèn)江 212000)

0 引 言

同時(shí)定位與地圖創(chuàng)建(simultaneous localization and mapping，SLAM)是探索未知環(huán)境的關(guān)鍵因素之一，尤其是在機(jī)器人自主導(dǎo)航領(lǐng)域更是扮演著至關(guān)重要的角色。SLAM的過(guò)程就是移動(dòng)機(jī)器人在未知環(huán)境中進(jìn)行探索，在不斷移動(dòng)的過(guò)程中通過(guò)自身攜帶的傳感器獲得的控制信息與觀測(cè)信息進(jìn)行環(huán)境地圖的增量式創(chuàng)建，并在建造的地圖中確定自身的位置。其中的控制信息和觀測(cè)信息都會(huì)存在不確定的噪聲，所以可以看出SLAM的本質(zhì)就是概率估計(jì)的問(wèn)題[1]。

一般由概率方法來(lái)解決SLAM問(wèn)題的經(jīng)典算法分為基于擴(kuò)展卡爾曼濾波SLAM(EKF-SLAM)算法和快速SLAM(FastSLAM)算法[2-4]。但在實(shí)際的應(yīng)用環(huán)境中，移動(dòng)機(jī)器人的運(yùn)動(dòng)模型與觀測(cè)模型一般是非線性的，EKF算法主要是通過(guò)一階泰勒展開式來(lái)近似非線性模型，所以避免不了引入線性化誤差[5]，這便導(dǎo)致該算法無(wú)法在大范圍的環(huán)境中運(yùn)行。Murphy[5]通過(guò)對(duì)全狀態(tài)濾波器分解，提出了一種基于Rao—Blackwellise粒子濾波器(RBPF)的新方法來(lái)解決SLAM問(wèn)題。隨后Montemerlo等[6]提出的FastSLAM算法，對(duì)解決SLAM問(wèn)題起到了很大的推動(dòng)作用。

FastSLAM主要的思想是將機(jī)器人的運(yùn)動(dòng)狀態(tài)與建立地圖分化成對(duì)機(jī)器人的運(yùn)動(dòng)估計(jì)以及基于位姿的路標(biāo)估計(jì)兩個(gè)主要的過(guò)程。其中在機(jī)器人運(yùn)動(dòng)估計(jì)中使用的則是粒子濾波進(jìn)行機(jī)器人位姿定位，地圖上的每個(gè)路標(biāo)估計(jì)則是使用的EKF估計(jì)[1]。相比一些傳統(tǒng)的SLAM算法，F(xiàn)astSLAM算法的計(jì)算量大大降低，但在使用粒子濾波進(jìn)行運(yùn)動(dòng)狀態(tài)估計(jì)的過(guò)程中很容易出現(xiàn)粒子退化這一類問(wèn)題[7]，此類問(wèn)題一直是眾多學(xué)者研究的熱點(diǎn)，也涌現(xiàn)了大量的較為優(yōu)秀的改進(jìn)策略。目前在算法改進(jìn)策略上有一大部分的研究者通過(guò)融入仿生算法對(duì)粒子濾波進(jìn)行優(yōu)化，因其具有很強(qiáng)的尋優(yōu)性能，能夠快速、智能地將粒子集中至高似然區(qū)域，有效地改善了樣本貧化和粒子多樣性喪失現(xiàn)象[8-9]，進(jìn)而提高了濾波精度。像基于果蠅算法優(yōu)化的粒子濾波算法[8]、基于蝙蝠算法的粒子濾波算法[9]等都是通過(guò)結(jié)合仿生算法進(jìn)行粒子優(yōu)化，其過(guò)程是初始化每個(gè)粒子最初時(shí)刻位置，并通過(guò)不斷迭代發(fā)現(xiàn)局部最優(yōu)值以及全局最優(yōu)值，進(jìn)而不斷更新位置向粒子群中最優(yōu)解的位置靠近。但這些結(jié)合仿生算法優(yōu)化后的粒子一定程度上存在盲目搜索，導(dǎo)致出現(xiàn)尋優(yōu)結(jié)果出現(xiàn)較多偏差、容易陷入早熟或收斂速度較慢的問(wèn)題。

針對(duì)FastSLAM算法存在的缺陷，融合蝴蝶算法進(jìn)行優(yōu)化，主要的改進(jìn)策略是將蝴蝶種群覓食的思想代入到FastSLAM的運(yùn)動(dòng)狀態(tài)估計(jì)中，傳統(tǒng)的蝴蝶算法中并沒(méi)有使用當(dāng)前時(shí)刻的測(cè)量值，從而使得算法結(jié)果存在較大的偏差。針對(duì)此問(wèn)題，該文引入機(jī)器人運(yùn)動(dòng)過(guò)程中最新時(shí)刻的觀測(cè)值和狀態(tài)值，另外傳統(tǒng)的蝴蝶算法在局部搜索飛行過(guò)程中，通過(guò)個(gè)體的香味進(jìn)行互相吸引，但需要整個(gè)群體進(jìn)行交互運(yùn)算，這便產(chǎn)生較大計(jì)算量，導(dǎo)致較差的實(shí)時(shí)性。因此引入自適應(yīng)香味半徑和自適應(yīng)調(diào)整飛行步長(zhǎng)因子[10]，同時(shí)在重采樣的過(guò)程中引入偏差修正指數(shù)加權(quán)平均(exponentially weighted average，EWA)算法[11]，分離出對(duì)估計(jì)精度影響較大的大權(quán)值粒子，對(duì)剩余部分的中小權(quán)值粒子進(jìn)行分布采樣，以提高粒子篩選的準(zhǔn)確性，自適應(yīng)調(diào)整重采樣粒子數(shù)，增加粒子的多樣性，減緩粒子退化。

1 機(jī)器人傳統(tǒng)的FastSLAM算法

1.1 FastSLAM的基本原理

p(X1:k,m|Z1:k,u1:k,n1:k)=

(1)

其中，X1:k為機(jī)器人的路徑估計(jì)；m為路標(biāo)位置的集合；Z1:k為機(jī)器人的觀測(cè)量；u1:k為機(jī)器人的控制輸入量；n1:k為數(shù)據(jù)關(guān)聯(lián)表，每個(gè)nk為k時(shí)刻觀測(cè)點(diǎn)到的路標(biāo)點(diǎn)。

1.2 FastSLAM算法的實(shí)現(xiàn)

FastSLAM算法基本上由五步構(gòu)成：機(jī)器人狀態(tài)預(yù)測(cè)、數(shù)據(jù)關(guān)聯(lián)、重要性采樣、地圖更新、重采樣。具體步驟如下：

(1)機(jī)器人的下一時(shí)刻的位置預(yù)測(cè)。基于每一時(shí)刻輸入的控制信息和機(jī)器人運(yùn)動(dòng)模型進(jìn)行預(yù)測(cè)。

(2)數(shù)據(jù)關(guān)聯(lián)。將觀測(cè)信息和各個(gè)粒子t時(shí)刻的路標(biāo)特征依次進(jìn)行數(shù)據(jù)關(guān)聯(lián)。

(3)重要性采樣。重要性權(quán)值可以根據(jù)下面的公式遞歸計(jì)算求得。

(2)

粒子的歸一化權(quán)值為：

(3)

(4)地圖更新。采用EKF算法關(guān)聯(lián)觀測(cè)信息更新每個(gè)特征的估計(jì)。

(5)重采樣。設(shè)置閾值并計(jì)算有效粒子數(shù)Neff。若Neff小于閾值則進(jìn)行重采樣。

重采樣過(guò)程能夠?qū)⒎稚顟B(tài)空間各處粒子較快地聚合到某些區(qū)域，但這樣可能存在的問(wèn)題是粒子過(guò)早地收斂陷入局部最優(yōu)解導(dǎo)致機(jī)器人運(yùn)動(dòng)狀態(tài)偏離真實(shí)狀態(tài)概率分布，從而使得粒子的估計(jì)狀態(tài)存在較大偏差。

因此，將改進(jìn)后的蝴蝶算法融合到FastSLAM算法上可以讓其在處理重要性采樣的時(shí)候，能夠充分考慮粒子間的作用關(guān)系，得到更加準(zhǔn)確的粒子均值與方差，這樣可以很好地保證整個(gè)系統(tǒng)在運(yùn)行過(guò)程的效率，大大地提高算法性能，在重采樣過(guò)程中，對(duì)自適應(yīng)重采樣進(jìn)行改進(jìn)，提高粒子多樣性，獲得更加準(zhǔn)確的定位與地圖。

2 基于改進(jìn)的蝴蝶算法優(yōu)化的FastSLAM算法

2.1 蝴蝶算法

蝴蝶算法(butterfly algorithm，BA)[13]是一種基于蝴蝶覓食行為的優(yōu)化算法。在收斂精度與速度上都要高于其他算法。其思想是在每個(gè)蝴蝶搜尋食物時(shí)，自身會(huì)發(fā)出具有一定濃度的香味，并可以通過(guò)香味的濃度吸引周圍的其他蝴蝶，其他蝴蝶則會(huì)朝著香味較多的個(gè)體進(jìn)行移動(dòng)[14]。影響蝴蝶的香味最主要的因素是感知形態(tài)、刺激強(qiáng)度以及冪指數(shù)。用方程表示為：

F=cIa

(4)

其中，F(xiàn)是香味濃度，c是感知形式，I是刺激強(qiáng)度，a是冪指數(shù)。

已知目標(biāo)函數(shù)f(x)，蝴蝶算法的步驟如下：

(a)初始化蝴蝶種群，并通過(guò)目標(biāo)函數(shù)f(xi)確定每只蝴蝶xi的刺激強(qiáng)度Ii。

(b)計(jì)算蝴蝶種群的個(gè)體適應(yīng)度值，根據(jù)這個(gè)適應(yīng)度值來(lái)確立在所定蝴蝶種群中處于最佳性能的那只蝴蝶。

(c)計(jì)算蝴蝶散發(fā)的香味?？紤]到環(huán)境因素的干擾，使用隨機(jī)數(shù)P作為搜索方式的更換標(biāo)志，有利于后期的位置更新。

(d)全局搜索。低香味的蝴蝶個(gè)體向全局適應(yīng)度最高的蝴蝶飛行，全局搜索的表達(dá)式為:

(5)

(e)局部搜索，蝴蝶個(gè)體進(jìn)行隨機(jī)飛行。局部搜索的表達(dá)式為:

(6)

為了避免蝴蝶進(jìn)入局部最優(yōu)，Lévy飛行被引入到算法中。

Lévy～u=t-λ，1<λ≤3

(7)

Lévy飛行可以有效提高搜索效率。一般λ取(1,3]。

2.2 改進(jìn)蝴蝶算法

改進(jìn)的蝴蝶算法粒子濾波[14]中提出引入粒子濾波中當(dāng)前時(shí)刻的觀測(cè)值，進(jìn)一步地提高算法的濾波精度。該文在此基礎(chǔ)上進(jìn)行優(yōu)化，采用最新時(shí)刻觀測(cè)和狀態(tài)值作為反饋信息對(duì)當(dāng)前目標(biāo)預(yù)估結(jié)果進(jìn)行修正，并重新定義蝴蝶算法的刺激強(qiáng)度公式表達(dá)：

(8)

(9)

2.2.1 自適應(yīng)香味半徑

蝴蝶算法具有很強(qiáng)的全局搜索能力，然而一旦個(gè)體的數(shù)量不斷增大，計(jì)算復(fù)雜度也會(huì)相應(yīng)增大。改進(jìn)的蝴蝶算法粒子濾波[14]提出的自適應(yīng)半徑，可以有效將蝴蝶個(gè)體間吸引的范圍限定在固定區(qū)域內(nèi)，進(jìn)而可以減少粒子數(shù)目的使用，將其引入到FastSLAM粒子濾波階段可以很好地降低系統(tǒng)的計(jì)算量。香味半徑的大小由香味濃度決定，其香味半徑與香味濃度成正比關(guān)系。對(duì)于兩只香味不同的蝴蝶，位置更新的條件則是任意兩個(gè)蝴蝶間距小于等于這兩個(gè)蝴蝶中香味濃度最大那個(gè)蝴蝶的香味半徑，否則不更新。該文定義的半徑的公式為：

r=η*Fi

(10)

其中，r為相對(duì)吸引半徑；Fi為種群個(gè)體的香味值；η作為調(diào)節(jié)半徑參數(shù)，一般在(0，1)之間取值。加入自適應(yīng)半徑之后相鄰粒子間吸引更加快速，則引入?yún)?shù)后的算法，在準(zhǔn)確率與運(yùn)行效率上都有所提高。

2.2.2 自適應(yīng)調(diào)整飛行步長(zhǎng)因子

模擬蝴蝶算法的實(shí)際運(yùn)行初期，每個(gè)預(yù)設(shè)的蝴蝶間距較大，較高的全局尋優(yōu)能力能夠使蝴蝶粒子較晚地陷入局部極值。在算法不斷迭代后期，蝴蝶的個(gè)體可移動(dòng)步長(zhǎng)逐漸變小，較強(qiáng)的局部尋優(yōu)能力有利于蝴蝶算法后期的收斂，不會(huì)因可移動(dòng)間距過(guò)大而跳過(guò)全局最優(yōu)解。因此固定的步長(zhǎng)因子會(huì)降低濾波整體的性能，導(dǎo)致預(yù)測(cè)精度低和收斂速度慢[15]。

針對(duì)以上問(wèn)題，該文設(shè)計(jì)一種非線性衰減函數(shù)，加入觀測(cè)噪聲的方差，保證模型對(duì)于實(shí)際目標(biāo)函數(shù)有較好的近似，兼顧了收斂速度與穩(wěn)定誤差，實(shí)現(xiàn)α值的自適應(yīng)調(diào)整：

(11)

α(i)×(rand÷2)

(12)

2.3 自適應(yīng)部分分布重采樣

FastSLAM中粒子濾波的重采樣一般采用的是自適應(yīng)重采樣，其基本思想是：若采樣后的有效粒子數(shù)小于設(shè)定的閾值，則進(jìn)行重采樣，否則不重采樣。此方法可以有效地緩解粒子退化、預(yù)估精度降低等問(wèn)題。為了進(jìn)一步提高粒子的多樣性，該文改進(jìn)自適應(yīng)重采樣方法，并通過(guò)基于偏差修正指數(shù)加權(quán)平均算法優(yōu)化粒子篩選，選出大權(quán)值的粒子，并對(duì)中小權(quán)值的粒子進(jìn)行分布重采樣，并設(shè)計(jì)如下策略：

計(jì)算粒子權(quán)重并按升序排列；

采用基于偏差修正指數(shù)加權(quán)平均算法得到粒子集的平均值分布曲線，其表達(dá)式如下：

(13)

該算法中的偏差修正項(xiàng)可以降低前期加權(quán)平均值的誤差，提供更加精確的平均值，進(jìn)而更好區(qū)分出大權(quán)值，對(duì)中小權(quán)值進(jìn)行采樣，有效緩解了粒子退化，提高了粒子的多樣性。

2.4 改進(jìn)后的FastSLAM算法實(shí)現(xiàn)

結(jié)合上面引入的改進(jìn)蝴蝶算法和自適應(yīng)部分分布重采樣的算法對(duì)FastSLAM進(jìn)行優(yōu)化，優(yōu)化具體步驟如下：

預(yù)測(cè)：由提議分布對(duì)當(dāng)前粒子集合進(jìn)行預(yù)測(cè)采樣，獲得下一時(shí)刻的粒子集合，sk～p(sk|sk-1,uk)。

仿生算法優(yōu)化粒子集合：將粒子集融合改進(jìn)后的蝴蝶算法，粒子集合根據(jù)自身的吸引半徑自適應(yīng)地更新位置尋找到最優(yōu)解，得到更加符合真實(shí)分布的后驗(yàn)概率密度。

重要性采樣：計(jì)算出融合蝴蝶算法優(yōu)化后粒子的后驗(yàn)位置和姿態(tài)建議分布，從建議分布中獲得新的粒子集，并歸一化權(quán)重。

地圖更新：結(jié)合最新觀測(cè)值更新機(jī)器人位姿和路標(biāo)位置。

重采樣：當(dāng)有效粒子數(shù)小于設(shè)置閾值時(shí)，采用改進(jìn)后的自適應(yīng)部分分布重采樣。

改進(jìn)后的FastSLAM中通過(guò)對(duì)粒子的仿生優(yōu)化使得粒子集更加符合真實(shí)分布，以及收斂速度上也有所提高，因此可以在下一時(shí)刻得到更好的初始值。

3 實(shí)驗(yàn)分析

該文利用MATLAB2012a仿真軟件分別對(duì)傳統(tǒng)FastSLAM、BA-FastSLAM與文中算法IBA-FastSLAM在稀疏路標(biāo)環(huán)境下進(jìn)行仿真實(shí)驗(yàn)并比較，以驗(yàn)證提出的算法的性能。

仿真中用到的運(yùn)動(dòng)模型與觀測(cè)模型如式(14)和式(15)所示。

(14)

(15)

其中，(xi,yi)為地圖中第i存在的路標(biāo)點(diǎn)；(ri,θi)為相對(duì)于機(jī)器人的位置傳感器所檢測(cè)到的路標(biāo)點(diǎn)的距離以及方向夾角；εt為觀測(cè)噪聲，其噪聲服從高斯分布。

仿真實(shí)驗(yàn)的機(jī)器人參數(shù)配置如表1所示。

表1 FastSLAM仿真實(shí)驗(yàn)運(yùn)動(dòng)參數(shù)配置

仿真實(shí)驗(yàn)中采用50個(gè)粒子，并建立約為200 m*160 m的定位與建圖的模擬環(huán)境，此環(huán)境一共有36個(gè)路標(biāo)點(diǎn)與18個(gè)航向點(diǎn)，在以上建立的環(huán)境下對(duì)FastSLAM、BA-FastSLAM、IBA-FastSLAM分別進(jìn)行移動(dòng)機(jī)器人定位與建圖的仿真實(shí)驗(yàn)，得到的實(shí)驗(yàn)結(jié)果如圖1～圖3所示。

圖1 FastSLAM

圖2 BA-FastSLAM

圖3 IBA-FastSLAM

由圖可知，利用仿生尋優(yōu)思想后一定程度上優(yōu)化了粒子位置與路標(biāo)位置，但依然存在較大的偏差，而文中在蝴蝶算法基礎(chǔ)上進(jìn)行改進(jìn)的算法得到的估計(jì)路徑更加貼合實(shí)際路徑，路標(biāo)的估計(jì)精度也相對(duì)提高。

(a)x方向

圖5 路標(biāo)估計(jì)誤差比較

對(duì)三種算法的性能進(jìn)行對(duì)比，如圖4、圖5所示。IBA-FastSLAM在位姿估計(jì)偏差以及路標(biāo)估計(jì)偏差上有少許部分稍微大于BA-FastSLAM和FastSLAM，但整體上隨著時(shí)間推移，IBA-FastSLAM不管是位姿估計(jì)精度還是路標(biāo)估計(jì)精度明顯要比BA-FastSLAM和FastSLAM這兩種算法高。

為了進(jìn)一步驗(yàn)證算法的穩(wěn)定性，分別利用20，40，60粒子數(shù)對(duì)兩種算法進(jìn)行仿真實(shí)驗(yàn)，計(jì)算兩種算法在不同粒子數(shù)時(shí)，連續(xù)運(yùn)行系統(tǒng)20次的位姿估計(jì)的均方根誤差的均值和方差以作為評(píng)價(jià)算法性能的標(biāo)準(zhǔn)，如圖6所示。

圖6 不同粒子數(shù)在位姿估計(jì)的均方根誤差的均值和方差

通過(guò)圖6所示，從整體上分析IBA-FastSLAM算法與原始的BA-FastSLAM在均方根誤差的均值和方差都要高于FastSLAM，這表明融合了蝴蝶算法后，粒子得到了優(yōu)化，使得系統(tǒng)的精度以及穩(wěn)定性都有明顯的提升，一定程度上表明了仿生算法的引入，可以很好地提高原始FastSLAM的性能。而文中提出的IBA-FastSLAM算法比原始的BA-FastSLAM算法在均方根的均值和方差上具有更高的精度與穩(wěn)定性，表明了未改進(jìn)的蝴蝶算法存在一定程度的缺陷，滿足不了系統(tǒng)的實(shí)際需求，進(jìn)一步說(shuō)明改進(jìn)后的蝴蝶算法性能上遠(yuǎn)遠(yuǎn)要優(yōu)于原始的蝴蝶算法。從不同粒子數(shù)的對(duì)比中可以看出，傳統(tǒng)的FastSLAM隨著粒子數(shù)的增加，誤差也隨之成非線性大幅度下降，可見，傳統(tǒng)FastSLAM對(duì)于粒子數(shù)的依賴較大，而BA-FastSLAM相對(duì)于傳統(tǒng)FastSLAM誤差都有所降低，但對(duì)于粒子數(shù)的依賴還是較大，IBA-FastSLAM均方根誤差的均值在不同粒子數(shù)之間變化，相對(duì)于BA-FastSLAM與FastSLAM都要較平穩(wěn)，其主要原因是改進(jìn)后的重采樣保證粒子多樣性。從這一點(diǎn)可以看出，提出的IBA-FastSLAM算法對(duì)于通過(guò)增加粒子數(shù)來(lái)緩解粒子退化，提高粒子多樣性以及濾波精度的需求相對(duì)降低，因此在選用粒子數(shù)量的時(shí)候可以適量減少采樣粒子數(shù)，來(lái)降低計(jì)算量。

為了對(duì)三種算法在綜合性能上進(jìn)行比較，設(shè)定粒子數(shù)為50，分別對(duì)三種算法連續(xù)運(yùn)行20次，得出三種算法在均方根的均值與方差以及運(yùn)行時(shí)間對(duì)比，如表2所示?？梢钥闯觯珺A-FastSLAM在預(yù)測(cè)精度與穩(wěn)定性上分別比FastSLAM提高了17.10%和27.99%，表明融合蝴蝶算法后的FastSLAM在預(yù)測(cè)精度和穩(wěn)定性上都有所提高，而運(yùn)行時(shí)間卻多于FastSLAM算法7.711秒，相當(dāng)于收斂速度降低了7.9%，可見原始的BA-FastSLAM算法存在一定計(jì)算量較大的問(wèn)題。而提出的IBA-FastSLAM均方根的均值和方差分別是0.379 5和0.231，在預(yù)測(cè)精度和穩(wěn)定性上分別比FastSLAM提高了40.6%和41.22%，同時(shí)比改進(jìn)前的BA-FastSLAM提高了28.34%和6.27%，表明在香味公式中融入最新觀測(cè)值和狀態(tài)值，以及引入香味半徑后的蝴蝶算法提高了FastSLAM中粒子的濾波能力，減少了誤差。IBA-FastSLAM在運(yùn)行時(shí)間上是95.721秒，與FastSLAM的運(yùn)行時(shí)間相比，縮短了3.876秒，而相對(duì)于BA-FastSLAM縮短了11.587秒，相當(dāng)于收斂速度提高了10.93%，主要原因是提出的自適應(yīng)蝴蝶步長(zhǎng)因子加快了粒子向最優(yōu)位置飛行速率。因此可以結(jié)合圖6所得出的結(jié)論推出，IBA-FastSLAM算法可以在保持粒子數(shù)較小的情況下實(shí)現(xiàn)更加準(zhǔn)確的預(yù)測(cè)，能從根本上改善粒子匱乏的現(xiàn)象以及粒子的多樣性，從而使系統(tǒng)更加具有實(shí)時(shí)性，一定程度提升了系統(tǒng)運(yùn)行的效率，因此能夠使移動(dòng)機(jī)器人在未知場(chǎng)景下進(jìn)行精確定位與建圖，從而合理地去決策與規(guī)劃路徑，更好地滿足移動(dòng)機(jī)器人的實(shí)際環(huán)境中的應(yīng)用需求。

表2 三種算法的均方根的均值與方差以及運(yùn)行時(shí)間對(duì)比

4 結(jié)束語(yǔ)

對(duì)FastSLAM的粒子濾波中的粒子集合融入了改進(jìn)后的蝴蝶算法，對(duì)重要性采樣過(guò)程進(jìn)行優(yōu)化，在粒子位置更新的過(guò)程中加入自適應(yīng)香味半徑和自適應(yīng)蝴蝶飛行調(diào)制步長(zhǎng)因子來(lái)控制蝴蝶種群尋優(yōu)的搜索范圍，能夠有效保證算法的運(yùn)行時(shí)間縮短和預(yù)測(cè)精度的提高，在重采樣過(guò)程中加入偏差修正指數(shù)加權(quán)算法劃分出穩(wěn)定與不穩(wěn)定的粒子集，并對(duì)劃分后放入不穩(wěn)定的粒子部分進(jìn)行分布重采樣，減少不穩(wěn)定的粒子對(duì)重采樣階段的干擾，進(jìn)而保證粒子的多樣性。通過(guò)仿真實(shí)驗(yàn)結(jié)果表明，提出的IBA-FastSLAM算法可以在不需要增加過(guò)多粒子數(shù)量的情況下，既可以改善粒子的退化和多樣性，又可以提升位姿與路標(biāo)預(yù)測(cè)精度與穩(wěn)定性，以及算法的收斂速度，進(jìn)而使機(jī)器人更實(shí)時(shí)地接近真實(shí)系統(tǒng)狀態(tài)分布。上述方法為移動(dòng)機(jī)器人的自主導(dǎo)航提供了一種精確的定位與建圖的方法。