本發(fā)明涉及三維地圖構(gòu)建，具體是涉及一種基于雷達的同步定位與建圖方法、裝置、設備及介質(zhì)。
背景技術(shù)：
1、精確的里程計和點云地圖是移動機器人自主探索、避障和運動規(guī)劃的前提，其中采用同步定位和建圖技術(shù)可以計算出里程計和構(gòu)建出點云地圖。現(xiàn)有技術(shù)通過激光雷達lidar采集的相鄰點云數(shù)據(jù)在估計移動機器人的狀態(tài)，進而通過移動機器人的狀態(tài)實現(xiàn)當前的同步定位和建圖。但是激光雷達lidar設置在移動機器人上，移動機器人的運動會導致激光雷達lidar采集到的點云數(shù)據(jù)存在運動畸變，即激光雷達lidar采集到的點云數(shù)據(jù)采集到的點云數(shù)據(jù)準確性較低，從而導致了同步定位和建圖的準確性較低。
2、因此，現(xiàn)有技術(shù)還有待改進和提高。
技術(shù)實現(xiàn)思路
1、為解決上述技術(shù)問題，本發(fā)明提供了一種基于雷達的同步定位與建圖方法、裝置、設備及介質(zhì)，解決了現(xiàn)有技術(shù)中同步定位和建圖準確性較低的問題。
2、為實現(xiàn)上述目的，本發(fā)明采用了以下技術(shù)方案：
3、第一方面，本發(fā)明提供一種基于雷達的同步定位與建圖方法，其中，包括：
4、獲取imu觀測數(shù)據(jù)，設置imu狀態(tài)參數(shù)，對所述imu觀測數(shù)據(jù)和所述imu狀態(tài)參數(shù)進行imu預積分處理，得到imu殘差因子；
5、獲取雷達采集的點云觀測數(shù)據(jù)，設置雷達狀態(tài)參數(shù)，依據(jù)所述點云觀測數(shù)據(jù)和所述雷達狀態(tài)參數(shù)，確定雷達殘差因子；
6、對所述imu殘差因子和所述雷達殘差因子應用光束平差法，得到目標狀態(tài)量，并依據(jù)所述目標狀態(tài)量實現(xiàn)同步定位與建圖，所述目標狀態(tài)量包括所述imu狀態(tài)參數(shù)所對應的狀態(tài)量和所述雷達狀態(tài)參數(shù)所對應的狀態(tài)量。
7、在一種實現(xiàn)方式中，對所述imu觀測數(shù)據(jù)和所述imu狀態(tài)參數(shù)進行imu預積分處理，得到imu殘差因子，包括：
8、確定所述imu觀測數(shù)據(jù)中的imu角速度和imu加速度；
9、確定所述imu狀態(tài)參數(shù)中的旋轉(zhuǎn)零偏和加速度零偏以及重力方向；
10、對所述imu角速度、所述imu加速度、所述旋轉(zhuǎn)零偏、所述加速度零偏、所述重力方向進行imu預積分處理，得到imu殘差因子。
11、在一種實現(xiàn)方式中，對所述imu角速度、所述imu加速度、所述旋轉(zhuǎn)零偏、所述加速度零偏、所述重力方向進行imu預積分處理，得到imu殘差因子，包括：
12、依據(jù)所述imu角速度和所述旋轉(zhuǎn)零偏，得到預積分旋轉(zhuǎn)增量和預積分旋轉(zhuǎn)測量值；
13、依據(jù)所述預積分旋轉(zhuǎn)增量和所述預積分旋轉(zhuǎn)測量值，得到旋轉(zhuǎn)殘差因子；
14、依據(jù)所述旋轉(zhuǎn)零偏、所述加速度零偏、所述重力方向、所述imu加速度，得到速度殘差因子；
15、依據(jù)所述預積分旋轉(zhuǎn)增量和所述imu加速度以及所述加速度零偏，得到預積分位移增量；
16、依據(jù)所述預積分位移增量，得到位移殘差因子；
17、依據(jù)所述旋轉(zhuǎn)殘差因、所述速度殘差因子和所述位移殘差因子，構(gòu)建imu殘差因子。
18、在一種實現(xiàn)方式中，依據(jù)所述點云觀測數(shù)據(jù)和所述雷達狀態(tài)參數(shù)，確定雷達殘差因子，包括；
19、確定雷達狀態(tài)參數(shù)中的雷達旋轉(zhuǎn)角度、雷達位置、雷達移動速度、雷達到imu的外參；
20、依據(jù)所述雷達旋轉(zhuǎn)角度、所述雷達位置、所述雷達移動速度、雷達到imu的所述外參，將所述點云觀測數(shù)據(jù)轉(zhuǎn)換至imu所在的坐標系，得到點云觀察轉(zhuǎn)換數(shù)據(jù)；
21、依據(jù)所述點云觀察轉(zhuǎn)換數(shù)據(jù)，確定雷達殘差因子。
22、在一種實現(xiàn)方式中，依據(jù)所述點云觀察轉(zhuǎn)換數(shù)據(jù)，確定雷達殘差因子，包括：
23、依據(jù)所述點云觀察轉(zhuǎn)換數(shù)據(jù)，確定雷達的邊緣特征殘差和面特征殘差；
24、依據(jù)所述邊緣特征殘差和所述面特征殘差之和，得到雷達殘差因子。
25、在一種實現(xiàn)方式中，對所述imu殘差因子和所述雷達殘差因子應用光束平差法，得到目標狀態(tài)量，包括：
26、對滑動窗口內(nèi)的多幀所述imu觀測數(shù)據(jù)所對應的所述imu殘差因子和滑動窗口內(nèi)的多幀所述點云觀測數(shù)據(jù)所對應的所述雷達殘差因子應用光束平差法，得到緊耦合的聯(lián)合狀態(tài)優(yōu)化模型；
27、計算所述聯(lián)合狀態(tài)優(yōu)化模型取得最小值時所對應的所述imu狀態(tài)參數(shù)的狀態(tài)量和所述雷達狀態(tài)參數(shù)的狀態(tài)量，得到目標狀態(tài)量。
28、在一種實現(xiàn)方式中，所述點云觀測數(shù)據(jù)為運動畸變補償之后的觀測數(shù)據(jù)，所述運動畸變補償?shù)姆绞?，包括?/p>
29、通過所述imu觀測數(shù)據(jù)對所述點云觀測數(shù)據(jù)進行運動畸變補償。
30、第二方面，本發(fā)明實施例還提供一種基于雷達的同步定位與建圖裝置，其中，所述裝置包括如下組成部分：
31、imu殘差因子計算模塊，用于獲取imu觀測數(shù)據(jù)，設置imu狀態(tài)參數(shù)，對所述imu觀測數(shù)據(jù)和所述imu狀態(tài)參數(shù)進行imu預積分處理，得到imu殘差因子；
32、雷達殘差因子計算模塊，用于獲取雷達采集的點云觀測數(shù)據(jù)，設置雷達狀態(tài)參數(shù)，依據(jù)所述點云觀測數(shù)據(jù)和所述雷達狀態(tài)參數(shù)，確定雷達殘差因子；
33、同步定位與建圖模塊，用于對所述imu殘差因子和所述雷達殘差因子應用光束平差法，得到目標狀態(tài)量，并依據(jù)所述目標狀態(tài)量實現(xiàn)同步定位與建圖，所述目標狀態(tài)量包括所述imu狀態(tài)參數(shù)所對應的狀態(tài)量和所述雷達狀態(tài)參數(shù)所對應的狀態(tài)量。
34、第三方面，本發(fā)明實施例還提供一種終端設備，其中，所述終端設備包括存儲器、處理器及存儲在所述存儲器中并可在所述處理器上運行的基于雷達的同步定位與建圖程序，所述處理器執(zhí)行所述基于雷達的同步定位與建圖程序時，實現(xiàn)上述所述的基于雷達的同步定位與建圖方法的步驟。
35、第四方面，本發(fā)明實施例還提供一種計算機可讀存儲介質(zhì)，所述計算機可讀存儲介質(zhì)上存儲有基于雷達的同步定位與建圖程序，所述基于雷達的同步定位與建圖程序被處理器執(zhí)行時，實現(xiàn)上述所述的基于雷達的同步定位與建圖方法的步驟。
36、有益效果：本發(fā)明對imu觀測數(shù)據(jù)和imu狀態(tài)參數(shù)進行imu預積分處理，得到imu殘差因子；根據(jù)雷達的點云觀察數(shù)據(jù)和雷達狀態(tài)參數(shù)，得到雷達殘差因子，對雷達殘差因子和imu殘差因子應用光束平差法，得到目標狀態(tài)量，目標狀態(tài)量即imu狀態(tài)參數(shù)的狀態(tài)量和雷達狀態(tài)參數(shù)的狀態(tài)量，根據(jù)這兩個狀態(tài)量實現(xiàn)同步定位和建圖。本發(fā)明采用本發(fā)明使用慣性測量單元imu來補償lidar幀之間的運動畸變，提高用于機器人狀態(tài)估計的點云數(shù)據(jù)的準確性，進而提高同步定位與建圖的準確性。
1.一種基于雷達的同步定位與建圖方法，其特征在于，包括：
2.如權(quán)利要求1所述的基于雷達的同步定位與建圖方法，其特征在于，對所述imu觀測數(shù)據(jù)和所述imu狀態(tài)參數(shù)進行imu預積分處理，得到imu殘差因子，包括：
3.如權(quán)利要求2所述的基于雷達的同步定位與建圖方法，其特征在于，對所述imu角速度、所述imu加速度、所述旋轉(zhuǎn)零偏、所述加速度零偏、所述重力方向進行imu預積分處理，得到imu殘差因子，包括：
4.如權(quán)利要求1所述的基于雷達的同步定位與建圖方法，其特征在于，依據(jù)所述點云觀測數(shù)據(jù)和所述雷達狀態(tài)參數(shù)，確定雷達殘差因子，包括；
5.如權(quán)利要求4所述的基于雷達的同步定位與建圖方法，其特征在于，依據(jù)所述點云觀察轉(zhuǎn)換數(shù)據(jù)，確定雷達殘差因子，包括：
6.如權(quán)利要求1所述的基于雷達的同步定位與建圖方法，其特征在于，對所述imu殘差因子和所述雷達殘差因子應用光束平差法，得到目標狀態(tài)量，包括：
7.如權(quán)利要求1所述的基于雷達的同步定位與建圖方法，其特征在于，所述點云觀測數(shù)據(jù)為運動畸變補償之后的觀測數(shù)據(jù)，所述運動畸變補償?shù)姆绞?，包括?/p>
8.一種基于雷達的同步定位與建圖裝置，其特征在于，所述裝置包括如下組成部分：
9.一種終端設備，其特征在于，所述終端設備包括存儲器、處理器及存儲在所述存儲器中并可在所述處理器上運行的基于雷達的同步定位與建圖程序，所述處理器執(zhí)行所述基于雷達的同步定位與建圖程序時，實現(xiàn)如權(quán)利要求1-7任一項所述的基于雷達的同步定位與建圖方法的步驟。
10.一種計算機可讀存儲介質(zhì)，其特征在于，所述計算機可讀存儲介質(zhì)上存儲有基于雷達的同步定位與建圖程序，所述基于雷達的同步定位與建圖程序被處理器執(zhí)行時，實現(xiàn)如權(quán)利要求1-7任一項所述的基于雷達的同步定位與建圖方法的步驟。