本發(fā)明涉及機(jī)器人導(dǎo)航和環(huán)境感知定位，具體涉及一種動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法。

背景技術(shù)：

1、目前，面向動(dòng)態(tài)環(huán)境的即時(shí)定位與地圖構(gòu)建(simultaneous?localization?andmapping，slam)技術(shù)在當(dāng)下已經(jīng)得到了多個(gè)領(lǐng)域的廣泛應(yīng)用。

2、然而，傳統(tǒng)的基于單一傳感器(激光雷達(dá)或視覺(jué)相機(jī))的slam技術(shù)容易受到動(dòng)態(tài)物體的干擾、特征點(diǎn)的稀缺以及容易受到遮擋和噪聲的影響，從而導(dǎo)致定位精度下降和地圖構(gòu)建不穩(wěn)定。

3、因此，亟需一種新的方法來(lái)有效融合激光與視覺(jué)信息以提高在動(dòng)態(tài)環(huán)境下的定位與地圖構(gòu)建的精度。

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

1、有鑒于此，本發(fā)明提供了一種動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法，以解決現(xiàn)有技術(shù)在動(dòng)態(tài)環(huán)境下的定位與地圖構(gòu)建的精度不足的技術(shù)問(wèn)題。

2、第一方面，本發(fā)明提供了一種動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法，包括：分別通過(guò)激光雷達(dá)、深度視覺(jué)相機(jī)和慣性測(cè)量模組獲取周圍環(huán)境的激光點(diǎn)云數(shù)據(jù)、帶有深度信息的二維圖像數(shù)據(jù)和機(jī)器人自身的姿態(tài)信息；對(duì)激光點(diǎn)云數(shù)據(jù)和二維圖像數(shù)據(jù)進(jìn)行時(shí)間同步、坐標(biāo)對(duì)齊和數(shù)據(jù)去噪處理，得到預(yù)處理后的激光點(diǎn)云數(shù)據(jù)和二維圖像數(shù)據(jù)；提取預(yù)處理后的激光點(diǎn)云數(shù)據(jù)和預(yù)處理后二維圖像數(shù)據(jù)的特征值，并對(duì)提取到的特征值進(jìn)行匹配融合，得到同時(shí)包含圖像特征信息的融合特征點(diǎn)云數(shù)據(jù)，將特征點(diǎn)云數(shù)據(jù)和姿態(tài)信息進(jìn)行優(yōu)化，獲得包含特征點(diǎn)云數(shù)據(jù)與定位信息的融合數(shù)據(jù)；將融合數(shù)據(jù)進(jìn)行后端優(yōu)化和回環(huán)檢測(cè)，實(shí)現(xiàn)地圖構(gòu)建和自身定位。

3、可選地，時(shí)間同步的過(guò)程包括：利用精確時(shí)鐘同步協(xié)議ptp獲取主時(shí)鐘發(fā)送同步報(bào)文的第一硬件時(shí)間戳、從時(shí)鐘接收同步報(bào)文的第二硬件時(shí)間戳、從時(shí)鐘發(fā)送延遲請(qǐng)求報(bào)文的第三硬件時(shí)間戳和主時(shí)鐘接收延遲請(qǐng)求報(bào)文的第四硬件時(shí)間戳；基于第一硬件時(shí)間戳、第二硬件時(shí)間戳、第三硬件時(shí)間戳和第四硬件時(shí)間戳計(jì)算主時(shí)鐘和從時(shí)鐘的同步誤差和同步時(shí)延，其中，主時(shí)鐘為接收激光點(diǎn)云數(shù)據(jù)和二維圖像數(shù)據(jù)的處理單元的時(shí)鐘，從時(shí)鐘為激光雷達(dá)或深度視覺(jué)相機(jī)的時(shí)鐘；基于同步誤差和同步時(shí)延對(duì)激光點(diǎn)云數(shù)據(jù)和二維圖像數(shù)據(jù)進(jìn)行初步時(shí)間同步；利用卡爾曼濾波算法融合多路徑時(shí)延數(shù)據(jù)，將時(shí)間偏移建模為動(dòng)態(tài)系統(tǒng)，使用卡爾曼濾波估計(jì)采集激光雷達(dá)和深度視覺(jué)相機(jī)的時(shí)鐘的時(shí)間偏移和漂移量；基于時(shí)間偏移和漂移量對(duì)初步時(shí)間同步后的激光點(diǎn)云數(shù)據(jù)和二維圖像數(shù)據(jù)的時(shí)間進(jìn)行校正。

4、可選地，坐標(biāo)對(duì)齊的過(guò)程包括：獲取激光雷達(dá)和深度視覺(jué)相機(jī)的旋轉(zhuǎn)矩陣和平移向量；獲取深度視覺(jué)相機(jī)的內(nèi)參矩陣；根據(jù)內(nèi)參矩陣、旋轉(zhuǎn)矩陣和平移向量將激光點(diǎn)云數(shù)據(jù)從點(diǎn)云坐標(biāo)系投影到像素坐標(biāo)系中，實(shí)現(xiàn)坐標(biāo)對(duì)齊。

5、可選地，數(shù)據(jù)去噪處理的過(guò)程包括：通過(guò)曼哈頓距離公式確定激光點(diǎn)云數(shù)據(jù)的閾值距離和目標(biāo)距離，基于閾值距離和目標(biāo)距離剔除噪點(diǎn)；采用自適應(yīng)直方圖均衡化對(duì)二維圖像數(shù)據(jù)進(jìn)行圖像增強(qiáng)處理，通過(guò)高斯濾波法消除二維圖像數(shù)據(jù)的噪聲，并使用深度學(xué)習(xí)模型對(duì)二維圖像數(shù)據(jù)進(jìn)行語(yǔ)義分割，提取每個(gè)像素的語(yǔ)義標(biāo)簽。

6、可選地，預(yù)處理后的激光點(diǎn)云數(shù)據(jù)的特征值包括點(diǎn)云特征，預(yù)處理后二維圖像數(shù)據(jù)的特征值包括顏色特征和語(yǔ)義特征；對(duì)應(yīng)地，提取預(yù)處理后的激光點(diǎn)云數(shù)據(jù)和預(yù)處理后二維圖像數(shù)據(jù)的特征值，并對(duì)提取到的特征值進(jìn)行匹配融合，得到同時(shí)包含圖像特征信息的融合特征點(diǎn)云數(shù)據(jù)，包括：基于坐標(biāo)對(duì)齊結(jié)果將預(yù)處理后的激光點(diǎn)云數(shù)據(jù)中的三維點(diǎn)和預(yù)處理后二維圖像數(shù)據(jù)中的rgb像素關(guān)聯(lián)；基于關(guān)聯(lián)的rgb像素的坐標(biāo)獲取每個(gè)三維點(diǎn)的顏色特征，將顏色特征添加到激光點(diǎn)云數(shù)據(jù)的每個(gè)三維點(diǎn)中，生成彩色點(diǎn)云；基于關(guān)聯(lián)的rgb像素的坐標(biāo)獲取每個(gè)三維點(diǎn)的語(yǔ)義特征，將點(diǎn)云特征和語(yǔ)義特征進(jìn)行融合，得到融合特征點(diǎn)云數(shù)據(jù)。

7、可選地，在基于關(guān)聯(lián)的rgb像素的坐標(biāo)獲取每個(gè)三維點(diǎn)的語(yǔ)義特征，將點(diǎn)云特征和語(yǔ)義特征進(jìn)行融合，得到融合特征點(diǎn)云數(shù)據(jù)之后，還包括：基于語(yǔ)義特征獲取融合特征點(diǎn)云數(shù)據(jù)中的動(dòng)態(tài)點(diǎn)云，并剔除動(dòng)態(tài)點(diǎn)云。

8、可選地，將融合數(shù)據(jù)進(jìn)行后端優(yōu)化和回環(huán)檢測(cè)，實(shí)現(xiàn)地圖構(gòu)建和自身定位，包括：將融合數(shù)據(jù)的觀測(cè)結(jié)果構(gòu)建成因子圖；采用非線性優(yōu)化算法對(duì)因子圖進(jìn)行優(yōu)化，獲得地圖特征信息和定位信息；基于地圖特征信息和定位信息進(jìn)行回環(huán)檢測(cè)，消除漂移誤差，實(shí)現(xiàn)地圖構(gòu)建和自身定位。

9、第二方面，本發(fā)明提供了一種動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建裝置，包括：數(shù)據(jù)采集模塊，用于分別通過(guò)激光雷達(dá)、深度視覺(jué)相機(jī)和慣性測(cè)量模組獲取周圍環(huán)境的激光點(diǎn)云數(shù)據(jù)、帶有深度信息的二維圖像數(shù)據(jù)和機(jī)器人自身的姿態(tài)信息；數(shù)據(jù)預(yù)處理模塊，用于對(duì)激光點(diǎn)云數(shù)據(jù)和二維圖像數(shù)據(jù)進(jìn)行時(shí)間同步、坐標(biāo)對(duì)齊和數(shù)據(jù)去噪處理，得到預(yù)處理后的激光點(diǎn)云數(shù)據(jù)和預(yù)處理后二維圖像數(shù)據(jù)；數(shù)據(jù)融合模塊，用于提取預(yù)處理后的激光點(diǎn)云數(shù)據(jù)和預(yù)處理后二維圖像數(shù)據(jù)的特征值，并對(duì)提取到的特征值進(jìn)行匹配融合，得到同時(shí)包含圖像特征信息的融合特征點(diǎn)云數(shù)據(jù)，將特征點(diǎn)云數(shù)據(jù)和姿態(tài)信息進(jìn)行優(yōu)化，獲得包含特征點(diǎn)云數(shù)據(jù)與定位信息的融合數(shù)據(jù)；定位與建圖模塊，用于將融合數(shù)據(jù)進(jìn)行后端優(yōu)化和回環(huán)檢測(cè)，實(shí)現(xiàn)地圖構(gòu)建和自身定位。

10、第三方面，本發(fā)明提供了一種計(jì)算機(jī)設(shè)備，包括：存儲(chǔ)器和處理器，存儲(chǔ)器和處理器之間互相通信連接，存儲(chǔ)器中存儲(chǔ)有計(jì)算機(jī)指令，處理器通過(guò)執(zhí)行計(jì)算機(jī)指令，從而執(zhí)行上述第一方面或其對(duì)應(yīng)的任一實(shí)施方式的動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法。

11、第四方面，本發(fā)明提供了一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，該計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)上存儲(chǔ)有計(jì)算機(jī)指令，計(jì)算機(jī)指令用于使計(jì)算機(jī)執(zhí)行上述第一方面或其對(duì)應(yīng)的任一實(shí)施方式的動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法。

12、本發(fā)明實(shí)施例的動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法具有以下效果：

13、本發(fā)明實(shí)施例的動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法，分別通過(guò)激光雷達(dá)、深度視覺(jué)相機(jī)和慣性測(cè)量模組獲取周圍環(huán)境的激光點(diǎn)云數(shù)據(jù)、帶有深度信息的二維圖像數(shù)據(jù)和機(jī)器人自身的姿態(tài)信息，對(duì)激光點(diǎn)云數(shù)據(jù)和二維圖像數(shù)據(jù)進(jìn)行時(shí)間同步、坐標(biāo)對(duì)齊和數(shù)據(jù)去噪處理，得到預(yù)處理后的激光點(diǎn)云數(shù)據(jù)和預(yù)處理后二維圖像數(shù)據(jù)，提取預(yù)處理后的激光點(diǎn)云數(shù)據(jù)和預(yù)處理后二維圖像數(shù)據(jù)的特征值，并對(duì)提取到的特征值進(jìn)行匹配融合，得到同時(shí)包含圖像特征信息的融合特征點(diǎn)云數(shù)據(jù)，將特征點(diǎn)云數(shù)據(jù)和姿態(tài)信息進(jìn)行優(yōu)化，獲得包含特征點(diǎn)云數(shù)據(jù)與定位信息的融合數(shù)據(jù)，將融合數(shù)據(jù)進(jìn)行后端優(yōu)化和回環(huán)檢測(cè)，實(shí)現(xiàn)地圖構(gòu)建和自身定位，能夠有效提高在動(dòng)態(tài)環(huán)境中定位與建圖的準(zhǔn)確性，通過(guò)該方法獲取的環(huán)境信息準(zhǔn)確，地圖構(gòu)建質(zhì)量高，當(dāng)前已能應(yīng)用在動(dòng)態(tài)環(huán)境下機(jī)器人的導(dǎo)航以及定位技術(shù)領(lǐng)域。

技術(shù)特征：

1.一種動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法，其特征在于，包括：

2.根據(jù)權(quán)利要求1所述的動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法，其特征在于，所述時(shí)間同步的過(guò)程包括：

3.根據(jù)權(quán)利要求1所述的動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法，其特征在于，所述坐標(biāo)對(duì)齊的過(guò)程包括：

4.根據(jù)權(quán)利要求1所述的動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法，其特征在于，所述數(shù)據(jù)去噪處理的過(guò)程包括：

5.根據(jù)權(quán)利要求1所述的動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法，其特征在于，預(yù)處理后的所述激光點(diǎn)云數(shù)據(jù)的特征值包括點(diǎn)云特征，預(yù)處理后所述二維圖像數(shù)據(jù)的特征值包括顏色特征和語(yǔ)義特征；

6.根據(jù)權(quán)利要求5所述的動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法，其特征在于，在基于關(guān)聯(lián)的rgb像素的坐標(biāo)獲取每個(gè)三維點(diǎn)的語(yǔ)義特征，將所述點(diǎn)云特征和所述語(yǔ)義特征進(jìn)行融合，得到融合特征點(diǎn)云數(shù)據(jù)之后，還包括：

7.根據(jù)權(quán)利要求1所述的動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法，其特征在于，所述將所述融合數(shù)據(jù)進(jìn)行后端優(yōu)化和回環(huán)檢測(cè)，實(shí)現(xiàn)地圖構(gòu)建和自身定位，包括：

8.一種動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建裝置，其特征在于，包括：

9.一種計(jì)算機(jī)設(shè)備，其特征在于，包括：

10.一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，其特征在于，所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)上存儲(chǔ)有計(jì)算機(jī)指令，所述計(jì)算機(jī)指令用于使計(jì)算機(jī)執(zhí)行權(quán)利要求1至7中任一項(xiàng)所述的動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法。

技術(shù)總結(jié)
本發(fā)明涉及機(jī)器人導(dǎo)航和環(huán)境感知定位技術(shù)領(lǐng)域，公開(kāi)了一種動(dòng)態(tài)環(huán)境下的激光雷達(dá)與視覺(jué)融合的定位與地圖構(gòu)建方法，該方法包括：獲取周圍環(huán)境的激光點(diǎn)云數(shù)據(jù)、帶有深度信息的二維圖像數(shù)據(jù)和機(jī)器人自身的姿態(tài)信息；對(duì)激光點(diǎn)云數(shù)據(jù)和二維圖像數(shù)據(jù)進(jìn)行預(yù)處理，得到激光點(diǎn)云數(shù)據(jù)和預(yù)處理后二維圖像數(shù)據(jù)；提取預(yù)處理后的激光點(diǎn)云數(shù)據(jù)和預(yù)處理后二維圖像數(shù)據(jù)的特征值，并對(duì)提取到的特征值進(jìn)行匹配融合，得到同時(shí)包含圖像特征信息的融合特征點(diǎn)云數(shù)據(jù)，將特征點(diǎn)云數(shù)據(jù)和姿態(tài)信息進(jìn)行優(yōu)化，獲得包含特征點(diǎn)云數(shù)據(jù)與定位信息的融合數(shù)據(jù)；將融合數(shù)據(jù)進(jìn)行后端優(yōu)化和回環(huán)檢測(cè)，實(shí)現(xiàn)地圖構(gòu)建和自身定位，本發(fā)明可以提高動(dòng)態(tài)環(huán)境下的定位與地圖構(gòu)建的精度。

技術(shù)研發(fā)人員：陳鵬,李林,趙欣雨,曾麗娜,劉魯鑫彧,劉盛捷,樂(lè)孜純,彭悅,李聯(lián)合,曲軼,李再金,劉國(guó)軍
受保護(hù)的技術(shù)使用者：海南師范大學(xué)
技術(shù)研發(fā)日：
技術(shù)公布日：2025/5/8