一種同時(shí)定位與地圖創(chuàng)建方法及裝置制造方法
【專(zhuān)利摘要】本申請(qǐng)?zhí)峁┮环N同時(shí)定位與地圖創(chuàng)建方法及裝置，通過(guò)對(duì)機(jī)器人的位姿初始值進(jìn)行高斯概率分布、離散近似，生成該機(jī)器人在下一時(shí)刻的多個(gè)位姿采樣，計(jì)算各個(gè)位姿采樣的權(quán)重并獲取與預(yù)先設(shè)置的采樣權(quán)重范圍相匹配的權(quán)重作為目標(biāo)權(quán)重，將與目標(biāo)權(quán)重對(duì)應(yīng)的位姿采樣確定為目標(biāo)位姿采樣，并獲取與位姿采樣相應(yīng)的感知信息，利用感知信息更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)，使得能夠更快速的實(shí)現(xiàn)收斂，以較少的粒子就能達(dá)到較高的定位精度，并且定位精度基本不受粒子數(shù)的影響。
【專(zhuān)利說(shuō)明】一種同時(shí)定位與地圖創(chuàng)建方法及裝置
【技術(shù)領(lǐng)域】
[0001]本申請(qǐng)涉及機(jī)器人定位與地圖創(chuàng)建【技術(shù)領(lǐng)域】，特別是涉及一種同時(shí)定位與地圖創(chuàng)
建方法及裝置。
【背景技術(shù)】
[0002]機(jī)器人定位和地圖創(chuàng)建問(wèn)題是移動(dòng)機(jī)器人導(dǎo)航研究中的關(guān)鍵問(wèn)題，可靠導(dǎo)航是自主機(jī)器人最基本、最重要的一項(xiàng)功能，對(duì)移動(dòng)機(jī)器人的研究有非常重要的意義。
[0003]在對(duì)機(jī)器人進(jìn)行定位和地圖創(chuàng)建的過(guò)程中主要是通過(guò)粒子濾波器來(lái)進(jìn)行計(jì)算的，粒子濾波器中的新位置粒子是從提議分布-運(yùn)動(dòng)模型中抽取的，如果提議分布和實(shí)際的后驗(yàn)分布的形狀相似，那么根據(jù)提議分布抽取的粒子在利用權(quán)重函數(shù)進(jìn)行補(bǔ)償后能夠很好的表示后驗(yàn)分布，但是由于提議分布沒(méi)有考慮當(dāng)前的感知信息，如果后驗(yàn)分布位于提議分布尾部較窄的一塊區(qū)域內(nèi)，則在權(quán)重函數(shù)取值較大區(qū)域?qū)?yīng)的采樣。且粒子濾波器往往存在早熟問(wèn)題，即在經(jīng)過(guò)若干次迭代之后，大多數(shù)的粒子權(quán)重都趨于零，從而只有少數(shù)的粒子真正對(duì)系統(tǒng)狀態(tài)的估計(jì)起作用。
[0004]現(xiàn)有技術(shù)中，為了使粒子能夠更好的表示系統(tǒng)的后驗(yàn)分布，Thrun等人提出了混合形式的提議分布。按照這種提議分布抽取的粒子能更好的表示系統(tǒng)的后驗(yàn)分布，但是這種方法使采樣階段的計(jì)算量大大增加。
[0005]為了減少粒子濾波的計(jì)算量，F(xiàn)ox提出了自適應(yīng)粒子濾波器，這種新型的粒子濾波器能夠根據(jù)系統(tǒng)狀態(tài)的不確定性自適應(yīng)的調(diào)整采樣數(shù)的多少，但是這種濾波器更容易產(chǎn)生早熟現(xiàn)象。
[0006]為了防止粒子濾波器的早熟現(xiàn)象，Milstein等人提出了基于聚類(lèi)的粒子濾波器，這種濾波器將采樣分成不同的類(lèi)，并通過(guò)保持各個(gè)類(lèi)的采樣數(shù)不變而防止采樣聚于某個(gè)局部區(qū)域，但是這種方法為了解決上述粒子濾波器存在的缺陷，失去了粒子濾波器將主要的計(jì)算量集中于系統(tǒng)狀態(tài)最有可能的區(qū)域的優(yōu)點(diǎn)。
[0007]因此，亟需一種同時(shí)定位與地圖創(chuàng)建方法及裝置，能夠更快速的實(shí)現(xiàn)收斂，以較少的粒子就能達(dá)到較高的定位精度，并且定位精度基本不受粒子數(shù)的影響。

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

[0008]有鑒于此，本申請(qǐng)實(shí)施例提供一種同時(shí)定位與地圖創(chuàng)建方法及裝置，能夠更快速的實(shí)現(xiàn)收斂，以較少的粒子就能達(dá)到較高的定位精度，并且定位精度基本不受粒子數(shù)的影響。
[0009]為了實(shí)現(xiàn)上述目的，本申請(qǐng)實(shí)施例提供的技術(shù)方案如下:
[0010]一種同時(shí)定位與地圖創(chuàng)建方法，包括:
[0011]對(duì)機(jī)器人的位姿初始值進(jìn)行高斯概率分布、離散近似，生成該機(jī)器人在下一時(shí)刻的多個(gè)位姿采樣；
[0012]分別計(jì)算各個(gè)所述位姿采樣的權(quán)重；[0013]在各個(gè)所述權(quán)重中獲取與預(yù)先設(shè)置的采樣權(quán)重范圍相匹配的權(quán)重，作為目標(biāo)權(quán)重；
[0014]將與所述目標(biāo)權(quán)重對(duì)應(yīng)的位姿采樣確定為目標(biāo)位姿采樣，獲取與所述目標(biāo)位姿采樣相應(yīng)的感知信息；
[0015]利用所述感知信息，更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)。
[0016]優(yōu)選的，還包括:接收用戶輸入的機(jī)器人的位姿初始值。
[0017]優(yōu)選的，當(dāng)利用所述感知信息，更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)之后，還包括:分別將各個(gè)所述目標(biāo)位姿采樣的值作為位姿初始值，執(zhí)行如上述權(quán)利要求1所述的同時(shí)定位與地圖創(chuàng)建方法。
[0018]優(yōu)選的，在分別計(jì)算各個(gè)所述位姿采樣的權(quán)重之前還包括:
[0019]計(jì)算當(dāng)前位姿采樣集的近似后驗(yàn)值；
[0020]根據(jù)所述近似后驗(yàn)值以及預(yù)先設(shè)置的閾值，判斷是否需要進(jìn)行重新采樣。
[0021]優(yōu)選的，還包括:將機(jī)器人的當(dāng)前感知信息融合到提議分布中。
[0022]優(yōu)選的，通過(guò)無(wú)偏變換對(duì)提議分布采樣粒子，控制所述粒子向后驗(yàn)分布的高概率區(qū)域移動(dòng)。
[0023]優(yōu)選的，所述利用所述感知信息，更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)，包括:
[0024]利用所述感知信息以及預(yù)先設(shè)置的無(wú)偏卡爾曼濾波器，更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)。
[0025]優(yōu)選的，還包括:根據(jù)所述感知信息，判斷是否需要更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)。
[0026]一種同時(shí)定位與地圖創(chuàng)建裝置，包括:位姿采樣單元、權(quán)重計(jì)算單元、重新采樣單元、感知信息獲取單元以及路標(biāo)更新單元，其中，
[0027]所述位姿采樣單元用于對(duì)機(jī)器人的位姿初始值進(jìn)行高斯概率分布、離散近似，生成該機(jī)器人在下一時(shí)刻的多個(gè)位姿米樣；
[0028]所述權(quán)重計(jì)算單元與所述位姿采樣單元相連接，用于分別計(jì)算各個(gè)所述位姿采樣的權(quán)重；
[0029]所述重新采樣單元與所述權(quán)重計(jì)算單元相連接，用于在各個(gè)所述權(quán)重中獲取與預(yù)先設(shè)置的采樣權(quán)重范圍相匹配的權(quán)重，作為目標(biāo)權(quán)重；
[0030]所述感知信息獲取單元與所述重新采樣單元相連接，用于將所述目標(biāo)權(quán)重對(duì)應(yīng)的位姿采樣確定為目標(biāo)位姿采樣，獲取與所述目標(biāo)位姿采樣相應(yīng)的感知信息；
[0031]所述路標(biāo)更新單元與所述感知信息獲取單元相連接，用于利用所述感知信息，更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)。
[0032]優(yōu)選的,還包括:位姿初始值接收單元，
[0033]所述位姿初始值接收單元與位姿采樣單元相連接，用于接收用戶輸入的機(jī)器人的位姿初始值。
[0034]本申請(qǐng)?zhí)峁┮环N同時(shí)定位與地圖創(chuàng)建方法及裝置，通過(guò)對(duì)機(jī)器人的位姿初始值進(jìn)行高斯概率分布、離散近似，生成該機(jī)器人在下一時(shí)刻的多個(gè)位姿采樣，計(jì)算各個(gè)位姿采樣的權(quán)重并獲取與預(yù)先設(shè)置的采樣權(quán)重范圍相匹配的權(quán)重作為目標(biāo)權(quán)重，將與目標(biāo)權(quán)重對(duì)應(yīng)的位姿采樣確定為目標(biāo)位姿采樣，并獲取與位姿采樣相應(yīng)的感知信息，利用感知信息更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)，使得能夠更快速的實(shí)現(xiàn)收斂，以較少的粒子就能達(dá)到較高的定位精度，并且定位精度基本不受粒子數(shù)的影響。
【專(zhuān)利附圖】

【附圖說(shuō)明】
[0035]為了更清楚地說(shuō)明本申請(qǐng)實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對(duì)實(shí)施例或現(xiàn)有技術(shù)描述中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見(jiàn)地，下面描述中的附圖僅僅是本申請(qǐng)中記載的一些實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù)這些附圖獲得其他的附圖。 [0036]圖1為本申請(qǐng)實(shí)施例一提供的一種同時(shí)定位與地圖創(chuàng)建方法流程圖；
[0037]圖2為本申請(qǐng)實(shí)施例一提供的一種問(wèn)題的圖形模式示意圖；
[0038]圖3為本申請(qǐng)實(shí)施例二提供的一種同時(shí)定位與地圖創(chuàng)建方法流程圖；
[0039]圖4為本申請(qǐng)實(shí)施例三提供的一種同時(shí)定位與地圖創(chuàng)建裝置的結(jié)構(gòu)示意圖。
【具體實(shí)施方式】
[0040]為了使本【技術(shù)領(lǐng)域】的人員更好地理解本申請(qǐng)中的技術(shù)方案，下面將結(jié)合本申請(qǐng)實(shí)施例中的附圖，對(duì)本申請(qǐng)實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實(shí)施例僅僅是本申請(qǐng)一部分實(shí)施例，而不是全部的實(shí)施例?；诒旧暾?qǐng)中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒(méi)有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他實(shí)施例，都應(yīng)當(dāng)屬于本申請(qǐng)保護(hù)的范圍。
[0041]實(shí)施例一:
[0042]圖1為本申請(qǐng)實(shí)施例一提供的一種同時(shí)定位與地圖創(chuàng)建方法流程圖。
[0043]如圖1所示，該方法包括:
[0044]S101、對(duì)機(jī)器人的位姿初始值進(jìn)行高斯概率分布、離散近似，生成該機(jī)器人在下一時(shí)刻的多個(gè)位姿采樣。
[0045]在本申請(qǐng)實(shí)施例中，對(duì)機(jī)器人的位姿初始值進(jìn)行高斯概率分布、離散近似，生成該機(jī)器人在下一時(shí)刻的多個(gè)位姿米樣的過(guò)程如下:
[0046]在本申請(qǐng)實(shí)施例中，當(dāng)機(jī)器人穿過(guò)一個(gè)未知環(huán)境時(shí)，設(shè)t時(shí)刻機(jī)器人位姿St= [xt, yt, 0 t]T,已經(jīng)觀測(cè)到的地圖為M,其中mk表示第k個(gè)路標(biāo),K表示已經(jīng)觀測(cè)的路標(biāo)數(shù)，kte {I,-,N}表示t時(shí)刻感知到的路標(biāo)索引號(hào)。系統(tǒng)的完整狀態(tài)可以表示為xt=[s1:t，M]τ，其中Spt=S1, S2,…，St表示機(jī)器人從I到t時(shí)刻的運(yùn)動(dòng)路徑，同樣，IV1表示t-ι到t時(shí)刻的運(yùn)動(dòng)控制信息，Zt表示機(jī)器人的當(dāng)前感知信息。機(jī)器人從位姿Stl開(kāi)始通過(guò)控制命令序列U0, U1,, IV1移動(dòng)，隨著機(jī)器人的移動(dòng)，附近的路標(biāo)被感知到，時(shí)刻t = I,感知到路標(biāo)Hl1,并獲得測(cè)量數(shù)據(jù)Z1 (包括距離和方向)，時(shí)刻t = 2，感知到路標(biāo)m2，并在時(shí)刻t=3，重新感知到路標(biāo)Hi1，現(xiàn)在已經(jīng)形成的地圖為同時(shí)定位與地圖創(chuàng)建的輸入信息是，路標(biāo)觀測(cè)信息z1:t，以及運(yùn)動(dòng)控制信息同時(shí)定位與地圖創(chuàng)建的目的是，根據(jù)輸入信息估計(jì)機(jī)器人運(yùn)動(dòng)路徑s1:t以及地圖M。[0047]在本申請(qǐng)實(shí)施例中，首先將當(dāng)前的感知信息Zt加入到傳統(tǒng)提議分布p(st| Sllt-/1), Zt)中，其中，St表示t時(shí)刻機(jī)器人位姿，Sllt=S1, S2,…，St表示機(jī)器人的運(yùn)動(dòng)路徑，表示機(jī)器人的運(yùn)動(dòng)控制信息，Zt表示機(jī)器人的當(dāng)前感知信息。通過(guò)將當(dāng)前感知信息Zt加入到傳統(tǒng)提議分布中，使從先驗(yàn)分布采樣的粒子向后驗(yàn)高概率區(qū)域移動(dòng)。
[0048]應(yīng)用無(wú)偏變換計(jì)算感知變量Zt的均值I；和方差C *精度可以達(dá)到二階泰勒擴(kuò)展，
設(shè)L是St的維數(shù)，主要分為以下幾步:
[0049]
【權(quán)利要求】
1.一種同時(shí)定位與地圖創(chuàng)建方法，其特征在于，包括: 對(duì)機(jī)器人的位姿初始值進(jìn)行高斯概率分布、離散近似，生成該機(jī)器人在下一時(shí)刻的多個(gè)位姿采樣； 分別計(jì)算各個(gè)所述位姿采樣的權(quán)重； 在各個(gè)所述權(quán)重中獲取與預(yù)先設(shè)置的采樣權(quán)重范圍相匹配的權(quán)重，作為目標(biāo)權(quán)重； 將與所述目標(biāo)權(quán)重對(duì)應(yīng)的位姿采樣確定為目標(biāo)位姿采樣，獲取與所述目標(biāo)位姿采樣相應(yīng)的感知信息； 利用所述感知信息，更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)。
2.根據(jù)權(quán)利要求1所述的方法，其特征在于，還包括:接收用戶輸入的機(jī)器人的位姿初始值。
3.根據(jù)權(quán)利要求1所述的方法，其特征在于，當(dāng)利用所述感知信息，更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)之后，還包括:分別將各個(gè)所述目標(biāo)位姿采樣的值作為位姿初始值，執(zhí)行如上述權(quán)利要求1所述的同時(shí)定位與地圖創(chuàng)建方法。
4.根據(jù)權(quán)利要求1所述的方法，其特征在于，在分別計(jì)算各個(gè)所述位姿采樣的權(quán)重之前還包括: 計(jì)算當(dāng)前位姿采樣集的近似`后驗(yàn)值； 根據(jù)所述近似后驗(yàn)值以及預(yù)先設(shè)置的閾值，判斷是否需要進(jìn)行重新采樣。
5.根據(jù)權(quán)利要求1所述的方法，其特征在于，還包括:將機(jī)器人的當(dāng)前感知信息融合到提議分布中。
6.根據(jù)權(quán)利要求5所述的方法，其特征在于，通過(guò)無(wú)偏變換對(duì)提議分布采樣粒子，控制所述粒子向后驗(yàn)分布的高概率區(qū)域移動(dòng)。
7.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述利用所述感知信息，更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)，包括: 利用所述感知信息以及預(yù)先設(shè)置的無(wú)偏卡爾曼濾波器，更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)。
8.根據(jù)權(quán)利要求7所述的方法，其特征在于，還包括:根據(jù)所述感知信息，判斷是否需要更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)。
9.一種同時(shí)定位與地圖創(chuàng)建裝置，其特征在于，包括:位姿采樣單元、權(quán)重計(jì)算單元、重新采樣單元、感知信息獲取單元以及路標(biāo)更新單元，其中， 所述位姿采樣單元用于對(duì)機(jī)器人的位姿初始值進(jìn)行高斯概率分布、離散近似，生成該機(jī)器人在下一時(shí)刻的多個(gè)位姿采樣； 所述權(quán)重計(jì)算單元與所述位姿采樣單元相連接，用于分別計(jì)算各個(gè)所述位姿采樣的權(quán)重； 所述重新采樣單元與所述權(quán)重計(jì)算單元相連接，用于在各個(gè)所述權(quán)重中獲取與預(yù)先設(shè)置的采樣權(quán)重范圍相匹配的權(quán)重，作為目標(biāo)權(quán)重； 所述感知信息獲取單元與所述重新采樣單元相連接，用于將所述目標(biāo)權(quán)重對(duì)應(yīng)的位姿采樣確定為目標(biāo)位姿采樣，獲取與所述目標(biāo)位姿采樣相應(yīng)的感知信息；所述路標(biāo)更新單元與所述感知信息獲取單元相連接，用于利用所述感知信息，更新預(yù)先設(shè)定的全局地圖中與所述目標(biāo)位姿采樣相對(duì)應(yīng)的位置相關(guān)聯(lián)的路標(biāo)。
10.根據(jù)權(quán)利要求9所述的裝置，其特征在于，還包括:位姿初始值接收單元， 所述位姿初始值接收單元與位姿采樣單元相連接，用于接收用戶輸入的機(jī)器人的位姿初 始值。
【文檔編號(hào)】G05D1/02GK103631264SQ201310646196
【公開(kāi)日】2014年3月12日 申請(qǐng)日期:2013年12月4日 優(yōu)先權(quán)日:2013年12月4日
【發(fā)明者】厲茂海, 林睿, 王振華, 陳國(guó)棟, 孫榮川 申請(qǐng)人:蘇州大學(xué)張家港工業(yè)技術(shù)研究院