[發(fā)明專(zhuān)利]一種用于室內(nèi)外場(chǎng)景的人為回環(huán)即時(shí)定位和建圖方法及系統(tǒng)

在審

• 申請(qǐng)?zhí)枺?/td>: 202011411092.4
• 申請(qǐng)日: 2020-12-03
• 公開(kāi)（公告）號(hào): CN112697158A
• 公開(kāi)（公告）日: 2021-04-23
• 發(fā)明（設(shè)計(jì)）人: 朱文俊;謝亮亮;易陽(yáng);馮李航
• 申請(qǐng)（專(zhuān)利權(quán)）人: 南京工業(yè)大學(xué)
• 主分類(lèi)號(hào): G01C21/32
• 分類(lèi)號(hào): G01C21/32
• 代理公司: 南京燦爛知識(shí)產(chǎn)權(quán)代理有限公司 32356
• 代理人: 吳亞

一種用于室內(nèi)外場(chǎng)景的人為回環(huán)即時(shí)定位和建圖方法，其特征在于，包括以下步驟：步驟一，定義位姿關(guān)系SLAM初始因子圖；步驟二，采集機(jī)器人定位數(shù)據(jù)，對(duì)第i幀定位數(shù)據(jù)輸入校正因子，通過(guò)EM算法估算校正因子；步驟三，通過(guò)矩陣變換串聯(lián)機(jī)器人前后變換的位姿，優(yōu)化校正因子；步驟四，通過(guò)校正因子約束優(yōu)化機(jī)器人位姿，消除累計(jì)誤差，得到全局一致性地圖。本申請(qǐng)消除累計(jì)誤差，得到全局一致性地圖。

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

本發(fā)明設(shè)計(jì)一種用于室內(nèi)外場(chǎng)景的人為回環(huán)即時(shí)定位和建圖方法及系統(tǒng)，屬于機(jī)器人應(yīng)用技術(shù)領(lǐng)域。

背景技術(shù)

構(gòu)建大規(guī)模且全局一致的地圖一直是機(jī)器人導(dǎo)航建圖的難題所在。全局一致地圖的制定可協(xié)助機(jī)器人較為準(zhǔn)確地按照指令進(jìn)行操作，進(jìn)而降低位姿估計(jì)誤差。傳統(tǒng)SLAM(Simultaneous Localization And Mapping，SLAM)技術(shù)僅僅采用視覺(jué)里程計(jì)來(lái)判斷機(jī)器人在地圖中的坐標(biāo)，效果不理想，原因主要有兩個(gè)：一是早期的視覺(jué)里程計(jì)由編碼器實(shí)現(xiàn)，位姿估計(jì)漂移太快；二是視覺(jué)里程計(jì)原理是僅計(jì)算相鄰圖像間的位置關(guān)系，這樣就容易導(dǎo)致上一個(gè)圖像誤差引入到當(dāng)前圖像的位置關(guān)系中。機(jī)器人經(jīng)長(zhǎng)時(shí)間運(yùn)行后累積的偏差會(huì)越來(lái)越大，最終將導(dǎo)致無(wú)法建立大規(guī)模且全局一致的地圖。

為了解決上述問(wèn)題，回環(huán)檢測(cè)技術(shù)使用外觀信息檢測(cè)機(jī)器人是否回到了先前位置，以期消除累積誤差。難點(diǎn)在于一開(kāi)始的微量誤差不易被察覺(jué)，機(jī)器人繞環(huán)路一圈后因誤差積累而無(wú)法閉合環(huán)路。特別是大范圍場(chǎng)景，即使采用高精度激光雷達(dá)也難以避免。現(xiàn)有技術(shù)專(zhuān)利“利用激光SLAM在長(zhǎng)走廊建圖的方法、裝置、系統(tǒng)、存儲(chǔ)介質(zhì)CN201910595880.4”、“一種基于距離度量學(xué)習(xí)的視覺(jué)SLAM回環(huán)檢測(cè)方法CN201910575905.4”都是被動(dòng)式的算法調(diào)整，建圖過(guò)程中的誤差無(wú)法實(shí)時(shí)控制，易導(dǎo)致構(gòu)建的矢量地圖達(dá)不到預(yù)期效果。

現(xiàn)有技術(shù)專(zhuān)利“基于單鏈序貫回溯Q學(xué)習(xí)的移動(dòng)機(jī)器人路徑規(guī)劃算法CN102799179A”利用單鏈序回溯Q學(xué)習(xí)方法循環(huán)往復(fù)地從初始狀態(tài)開(kāi)始尋找到達(dá)目標(biāo)狀態(tài)的路徑，并不斷迭代和優(yōu)化狀態(tài)的Q值，直到收斂為止。該方法屬于被動(dòng)式地通過(guò)算法尋找最優(yōu)參數(shù)，并沒(méi)有能實(shí)時(shí)校準(zhǔn)機(jī)器人在每一步迭代過(guò)程中產(chǎn)生的定位誤差。

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

本發(fā)明的目的在于：針對(duì)現(xiàn)有技術(shù)的不足，提供一種用于室內(nèi)外場(chǎng)景的人為回環(huán)即時(shí)定位和建圖方法及系統(tǒng)，能實(shí)時(shí)校準(zhǔn)機(jī)器人在每一步迭代過(guò)程中產(chǎn)生的定位誤差。

本發(fā)明具體技術(shù)方案為：

一種用于室內(nèi)外場(chǎng)景的人為回環(huán)即時(shí)定位和建圖方法，其特征在于，包括以下步驟：

步驟一，定義位姿關(guān)系SLAM初始因子圖；

步驟二，采集機(jī)器人定位數(shù)據(jù)，對(duì)第i幀定位數(shù)據(jù)輸入校正因子，通過(guò)EM算法估算校正因子；

步驟三，通過(guò)矩陣變換串聯(lián)機(jī)器人前后變換的位姿，優(yōu)化校正因子；

步驟四，通過(guò)校正因子約束優(yōu)化機(jī)器人位姿，消除累計(jì)誤差，得到全局一致性地圖。

步驟一具體包括以下步驟：

定義機(jī)器人在地圖中的初始位姿及其約束關(guān)系的初始因子圖G₀＝{X,F}；其中X為機(jī)器人初始行駛軌跡(機(jī)器人初始行駛軌跡不包括后續(xù)回環(huán)過(guò)程中的行駛軌跡)上的位姿估計(jì)集合，F(xiàn)＝{R}，R表示測(cè)量值和觀測(cè)值之間的相對(duì)位姿(相對(duì)位姿約束指的是觀測(cè)值與測(cè)量值之間的相對(duì)位姿)。

步驟二具體包括以下步驟：

(201)，采集機(jī)器人定位數(shù)據(jù)，在機(jī)器人定位數(shù)據(jù)的第i幀輸入用于修正地圖(地圖指的就是機(jī)器人當(dāng)前所位于的矢量地圖，包含了機(jī)器人當(dāng)前位姿數(shù)據(jù))的校正因子；