本發(fā)明屬于自主導(dǎo)航與機器人，具體說是一種三維激光特征與4d毫米波雷達(dá)特征融合slam方法。

背景技術(shù)：

1、隨著自主導(dǎo)航技術(shù)的不斷發(fā)展，三維激光雷達(dá)(lidar)在即時定位與地圖構(gòu)建(slam)領(lǐng)域扮演著愈發(fā)重要的角色，特別是在要求高精度與可靠性的應(yīng)用中，如移動機器人、agv、服務(wù)機器人、自動駕駛汽車等。盡管現(xiàn)有的三維激光slam算法在處理點云數(shù)據(jù)以及構(gòu)建環(huán)境模型、定位方面取得了顯著進(jìn)展，然而它們的前端算法大多只集中在提取簡單的幾何特征，對于點云中蘊含的豐富特征并未進(jìn)行充分挖掘與融合，導(dǎo)致在復(fù)雜、多變環(huán)境中定位精度和環(huán)境理解能力受限，并且針對戶外環(huán)境，面對極端天氣時導(dǎo)航的精度與穩(wěn)定性受到嚴(yán)重考驗。

2、當(dāng)前主流的三維激光slam算法，如loam、lego-loam、lio-sam等，雖然在處理三維激光雷達(dá)數(shù)據(jù)時表現(xiàn)良好，但是對于復(fù)雜的工業(yè)生產(chǎn)環(huán)境，算法的效率與適應(yīng)性面臨挑戰(zhàn)。loam算法利用點云中的線特征和平面特征進(jìn)行位姿估計，提高了定位精度，但是該算法對于計算資源要求較高，較難在資源受限的嵌入式系統(tǒng)上高效運行，并且數(shù)據(jù)預(yù)處理相對簡單，沒有充分過濾噪聲或異常值，lego-loam相比loam進(jìn)行了優(yōu)化，更適合在資源有限的平臺上運行，特別優(yōu)化了對地面特征的處理，提高了在復(fù)雜環(huán)境下的穩(wěn)定性和準(zhǔn)確性，并且引入了關(guān)鍵幀概念、回環(huán)檢測以及位姿圖優(yōu)化，減少了累積誤差，然而前端配準(zhǔn)仍是基于提取環(huán)境中線特征和面特征的方法，針對生產(chǎn)環(huán)境復(fù)雜的工廠，由于廠區(qū)面積大，結(jié)構(gòu)復(fù)雜，因此精度仍然不高，lio-sam算法將激光雷達(dá)和慣性測量單元(imu)數(shù)據(jù)通過因子圖優(yōu)化緊密耦合，提高了定位穩(wěn)定性和準(zhǔn)確性，該算法同時融合了gps數(shù)據(jù)，增強了戶外動態(tài)環(huán)境的適應(yīng)能力，在實時建圖的同時對位姿平滑優(yōu)化，使得地圖質(zhì)量更高，定位更準(zhǔn)確，lio-sam的前端配準(zhǔn)算法并沒有對環(huán)境中其他特征進(jìn)行提取和描述，采用角點和面點的特征提取方案，針對空曠的倉庫和結(jié)構(gòu)復(fù)雜、堆貨雜亂的工業(yè)環(huán)境，適應(yīng)性降低，運算復(fù)雜度增高。為了應(yīng)對發(fā)生退化的快速運動、噪聲或雜亂環(huán)境，fast-lio算法采用緊耦合迭代卡爾曼濾波器來融合激光雷達(dá)特征和imu測量數(shù)據(jù)，提升了運算速度與定位精度，然而面對戶外的生產(chǎn)環(huán)境，并不能克服極端天氣(如，雨、雪、霧)對定位的干擾，導(dǎo)航能力降低?；诩す馀c相機融合的slam方案雖然可以應(yīng)對堆貨雜亂的施工場景，可是面對極端天氣的考驗，定位效果仍不理想，而且相機極易受到自然環(huán)境變化所來帶的干擾，使得導(dǎo)航的魯棒性降低。毫米波雷達(dá)因其能夠有效穿透雨霧等惡劣天氣條件而被廣泛應(yīng)用于多種場景，然而在實際應(yīng)用中，毫米波雷達(dá)通常僅利用其自身的觀測信息進(jìn)行環(huán)境感知，而未與其它傳感器(如激光雷達(dá))的數(shù)據(jù)進(jìn)行融合處理。這種單一的感知方式在極端天氣條件下(例如暴雨、大霧等)，特別是在戶外工業(yè)生產(chǎn)環(huán)境中，可能會導(dǎo)致位置估計的準(zhǔn)確性下降。

3、如何在復(fù)雜多變的環(huán)境中實現(xiàn)穩(wěn)定且精確的定位仍然是一個亟待解決的技術(shù)難題，因此針對工業(yè)生產(chǎn)場景，尤其是如工業(yè)園區(qū)、物流中心等復(fù)雜的施工環(huán)境，如何應(yīng)對極端天氣的影響，保證魯棒性和高精度定位是工業(yè)移動機器人自主導(dǎo)航的關(guān)鍵問題。

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

1、為了解決上述問題，本發(fā)明的目的在于提供一種三維激光特征與4d毫米波雷達(dá)特征融合的slam(simultaneous?localization?and?mapping，同步定位與建圖)方法，該方法通過結(jié)合激光雷達(dá)和毫米波雷達(dá)的優(yōu)勢，在極端天氣環(huán)境下對復(fù)雜的工業(yè)生產(chǎn)園區(qū)提取信息豐富、約束性強的激光點云特征與4d毫米波雷達(dá)特征，并將多種特征融合配準(zhǔn)，提升定位精度與穩(wěn)定性。

2、本發(fā)明為實現(xiàn)上述目的所采用的技術(shù)方案是：一種三維激光特征與4d毫米波雷達(dá)特征融合slam方法，包括以下步驟：

3、1)上位機獲取三維激光原始點云和4d毫米波雷達(dá)數(shù)據(jù)，并分別進(jìn)行預(yù)處理，得到處理后的三維激光點云和處理后的雷達(dá)數(shù)據(jù)；

4、2)對處理后的三維激光點云進(jìn)行特征提取，得到對應(yīng)特征的點云；對處理后的雷達(dá)數(shù)據(jù)進(jìn)行目標(biāo)分類，得到4d毫米波雷達(dá)特征；

5、3)將步驟2)中對應(yīng)特征的三維激光點云進(jìn)一步特征提取，提取方柱特征、圓柱特征點云，并將不同特征類型的點云進(jìn)行分類；

6、4)將步驟3)中的激光特征與步驟2)中的4d毫米波雷達(dá)特征進(jìn)行時空配準(zhǔn)，并基于時空配準(zhǔn)的結(jié)果，將激光雷達(dá)檢測到的特征與4d毫米波雷達(dá)的特征信息進(jìn)行關(guān)聯(lián)融合，得到融合后的相同特征類型的數(shù)據(jù)；

7、5)對步驟4)中融合后的相同特征類型的數(shù)據(jù)進(jìn)行特征匹配與幾何關(guān)系約束，以實現(xiàn)點云配準(zhǔn)；

8、6)基于點云配準(zhǔn)，結(jié)合上位機后端回環(huán)檢測算法，回環(huán)成功后優(yōu)化全局位姿，輸出地圖坐標(biāo)系下的6自由度的全局位姿。

9、所述步驟1)，具體為：

10、1-1)利用高頻imu慣性單元對三維激光點云進(jìn)行畸變補償；

11、1-2)將畸變補償后的三維激光點云通過dbscan聚類算法進(jìn)行去噪處理；

12、1-3)對三維激光點云進(jìn)行體素化，以降低后續(xù)處理的復(fù)雜度并保持關(guān)鍵結(jié)構(gòu)信息。

13、步驟2)中，所述對處理后的三維激光點云進(jìn)行特征提取，得到對應(yīng)特征的點云，具體為：

14、所述提取三維激光點云特征，包括：角點特征、面點特征、地面點特征；

15、2-1)基于高度閾值與坡度分析，并采用ransac算法，提取三維激光點云的地面特征點云；

16、2-2)根據(jù)曲率計算結(jié)果，分別提取角點特征點云與面點特征點云。

17、步驟2)，所述對處理后的雷達(dá)數(shù)據(jù)進(jìn)行目標(biāo)分類，得到4d毫米波雷達(dá)特征，具體為：

18、(1)對雷達(dá)數(shù)據(jù)進(jìn)行聚類，以去除4d毫米波雷達(dá)中的虛假目標(biāo)，提取出運動目標(biāo)；

19、(2)利用聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)法對4d毫米波雷達(dá)目標(biāo)進(jìn)行分類，使用卡爾曼濾波器進(jìn)行動態(tài)目標(biāo)跟蹤；

20、(3)標(biāo)記靜態(tài)物體，并對靜態(tài)物體進(jìn)行面特征提取。

21、所述步驟3)，具體為：

22、3-1)對聚類的點云簇計算最小外接矩形，根據(jù)矩形長寬比閾值進(jìn)行篩選，滿足要求的點云簇加入柱特征集合；

23、對于每個聚類簇cj計算其在三維空間中的最小外接長方體，其邊長定義為lj，wj，hj，分別對應(yīng)長方體的長、寬和高。為了識別柱狀目標(biāo)，預(yù)設(shè)一個長寬比閾值λar，并結(jié)合高度限制，定義三維長方體的長寬比為：

24、

25、定義每個聚類簇cj的高度范圍hj為：

26、hj＝zmax-zmin

27、其中，zmax是點云簇中z軸方向上的最大坐標(biāo)值，zmin是點云簇中z軸方向上的最小坐標(biāo)值，定義預(yù)設(shè)的高度閾值范圍為[hmin,hmax]；

28、柱狀特征，需要滿足以下條件：

29、arj≤λar且hmin≤hj≤hmax

30、最終的柱特征集合ccylinder被定義為：

31、ccylinder＝{cj|arj≤λar且hmin≤hj≤hmax}

32、3-2)對柱特征集合中的每個點云簇分別提取法線信息，對柱特征集合中的每個點云簇進(jìn)行角點特征、面點特征分布分析，即：

33、面點特征連續(xù)且兩端帶有角點特征，面點特征的法線呈垂直分布的點云簇標(biāo)記為方柱特征；

34、3-3)點云簇表現(xiàn)為：面點特征且具有相似法線方向并緊密排列，對此標(biāo)記為圓柱特征。

35、所述將激光雷達(dá)檢測到的特征與4d毫米波雷達(dá)的特征信息進(jìn)行關(guān)聯(lián)融合，得到融合后的相同特征類型的數(shù)據(jù)，包括以下步驟：

36、4-1)將激光雷達(dá)與4d毫米波雷達(dá)的特征進(jìn)行關(guān)聯(lián)，采用聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)法,該算法通過最大化聯(lián)合概率，找到兩個傳感器中相同目標(biāo)的最佳匹配，定義為激光雷達(dá)提取的第i個柱特征簇，為4d毫米波雷達(dá)檢測的第k個運動目標(biāo)簇，關(guān)聯(lián)目標(biāo)通過聯(lián)合概率公式進(jìn)行匹配：

37、

38、其中，ppos是根據(jù)位置相似度計算的概率，基于激光雷達(dá)和毫米波雷達(dá)中點云簇的中心距離；pvel是是基于速度特征(從毫米波雷達(dá)得到)的概率；psize是根據(jù)兩者的點云簇尺寸進(jìn)行比較的概率；

39、4-2)當(dāng)匹配概率超過一定閾值pthresh時，認(rèn)為激光雷達(dá)的柱特征點云簇與毫米波雷達(dá)的運動目標(biāo)點云簇是同一個目標(biāo)。

40、所述步驟5)，具體為：

41、標(biāo)記為相同特征類型的數(shù)據(jù)簇間進(jìn)行配準(zhǔn)，利用角點、面點、地面點、方柱、圓柱的幾何關(guān)系進(jìn)行約束優(yōu)化；

42、具體約束條件如下：

43、5-1)利用地面點約束：

44、地面點云簇cground的特征是它們在同一個平面上。因此，對于被標(biāo)記為地面點的點云簇，設(shè)定平面約束，假設(shè)地面點滿足平面方程：

45、ax+by+cz+d＝0

46、其中，[a,b,c]是平面法向量，d是平面偏移量，通過優(yōu)化點云簇的擬合程度來最小化所有地面點到該平面的距離：

47、

48、通過最小化該誤差，使得地面點簇與地面平面對齊，從而提高配準(zhǔn)的精度；

49、5-2)利用角點約束：

50、角點ccorner通常是邊緣處的特征點，具有明確的幾何關(guān)系；對于角點，要求它們與其他點云簇在其局部空間內(nèi)的邊緣線保持一致；角點的約束是其應(yīng)該接近某條直線，設(shè)直線方程為：

51、r(t)＝r0+tv

52、其中，r0是直線的起點，v是方向向量，優(yōu)化目標(biāo)是最小化角點pi到該直線的距離：

53、

54、通過最小化該誤差，保證角點與邊緣線之間的幾何一致性。

55、5-3)利用面點約束：

56、面點csurface通常位于較為平滑的表面。面點的約束是這些點應(yīng)該滿足某個局部平面的擬合。面點滿足局部平面方程：

57、a′x+b′y+c′z+d′＝0

58、通過最小化面點到局部平面的距離誤差：

59、

60、確保面點與相應(yīng)的平面特征對齊。

61、5-4)利用方柱特征與圓柱特征約束：

62、對于方柱或圓柱特征點云ccylinder和crectangular的幾何約束關(guān)系如下：

63、圓柱特征：圓柱點云的約束條件是所有點應(yīng)該滿足圓柱體的方程。假設(shè)圓柱的軸向為z軸，圓柱的半徑為r，圓柱上的點滿足：

64、(x-x0)2+(y-y0)2＝r2

65、優(yōu)化目標(biāo)為最小化所有點到該圓柱表面的距離：

66、

67、方柱特征：方柱的約束是所有點云應(yīng)與矩形框架對齊。方柱點云可以用最小外接矩形來逼近，通過最小化方柱點云與矩形邊框之間的距離來優(yōu)化：

68、

69、其中，d(pi,rectangle)是點pi到最小外接矩形的距離。

70、5-5)將上述各類幾何約束結(jié)合在一起，形成整體的優(yōu)化目標(biāo)函數(shù)：

71、εtotal＝λgroundεground+λcornerεcorner+λsurfaceεsurface+λcylinderεcylinder+λrectangularεrectangular

72、其中，λground，λcorner，λsurface，λcylinder，λrectangular是不同約束項的權(quán)重系數(shù)，通過調(diào)節(jié)這些系數(shù)可以控制各類約束對最終優(yōu)化結(jié)果的影響。

73、所述步驟6)，具體為：

74、6-1)設(shè)當(dāng)前時刻的位姿為tcurrent，包括位置信息pcurr和方向信息qcyrr，與歷史關(guān)鍵幀中的位姿tprev進(jìn)行比較，當(dāng)兩者的位姿差異小于一定閾值時，觸發(fā)回環(huán)檢測：

75、dpos＝||pcurr-pprev||<∈pos

76、drot＝||qcurr-qprev||<∈rot

77、其中，∈pos和∈rot是位置和旋轉(zhuǎn)的閾值；

78、6-2)對于觸發(fā)了回環(huán)檢測的關(guān)鍵幀，將當(dāng)前幀與之前匹配的關(guān)鍵幀提取的特征點云進(jìn)行匹配，利用相似變換矩陣tloop，包括旋轉(zhuǎn)矩陣rloop和平移向量tloop，來進(jìn)行特征點云的配準(zhǔn)，最小化特征點云的距離誤差；

79、6-3)通過回環(huán)檢測，將當(dāng)前幀tcurrent與先前的幀tprev之間的相對位姿加入到全局位姿圖中，構(gòu)造圖優(yōu)化問題，將每個關(guān)鍵幀視為圖中的節(jié)點，每兩幀之間的相對位姿作為圖中的邊；目標(biāo)是優(yōu)化整個圖，使得每個幀的位姿更加準(zhǔn)確；

80、設(shè)關(guān)鍵幀的位姿圖為t＝{t1,t2,…,tn}，其中每個關(guān)鍵幀的位姿ti是其位置和旋轉(zhuǎn)的組合，圖優(yōu)化的目標(biāo)是最小化所有相鄰幀之間的相對誤差：

81、

82、其中，tij是觀察到的相對位姿，調(diào)整每個關(guān)鍵幀的位姿，使得整體誤差最小化。

83、一種三維激光特征與4d毫米波雷達(dá)特征融合slam系統(tǒng)，包括存儲器和處理器；所述存儲器，用于存儲計算機程序；所述處理器，用于當(dāng)執(zhí)行所述計算機程序時，實現(xiàn)一種三維激光特征與4d毫米波雷達(dá)特征融合slam方法。

84、一種計算機可讀存儲介質(zhì)，所述存儲介質(zhì)上存儲有計算機程序，當(dāng)所述計算機程序被處理器執(zhí)行時，實現(xiàn)一種三維激光特征與4d毫米波雷達(dá)特征融合slam方法。

85、本發(fā)明具有以下有益效果及優(yōu)點：

86、1.本發(fā)明在極端天氣環(huán)境下對復(fù)雜的工業(yè)生產(chǎn)園區(qū)提取信息豐富、約束性強的激光點云特征與4d毫米波雷達(dá)特征，并將多種特征融合配準(zhǔn)，提升定位精度與穩(wěn)定性。

87、2.本發(fā)明具有全面的特征提?。罕景l(fā)明中的方法能夠有效地從三維激光點云中提取角點、面點、地面點等多種特征類型，并識別出特殊的結(jié)構(gòu)特征，如方柱和圓柱等，提高了環(huán)境理解的全面性和準(zhǔn)確性。

88、3.本發(fā)明具有高效的特征融合：通過將激光雷達(dá)與4d毫米波雷達(dá)的數(shù)據(jù)進(jìn)行融合，不僅增強了系統(tǒng)的魯棒性，還能在極端天氣條件下保持良好的定位性能。

89、4.本發(fā)明的前端約束配準(zhǔn)：通過對不同特征類型的“激光與4d毫米波雷達(dá)融合數(shù)據(jù)”進(jìn)行前端約束配準(zhǔn)，有效減少了點云配準(zhǔn)時的誤匹配與錯匹配概率，提高了定位精度和穩(wěn)定性。

90、5.本發(fā)明極端天氣適應(yīng)性：本發(fā)明方法特別適用于工業(yè)生產(chǎn)場景，如工業(yè)園區(qū)、物流中心等復(fù)雜的工廠環(huán)境，能夠有效應(yīng)對極端天氣帶來的挑戰(zhàn)，保證了移動機器人的高效作業(yè)；將激光雷達(dá)與4d毫米波雷達(dá)的點云進(jìn)行融合匹配，實現(xiàn)魯棒性更強、定位更準(zhǔn)確的slam系統(tǒng)。