本發(fā)明屬于機(jī)器人定位技術(shù)領(lǐng)域，特別涉及一種適用于變電站環(huán)境下的巡檢機(jī)器人定位方法。

背景技術(shù)：

變電站設(shè)備巡檢為變電站安全運(yùn)行提供基本保障，而變電站穩(wěn)定、安全的運(yùn)行是人民日常生活、工業(yè)生產(chǎn)正常進(jìn)行的重要條件，隨著變電站自動化水平提高，變電站設(shè)備可靠運(yùn)行面臨更加嚴(yán)峻的考驗(yàn)。由于變電站巡檢任務(wù)的復(fù)雜性，目前，變電站設(shè)備巡檢主要是工作人員借助一些檢測設(shè)備進(jìn)行檢查的方式，檢查人員的主要工作就是對站內(nèi)大量的儀表讀數(shù)進(jìn)行記錄以及對特定區(qū)域的溫度測量，由于人工巡檢諸如操作人員勞動強(qiáng)度、現(xiàn)場環(huán)境等不確定因素影響，檢查結(jié)果可靠性差，漏檢和錯誤檢查情況較多，進(jìn)而可能造成較大經(jīng)濟(jì)損失，甚至引起嚴(yán)重安全事故，變電站自主巡檢機(jī)器人代替人工進(jìn)行巡檢成為智能電網(wǎng)的發(fā)展趨勢。自主導(dǎo)航是機(jī)器人進(jìn)行自主巡檢的關(guān)鍵步驟，機(jī)器人的準(zhǔn)確定位是實(shí)現(xiàn)自主導(dǎo)航的前提。

定位是移動機(jī)器人研究的熱點(diǎn)，也是機(jī)器人自主導(dǎo)航最基本的環(huán)節(jié)，對于實(shí)現(xiàn)變電站巡檢機(jī)器人具有重要的理論意義和實(shí)用價值。如果移動機(jī)器人在導(dǎo)航過程中僅僅依靠編碼器的信息進(jìn)行位置估計，那么由于測程法系統(tǒng)誤差和非系統(tǒng)誤差會引起無界的誤差累積，甚至導(dǎo)致機(jī)器人導(dǎo)航任務(wù)的失敗，因此需要借助外界的傳感器信息修正測程法的定位誤差，提高機(jī)器人的定位精度，本文中所用的觀測傳感器為激光傳感器。

近年來，基于概率的絕對定位方法引起了國內(nèi)外學(xué)者的注意，成為機(jī)器人定位研究的熱點(diǎn)，出現(xiàn)了一大批文獻(xiàn)。在定位過程中，存在很多不確定性因素：首先機(jī)器人本身具有不確定性：如里程計誤差累積和傳感器噪聲數(shù)據(jù)；其次機(jī)器人所處環(huán)境也是不可預(yù)知的：人的走動和物體的移動所造成的環(huán)境變化。由于這些不確定性因素，使定位變得更加困難，因此近年來,越來越多的研究者把概率理論應(yīng)用到移動機(jī)器人定位中。這些研究理論的基礎(chǔ)是貝葉斯濾波，貝葉斯濾波是概率定位方法的理論基礎(chǔ)，貝葉斯濾波用傳感器測量數(shù)據(jù)去估計一個動態(tài)系統(tǒng)的未知狀態(tài)，其核心思想就是：以當(dāng)前為止所收集的數(shù)據(jù)為條件，遞歸估計狀態(tài)空間后驗(yàn)概率密度。本文基本的定位算法采用基于粒子濾波器的蒙特卡洛定位算法。

變電站環(huán)境主要由兩類特點(diǎn)的環(huán)境組成，一種是靜態(tài)環(huán)境，一種是每隔一段時間就會發(fā)生變化的動態(tài)環(huán)境。變電站內(nèi)的環(huán)境大部分屬于靜態(tài)環(huán)境，但還有一部分環(huán)境會定期發(fā)生變化，這種環(huán)境內(nèi)的參照物主要是灌木叢，變電站內(nèi)的灌木是生長的，地圖創(chuàng)建完成以后過一段時間灌木叢的形狀就會發(fā)生改變，導(dǎo)致機(jī)器人自主導(dǎo)航時激光掃描到的數(shù)據(jù)無法和地圖匹配，從而導(dǎo)致機(jī)器人定位錯誤，使機(jī)器人無法完成變電站的巡檢任務(wù)，此外，灌木叢每隔一段時間就會進(jìn)行修剪，每次修剪后和修剪前形狀也不同，同樣會導(dǎo)致定位錯誤。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是為了提供一種能夠兼顧靜態(tài)環(huán)境和動態(tài)環(huán)境，適用于變電站環(huán)境下的巡檢機(jī)器人定位方法。

為此，本發(fā)明的技術(shù)方案是：一種適用于變電站環(huán)境下的巡檢機(jī)器人定位方法，所述巡檢機(jī)器人具有運(yùn)動控制系統(tǒng)、慣性導(dǎo)航系統(tǒng)、激光掃描傳感器，所用軟件系統(tǒng)為機(jī)器人操作系統(tǒng)ros；其特征在于：機(jī)器人處于靜態(tài)環(huán)境時，采用的是蒙特卡洛定位方法，通過激光掃描環(huán)境信息和地圖進(jìn)行匹配；當(dāng)機(jī)器人進(jìn)入和建圖時環(huán)境相比發(fā)生改變了的區(qū)域時，不再使用已有地圖定位，采用slam算法實(shí)時建圖并定位，當(dāng)機(jī)器人離開該區(qū)域，再次進(jìn)入靜態(tài)環(huán)境時，再次切換回蒙特卡洛定位；兩種定位方式之間的切換由一個定位管理節(jié)點(diǎn)實(shí)現(xiàn)；

具體包括以下步驟：

1）機(jī)器人開機(jī)時，默認(rèn)使用的是蒙特卡洛定位算法，機(jī)器人會加載變電站環(huán)境地圖，并加載機(jī)器人初始位姿信息發(fā)布到/initialpose主題；

2）蒙特卡洛定位節(jié)點(diǎn)接收/initialpose主題消息并初始化機(jī)器人位姿，機(jī)器人接受巡檢任務(wù)后開始自主導(dǎo)航，蒙特卡洛算法實(shí)時追蹤機(jī)器人位姿，實(shí)現(xiàn)機(jī)器人的定位并發(fā)布機(jī)器人的實(shí)時定位結(jié)果到/amcl_current_pose主題；

3）定位管理節(jié)點(diǎn)收聽/amcl_current_pose主題消息，定位管理節(jié)點(diǎn)根據(jù)定位結(jié)果及提前預(yù)設(shè)的灌木叢區(qū)域坐標(biāo)判斷機(jī)器人是否進(jìn)入灌木叢區(qū)域；

4）當(dāng)機(jī)器人進(jìn)入灌木叢區(qū)域，定位管理節(jié)點(diǎn)首先發(fā)送消息到/amcl_block_flag主題，蒙特卡洛定位節(jié)點(diǎn)接收到/amcl_block_flag主題消息后將自身阻塞，然后定位管理節(jié)點(diǎn)發(fā)送消息到/gmap_initialpose主題；

5）slam算法節(jié)點(diǎn)接收到/gmap_initialpose主題消息后初始化slam算法并實(shí)時發(fā)布其定位結(jié)果到/gmap_current_pose主題；

6）定位管理節(jié)點(diǎn)收聽/gmap_current_pose主題消息并判斷機(jī)器人是否離開灌木叢區(qū)域；

7）當(dāng)機(jī)器人離開灌木叢區(qū)域時，定位管理節(jié)點(diǎn)發(fā)送消息到/gmap_block_flag主題，slam算法接收到/gmap_block_flag主題消息后將自身阻塞，然后定位管理節(jié)點(diǎn)再發(fā)布機(jī)器人位姿消息到/initialpose主題，初始化蒙特卡洛定位節(jié)點(diǎn)。

進(jìn)一步地，所述蒙特卡洛定位和slam算法的坐標(biāo)系之間的坐標(biāo)樹關(guān)系都是地圖坐標(biāo)系—里程計坐標(biāo)系—機(jī)器人坐標(biāo)系—激光坐標(biāo)系；slam算法在初始時刻新建地圖的地圖坐標(biāo)系和里程計坐標(biāo)系是重合的，加入一個補(bǔ)償坐標(biāo)系，取代原始slam算法中的里程計坐標(biāo)系；由slam算法切換到蒙特卡洛定位時，只需將機(jī)器人在slam算法中的定位結(jié)果作為蒙特卡洛定位的初始位姿即可；由蒙特卡洛定位切換到slam算法時，利用蒙特卡洛定位結(jié)果作為slam初始化時機(jī)器人的位姿，機(jī)器人在里程計坐標(biāo)系下的坐標(biāo)并沒有發(fā)生變化，補(bǔ)償坐標(biāo)系和里程計坐標(biāo)系的關(guān)系與蒙特卡洛算法結(jié)束時地圖坐標(biāo)系和里程計坐標(biāo)系的關(guān)系一致并保持此變換，slam初始化時地圖坐標(biāo)系和補(bǔ)償坐標(biāo)系重合，然后根據(jù)slam中機(jī)器人的定位結(jié)果來計算地圖坐標(biāo)系和補(bǔ)償坐標(biāo)系之間的關(guān)系。

進(jìn)一步地，所述蒙特卡洛定位算法包括以下步驟：

a1)初始化：初始時刻，從先驗(yàn)概率分布抽取樣本大小為n的粒子集；

a2)運(yùn)動更新階段：在k時刻，利用k-1時刻的粒子集和機(jī)器人運(yùn)動模型預(yù)測機(jī)器人位姿分布粒子集，表示運(yùn)動控制信息；

a3)觀測更新階段：利用k時刻的傳感器測量數(shù)據(jù)和觀測模型，更新預(yù)測粒子集中的粒子的權(quán)重，并對權(quán)重進(jìn)行歸一化處理，得到更新后的近似的機(jī)器人后驗(yàn)概率分布粒子集；

a4)根據(jù)粒子權(quán)重進(jìn)行重采樣，重采樣階段會復(fù)制權(quán)重高的粒子，消除權(quán)重小的粒子，這樣大部分粒子會集中在機(jī)器人真實(shí)位姿附近，然后時刻返回步驟a2)進(jìn)行下一輪迭代。

進(jìn)一步地，所述slam算法包括以下步驟：

b1)初始化：當(dāng)機(jī)器人駛?cè)牍嗄緟矃^(qū)域，由蒙特卡洛定位切換到slam算法，實(shí)時創(chuàng)建地圖并基于新建地圖實(shí)現(xiàn)定位，基于粒子濾波的slam算法每個粒子單獨(dú)維護(hù)一副變電站地圖，初始化信息是蒙特卡洛方法最后發(fā)布的機(jī)器人位姿和當(dāng)前里程計信息，為保持新建地圖坐標(biāo)系統(tǒng)和蒙特卡洛定位所用地圖坐標(biāo)系統(tǒng)的一致，slam初始化粒子位姿為蒙特卡洛算法最后發(fā)布的機(jī)器人位姿，slam算法中加入一個補(bǔ)償坐標(biāo)系，初始時刻補(bǔ)償坐標(biāo)系與地圖坐標(biāo)系重合，此后通過slam中機(jī)器人的定位結(jié)果對這兩個坐標(biāo)系之間的關(guān)系進(jìn)行校正，補(bǔ)償坐標(biāo)系和里程計坐標(biāo)系始終保持固定變換；

b2)采樣階段：首先利用機(jī)器人運(yùn)動模型

，

即里程計數(shù)據(jù)計算每個粒子下一時刻的位置，是t-1時刻第i個粒子所維護(hù)的地圖；為了實(shí)現(xiàn)上述在高似然區(qū)域采樣的目的，我們對每個粒子周圍一定范圍的位置進(jìn)行迭代，在這些位置中選出機(jī)器人在此位置時能使當(dāng)前激光和地圖匹配最好的位置，作為該粒子下一步的采樣結(jié)果；

b3)權(quán)重更新：粒子權(quán)重根據(jù)各粒子位姿利用當(dāng)前激光數(shù)據(jù)與各粒子所維護(hù)的地圖進(jìn)行匹配，根據(jù)匹配得分計算每個粒子的權(quán)重；

b4)重采樣：有效粒子系數(shù)，當(dāng)有效粒子系數(shù)低于給定閾值時進(jìn)行重采樣；當(dāng)采樣符合真實(shí)分布時，各粒子權(quán)重相似，當(dāng)采樣分布與真實(shí)分布偏差越大，粒子的權(quán)重方差會越大，因此有效粒子系數(shù)可以作為采樣粒子是否逼近真實(shí)分布的有效測度；當(dāng)時進(jìn)行重采樣，當(dāng)時跳過重采樣步驟，n表示粒子數(shù)。

本發(fā)明中的/initialpose主題為蒙特卡洛算法初始化位姿主題，/amcl_current_pose主題為蒙特卡洛定位位姿主題，/amcl_block_flag主題為蒙特卡洛算法阻塞主題，/gmap_initialpose主題為slam算法初始化主題，/gmap_current_pose主題為slam算法定位位姿主題，/gmap_block_flag主題為slam算法阻塞主題。機(jī)器人操作系統(tǒng)的通信特點(diǎn)是可通過“主題”的方式進(jìn)行通信，某個或某些節(jié)點(diǎn)作為發(fā)布者發(fā)布消息到特定主題，收聽該主題的節(jié)點(diǎn)（即訂閱者，一個或多個節(jié)點(diǎn)）收聽到該主題消息后可進(jìn)行對應(yīng)操作，發(fā)布者和訂閱者之間并不需要知道彼此的存在。

在變電站環(huán)境下，單純采用基于已有地圖的蒙特卡洛定位方法進(jìn)行機(jī)器人定位的局限性在于當(dāng)機(jī)器人導(dǎo)航進(jìn)入灌木叢區(qū)域時由于激光掃描到的數(shù)據(jù)和地圖中的信息匹配不準(zhǔn)導(dǎo)致定位錯誤，單純采用slam方式導(dǎo)航，需要進(jìn)行實(shí)時建圖，變電站環(huán)境規(guī)模大，由于slam算法復(fù)雜度高，很快會建圖失敗進(jìn)而導(dǎo)致導(dǎo)航失敗。本發(fā)明融合兩種算法的特點(diǎn)，在變電站的靜態(tài)環(huán)境中使用蒙特卡洛方法進(jìn)行定位，在灌木叢區(qū)域切換為slam方式，使機(jī)器人可在整個變電站環(huán)境下實(shí)現(xiàn)準(zhǔn)確定位，有效完成變電站巡檢任務(wù)。

附圖說明

以下結(jié)合附圖和本發(fā)明的實(shí)施方式來作進(jìn)一步詳細(xì)說明

圖1為本發(fā)明的定位管理框圖。

具體實(shí)施方式

本實(shí)施例提出一種組合定位的方式，在靜態(tài)環(huán)境中使用蒙特卡洛方法基于已有地圖進(jìn)行定位，當(dāng)進(jìn)入到灌木叢區(qū)域時，使用slam的方式進(jìn)行導(dǎo)航，即在灌木叢區(qū)域，不再使用已有的地圖進(jìn)行機(jī)器人的位姿估計，而是進(jìn)行實(shí)時建圖并基于新建地圖來實(shí)現(xiàn)機(jī)器人的定位。

本實(shí)施例所用平臺機(jī)器人須具有運(yùn)動控制系統(tǒng)、慣性導(dǎo)航系統(tǒng)、激光掃描傳感器，而所用軟件系統(tǒng)為機(jī)器人操作系統(tǒng)ros。機(jī)器人在兩種定位算法之間進(jìn)行切換時，由于定位使用的不是同一地圖，因此使用的坐標(biāo)系統(tǒng)也會有差別，若要求兩種定位算法的切換不影響機(jī)器人的自主導(dǎo)航，則要求在不同的地圖坐標(biāo)系統(tǒng)中，機(jī)器人的坐標(biāo)是一致的。

默認(rèn)情況下蒙特卡洛定位和slam算法的坐標(biāo)系之間的坐標(biāo)樹關(guān)系都是地圖坐標(biāo)系-里程計坐標(biāo)系-機(jī)器人坐標(biāo)系-激光坐標(biāo)系。蒙特卡洛定位是基于已有地圖的，因此地圖坐標(biāo)系已經(jīng)固定，初始化時只需要告訴算法初始時刻機(jī)器人在地圖坐標(biāo)系下的坐標(biāo)即可。

在由slam算法切換到蒙特卡洛定位時，只需將機(jī)器人在slam算法中的定位結(jié)果作為蒙特卡洛定位的初始位姿即可，在由蒙特卡洛定位切換到slam算法時，此時由于既沒有地圖也沒有機(jī)器人位姿，所以比較麻煩，在蒙特卡洛定位中直接定位結(jié)果計算頻率較低，而機(jī)器人在里程計坐標(biāo)系下的坐標(biāo)更新較快并且里程計短期內(nèi)結(jié)果比較準(zhǔn)確，利用里程計該特點(diǎn)，導(dǎo)航時使用的定位結(jié)果由地圖坐標(biāo)系到里程計坐標(biāo)系的變化加上機(jī)器人在里程計坐標(biāo)系下的坐標(biāo)求得，蒙特卡洛方法來計算地圖坐標(biāo)系和里程計坐標(biāo)系之間的變化，在切換到slam算法時，需要利用蒙特卡洛定位結(jié)果作為slam初始化時機(jī)器人的位姿，機(jī)器人在里程計坐標(biāo)系下的坐標(biāo)并沒有發(fā)生變化，為保持地圖坐標(biāo)系和里程計坐標(biāo)系的變換關(guān)系也不變，需要加入一個補(bǔ)償坐標(biāo)系，該補(bǔ)償坐標(biāo)系和里程計坐標(biāo)系的關(guān)系與蒙特卡洛算法結(jié)束時地圖坐標(biāo)系和里程計坐標(biāo)系的關(guān)系一致并保持此變換，slam初始化時機(jī)器人坐標(biāo)系和補(bǔ)償坐標(biāo)系重合，然后根據(jù)slam中機(jī)器人的定位結(jié)果來計算地圖坐標(biāo)系和補(bǔ)償坐標(biāo)系之間的關(guān)系。兩種定位方式之間的切換由一個定位管理節(jié)點(diǎn)實(shí)現(xiàn)，定位模塊的結(jié)構(gòu)如圖1所示。

兩種定位方式之間的切換由一個定位管理節(jié)點(diǎn)實(shí)現(xiàn)；具體包括以下步驟：

1）機(jī)器人開機(jī)時，默認(rèn)使用的是蒙特卡洛定位算法，機(jī)器人會加載變電站環(huán)境地圖，并加載機(jī)器人初始位姿信息發(fā)布到/initialpose主題（蒙特卡洛算法初始化位姿主題）；

2）蒙特卡洛定位節(jié)點(diǎn)接收/initialpose主題（蒙特卡洛算法初始化位姿主題）消息并初始化機(jī)器人位姿，機(jī)器人接受巡檢任務(wù)后開始自主導(dǎo)航，蒙特卡洛算法實(shí)時追蹤機(jī)器人位姿，實(shí)現(xiàn)機(jī)器人的定位并發(fā)布機(jī)器人的實(shí)時定位結(jié)果到/amcl_current_pose主題（蒙特卡洛定位位姿主題）；

3）定位管理節(jié)點(diǎn)收聽/amcl_current_pose主題（蒙特卡洛定位位姿主題）消息，定位管理節(jié)點(diǎn)根據(jù)定位結(jié)果及提前預(yù)設(shè)的灌木叢區(qū)域坐標(biāo)判斷機(jī)器人是否進(jìn)入灌木叢區(qū)域；

4）當(dāng)機(jī)器人進(jìn)入灌木叢區(qū)域，定位管理節(jié)點(diǎn)首先發(fā)送消息到/amcl_block_flag主題（蒙特卡洛算法阻塞主題），蒙特卡洛定位節(jié)點(diǎn)接收到/amcl_block_flag主題（蒙特卡洛算法阻塞主題）消息后將自身阻塞，然后定位管理節(jié)點(diǎn)發(fā)送消息到/gmap_initialpose主題（slam算法初始化主題）；

5）slam算法節(jié)點(diǎn)接收到/gmap_initialpose主題（slam算法初始化主題）消息后初始化slam算法并實(shí)時發(fā)布其定位結(jié)果到/gmap_current_pose主題（slam算法定位位姿主題）；

6）定位管理節(jié)點(diǎn)收聽/gmap_current_pose主題（slam算法定位位姿主題）消息并判斷機(jī)器人是否離開灌木叢區(qū)域；

7）當(dāng)機(jī)器人離開灌木叢區(qū)域時，定位管理節(jié)點(diǎn)發(fā)送消息到/gmap_block_flag主題（slam算法阻塞主題），slam算法接收到/gmap_block_flag主題（slam算法阻塞主題）消息后將自身阻塞，然后定位管理節(jié)點(diǎn)再發(fā)布機(jī)器人位姿消息到/initialpose主題（蒙特卡洛算法初始化位姿主題），初始化蒙特卡洛定位節(jié)點(diǎn)。

本實(shí)施例所述的蒙特卡洛定位算法包括以下步驟：

a1)初始化：初始時刻，從先驗(yàn)概率分布抽取樣本大小為n的粒子集；蒙特卡洛定位算法在兩種情況下需要進(jìn)行初始化，一是機(jī)器人開機(jī)啟動時，此時會從配置文件讀取默認(rèn)的機(jī)器人初始位姿參數(shù)，二是機(jī)器人導(dǎo)航駛出灌木叢區(qū)域，由slam算法切換到蒙特卡洛定位時，此時需要基于已有地圖，利用slam算法最后時刻發(fā)布的機(jī)器人位姿來對蒙特卡洛算法進(jìn)行初始化。

a2)運(yùn)動更新階段：在k時刻，利用k-1時刻的粒子集和機(jī)器人運(yùn)動模型預(yù)測機(jī)器人位姿分布粒子集，表示運(yùn)動控制信息；在本實(shí)施例中，選擇里程計模型作為機(jī)器人的運(yùn)動模型，以機(jī)器人的里程計信息作為運(yùn)動更新的條件，對下一時刻的粒子位置進(jìn)行預(yù)測。

a3)觀測更新階段：利用k時刻的傳感器測量數(shù)據(jù)和觀測模型，更新預(yù)測粒子集中的粒子的權(quán)重，并對權(quán)重進(jìn)行歸一化處理，得到更新后的近似的機(jī)器人后驗(yàn)概率分布粒子集；本實(shí)施例使用似然場模型作為機(jī)器人的觀測模型，似然場模型可以克服光束投影模型中的缺陷，并且在實(shí)踐中取得了非常好的效果，似然場模型不再根據(jù)射線投影法去求解激光在地圖中的投影，而是直接將激光端點(diǎn)投影到地圖中，根據(jù)其投影與地圖中距離其最近的障礙物的距離確定其似然性，在似然場中，距離障礙物越近，則該處似然性越大，距離障礙物越遠(yuǎn)，似然性越低。

a4)根據(jù)粒子權(quán)重進(jìn)行重采樣，重采樣階段會復(fù)制權(quán)重高的粒子，消除權(quán)重小的粒子，這樣大部分粒子會集中在機(jī)器人真實(shí)位姿附近，然后時刻返回步驟a2)進(jìn)行下一輪迭代。

本實(shí)施例所述的slam算法包括以下步驟：

b1)初始化：當(dāng)機(jī)器人駛?cè)牍嗄緟矃^(qū)域，由蒙特卡洛定位切換到slam算法，實(shí)時創(chuàng)建地圖并基于新建地圖實(shí)現(xiàn)定位，基于粒子濾波的slam算法每個粒子單獨(dú)維護(hù)一副變電站地圖，初始化信息是蒙特卡洛方法最后發(fā)布的機(jī)器人位姿和當(dāng)前里程計信息，為保持新建地圖坐標(biāo)系統(tǒng)和蒙特卡洛定位所用地圖坐標(biāo)系統(tǒng)的一致，slam初始化粒子位姿為蒙特卡洛算法最后發(fā)布的機(jī)器人位姿，slam算法中加入一個補(bǔ)償坐標(biāo)系，初始時刻補(bǔ)償坐標(biāo)系與地圖坐標(biāo)系重合，此后通過slam中機(jī)器人的定位結(jié)果對這兩個坐標(biāo)系之間的關(guān)系進(jìn)行校正，補(bǔ)償坐標(biāo)系和里程計坐標(biāo)系始終保持固定變換。

b2)采樣階段：傳統(tǒng)基于粒子濾波的slam算法以機(jī)器人運(yùn)動模型為建議分布來近似機(jī)器人真實(shí)后驗(yàn)概率分布進(jìn)行采樣，但這種方法需要大量的粒子才能較好的表示機(jī)器人的真實(shí)分布，受限于粒子濾波slam計算量的因素，過多粒子無法滿足實(shí)時性要求，有文獻(xiàn)證明最優(yōu)的建議分布是，當(dāng)機(jī)器人搭載激光傳感器時，觀測模型的概率分布程尖峰形式，此時在中觀測模型占據(jù)主導(dǎo)作用，而在觀測模型的概率分布區(qū)域，運(yùn)動模型的概率分布可近似為一個常數(shù)，此時機(jī)器人的的建議分布

。

因此采樣階段首先利用機(jī)器人運(yùn)動模型，即里程計數(shù)據(jù)計算每個粒子下一時刻的位置，為了實(shí)現(xiàn)上述在高似然區(qū)域采樣的目的，我們對每個粒子周圍一定范圍的位置進(jìn)行迭代，在這些位置中選出機(jī)器人在此位置時能使當(dāng)前激光和地圖匹配最好的位置，作為該粒子下一步的采樣結(jié)果。采樣步驟在高似然區(qū)域進(jìn)行采樣可大大減少所需粒子數(shù)，從而大大減小計算量。

b3)權(quán)重更新：粒子權(quán)重，根據(jù)各粒子位姿利用當(dāng)前激光數(shù)據(jù)與各粒子所維護(hù)的地圖進(jìn)行匹配，根據(jù)匹配得分計算每個粒子的權(quán)重；表示采樣所用的建議分布。

b4)重采樣：在重采樣階段，會刪除權(quán)重較小的粒子，復(fù)制權(quán)重較大的粒子由于只有少數(shù)粒子能較好的代表機(jī)器人的真實(shí)位置，所以重采樣步驟是必須的，但過于頻繁的重采樣會導(dǎo)致粒子匱乏問題，因此引入有效粒子系數(shù)，當(dāng)有效粒子系數(shù)低于給定閾值時進(jìn)行重采樣；當(dāng)采樣符合真實(shí)分布時，各粒子權(quán)重相似，當(dāng)采樣分布與真實(shí)分布偏差越大，粒子的權(quán)重方差會越大，因此有效粒子系數(shù)可以作為采樣粒子是否逼近真實(shí)分布的有效測度；當(dāng)時進(jìn)行重采樣，當(dāng)時跳過重采樣步驟，n代表的是粒子數(shù)，即中的n。