同步定位與地圖構(gòu)建方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種移動(dòng)機(jī)器人導(dǎo)航領(lǐng)域，特別是涉及一種同步定位與地圖構(gòu)建方 法。
【背景技術(shù)】
[0002] 自主移動(dòng)機(jī)器人的同步定位與地圖構(gòu)建（simultaneouslocalizationand mapping，SLAM)問(wèn)題可描述為：在未知環(huán)境中，移動(dòng)機(jī)器人通過(guò)機(jī)載傳感器（如里程計(jì)、視 覺(jué)傳感器、超聲波及激光等）來(lái)感知環(huán)境信息，逐步構(gòu)建周圍環(huán)境地圖，同時(shí)運(yùn)用此地圖對(duì) 其位置和姿態(tài)進(jìn)行估計(jì)。該問(wèn)題一直是移動(dòng)機(jī)器人研究領(lǐng)域的熱點(diǎn)和難點(diǎn)，被認(rèn)為是能否 真正實(shí)現(xiàn)機(jī)器人自主導(dǎo)航的關(guān)鍵問(wèn)題，具有廣闊的應(yīng)用前景。
[0003] 現(xiàn)有解決SLAM問(wèn)題采用的方法有粒子濾波器和擴(kuò)展卡爾曼濾波器等方法，但不 能很好地表示提議分布，隨著重采樣的反復(fù)運(yùn)用，使得粒子退化嚴(yán)重，降低粒子多樣性，所 得到的2-D柵格地圖的精度不高，同時(shí)存在復(fù)雜度高、執(zhí)行效率低等問(wèn)題。

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

[0004] 本發(fā)明旨在至少解決現(xiàn)有技術(shù)中存在的技術(shù)問(wèn)題，特別創(chuàng)新地提出了一種同步定 位與地圖構(gòu)建方法。
[0005] 為了實(shí)現(xiàn)本發(fā)明的上述目的，本發(fā)明提供了一種同步定位與地圖構(gòu)建方法，包括 以下步驟：
[0006]S1，初始化系統(tǒng)狀態(tài)：
[0007] 當(dāng)t=0時(shí)，根據(jù)機(jī)器人運(yùn)動(dòng)模型先驗(yàn)概率p (X。）選取N個(gè)粒子，記為,i= 1，2,…，N，所述N為正整數(shù)，每個(gè)粒子對(duì)應(yīng)的權(quán)重為4,
[0008]S2,計(jì)算優(yōu)化的混合提議分布；
[0009] S3,在該提議分布中采樣粒子；
[0010] S4,計(jì)算并更新權(quán)重；
[0011]S5,計(jì)算有效粒子數(shù)Nrff，當(dāng)有效粒子數(shù)Nrff小于預(yù)先設(shè)定的閾值Nth時(shí)，進(jìn)行重采 樣；否則直接執(zhí)行步驟S6;
[0012] S6,更新地圖，返回步驟S2。
[0013] 以退火參數(shù)調(diào)控運(yùn)動(dòng)模型和觀測(cè)模型在混合提議分布中的比重，使得改進(jìn)后的提 議分布更加接近真實(shí)狀態(tài)；融入當(dāng)前最新激光傳感器觀測(cè)信息，使得重要性權(quán)重的方差較 小，減少所需粒子數(shù)，并保持粒子多樣性，在不同環(huán)境下能在線創(chuàng)建高精度的2-D柵格地 圖。
[0014] 在本發(fā)明的一種優(yōu)選實(shí)施方式中，混合提議分布的計(jì)算方法為：
[0015] 利用退火參數(shù)α來(lái)優(yōu)化控制里程計(jì)運(yùn)動(dòng)模型J和觀測(cè)模型 扒之間的比例，此時(shí)，混合提議分布的表示形式為：
[0016]
[0017] 其中α為退火參數(shù)，取值范圍[0, 1] 表示第i個(gè)粒子在t時(shí)刻的 狀態(tài)由該粒子在t-Ι時(shí)刻的狀態(tài)和t-Ι時(shí)刻的里程計(jì)的控制輸入u決定；jm丨表 示在t時(shí)刻的觀測(cè)信息由第i個(gè)粒子在t-Ι時(shí)刻的攜帶的地圖信息和該粒子在t時(shí)刻的狀 態(tài)決定。
[0018] 融入當(dāng)前最新激光測(cè)距儀的觀測(cè)信息，以退火參數(shù)調(diào)控結(jié)合里程計(jì)運(yùn)動(dòng)模型和觀 測(cè)模型在混合提議分布中的比重，使提議分布更加接近真實(shí)分布，有效地降低粒子數(shù)量，并 保持粒子多樣性，大大減小計(jì)算復(fù)雜度。
[0019] 在本發(fā)明的一種更加優(yōu)選實(shí)施方式中，當(dāng)提議分布由里程計(jì)運(yùn)動(dòng)模型起主導(dǎo)作用 時(shí)，設(shè)置退火參數(shù)α為0.6 ;當(dāng)融入激光測(cè)距儀的觀測(cè)模型更接近真實(shí)狀態(tài)分布時(shí)，則增加 觀測(cè)模型的比率，設(shè)置α為0.02。
[0020] 通過(guò)退火參數(shù)優(yōu)化控制兩者在提議分布中的比重，當(dāng)激光傳感器的觀測(cè)信息的精 度明顯高于里程計(jì)運(yùn)動(dòng)模型時(shí)，使得盡可能多的粒子分布在觀測(cè)信息高似然的可行區(qū)域， 使粒子權(quán)重不會(huì)出現(xiàn)明顯差別，保持粒子多樣性。
[0021] 在本發(fā)明的一種優(yōu)選實(shí)施方式中，權(quán)重的計(jì)算方法為：
[0022]
[0023] 其中ρ(Λ·;(?) | )表示第i個(gè)粒子在t時(shí)刻的狀態(tài)由該粒子在t_l時(shí)刻的狀態(tài) 和t-Ι時(shí)刻的里程計(jì)的控制輸入U(xiǎn)決定；1?? 表示在t時(shí)刻的觀測(cè)信息由第i個(gè) 粒子在t-ι時(shí)刻的攜帶的地圖信息和該粒子在t時(shí)刻的狀態(tài)決定；ufil表示第i個(gè)粒子在t-i時(shí)刻的權(quán)重。
[0024] 在本發(fā)明的一種優(yōu)選實(shí)施方式中，有效粒子數(shù)Nrff的計(jì)算方法為：
[0025]
[0026] 其中N為粒子數(shù)目個(gè)數(shù)，為歸一化重要性權(quán)重，_為第i 個(gè)粒子在t時(shí)刻的權(quán)重。
[0027] 有效粒子數(shù)Nrff保證了足夠數(shù)量的粒子數(shù)來(lái)近似目標(biāo)分布，呈現(xiàn)真實(shí)的后驗(yàn)概率， 防止粒子退化，保持粒子多樣性。
[0028] 在本發(fā)明的一種優(yōu)選實(shí)施方式中，重采樣的計(jì)算方法包括以下步驟：
[0029] a.設(shè)置高閾值權(quán)重％ =-^χΗ和低閾值權(quán)重％ 所述η為大于l的正數(shù)， Ν % 根據(jù)粒子權(quán)重與門(mén)限值的大小關(guān)系將粒子分成兩部分：
[0030] I部分為高權(quán)重粒子域和低權(quán)重粒子域：
[0031]
[0032]II部分為中等權(quán)重粒子域：
[0033] ' 1=1
5
[0034] 其中PV=NH+隊(duì)表示高權(quán)重粒子域與低權(quán)重粒子域的粒子數(shù)量之和，N-PV表示中 等權(quán)重粒子域的粒子數(shù)量，xP表示第i個(gè)粒子在t時(shí)刻的狀態(tài)，wf表示第i個(gè)粒子在t時(shí) 刻的權(quán)重；
[0035]b.決定第i個(gè)粒子被復(fù)制的次數(shù)n1，對(duì)于II部分中的粒子，直接復(fù)制；對(duì)I部分 的粒子，根據(jù)權(quán)重由高到低給粒子排序，依次設(shè)定粒子的序號(hào)為1、2、3、……PV，設(shè)置每個(gè) 粒子被選擇的概率：
[0036]
[0037] 其中rank(i)為某一粒子排序后的序號(hào)，arank和βrank為系數(shù)，當(dāng)粒子數(shù)確定時(shí)， arank= 2-βrank，1 彡βrank< 2,取值為：
[0038] ^
厶：，：
[0039] 如果第i個(gè)粒子的概率小于其被選擇的概率，即p(ir= (1/NJ<p(i)，則 該粒子被舍棄；如果第i個(gè)粒子的概率大于或者等于其被選擇的概率，即P(ir= (1/ PV)多p(i)，則該粒子被復(fù)制，被復(fù)制的次數(shù)η;由以下決定：
[0040]
[0041] L:」表示舍余取整，
[0042] 如果粒子被復(fù)制的總次數(shù)小于采樣前的高權(quán)重和低權(quán)重的粒子總數(shù)，則依次增加 由最高到低權(quán)重粒子被復(fù)制的次數(shù)，使得粒子被復(fù)制的總次數(shù)等于采樣前的高權(quán)重和低權(quán) 重的粒子總數(shù)；如果粒子被復(fù)制的總次數(shù)大于采樣前的高權(quán)重和低權(quán)重的粒子總數(shù)，則依 次減少由最低到高權(quán)重粒子被復(fù)制的次數(shù)，使得粒子被復(fù)制的總次數(shù)等于采樣前的高權(quán)重 和低權(quán)重的粒子總數(shù)；
[0043] 復(fù)制后高權(quán)重的各粒子的權(quán)重為W;( *為高權(quán)重粒子的權(quán)重，叫*為對(duì) 應(yīng)各粒子增加或者減少后實(shí)際被復(fù)制的次數(shù)；其余粒子權(quán)重保持不變。
[0044] 改進(jìn)的方法能減少運(yùn)行時(shí)間，降低計(jì)算復(fù)雜度，增加算法的執(zhí)行效率；同時(shí)通過(guò)控 制權(quán)值的門(mén)限值增加粒子多樣性，緩解粒子退化，使得所建地圖更精確。
[0045] 在本發(fā)明的一種更加優(yōu)選實(shí)施方式中，Η為2,L為0. 5。
[0046] 在本發(fā)明的一種優(yōu)選實(shí)施方式中，更新地圖的計(jì)算方法為：
[0047] 根據(jù)描述機(jī)器人軌跡的粒子的位姿xf和歷史觀測(cè)信息來(lái)更新計(jì)算對(duì)應(yīng)的地 圖的后驗(yàn)概率ρΟΥ?Ο1，~)，其中為第i粒子在[1，t]時(shí)間段的狀態(tài)，Zl:t為[1，t]時(shí) 間段的觀測(cè)信息，/為第i粒子的地圖信息由第i粒子在[1，t]時(shí)間段的狀 態(tài)和[i，t]時(shí)間段的觀測(cè)信息決定。
[0048] 綜上所述，由于采用了上述技術(shù)方案，本發(fā)明的有益效果是：在相同環(huán)境下使用相 同粒子數(shù)，改進(jìn)方法提高機(jī)器人建立2-D柵格地圖的精度，增加移動(dòng)機(jī)器人SLAM的魯棒性， 保持粒子多樣性，防