本發(fā)明屬于三維重建領(lǐng)域，涉及多傳感器融合、計(jì)算機(jī)視覺、關(guān)鍵幀匹配、全局優(yōu)化等，特別是集成了激光雷達(dá)(lidar)和視覺-慣性里程計(jì)(vio)系統(tǒng)，具體涉及一種基于回環(huán)檢測(cè)及視覺-慣性里程計(jì)的激光雷達(dá)建圖方法。

背景技術(shù)：

1、在動(dòng)態(tài)和不確定的環(huán)境中，精確的定位和地圖構(gòu)建對(duì)于自動(dòng)駕駛車輛和高級(jí)機(jī)器人來說至關(guān)重要?；谝曈X的方法特別適合于場(chǎng)所識(shí)別，但其對(duì)初始化、光照和范圍的敏感性使得單獨(dú)使用它們來支持自主導(dǎo)航系統(tǒng)時(shí)變得不可靠；基于激光雷達(dá)(lidar)和慣性測(cè)量單元(imu)的方法備受青睞，但它們對(duì)初始姿態(tài)估計(jì)的依賴、累積誤差的快速增長(zhǎng)以及缺乏外部環(huán)境參照點(diǎn)的問題使得僅依賴lidar和imu進(jìn)行長(zhǎng)期自主導(dǎo)航顯得不夠可靠。視覺-慣性里程計(jì)(vio)利用相機(jī)來估計(jì)設(shè)備的運(yùn)動(dòng)，但受限于相機(jī)視野和光照條件，而激光雷達(dá)提供了高精度的距離測(cè)量，但缺乏用于估計(jì)設(shè)備姿態(tài)的足夠信息。為了克服這些限制，一些系統(tǒng)開始將vio與激光雷達(dá)數(shù)據(jù)結(jié)合，以獲得更準(zhǔn)確和魯棒的導(dǎo)航結(jié)果。回環(huán)檢測(cè)方法通過有效糾正累積的定位誤差，提高長(zhǎng)期導(dǎo)航的準(zhǔn)確性。當(dāng)系統(tǒng)識(shí)別到當(dāng)前位置與之前訪問過的地點(diǎn)相匹配時(shí)，回環(huán)檢測(cè)通過調(diào)整之前的軌跡估計(jì)來消除由于傳感器噪聲、環(huán)境變化或動(dòng)態(tài)障礙物引起的誤差，特別適用于復(fù)雜或大規(guī)模的環(huán)境中，能顯著提升地圖的一致性和整體導(dǎo)航系統(tǒng)的魯棒性，增強(qiáng)了系統(tǒng)對(duì)環(huán)境變化的適應(yīng)性，使其在面對(duì)長(zhǎng)時(shí)間運(yùn)行和不斷變化的場(chǎng)景時(shí)能夠維持高水平的定位精度。

2、在現(xiàn)有技術(shù)中存在著包括傳感器誤差累積、環(huán)境變化以及動(dòng)態(tài)障礙物引起的定位誤差等問題。這些問題會(huì)影響自主導(dǎo)航系統(tǒng)的準(zhǔn)確性和魯棒性，而回環(huán)檢測(cè)能夠幫助系統(tǒng)在面對(duì)這些挑戰(zhàn)時(shí)進(jìn)行有效的修正，從而提高導(dǎo)航系統(tǒng)的性能和可靠性。

技術(shù)實(shí)現(xiàn)思路

1、針對(duì)現(xiàn)有技術(shù)中存在的不足，本發(fā)明提供了一種基于回環(huán)檢測(cè)及視覺-慣性里程計(jì)的激光雷達(dá)建圖方法，將視覺-慣性里程計(jì)(vio)和激光雷達(dá)數(shù)據(jù)與回環(huán)檢測(cè)相結(jié)合。通過綜合利用vio對(duì)環(huán)境的豐富感知、激光雷達(dá)提供的高精度距離測(cè)量以及回環(huán)檢測(cè)的糾正功能，我們的方法能夠顯著提高自主導(dǎo)航系統(tǒng)的準(zhǔn)確性和魯棒性。在動(dòng)態(tài)和不確定的環(huán)境中，該方法能夠有效地解決傳感器誤差累積、環(huán)境變化以及動(dòng)態(tài)障礙物引起的定位誤差等技術(shù)問題，為自動(dòng)駕駛車輛和高級(jí)機(jī)器人在復(fù)雜環(huán)境中的導(dǎo)航提供了更可靠和有效的解決方案。

2、本發(fā)明利用現(xiàn)有的消費(fèi)級(jí)realsense相機(jī)和livox雷達(dá)，以點(diǎn)云圖、深度圖像及慣性數(shù)據(jù)作為輸入，設(shè)計(jì)了一種基于回環(huán)檢測(cè)及視覺-慣性里程計(jì)的激光雷達(dá)建圖方法。在長(zhǎng)時(shí)間運(yùn)行的三維建圖系統(tǒng)中，由于傳感器的噪聲和慣性測(cè)量單元的漂移，系統(tǒng)存在累積誤差，這將導(dǎo)致系統(tǒng)定位和姿態(tài)的準(zhǔn)確性下降。為了解決這個(gè)問題，我們?cè)诩す饫走_(dá)建圖過程中采用回環(huán)檢測(cè)技術(shù)，在建圖過程中選定激光雷達(dá)關(guān)鍵幀，當(dāng)系統(tǒng)檢測(cè)到當(dāng)前掃描幀與歷史激光雷達(dá)關(guān)鍵幀之間存在潛在匹配時(shí)，即觸發(fā)回環(huán)檢測(cè)，對(duì)這兩個(gè)時(shí)間點(diǎn)之間的軌跡進(jìn)行調(diào)整實(shí)現(xiàn)對(duì)由于傳感器噪聲、環(huán)境變化或動(dòng)態(tài)障礙物引起的誤差的消除。激光雷達(dá)慣性里程計(jì)主要提供距離測(cè)量，但對(duì)于環(huán)境的顏色、紋理等細(xì)節(jié)信息的感知能力相對(duì)有限，同時(shí)由于對(duì)初始姿態(tài)估計(jì)的依賴、累積誤差的快速增長(zhǎng)以及缺乏外部環(huán)境參照點(diǎn)的問題，將嚴(yán)重的影響地圖的完整性和地圖的可視化友好性。為了解決這個(gè)問題，我們采用視覺-慣性里程計(jì)數(shù)據(jù)的姿態(tài)優(yōu)化方法。利用視覺-慣性里程計(jì)提供的數(shù)據(jù)，使用姿態(tài)估計(jì)算法來估計(jì)設(shè)備的當(dāng)前位姿，并用姿態(tài)優(yōu)化算法對(duì)估計(jì)得到的姿態(tài)進(jìn)行優(yōu)化。

3、系統(tǒng)概述如附圖1所示

4、本發(fā)明綜合利用激光雷達(dá)、imu和視覺-慣性里程計(jì)等傳感器數(shù)據(jù)，以估計(jì)機(jī)器人的狀態(tài)和軌跡。激光雷達(dá)負(fù)責(zé)構(gòu)建全局地圖的幾何結(jié)構(gòu)，通過對(duì)輸入的lidar掃描進(jìn)行注冊(cè)，并通過最小化點(diǎn)-平面殘差來估計(jì)機(jī)器人的狀態(tài)。視覺-慣性里程計(jì)(vio)則負(fù)責(zé)構(gòu)建地圖的紋理，它利用輸入圖像呈現(xiàn)每個(gè)點(diǎn)的rgb顏色，并通過最小化幀到幀的pnp重投影誤差和幀到地圖的光度誤差來更新系統(tǒng)狀態(tài)。同時(shí)，本發(fā)明還集成了回環(huán)檢測(cè)功能，以進(jìn)一步提高狀態(tài)和軌跡的估計(jì)精度。

5、本發(fā)明具體實(shí)施步驟如下：

6、步驟1：選定關(guān)鍵幀；

7、當(dāng)激光雷達(dá)掃描到達(dá)時(shí)，首先進(jìn)行特征提取。通過評(píng)估局部區(qū)域上點(diǎn)的粗糙度來提取邊緣和平面特征。將時(shí)刻i激光雷達(dá)掃描中提取的邊緣和平面特征分別表示為和在時(shí)刻i提取的所有特征組成一個(gè)激光雷達(dá)幀fi。當(dāng)機(jī)器人姿態(tài)的變化超過用戶定義的閾值時(shí)，獲取一個(gè)新的機(jī)器人狀態(tài)x。當(dāng)時(shí)刻i+1時(shí)機(jī)器人姿態(tài)的變化與之前的狀態(tài)xi相比超過任意一項(xiàng)用戶定義的閾值時(shí)，選擇激光雷達(dá)幀fi+1作為關(guān)鍵幀。新保存的關(guān)鍵幀fi+1與新的機(jī)器人狀態(tài)xi+1相關(guān)聯(lián)。兩個(gè)關(guān)鍵幀fi和fi+1之間的激光雷達(dá)幀被丟棄。所述的用戶定義的閾值包括機(jī)器人位置閾值和機(jī)器人旋轉(zhuǎn)變化閾值。

8、進(jìn)一步的，所述的機(jī)器人位置閾值和機(jī)器人旋轉(zhuǎn)變化閾值分別選擇為1m和10°。

9、步驟2：掃描幀匹配；

10、當(dāng)一個(gè)新的機(jī)器人狀態(tài)xi+1被獲取時(shí)，首先搜索歷史關(guān)鍵幀，找到在歐氏空間中接近xi+1的先驗(yàn)狀態(tài)。該先驗(yàn)狀態(tài)提供一個(gè)初始估計(jì)，以便對(duì)當(dāng)前機(jī)器人狀態(tài)進(jìn)行更準(zhǔn)確的定位。

11、步驟3：觸發(fā)回環(huán)檢測(cè)；

12、如果檢測(cè)到當(dāng)前掃描幀與某個(gè)歷史關(guān)鍵幀之間存在潛在匹配，即當(dāng)前掃描幀的機(jī)器人狀態(tài)和歷史關(guān)鍵幀的機(jī)器人狀態(tài)的變化小于設(shè)定的匹配閾值時(shí)，觸發(fā)回環(huán)檢測(cè)。

13、步驟4：掃描匹配和軌跡調(diào)整；

14、當(dāng)觸發(fā)回環(huán)檢測(cè)時(shí)，對(duì)當(dāng)前掃描幀與匹配的歷史關(guān)鍵幀之間的軌跡進(jìn)行掃描匹配，利用激光雷達(dá)數(shù)據(jù)進(jìn)行點(diǎn)云配準(zhǔn)，以估計(jì)兩個(gè)幀對(duì)應(yīng)時(shí)間點(diǎn)之間的相對(duì)運(yùn)動(dòng)。這個(gè)相對(duì)運(yùn)動(dòng)被稱為回環(huán)閉合約束。

15、步驟5：誤差消除；

16、獲取相對(duì)運(yùn)動(dòng)后，使用這個(gè)信息來調(diào)整當(dāng)前掃描幀與匹配的歷史關(guān)鍵幀之間的軌跡，以減小由于傳感器噪聲、環(huán)境變化或動(dòng)態(tài)障礙物引起的誤差。具體地，利用相對(duì)運(yùn)動(dòng)信息來更新軌跡，使得更新后的軌跡與獲取的相對(duì)運(yùn)動(dòng)更加一致。

17、步驟6：視覺-慣性里程計(jì)的姿態(tài)優(yōu)化；

18、在進(jìn)行回環(huán)檢測(cè)和點(diǎn)云配準(zhǔn)后，通過視覺-慣性里程計(jì)(vio)渲染全局地圖的紋理，并通過最小化光度誤差來優(yōu)化機(jī)器人狀態(tài)。具體來說，將關(guān)鍵幀從全局地圖投影到對(duì)應(yīng)時(shí)刻的圖像中，然后在擴(kuò)展卡爾曼濾波(esikf)框架內(nèi)通過最小化地圖點(diǎn)的光度誤差迭代地優(yōu)化機(jī)器人狀態(tài)。光度誤差利用單個(gè)地圖點(diǎn)的顏色計(jì)算得到。為了確保魯棒和快速的收斂，首先利用幀間光流跟蹤地圖點(diǎn)，并通過最小化被跟蹤地圖點(diǎn)的透視投影誤差來優(yōu)化系統(tǒng)的狀態(tài)。然后，通過最小化被跟蹤地圖點(diǎn)之間的幀到地圖光度誤差來進(jìn)一步優(yōu)化系統(tǒng)的狀態(tài)估計(jì)。

19、本發(fā)明有益效果如下：

20、相對(duì)于現(xiàn)有技術(shù)，本發(fā)明方法將視覺慣性里程計(jì)(vio)、激光雷達(dá)數(shù)據(jù)和回環(huán)檢測(cè)三者相結(jié)合，以彌補(bǔ)各自單獨(dú)使用時(shí)的局限性。通過在導(dǎo)航系統(tǒng)中引入回環(huán)檢測(cè)功能，我們能夠有效地糾正傳感器誤差累積、環(huán)境變化以及動(dòng)態(tài)障礙物引起的定位誤差，提高了長(zhǎng)期導(dǎo)航的準(zhǔn)確性和魯棒性。這種綜合的方法能夠在不同環(huán)境條件下獲取更準(zhǔn)確的位置信息，提高導(dǎo)航系統(tǒng)的定位精度，并使其更具魯棒性，能夠適應(yīng)各種復(fù)雜環(huán)境，避免了長(zhǎng)時(shí)間運(yùn)行中導(dǎo)航系統(tǒng)性能下降的問題，為自動(dòng)駕駛車輛和高級(jí)機(jī)器人的導(dǎo)航提供了更可靠和有效的解決方案。