專利名稱：移動機器人的粒子定位方法及其裝置的制作方法
移動機器人的粒子定位方法及其裝置
技術(shù)領(lǐng)域：
本發(fā)明涉及概率推理、智能系統(tǒng)和機器人控制領(lǐng)域，尤其涉及智能移動機器人在 探測未知環(huán)境中的即時定位與地圖創(chuàng)建技術(shù)。
背景技術(shù)：
目前智能移動機器人研究領(lǐng)域的即時定位與地圖創(chuàng)建算法主要采用概率估計算 法，如卡爾曼濾波器(KF)、擴展卡爾曼濾波器(EKF)、無跡卡爾曼濾波器(UKF)、最大似然估 計(MLE)、粒子濾波器(PF)、Rao-Blackwellized粒子濾波器(RBPF)、Markov定位算法以 及高斯濾波等。KF和EKF方法是環(huán)境建模領(lǐng)域的經(jīng)典算法，其最大的優(yōu)點是能夠在線估計 地圖中所有元素的后驗概率，該方法意義明確、簡潔易實現(xiàn)。MLE是目前解決數(shù)據(jù)關(guān)聯(lián)問 題的最佳方案，Thrun等提出能夠適應(yīng)任意噪聲分布、易實現(xiàn)的Monte Carlo定位(MCL)， 其算法關(guān)鍵是用一組加權(quán)的樣本(或稱之為“粒子”)表示待估計狀態(tài)(機器人位置或/ 及路標(biāo)位置)的后驗概率分布。后來眾多學(xué)者提出如UPF(UnSCented ParticleFilter), APF(Adaptive PF)、均取得了一定效果。Murphy、Montemerlo等人基于馬爾可夫理論與擴 展卡爾曼濾波及粒子濾波算法相結(jié)合的方法，進行移動機器人的定位與建圖研究，并將基 于 RBPF(Rao-Blackwellized Particle Filter)的 FastSLAM 算法(包括 FastSLAMl. O， hstSLAM2. O算法)應(yīng)用于維多利亞公園的地圖創(chuàng)建和定位。粒子濾波器適用于任何能用 狀態(tài)空間模型以及傳統(tǒng)的卡爾曼濾波表示的非線性系統(tǒng)，精度可以逼近最優(yōu)估計，所以受 到高度重視?，F(xiàn)有技術(shù)的缺點在于(1)對于KF和EKF方法，隨著環(huán)境特征的增加，協(xié)方差矩陣中的元素成平方級的增 加，造成沉重的運算負(fù)擔(dān)。KF/EKF最大的缺陷就是假設(shè)系統(tǒng)中的不確定性符合高斯分布，因 此對系統(tǒng)中的多模分布無能為力。雖然UKF方法提高了 EKF的性能，但UKF仍存在很多約 束，起始位姿要求已知是高斯分布的，非高斯分布的運動和觀測模型會產(chǎn)生問題，因為噪聲 也不一定是高斯分布的。(2)MLE必須花費大量的時間維護和搜索若干步子地圖，因而無法像KF那樣增量 式建立地圖，不適合在線應(yīng)用。(3)粒子濾波存在計算量與存儲量大以及無法避免的粒子多樣性退化 (Degeneracy)問題，重采樣在一定程度上可以減小退化，但卻帶來了粒子耗盡(Sample Impoverishment)問題。

發(fā)明內(nèi)容本發(fā)明所要解決的技術(shù)問題是，提供一種移動機器人的粒子定位方法及其裝置， 能夠減少了對非線性模型的線性化所引起的信息丟失和誤差增加，降低了粒子定位算法的 復(fù)雜度，有效防止粒子重采樣的退化與耗盡問題，提高算法的實時在線運行速度和準(zhǔn)確度。為了解決上述問題，本發(fā)明提供了一種移動機器人的粒子定位方法，包括如下步驟(a)以無跡卡爾曼濾波算法進行路標(biāo)估計；(b)獲得多個采樣粒子的近似分布定位信 息，并采用無跡卡爾曼濾波算法進行輔助定位；(C)對采樣的粒子進行基于節(jié)點內(nèi)的遺傳 算法優(yōu)化。作為可選的技術(shù)方案，所述步驟(b)進一步包括(bl)定位通過采樣獲得多個采 樣粒子；( )計算每個粒子的分布函數(shù)；(b!3)采用無跡卡爾曼濾波的方法獲得粒子的建議 分布函數(shù)；(b4)當(dāng)采用粒子的分布函數(shù)與采用無跡卡爾曼濾波方法獲得的建議分布函數(shù) 計算的粒子定位距離超過預(yù)設(shè)值，則對粒子進行重新采樣，并重復(fù)上述步驟。作為可選的技術(shù)方案，所述預(yù)設(shè)值由本方法的實施者自行確定。本發(fā)明進一步提供了一種移動機器人的粒子定位裝置，包括如下模塊路標(biāo)估計 模塊用于以無跡卡爾曼濾波算法進行路標(biāo)估計；粒子定位模塊用于獲得多個采樣粒子 的近似分布定位信息，并采用無跡卡爾曼濾波算法進行輔助定位；定位優(yōu)化模塊用于對 采樣的粒子進行基于節(jié)點內(nèi)的遺傳算法優(yōu)化。作為可選的技術(shù)方案，所述粒子定位模塊進一步包括定位單元用于通過采樣 獲得多個采樣粒子；分布函數(shù)計算單元用于計算每個粒子的分布函數(shù)；建議分布函數(shù)計 算單元用于采用無跡卡爾曼濾波的方法獲得粒子的建議分布函數(shù)；比較檢驗單元用于 當(dāng)采用粒子的分布函數(shù)與采用無跡卡爾曼濾波方法獲得的建議分布函數(shù)計算的粒子定位 距離超過預(yù)設(shè)值，則對粒子進行重新采樣，并重復(fù)實施本模塊的所有單元。作為可選的技術(shù)方案，所述預(yù)設(shè)值由本裝置的使用者自行確定。本發(fā)明的優(yōu)點在于(1)在路標(biāo)估計計算中，提出以UKF算法取代基于EKF算法的路標(biāo)估計，避免了計 算龐大的雅克比矩陣推導(dǎo)，以對非線性模型的高斯近似取代對非線性模型的線性化近似， 減少有用信息數(shù)據(jù)的丟失和誤差。有效地降低了整個粒子濾波定位算法的復(fù)雜度，提高實 時在線的運算速度和準(zhǔn)確度；(2)提出了基于UKF輔助建議分布的粒子濾波定位方法，方法的具體內(nèi)容為提出 基于粒子近似分布與UKF輔助建議分布距離的概念，取預(yù)先設(shè)定的距離值作為粒子自適應(yīng) 重采樣的條件，這一閾值是衡量粒子退化的重要條件，防止粒子過度偏離真實后驗分布區(qū) 域。并且在粒子重采樣的過程中，部分粒子從UKF輔助建議分布函數(shù)中抽取的，以此提高和 彌補粒子的質(zhì)量和多樣性，使粒子向真實后驗分布區(qū)流動；(3)提出了基于拓?fù)涔?jié)點重要性權(quán)值概念，對采樣的粒子進行基于節(jié)點內(nèi)的遺傳 算法優(yōu)化，增強了粒子多樣性，防止粒子的退化。

附圖1所示為本發(fā)明所述移動機器人的粒子定位方法具體實施方式
的實施步驟 示意圖；附圖2所示為上述具體實施方式
中粒子近似分布與UKF輔助分布在實際情況中受 誤差干擾的情況示意圖；附圖3所示為上述具體實施方式
所述采樣粒子的拓?fù)湎到y(tǒng)示意圖；附圖4與附圖5所示為上述具體實施方式
的技術(shù)效果示意圖；附圖6與附圖7所示是本發(fā)明所述移動機器人的粒子定位裝置具體實施方式
的結(jié)構(gòu)示意圖。
具體實施方式下面結(jié)合附圖對本發(fā)明提供的移動機器人的粒子定位方法及其裝置的具體實施 方式做詳細(xì)說明。首先給出本發(fā)明所述移動機器人的粒子定位方法的具體實施方式
。附圖1所示是本發(fā)明所述方法的具體實施方式
的實施步驟示意圖，包括步驟 Sll,以無跡卡爾曼濾波算法進行路標(biāo)估計；步驟S12，獲得多個采樣粒子的近似分布定位 信息，并采用無跡卡爾曼濾波算法進行輔助定位；步驟S13，對采樣的粒子進行基于節(jié)點內(nèi) 的遺傳算法優(yōu)化。參考步驟S11，以無跡卡爾曼濾波算法進行路標(biāo)估計。本步驟以UKF的路標(biāo)估計算法來取代現(xiàn)有技術(shù)中的標(biāo)準(zhǔn)!^astSLAM算法中的基于 EKF的路標(biāo)估計。本步驟利用前一時刻估計得到的路標(biāo)特征的均值和方差來選取一些Sigma點，假 設(shè)k-Ι時刻的第η個路標(biāo)特征估計的均值和方差為/mk’k-l, Kk^l。
權(quán)利要求
1.一種移動機器人的粒子定位方法，其特征在于，包括如下步驟(a)以無跡卡爾曼濾波算法進行路標(biāo)估計；(b)獲得多個采樣粒子的近似分布定位信息，并采用無跡卡爾曼濾波算法進行輔助定位；(c)對采樣的粒子進行基于節(jié)點內(nèi)的遺傳算法優(yōu)化。
2.根據(jù)權(quán)利要求1所述的移動機器人的粒子定位方法，其特征在于，所述步驟(b)進一 步包括(bl)定位通過采樣獲得多個采樣粒子； (b2)計算每個粒子的分布函數(shù)；(b3)采用無跡卡爾曼濾波的方法獲得粒子的建議分布函數(shù)；(b4)當(dāng)采用粒子的分布函數(shù)與采用無跡卡爾曼濾波方法獲得的建議分布函數(shù)計算的 粒子定位距離超過預(yù)設(shè)值，則對粒子進行重新采樣，并重復(fù)上述步驟。
3.根據(jù)權(quán)利要求2所述的移動機器人的粒子定位方法，其特征在于，所述預(yù)設(shè)值由本 方法的實施者自行確定。
4.一種移動機器人的粒子定位裝置，其特征在于，包括如下模塊 路標(biāo)估計模塊用于以無跡卡爾曼濾波算法進行路標(biāo)估計；粒子定位模塊用于獲得多個采樣粒子的近似分布定位信息，并采用無跡卡爾曼濾波 算法進行輔助定位；定位優(yōu)化模塊用于對采樣的粒子進行基于節(jié)點內(nèi)的遺傳算法優(yōu)化。
5.根據(jù)權(quán)利要求4所述的移動機器人的粒子定位裝置，其特征在于，所述粒子定位模 塊進一步包括定位單元用于通過采樣獲得多個采樣粒子； 分布函數(shù)計算單元用于計算每個粒子的分布函數(shù)；建議分布函數(shù)計算單元用于采用無跡卡爾曼濾波的方法獲得粒子的建議分布函數(shù)； 比較檢驗單元用于當(dāng)采用粒子的分布函數(shù)與采用無跡卡爾曼濾波方法獲得的建議分 布函數(shù)計算的粒子定位距離超過預(yù)設(shè)值，則對粒子進行重新采樣，并重復(fù)實施本模塊的所 有單元。
6.根據(jù)權(quán)利要求5所述的移動機器人的粒子定位裝置，其特征在于，所述預(yù)設(shè)值由本 裝置的使用者自行確定。
全文摘要
本發(fā)明提供了一種移動機器人的粒子定位方法，包括如下步驟(a)以無跡卡爾曼濾波算法進行路標(biāo)估計；(b)獲得多個采樣粒子的近似分布定位信息，并采用無跡卡爾曼濾波算法進行輔助定位；(c)對采樣的粒子進行基于節(jié)點內(nèi)的遺傳算法優(yōu)化。本發(fā)明的優(yōu)點在于(1)在路標(biāo)估計計算中，提出以UKF算法取代基于EKF算法的路標(biāo)估計，避免了計算龐大的雅克比矩陣推導(dǎo)；(2)提出了基于UKF輔助建議分布的粒子濾波定位方法；(3)提出了基于拓?fù)涔?jié)點重要性權(quán)值概念，增強了粒子多樣性，防止粒子的退化。
文檔編號G01S5/00GK102103815SQ20091020128
公開日2011年6月22日 申請日期2009年12月17日 優(yōu)先權(quán)日2009年12月17日
發(fā)明者王海軍 申請人:上海電機學(xué)院