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