[發(fā)明專利]一種蛇形機(jī)器人基于激光測(cè)距儀的地圖創(chuàng)建方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201410510657.2
• 申請(qǐng)日: 2014-09-28
• 公開（公告）號(hào): CN104236551B
• 公開（公告）日: 2017-07-28
• 發(fā)明（設(shè)計(jì)）人: 王超杰;趙旭;蘇中;連曉峰
• 申請(qǐng)（專利權(quán)）人: 北京信息科技大學(xué);北京理工大學(xué)
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20
• 代理公司: 北京市合德專利事務(wù)所11244
• 代理人: 王文會(huì),劉榜美

技術(shù)領(lǐng)域

本發(fā)明屬于移動(dòng)機(jī)器人地圖創(chuàng)建技術(shù)領(lǐng)域，特別涉及一種基于激光測(cè)距儀在未知環(huán)境中的地圖創(chuàng)建方法。

背景技術(shù)

移動(dòng)機(jī)器人通常是基于環(huán)境地圖才能夠?qū)崿F(xiàn)定位和導(dǎo)航，所以地圖創(chuàng)建是研究移動(dòng)機(jī)器人的一個(gè)基本問題。SLAM問題在智能移動(dòng)機(jī)器人領(lǐng)域的重要性受到了廣泛的關(guān)注，出現(xiàn)了許多較為成熟的理論和實(shí)現(xiàn)方法。目前有很多針對(duì)SLAM問題的研究，SLAM常見的算法有EKF-SLAM算法、FastSLAM算法、DP-SLAM算法等。

基于EKF-SLAM的算法在不確定信息的表達(dá)上非常簡(jiǎn)潔和高效，它采用一個(gè)多維的高斯模型N(μ_t,∑_t)來描述機(jī)器人位姿和地圖的聯(lián)合后驗(yàn)分布，其維度為(2N+3)，N為環(huán)境特征的數(shù)目，但EKF-SLAM算法難以解決高度非線性和數(shù)據(jù)關(guān)聯(lián)問題。

FastSLAM算法運(yùn)用RBPF思想對(duì)后驗(yàn)概率進(jìn)行因式分解，將SLAM問題分解成機(jī)器人路徑估計(jì)和基于路徑估計(jì)的地圖創(chuàng)建兩個(gè)子問題，其中路徑的估計(jì)采用粒子濾波器，環(huán)境特征的估計(jì)采用EKF。其相比于傳統(tǒng)的EKF-SLAM算法，降低了算法的復(fù)雜度，效率更高，但計(jì)算量仍然比較大，程序?qū)崿F(xiàn)復(fù)雜。

基于粒子濾波的DP-SLAM算法是通過粒子濾波保持機(jī)器人的位姿和地圖的聯(lián)合概率分布，采用的是子地圖系統(tǒng)，它為每個(gè)粒子創(chuàng)建一個(gè)子地圖，然后再進(jìn)行子地圖關(guān)聯(lián)來獲得一幅完整的環(huán)境地圖。這個(gè)算法在計(jì)算和環(huán)境地圖創(chuàng)建過程中會(huì)消耗大量的內(nèi)存。此外，該算法需要一個(gè)功能強(qiáng)大且價(jià)格昂貴的激光傳感器(SICK)。

大多數(shù)SLAM算法都是針對(duì)緩慢移動(dòng)的機(jī)器人進(jìn)行測(cè)試，速度一般不超過1米/秒。到目前為止幾乎所有SLAM算法的計(jì)算和實(shí)現(xiàn)都需要大量代碼，要理解并測(cè)試他們的算法需要花費(fèi)很大精力。

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

本發(fā)明所要解決的技術(shù)問題在于克服現(xiàn)有技術(shù)的不足，提供一種MiniSLAM算法，具有算法簡(jiǎn)單易懂、計(jì)算量少、適用于快速移動(dòng)的蛇形機(jī)器人等優(yōu)點(diǎn)。

本發(fā)明采用以下技術(shù)方案：

一種蛇形機(jī)器人基于激光測(cè)距儀的地圖創(chuàng)建方法，包括如下步驟：

步驟(1)：通過激光測(cè)距儀進(jìn)行環(huán)境感知，獲取環(huán)境信息；

步驟(2)：將當(dāng)前激光測(cè)距儀的測(cè)量值與當(dāng)前地圖進(jìn)行匹配；

步驟(3)：對(duì)蛇形機(jī)器人粒子集的粒子位姿及權(quán)值進(jìn)行更新；

步驟(4)：進(jìn)行地圖更新。

步驟(1)中，所述激光測(cè)距儀為測(cè)距范圍20mm到4000mm的URG04-LX激光測(cè)距儀，該激光測(cè)距儀的最大測(cè)量距離小于4m；采用閾值濾波方法對(duì)掃描的激光數(shù)據(jù)進(jìn)行預(yù)處理濾除噪聲點(diǎn)；并采用一個(gè)恒定的縱向和轉(zhuǎn)速值對(duì)每次激光掃描值進(jìn)行糾正。所述采用恒定的縱向和轉(zhuǎn)速值對(duì)激光數(shù)據(jù)糾正并非本領(lǐng)域技術(shù)人員的常規(guī)技術(shù)實(shí)踐，本申請(qǐng)是在蛇形機(jī)器人建圖的過程中對(duì)蛇形機(jī)器人的運(yùn)動(dòng)步態(tài)進(jìn)行建模，分析運(yùn)動(dòng)步態(tài)對(duì)激光數(shù)據(jù)及建圖的影響，創(chuàng)造性的采用恒定的縱向和轉(zhuǎn)速值對(duì)激光數(shù)據(jù)進(jìn)行糾正。

進(jìn)一步的，步驟(2)中，所述激光測(cè)距儀每一次掃描完后，會(huì)將觀測(cè)值與當(dāng)前地圖中的特征點(diǎn)進(jìn)行匹配；當(dāng)與環(huán)境中的特征點(diǎn)較遠(yuǎn)時(shí)，依據(jù)最近鄰數(shù)據(jù)關(guān)聯(lián)準(zhǔn)則，選取距離觀測(cè)值最近的環(huán)境路標(biāo)作為其匹配點(diǎn)；環(huán)境特征點(diǎn)間的距離相距越遠(yuǎn)，誤匹配率越低，當(dāng)誤匹配率小于最大匹配誤差閾值時(shí)，認(rèn)為匹配成功。

進(jìn)一步的，步驟(3)所述的對(duì)蛇形機(jī)器人粒子集的粒子位姿及權(quán)值進(jìn)行更新，采用的是Monte-Carlo算法；每次蛇形機(jī)器人感知完環(huán)境后這些粒子會(huì)重新采樣；算法結(jié)束時(shí)，粒子會(huì)收斂于蛇形機(jī)器人的實(shí)際位置；在每個(gè)時(shí)刻，由初始狀態(tài)和到當(dāng)前時(shí)刻所有的測(cè)量值Z^k＝{z_i,i＝1...k}估計(jì)當(dāng)前蛇形機(jī)器人的狀態(tài)。

進(jìn)一步的，所述Monte-Carlo算法包含3個(gè)階段：

預(yù)測(cè)階段：預(yù)測(cè)階段用運(yùn)動(dòng)模型以概率密度函數(shù)的形式來預(yù)測(cè)當(dāng)前蛇形機(jī)器人的位姿；假設(shè)當(dāng)前的狀態(tài)x_k僅依賴于之前的狀態(tài)x_k-1和已知的控制輸入u_k-1，該運(yùn)動(dòng)模型被認(rèn)定為條件密度p(x_k|x_k-1,u_k-1)，對(duì)于一階馬爾可夫過程，先驗(yàn)概率密度就可以通過積分得到