本發(fā)明屬于測(cè)量定位，具體涉及一種基于imu和毫米波雷達(dá)的三維重建方法。

背景技術(shù)：

1、近年來(lái)，隨著計(jì)算機(jī)技術(shù)和傳感器技術(shù)的發(fā)展，目標(biāo)三維重建成為研究熱點(diǎn)且應(yīng)用市場(chǎng)需求大幅增加，slam技術(shù)成為目標(biāo)三維重建的關(guān)鍵技術(shù)之一。但是應(yīng)用場(chǎng)景的多樣化、復(fù)雜化，這對(duì)高精度、高性能slam技術(shù)提出更大挑戰(zhàn)。

2、目前成熟的slam技術(shù)包括視覺slam技術(shù)和激光雷達(dá)slam技術(shù)，但是由于光學(xué)相機(jī)和激光雷達(dá)不具備全天時(shí)全天候的工作能力，因此該類技術(shù)不再適用于濃霧、大雪、濃煙等場(chǎng)景。而且激光slam和視覺slam濾除動(dòng)態(tài)目標(biāo)的方法主要分為基于點(diǎn)云分割的濾除算法、基于可見性的濾除算法和基于柵格的濾除算法三類，該類算法復(fù)雜度高，操作難度大。

3、現(xiàn)也有使用毫米波雷達(dá)進(jìn)行slam的技術(shù)，但現(xiàn)有的大部分毫米波雷達(dá)slam算法是根據(jù)激光雷達(dá)slam算法修改優(yōu)化的，由于激光雷達(dá)點(diǎn)云和毫米波雷達(dá)點(diǎn)云密度和精度存在明顯差異，所以算法性能較差。

4、隨著毫米波雷達(dá)的更新迭代，由傳統(tǒng)的3d毫米波雷達(dá)升級(jí)為4d毫米波雷達(dá)，不僅具備探測(cè)目標(biāo)三維空間信息的能力，而且4d成像毫米波雷達(dá)相較傳統(tǒng)3d毫米波雷達(dá)天線數(shù)量增加，提高了點(diǎn)云精度和點(diǎn)云密度。雖然毫米波雷達(dá)點(diǎn)云精度和點(diǎn)云密度明顯提升，但是仍不能達(dá)到激光雷達(dá)點(diǎn)云的精度和密度，因此使用單一的毫米波雷達(dá)slam不足以應(yīng)對(duì)復(fù)雜多變環(huán)境的挑戰(zhàn)。

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

1、本發(fā)明提供一種基于imu和毫米波雷達(dá)的三維重建方法，利用imu輔助4d毫米波雷達(dá)slam提高目標(biāo)三維重建精度。

2、為實(shí)現(xiàn)上述技術(shù)目的，本發(fā)明采用如下技術(shù)方案：

3、一種基于imu和毫米波雷達(dá)的三維重建方法，包括：

4、步驟1，將imu和4d毫米波雷達(dá)搭載于同一移動(dòng)設(shè)備上，且imu和4d毫米波雷達(dá)相對(duì)靜止；將移動(dòng)設(shè)備在待三維重建的目標(biāo)周圍移動(dòng)，通過(guò)imu采集移動(dòng)設(shè)備在各時(shí)刻的運(yùn)動(dòng)參數(shù)，通過(guò)4d毫米波雷達(dá)采集目標(biāo)的連續(xù)多幀點(diǎn)云數(shù)據(jù)；

5、步驟2，從目標(biāo)的點(diǎn)云數(shù)據(jù)中提取速度信息，并基于速度信息濾除點(diǎn)云數(shù)據(jù)中的動(dòng)態(tài)點(diǎn)云；

6、步驟3，獲取雷達(dá)第j幀采集的點(diǎn)云數(shù)據(jù)以及imu在該幀時(shí)刻采集的運(yùn)動(dòng)參數(shù)，通過(guò)構(gòu)建imu在第j幀時(shí)的局部旋轉(zhuǎn)矩陣和局部平移矩陣，將第j幀的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到第j-1幀的雷達(dá)局部坐標(biāo)系下；重復(fù)步驟3，直到所有幀的點(diǎn)云數(shù)據(jù)均轉(zhuǎn)換到同一幀的雷達(dá)局部坐標(biāo)系下；其中，；

7、步驟4，對(duì)在同一幀雷達(dá)局部坐標(biāo)系下的所有點(diǎn)云數(shù)據(jù)，進(jìn)行去噪處理；

8、步驟5，通過(guò)對(duì)相鄰幀去噪后的點(diǎn)云數(shù)據(jù)進(jìn)行匹配，獲得4d毫米波雷達(dá)在各幀時(shí)的位姿；

9、步驟6，以4d毫米波雷達(dá)在各幀時(shí)的位姿作為觀測(cè)數(shù)據(jù)，以imu采集的運(yùn)動(dòng)參數(shù)作為控制輸入，采用卡爾曼濾波算法對(duì)各幀時(shí)的位姿進(jìn)行優(yōu)化調(diào)整；

10、步驟7，根據(jù)步驟6得到的各幀位姿，以及經(jīng)步驟4去噪后的各幀點(diǎn)云數(shù)據(jù)，構(gòu)建目標(biāo)的三維地圖。

11、進(jìn)一步的，步驟2中，若點(diǎn)云中某個(gè)點(diǎn)的速度超過(guò)4d毫米波雷達(dá)的速度分辨率，則判定該點(diǎn)為動(dòng)態(tài)點(diǎn)云并做濾除處理。

12、進(jìn)一步的，步驟3將第j幀的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到第j-1幀的雷達(dá)局部坐標(biāo)系下，具體為：

13、步驟3.1，根據(jù)imu在雷達(dá)第j幀時(shí)采集的運(yùn)動(dòng)參數(shù)，構(gòu)建imu在第j幀時(shí)的局部旋轉(zhuǎn)矩陣和局部平移矩陣：

14、

15、

16、式中，分別為雷達(dá)第j幀時(shí)繞其局部坐標(biāo)系中x、y、z軸的旋轉(zhuǎn)角度，分別為雷達(dá)第j幀時(shí)沿其局部坐標(biāo)系中x、y、z軸的位移，由imu采集到的三軸角速度和三軸加速度得到：

17、

18、

19、其中，為采集間隔，分別為第j-1幀時(shí)繞其局部坐標(biāo)系中x、y、z軸的旋轉(zhuǎn)角度，分別為雷達(dá)第j-1幀時(shí)沿其局部坐標(biāo)系中x、y、z軸的位移，分別為第j-1幀時(shí)繞其局部坐標(biāo)系中x、y、z軸的角速度，分別為第j-1幀時(shí)沿其局部坐標(biāo)系中x、y、z軸的速度，分別為第j-1幀時(shí)沿其局部坐標(biāo)系中x、y、z軸的加速度；

20、步驟3.2，根據(jù)構(gòu)建的局部旋轉(zhuǎn)矩陣和局部平移矩陣，將雷達(dá)第j幀采集的點(diǎn)云數(shù)據(jù)，轉(zhuǎn)換到第j-1幀的雷達(dá)局部坐標(biāo)系下：

21、

22、式中，為雷達(dá)第j幀采集的三維點(diǎn)云數(shù)據(jù)，為雷達(dá)第j幀采集點(diǎn)云數(shù)據(jù)在第j-1幀雷達(dá)局部坐標(biāo)系下的三維坐標(biāo)。

23、進(jìn)一步的，通過(guò)改進(jìn)dbscan算法對(duì)同一幀雷達(dá)局部坐標(biāo)系下的所有點(diǎn)云數(shù)據(jù)，進(jìn)行去噪處理，具體包括：

24、步驟4.1，獲取4d毫米波雷達(dá)的角精度和該同一幀雷達(dá)局部坐標(biāo)系下的所有點(diǎn)云數(shù)據(jù)，計(jì)算點(diǎn)云數(shù)據(jù)中各點(diǎn)在笛卡爾坐標(biāo)系下考慮角精度的坐標(biāo)；其中，分別為點(diǎn)在笛卡爾坐標(biāo)系下的三軸位置，分別為點(diǎn)在笛卡爾坐標(biāo)系下的三軸精度誤差；

25、步驟4.2，針對(duì)點(diǎn)云數(shù)據(jù)中各點(diǎn)，基于其坐標(biāo)構(gòu)建對(duì)應(yīng)的橢球面，并以該橢球面作為點(diǎn)的ε-鄰域；其中，基于點(diǎn)云坐標(biāo)構(gòu)建對(duì)應(yīng)的橢球面為：

26、

27、式中，為點(diǎn)對(duì)應(yīng)的橢球面上的點(diǎn)坐標(biāo)；

28、步驟4.3，基于各點(diǎn)的ε-鄰域，采用dbscan算法確定點(diǎn)云數(shù)據(jù)中的噪聲點(diǎn)并濾除。

29、進(jìn)一步的，所述計(jì)算點(diǎn)云數(shù)據(jù)中各點(diǎn)在笛卡爾坐標(biāo)系下考慮角精度的坐標(biāo)，具體為：

30、（1）將點(diǎn)在雷達(dá)局部坐標(biāo)系下的坐標(biāo)，轉(zhuǎn)換為笛卡爾坐標(biāo)系下的坐標(biāo)：

31、

32、式中，分別為點(diǎn)在雷達(dá)局部坐標(biāo)系下的距離、俯仰角和水平角；

33、（2）根據(jù)4d毫米波雷達(dá)的角精度，計(jì)算點(diǎn)在笛卡爾坐標(biāo)系下的精度誤差：

34、

35、式中，為4d毫米波雷達(dá)的俯仰角精度和水平角精度；

36、（3）綜合點(diǎn)在笛卡爾坐標(biāo)系下的坐標(biāo)和精度誤差，得到點(diǎn)在笛卡爾坐標(biāo)系下考慮角精度的坐標(biāo)：

37、

38、式中，為點(diǎn)在笛卡爾坐標(biāo)系下考慮角精度的三軸位置。

39、進(jìn)一步的，步驟5采用迭代最近點(diǎn)的方法對(duì)相鄰幀去噪后的點(diǎn)云數(shù)據(jù)進(jìn)行匹配，具體以相鄰幀的同一目標(biāo)點(diǎn)云的距離最小化作為優(yōu)化目標(biāo)，建立優(yōu)化目標(biāo)函數(shù)為：

40、

41、式中，分別為4d毫米波雷達(dá)在第j幀時(shí)的旋轉(zhuǎn)矩陣和平移矩陣，分別為點(diǎn)i在第j-1幀、第j幀時(shí)的三軸位置坐標(biāo)，n為第j-1幀、第j幀共有的目標(biāo)點(diǎn)個(gè)數(shù)。

42、進(jìn)一步的，步驟6采用卡爾曼濾波算法對(duì)各幀時(shí)的位姿進(jìn)行優(yōu)化調(diào)整，具體包括：

43、步驟6.1，獲取imu在第j-1幀時(shí)采集的運(yùn)動(dòng)參數(shù)、4d毫米波雷達(dá)在第j幀時(shí)的位姿以及采用卡爾曼濾波算法優(yōu)化調(diào)整得到第j-1幀時(shí)的最優(yōu)位姿；

44、步驟6.2，根據(jù)獲取的第j-1幀的運(yùn)動(dòng)參數(shù)和優(yōu)化調(diào)整后的最優(yōu)位姿，預(yù)測(cè)第j幀時(shí)的位姿：

45、

46、式中，、分別為第j幀時(shí)的位姿狀態(tài)轉(zhuǎn)移矩陣和控制矩陣；通過(guò)初始化給定；

47、步驟6.3，預(yù)測(cè)第j幀時(shí)的位姿狀態(tài)協(xié)方差矩陣：

48、

49、式中，表示預(yù)測(cè)得到的位姿狀態(tài)協(xié)方差矩陣，表示第j-1幀時(shí)的位姿狀態(tài)協(xié)方差矩陣，為過(guò)程噪聲協(xié)方差矩陣；

50、步驟6.4，計(jì)算卡爾曼增益：

51、

52、式中，表示第j幀時(shí)的卡爾曼增益，表示第j幀時(shí)的觀測(cè)矩陣，為觀測(cè)噪聲協(xié)方差矩陣；

53、步驟6.5，取第j幀時(shí)的位姿作為觀測(cè)數(shù)據(jù)，對(duì)預(yù)測(cè)得到的位姿進(jìn)行優(yōu)化調(diào)整，得到優(yōu)化調(diào)整后的第j幀的最優(yōu)位姿：

54、

55、步驟6.6，若第j幀是最后一幀，則已完成對(duì)所有幀的最優(yōu)位姿調(diào)整；否則更新第j幀時(shí)的位姿狀態(tài)協(xié)方差矩陣，并返回步驟6.1，繼續(xù)對(duì)下一幀的最優(yōu)位姿進(jìn)行優(yōu)化調(diào)整；其中，更新第j幀時(shí)的位姿狀態(tài)協(xié)方差矩陣，表示為：

56、

57、式中，表示對(duì)角線全為1、其余全為0的單位矩陣。

58、進(jìn)一步的，在imu采集運(yùn)動(dòng)參數(shù)以及4d毫米波雷達(dá)采集點(diǎn)云數(shù)據(jù)的過(guò)程中，基于gps聯(lián)合移動(dòng)設(shè)備上的工控機(jī)對(duì)imu和4d毫米波雷達(dá)進(jìn)行時(shí)間同步，即：

59、當(dāng)gps信號(hào)存在時(shí)，使用gps信號(hào)對(duì)imu和4d毫米波雷達(dá)進(jìn)行授時(shí)；

60、當(dāng)gps信號(hào)不存在時(shí)，使用工控機(jī)的時(shí)間戳對(duì)imu和4d毫米波雷達(dá)進(jìn)行授時(shí)。

61、進(jìn)一步的，移動(dòng)設(shè)備在待三維重建的目標(biāo)周圍移動(dòng)過(guò)程中，若根據(jù)gps信號(hào)判斷移動(dòng)設(shè)備在某幀與之前另一幀處于同一位置，則：將所述某幀時(shí)的位姿，修改為所述另一幀時(shí)的位姿。

62、與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果為：

63、1、相比單幀毫米波雷達(dá)點(diǎn)云聚類，本發(fā)明通過(guò)imu輔助多幀毫米波雷達(dá)點(diǎn)云匹配，改進(jìn)dbscan算法可以更準(zhǔn)確的去掉離群點(diǎn)和雜波點(diǎn)云，提高點(diǎn)云質(zhì)量。

64、2、雖然dbscan可以有效的提高毫米波雷達(dá)點(diǎn)云的質(zhì)量，但是毫米波雷達(dá)點(diǎn)云精度較低，僅依靠毫米波雷達(dá)進(jìn)行三維重建效果一般，本發(fā)明利用卡爾曼濾波聯(lián)合優(yōu)化imu和毫米波雷達(dá)里程計(jì)得到高精度的位姿，提高三維重建的精度。

65、3、本發(fā)明gps和工控機(jī)聯(lián)合授時(shí)確保了系統(tǒng)時(shí)間的連續(xù)行，利用gps完成回環(huán)檢測(cè)，可以有效的減少計(jì)算量。

66、4、本發(fā)明解決了光學(xué)相機(jī)和激光雷達(dá)在云霧等極端天氣下工作能力受限導(dǎo)致三維重建誤差較大的問(wèn)題。