[發(fā)明專(zhuān)利]一種激光建圖定位方法、裝置、存儲(chǔ)介質(zhì)及電子設(shè)備

有效

• 申請(qǐng)?zhí)枺?/td>: 202010965430.2
• 申請(qǐng)日: 2020-09-14
• 公開(kāi)（公告）號(hào): CN112068154B
• 公開(kāi)（公告）日: 2022-12-13
• 發(fā)明（設(shè)計(jì)）人: 尹靖淳;王運(yùn)志
• 申請(qǐng)（專(zhuān)利權(quán)）人: 中科院軟件研究所南京軟件技術(shù)研究院
• 主分類(lèi)號(hào): G01S17/89
• 分類(lèi)號(hào): G01S17/89;G01C21/20
• 代理公司: 北京三聚陽(yáng)光知識(shí)產(chǎn)權(quán)代理有限公司 11250
• 代理人: 李靜玉

本發(fā)明公開(kāi)了一種激光建圖定位方法、裝置、存儲(chǔ)介質(zhì)及電子設(shè)備，包括采用占據(jù)柵格地圖來(lái)進(jìn)行環(huán)境表示；運(yùn)動(dòng)跟蹤線(xiàn)程中相鄰兩幀激光雷達(dá)進(jìn)行掃描匹配，計(jì)算優(yōu)化的幀間位姿運(yùn)動(dòng)矩陣；當(dāng)前幀激光雷達(dá)數(shù)據(jù)與占據(jù)柵格地圖匹配，調(diào)整幀間位姿運(yùn)動(dòng)矩陣。建圖線(xiàn)程中由新插入的關(guān)鍵幀和關(guān)鍵幀隊(duì)列構(gòu)建共視幀約束，檢測(cè)到閉環(huán)時(shí)，構(gòu)建閉環(huán)幀約束，采用增量式優(yōu)化位姿因子圖更新關(guān)鍵幀隊(duì)列位姿，實(shí)現(xiàn)全局地圖的更新。本發(fā)明通過(guò)兩次匹配，確保機(jī)器人運(yùn)動(dòng)過(guò)程中局部建圖準(zhǔn)確可靠；對(duì)于原始位姿圖非線(xiàn)性最小二乘優(yōu)化問(wèn)題的求解，將圖式結(jié)構(gòu)轉(zhuǎn)換為樹(shù)狀結(jié)構(gòu)，并將非線(xiàn)性?xún)?yōu)化問(wèn)題的求解轉(zhuǎn)換為最新幀相關(guān)的共視幀的增量式求解，大大提高計(jì)算效率，減小資源的耗費(fèi)。

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

本發(fā)明涉及定位和建圖技術(shù)領(lǐng)域，具體涉及一種激光建圖定位方法、裝置、存儲(chǔ)介質(zhì)及電子設(shè)備。

背景技術(shù)

激光SLAM(simultaneous localization and mapping，即時(shí)定位與地圖構(gòu)建)一般是指裝備平面單線(xiàn)激光雷達(dá)的輪式移動(dòng)機(jī)器人，通過(guò)運(yùn)動(dòng)來(lái)使用激光雷達(dá)掃描周?chē)覂?nèi)環(huán)境進(jìn)行建圖，同時(shí)在此地圖中估算機(jī)器人位姿的技術(shù)。所使用的傳感器除了激光雷達(dá)，還可以使用輪速里程計(jì)，慣導(dǎo)陀螺儀等。現(xiàn)有方案一般通過(guò)激光掃描匹配來(lái)計(jì)算幀間位姿變換，再通過(guò)粒子濾波進(jìn)行狀態(tài)更新迭代，或通過(guò)位姿圖優(yōu)化來(lái)消除大范圍建圖過(guò)程中的積累誤差。

粒子濾波方法采用一系列的粒子來(lái)表示機(jī)器人的位姿和地圖信息，通過(guò)里程計(jì)信息作為運(yùn)動(dòng)先驗(yàn)和掃描匹配激光到地圖上的匹配度來(lái)推算概率分布函數(shù)(proposaldistribution)。同時(shí)對(duì)粒子根據(jù)激光匹配度來(lái)進(jìn)行權(quán)重系數(shù)的評(píng)估，再根據(jù)這些評(píng)估權(quán)重系統(tǒng)的大小，來(lái)對(duì)于粒子進(jìn)行篩選，確保雷達(dá)數(shù)據(jù)與地圖匹配好的粒子能夠在多次迭代篩選中大概率的存活下來(lái)，從而完成建圖。該方法不足之處在于無(wú)法消除建圖過(guò)程中的累計(jì)誤差，且通過(guò)激光掃描匹配強(qiáng)依賴(lài)于里程計(jì)信息。一旦里程計(jì)信息失效，如出現(xiàn)輪子打滑，空轉(zhuǎn)，胎壓不穩(wěn)，或者機(jī)器人被意外推動(dòng)，挪移，就會(huì)出現(xiàn)建圖花掉的情形。

位姿圖優(yōu)化方法在局部通過(guò)掃描匹配來(lái)計(jì)算幀間位姿，使用的是相關(guān)度匹配和基于高斯牛頓的最小化地圖灰度差的方法；全局通過(guò)優(yōu)化整個(gè)位姿圖來(lái)優(yōu)化整體軌跡，閉環(huán)檢測(cè)使用分支定界加速的相關(guān)度匹配和基于高斯牛頓的最小化地圖灰度差的方法來(lái)構(gòu)造位姿圖的閉環(huán)運(yùn)動(dòng)約束。該方案可以單獨(dú)直接使用雷達(dá)數(shù)據(jù)(主要對(duì)于雷達(dá)數(shù)據(jù)源質(zhì)量較高或機(jī)器人運(yùn)動(dòng)速度較低情形)，也可以使用輪速里程計(jì)及/或慣導(dǎo)來(lái)提供運(yùn)動(dòng)先驗(yàn)，同時(shí)使用雷達(dá)數(shù)據(jù)。不足之處在于一方面幀間匹配依然存在錯(cuò)誤估算的情況，對(duì)于邊建圖邊導(dǎo)航的探索模式中容易導(dǎo)致運(yùn)動(dòng)規(guī)劃控制無(wú)法正常進(jìn)行；另一方面全局位姿圖優(yōu)化過(guò)程中所使用的加速分支定界來(lái)根據(jù)相關(guān)度匹配尋找相似子圖并計(jì)算幀間運(yùn)動(dòng)而構(gòu)建運(yùn)動(dòng)約束的位姿圖優(yōu)化方法，對(duì)于計(jì)算資源的耗費(fèi)是非常顯著的。就算使用多線(xiàn)程計(jì)算的方式，計(jì)算負(fù)載也很難降下來(lái)。

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

有鑒于此，本發(fā)明實(shí)施例提供了一種激光建圖定位方法、裝置、存儲(chǔ)介質(zhì)及電子設(shè)備，以解決現(xiàn)有技術(shù)在激光建圖定位過(guò)程中機(jī)器人的運(yùn)動(dòng)跟蹤過(guò)程抗干擾能力不夠強(qiáng)問(wèn)題，以及位姿圖優(yōu)化過(guò)程存在非線(xiàn)性?xún)?yōu)化問(wèn)題不夠高效耗時(shí)過(guò)多的問(wèn)題。

本發(fā)明提出的技術(shù)方案如下：

本發(fā)明實(shí)施例第一方面提供一種激光建圖定位方法，該方法包括：根據(jù)獲取的第一幀激光雷達(dá)數(shù)據(jù)構(gòu)建局部的占據(jù)柵格地圖；根據(jù)運(yùn)動(dòng)跟蹤線(xiàn)程中當(dāng)前幀激光雷達(dá)數(shù)據(jù)與上一幀激光雷達(dá)數(shù)據(jù)進(jìn)行匹配，計(jì)算得到優(yōu)化的幀間位姿運(yùn)動(dòng)矩陣；根據(jù)當(dāng)前幀激光雷達(dá)數(shù)據(jù)與占據(jù)柵格地圖進(jìn)行匹配，對(duì)所述優(yōu)化的幀間位姿運(yùn)動(dòng)矩陣進(jìn)行調(diào)整，得到精確的當(dāng)前幀位姿；根據(jù)當(dāng)前幀和當(dāng)前關(guān)鍵幀之間的距離更新局部地圖或重新構(gòu)建局部地圖；根據(jù)新插入的關(guān)鍵幀構(gòu)建的運(yùn)動(dòng)約束，增量式優(yōu)化更新關(guān)鍵幀隊(duì)列中各個(gè)關(guān)鍵幀的位姿，得到更新后的全局地圖。