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

背景技術(shù)：

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

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

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

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

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

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

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

具體包括以下步驟：

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

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

3）定位管理節(jié)點(diǎn)收聽(tīng)/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í)時(shí)發(fā)布其定位結(jié)果到/gmap_current_pose主題；

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

7）當(dāng)機(jī)器人離開(kāi)灌木叢區(qū)域時(shí)，定位管理節(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)樹(shù)關(guān)系都是地圖坐標(biāo)系—里程計(jì)坐標(biāo)系—機(jī)器人坐標(biāo)系—激光坐標(biāo)系；slam算法在初始時(shí)刻新建地圖的地圖坐標(biāo)系和里程計(jì)坐標(biāo)系是重合的，加入一個(gè)補(bǔ)償坐標(biāo)系，取代原始slam算法中的里程計(jì)坐標(biāo)系；由slam算法切換到蒙特卡洛定位時(shí)，只需將機(jī)器人在slam算法中的定位結(jié)果作為蒙特卡洛定位的初始位姿即可；由蒙特卡洛定位切換到slam算法時(shí)，利用蒙特卡洛定位結(jié)果作為slam初始化時(shí)機(jī)器人的位姿，機(jī)器人在里程計(jì)坐標(biāo)系下的坐標(biāo)并沒(méi)有發(fā)生變化，補(bǔ)償坐標(biāo)系和里程計(jì)坐標(biāo)系的關(guān)系與蒙特卡洛算法結(jié)束時(shí)地圖坐標(biāo)系和里程計(jì)坐標(biāo)系的關(guān)系一致并保持此變換，slam初始化時(shí)地圖坐標(biāo)系和補(bǔ)償坐標(biāo)系重合，然后根據(jù)slam中機(jī)器人的定位結(jié)果來(lái)計(jì)算地圖坐標(biāo)系和補(bǔ)償坐標(biāo)系之間的關(guān)系。

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

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

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

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

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

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

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

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

，

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

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

b4)重采樣：有效粒子系數(shù)，當(dāng)有效粒子系數(shù)低于給定閾值時(shí)進(jìn)行重采樣；當(dāng)采樣符合真實(shí)分布時(shí)，各粒子權(quán)重相似，當(dāng)采樣分布與真實(shí)分布偏差越大，粒子的權(quán)重方差會(huì)越大，因此有效粒子系數(shù)可以作為采樣粒子是否逼近真實(shí)分布的有效測(cè)度；當(dāng)時(shí)進(jìn)行重采樣，當(dāng)時(shí)跳過(guò)重采樣步驟，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)是可通過(guò)“主題”的方式進(jìn)行通信，某個(gè)或某些節(jié)點(diǎn)作為發(fā)布者發(fā)布消息到特定主題，收聽(tīng)該主題的節(jié)點(diǎn)（即訂閱者，一個(gè)或多個(gè)節(jié)點(diǎn)）收聽(tīng)到該主題消息后可進(jìn)行對(duì)應(yīng)操作，發(fā)布者和訂閱者之間并不需要知道彼此的存在。

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

附圖說(shuō)明

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

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

具體實(shí)施方式

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

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

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

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

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

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

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

3）定位管理節(jié)點(diǎn)收聽(tīng)/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í)時(shí)發(fā)布其定位結(jié)果到/gmap_current_pose主題（slam算法定位位姿主題）；

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

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

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

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

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

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

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

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

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

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

。

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

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

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