t-Ι時刻的觀測值和t-Ι時刻施加的控制量，由機 器人位姿xf丨和里程計控制信息uh估計機器人初始位姿xf:) = 。
[0043] S2，根據(jù)地圖執(zhí)行掃描匹配方法，通過x := _arg maxB | .??，為>)來判斷掃描匹 配是否成功;掃描匹配方法的步驟為：
[0044] 首先由后驗分布遞歸貝葉斯濾波器結(jié)合傳感器數(shù)據(jù)來估計機器人的位姿，形成初 始樣本集為：
[0045] x(r, Ι Ε 0046] 其中為各位姿采樣集合，Χ1，Χ2, . . .，xt代表每一次機器人可能的位姿。
[0047] 樣太隼的后駘概率密庠為從1|冊4.5,〇，
[0048]
[0049] 其中，為能使得該后驗概率密度函數(shù)最大的值。
[0050] S3,在軌跡的提議分布中進行粒子采樣;在本實施方式中，具體計算方法為：
[0051 ]觀測模型的似然函數(shù)為：
[0052] Lii：) = {x\p{zl\mi；_\,x)>￡}
[0053] 將提議分布近似為高斯分布：
[0054] 祕 Α.+t+iv' '
[0055] 其高斯分布參數(shù)wf和Σ丨"分布為：
>1 /H[0058] 其中，L(1)是觀測模型的似然函數(shù)，X是該似然函數(shù)的自變量，是觀測
[0056]
[0057] 模型的概率表達，zt是觀測值，丨是t-i時刻的地目
）為歸一化因子， if是里程計讀數(shù)，{幻}取里程計的最近一次讀數(shù)。
[0061 ] 可對上式進一步簡化：[0062]
[0059] S4,計算完步驟S3中涉及的提議分布之后，在本實施方式中，每個粒子的權(quán)重的計 算方法+-
[0060]
[0063]其中#表示t時刻第i個粒子的權(quán)重大小，|·,Mg)是運動模型的概率表達。
[0064] S5,對S4每個粒子權(quán)重大小計算之后，在本實施方式中，在高斯分布的基礎(chǔ)上進行 粒子重采樣，具體是通過排序粒子權(quán)重大小，分散高權(quán)重粒子獲得重采樣后的新粒子，在高 斯分布中進行粒子重采樣的計算方法：
[0065]
[0066]
[0067]其中α是調(diào)整分布程度的參數(shù)，i是進入采樣區(qū)粒子的序列數(shù)，j是當前新得到粒子 的序列數(shù)，Σ k表示新粒子離散程度的方差，設(shè)置方差來分散高權(quán)重粒子。
[0068] S6,根據(jù)機器人位姿和觀測信息計算地圖并進行地圖更新。
[0069] 以上這些實施例應(yīng)理解為僅用于說明本發(fā)明而不用于限制本發(fā)明的保護范圍。在 閱讀了本發(fā)明的記載的內(nèi)容之后，技術(shù)人員可以對本發(fā)明作各種改動或修改，這些等效變 化和修飾同樣落入本發(fā)明權(quán)利要求所限定的范圍。
【主權(quán)項】
1. 一種基于高斯分布的移動機器人同步定位與地圖構(gòu)建方法，其特征在于，包括W下 步驟： 51, 輸入t-1時刻的粒子集，其中移動機器人在每個點的運行軌跡為粒子，移動機器人 t-1時刻的觀測值和t-1時刻施加的控制量，由機器人位姿和里程計控制信息估計機器人初 始位姿； 52, 根據(jù)移動機器人的初始位姿和傳感器觀測數(shù)據(jù)創(chuàng)建地圖，并根據(jù)地圖執(zhí)行掃描匹 配方法得出預(yù)估的移動機器人的軌跡； 53, 根據(jù)步驟S2掃描匹配方法得出移動機器人的軌跡，求出移動機器人軌跡的提議分 布，將提議分布近似為高斯分布，在高斯分布中進行粒子采樣； S4，計算每個粒子的權(quán)重并更新各粒子權(quán)值； S5，在高斯分布的基礎(chǔ)上進行粒子重采樣，具體是通過排序粒子權(quán)重大小，分散高權(quán)重 粒子獲得重采樣后的新粒子； S6,根據(jù)機器人位姿和觀測信息計算地圖并進行地圖更新。2. 根據(jù)權(quán)利要求1所述的一種基于高斯分布的移動機器人同步定位與地圖構(gòu)建方法， 其特征在于，步驟S2掃描方法通過;.坤> =arg.ffiax:.巧州.W班馬,吵）來判斷掃描匹配是否成 功，掃描匹配方法的步驟為： 首先由后驗分布遞歸貝葉斯濾波器結(jié)合傳感器數(shù)據(jù)來估計機器人的位姿，形成初始樣 本集為： X'…二林I，兩，….，A} 其中皆 > 為各位姿采樣集合，X1，X2, ...，Xt代表每一次機器人可能的位姿。 樣本集的后驗概率密度為'fW "料，2,，皆1)， 片' =Sirgmax /,(.\-|,";''|，7,，誠)) 其中，為能使得該后驗概率密度函數(shù)最大的值。3. 根據(jù)權(quán)利要求1或2所述的一種基于高斯分布的移動機器人同步定位與地圖構(gòu)建方 法，其特征在于，步驟S3求取軌跡提議分布的計算方法為： 首先根據(jù)觀測模型的似然函數(shù)為： 公。=林 |p(Z, I 將提議分布近似為高斯分布：其高斯分布參數(shù)皆i '和巧1分布為：其中，是觀測模型的似然函數(shù)，X是該似然函數(shù)的自變量，Pb I ?媽，對是觀測模型的 概率表達，Zt是觀測值，?諧是t-1時刻的地圖，口W =|；興非趙^.)為歸一化因子，if是 M 里程計讀數(shù)，{ ^ }取里程計的最近一次讀數(shù)。4. 根據(jù)權(quán)利要求3所述的一種基于高斯分布的移動機器人同步定位與地圖構(gòu)建方法， 其特征在于，步驟S4中每個粒子的權(quán)重的計算方法為：對每個粒子的權(quán)重進一步簡化：其中 表示t時刻第i個粒子的權(quán)重大小，PCr 1進1,，《,_,)是運動模型的概率表達。5. 根據(jù)權(quán)利要求1或2所述的一種基于高斯分布的移動機器人同步定位與地圖構(gòu)建方 法，其特征在于，步驟S5中，高斯分布中進行粒子重采樣的計算方法：其中a是調(diào)整分布程度的參數(shù)，i是進入采樣區(qū)粒子的序列數(shù)，j是當前新得到粒子的序 列數(shù)，Sk表示新粒子離散程度的方差，設(shè)置方差來分散高權(quán)重粒子。
【專利摘要】本發(fā)明請求保護一種基于高斯分布重采樣Rao-Blackwellized粒子濾波的移動機器人同時定位與地圖構(gòu)建方法，該方法包括步驟：S1，由機器人位姿和里程計控制信息估計機器人初始位姿；S2，根據(jù)地圖執(zhí)行掃描匹配方法；S3，在軌跡的提議分布中進行粒子采樣；S4，計算每個粒子的權(quán)重并更新各粒子權(quán)值；S5，在高斯分布的基礎(chǔ)上進行粒子重采樣，具體是通過排序粒子權(quán)重大小，分散高權(quán)重粒子獲得重采樣后的新粒子；S6，根據(jù)機器人位姿和觀測信息計算地圖并進行地圖更新。本發(fā)明可以獲得可靠的柵格地圖精度。
【IPC分類】G01C21/32
【公開號】CN105509755
【申請?zhí)枴緾N201510847459
【發(fā)明人】張毅, 鄭瀟峰, 羅元
【申請人】重慶郵電大學(xué)
【公開日】2016年4月20日
【申請日】2015年11月27日