一種基于高斯分布的移動(dòng)機(jī)器人同步定位與地圖構(gòu)建方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于移動(dòng)機(jī)器人導(dǎo)航領(lǐng)域，特別是一種高斯分布的同時(shí)定位與地圖構(gòu)建方 法
【背景技術(shù)】
[0002] 移動(dòng)機(jī)器人的同時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping, SLAM)是移動(dòng)機(jī)器人在其自身位置不確定的條件下，在完全未知環(huán)境中根據(jù)自身的位姿估 計(jì)和傳感器觀測(cè)數(shù)據(jù)創(chuàng)建地圖。SLAM最早由Smith, Self和Cheeseman提出，它解決了從移動(dòng) 機(jī)器人得到一系列觀測(cè)值從而構(gòu)建未知關(guān)鍵地圖的問(wèn)題。
[0003] SLAM問(wèn)題的研究中，對(duì)于非線性、非高斯系統(tǒng)的狀態(tài)估計(jì)，目前研究熱點(diǎn)是基于序 列蒙特卡羅方法(Sequential Monte Car 1 〇，SMC)的粒子濾波算法。該算法對(duì)系統(tǒng)噪聲沒(méi)有 限制，它能通過(guò)預(yù)測(cè)和更新系統(tǒng)概論密度函數(shù)的采樣集來(lái)近似非線性系統(tǒng)的遞歸貝葉斯估 計(jì)。Murphy等人將Rao-Blackwellized粒子濾波(Rao-Blackwellized Particle Filter， RBPF)算法作為一種新的方式來(lái)處理SLAM問(wèn)題。隨后，Montemerlo等人于2003年提出了一種 基于Rao-Blackwellized粒子濾波器的FastSLAM算法(RBPF-SLAM)。相比適合高斯線性系統(tǒng) 的EKF-SLAM算法，RBPF-SLAM算法根據(jù)SLAM問(wèn)題的條件獨(dú)立特性，將高維的移動(dòng)機(jī)器人軌跡 和環(huán)境地圖聯(lián)合后驗(yàn)概率密度估計(jì)解耦為低維狀態(tài)估計(jì)，從而解決了狀態(tài)空間SLAM算法的 維數(shù)災(zāi)難問(wèn)題，提高了 SLAM的求解效率。
[0004] 然而隨著粒子權(quán)值的方差隨時(shí)間遞增，粒子退化現(xiàn)象是不可避免的。為了減少粒 子退化現(xiàn)象，引入重采樣方法，即舍棄權(quán)值較小的粒子，復(fù)制權(quán)值較大的粒子，產(chǎn)生多個(gè)等 權(quán)值的粒子。這樣雖然改善了粒子退化，但同時(shí)造成粒子匱乏。為此，自適應(yīng)的重采樣的方 法成為研究重點(diǎn)，Moral等人對(duì)自適應(yīng)重采樣策略及收斂性進(jìn)行了詳細(xì)分析，一些自適應(yīng)改 進(jìn)重采樣的SLAM算法被提出，雖然這些算法能夠抑制樣本匱乏現(xiàn)象，提高粒子濾波算法的 估計(jì)準(zhǔn)確性，但是該算法的代價(jià)仍然是增加了計(jì)算時(shí)間，雖然可以以較少的粒子數(shù)目來(lái)實(shí) 現(xiàn)算法，但也僅僅能達(dá)到有限的精度與時(shí)間上的平衡。因此高斯分布重采樣移動(dòng)機(jī)器人同 時(shí)定位與地圖構(gòu)建方法的研究RBPF-SLAM中保持粒子多樣性，避免粒子匱乏、同時(shí)保證可靠 精確的估計(jì)有非常重要的意義。

【發(fā)明內(nèi)容】

[0005] 針對(duì)現(xiàn)有技術(shù)的不足，提出了一種可獲得可靠的柵格地圖精度、保持粒子多樣性 的基于高斯分布的移動(dòng)機(jī)器人同步定位與地圖構(gòu)建方法。本發(fā)明的技術(shù)方案如下：一種基 于高斯分布的移動(dòng)機(jī)器人同步定位與地圖構(gòu)建方法，其包括以下步驟：
[0006] S1，輸入t-Ι時(shí)刻的粒子集，其中移動(dòng)機(jī)器人在每個(gè)點(diǎn)的運(yùn)行軌跡為粒子，移動(dòng)機(jī) 器人t-Ι時(shí)刻的觀測(cè)值和t-Ι時(shí)刻施加的控制量，由機(jī)器人位姿和里程計(jì)控制信息估計(jì)機(jī)器 人初始位姿；
[0007] S2,根據(jù)移動(dòng)機(jī)器人的初始位姿和傳感器觀測(cè)數(shù)據(jù)創(chuàng)建地圖，并根據(jù)地圖執(zhí)行掃 描匹配方法得出移動(dòng)機(jī)器人的軌跡；
[0008] S3,根據(jù)步驟S2掃描匹配方法得出移動(dòng)機(jī)器人的軌跡，求出移動(dòng)機(jī)器人軌跡的提 議分布，將提議分布近似為高斯分布，在高斯分布中進(jìn)行粒子采樣；
[0009] S4,計(jì)算每個(gè)粒子的權(quán)重并更新各粒子權(quán)值；
[001 0] S5，在高斯分布的基礎(chǔ)上進(jìn)行粒子重采樣，具體是通過(guò)排序粒子權(quán)重大小，分散高 權(quán)重粒子獲得重采樣后的新粒子；
[0011] S6,根據(jù)機(jī)器人位姿和觀測(cè)信息計(jì)算地圖并進(jìn)行地圖更新。
[0012] 進(jìn)一步的，步驟S2掃描方法通過(guò)廣=argmax_ /)(.v|來(lái)判斷掃描匹配是 否成功。掃描匹配方法的步驟為：
[0013] 首先由后驗(yàn)分布遞歸貝葉斯濾波器結(jié)合傳感器數(shù)據(jù)來(lái)估計(jì)機(jī)器人的位姿，形成初 始樣本集為：
[0014] - {xvx2^,.xt\
[0015] 其中為各位姿采樣集合，X1，X2, . . .，Xt代表每一次機(jī)器人可能的位姿。
[0016] 樣本集的后驗(yàn)概率密度為/)(x| ，
[0017]
[0018] 其中，為能使得該后驗(yàn)概率密度函數(shù)最大的值。
[0019] 進(jìn)一步的，步驟S3求取軌跡提議分布的計(jì)算方法為：
[0020] 首先根據(jù)觀測(cè)模型的似然函數(shù)為：
[0021] lif)
[0022] 將提議分布近似為高斯分布：
[0023]
[0024] 其高斯分布參數(shù)和Σ;"分布為：
[0025]
[0026] ' Ψ： Μ ' .......
[0027] 其中，L(1)是觀測(cè)模型的似然函數(shù)，X是該似然函數(shù)的自變量，| 是觀測(cè) 模型的概率表達(dá)，Zt是觀測(cè)值，《g是t-i時(shí)刻的地圖％歸一化因子， 尸1
if是里程計(jì)讀數(shù)，{幻}取里程計(jì)的最近一次讀數(shù)。
[0028] 講一步的，步碟S4中毎個(gè)粒子的權(quán)雷的計(jì)筧方法為：
[0029]
[0030]對(duì)每個(gè)粒子的權(quán)重進(jìn)一步簡(jiǎn)化： 「00311
)
[0032] 其中wf表示t時(shí)刻第i個(gè)粒子的權(quán)重大小，；Om)是運(yùn)動(dòng)模型的概率表達(dá)。
[0033] 進(jìn)一步的，步驟S5中，高斯分布中進(jìn)行粒子重采樣的計(jì)算方法：
[0034]
[0035]
[0036] 其中α是調(diào)整分布程度的參數(shù)，i是進(jìn)入采樣區(qū)粒子的序列數(shù)，j是當(dāng)前新得到粒子 的序列數(shù)，Σ k表示新粒子離散程度的方差，設(shè)置方差來(lái)分散高權(quán)重粒子。
[0037]本發(fā)明的優(yōu)點(diǎn)及有益效果如下：
[0038] 本發(fā)明提供了一種在高斯分布的基礎(chǔ)上分散高權(quán)重粒子獲得重采樣后的新粒子， 保證了粒子多樣性，從而避免粒子匱乏而造成柵格地圖構(gòu)建不精確。在粒子數(shù)減少的條件 下保持可靠的估計(jì)，有效地減輕了計(jì)算壓力。
【附圖說(shuō)明】
[0039] 圖1是本發(fā)明提供優(yōu)選實(shí)施例基于高斯分布的移動(dòng)機(jī)器人同步定位與地圖構(gòu)建方 法流程圖。
【具體實(shí)施方式】
[0040] 以下結(jié)合附圖，對(duì)本發(fā)明作進(jìn)一步說(shuō)明：
[0041] 如圖1所示，本發(fā)明提供了一種基于高斯分布的移動(dòng)機(jī)器人同步定位與地圖構(gòu)建， 其特征在于，包括以下步驟：
[0042] S1，輸入量為t-Ι時(shí)刻的粒子集，