本技術(shù)涉及自動導(dǎo)航及定位，更具體地，涉及一種定位方法與地圖構(gòu)建方法。

背景技術(shù)：

1、目前，隨著激光雷達技術(shù)和計算機視覺技術(shù)的迅速發(fā)展，激光雷達slam(simultaneous?localization?and?mapping，簡稱slam，即：同步定位與建圖)因其高精度和穩(wěn)定性而廣泛應(yīng)用于自動駕駛車輛和服務(wù)型自動駕駛汽車中。然而，現(xiàn)有的slam算法在復(fù)雜環(huán)境下仍面臨著誤差累積和高度漂移的問題，尤其是在長時間或大范圍運行時，這些問題會顯著降低slam系統(tǒng)的精度和魯棒性。在動態(tài)、復(fù)雜的環(huán)境中，激光雷達等傳感器可能會采集到大量的動態(tài)噪聲點和無效點，這些噪聲數(shù)據(jù)會嚴重干擾slam算法的配準精度，進而影響其導(dǎo)航和任務(wù)執(zhí)行的效果。

2、在點云數(shù)據(jù)的處理和自動駕駛汽車的位姿估計的過程中，掃描幀到局部地圖的配準方式雖然簡單快捷，但其固有的缺陷在于誤差的快速累積，這對于長時間運行的系統(tǒng)來說會導(dǎo)致顯著的建圖精度衰減。其每次配準僅依賴于連續(xù)掃描幀間的相對變換，任何單次測量的偏差都可能在隨后的配準中被放大，從而在全局尺度上積累成為可觀的誤差。雖然傳統(tǒng)的損失函數(shù)和回環(huán)檢測技術(shù)能夠在一定程度上減輕這些問題，但其主要僅是基于幾何特征，從而限制了其在復(fù)雜環(huán)境下的應(yīng)用效果。

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

1、針對現(xiàn)有技術(shù)的至少一個缺陷或改進需求，本技術(shù)提供了一種定位方法與地圖構(gòu)建方法，用于至少提升定位精度或建圖質(zhì)量。

2、為實現(xiàn)上述目的，第一方面，本技術(shù)提供了一種定位方法，包括：

3、獲取目標物移動時的原始點云數(shù)據(jù)，提取特征指標；

4、融合所述原始點云數(shù)據(jù)的持久特征和強度特征，構(gòu)建強度分布指標函數(shù)，并聯(lián)立預(yù)設(shè)的有效特征點過濾閾值以剔除掉特征點集合中的動態(tài)點；

5、將所述強度分布指標函數(shù)的函數(shù)值作為權(quán)重值，通過最小化邊緣特征點到全局地圖線特征以及平面特征點到全局地圖面特征的距離加權(quán)和處理，獲取所述目標物的最終位姿估計值。

6、進一步地，所述融合所述原始點云數(shù)據(jù)的持久特征和強度特征，構(gòu)建強度分布指標函數(shù)，并聯(lián)立預(yù)設(shè)的有效特征點過濾閾值以剔除掉特征點集合中的動態(tài)點包括：

7、所構(gòu)建的所述強度分布指標函數(shù)的表達式包括：

8、

9、其中，表示強度分布指標值，表示第k0幀掃描幀中局部地圖m中的一個特征點，k表示掃描幀的編號；ωp和ωi分別表示持久特征指標和強度分布指標的權(quán)重值；表示持久特征指標值，γ∈[0,1]和τ表示用于衡量時間間隔對所述持久特征指標值的影響的參數(shù)，表示在第τ幀中是否存在一個特征點可以與相匹配的匹配評判值；(σi-θi)表示強度特征指標值，σi表示特征點最近鄰點強度分布值，θi表示有效特征點強度閾值；

10、若則將此時的特征點認定為有效特征點并予以保留；若則將此時的特征點認定為無效特征點并將其剔除；θpi表示所述有效特征點過濾閾值。

11、進一步地，所述目標物的最終位姿估計值的表達式包括：

12、

13、其中，表示所述目標物的最終位姿估計值；表示邊緣特征點近鄰點的強度分布指標值；表示平面特征點近鄰點的強度分布指標值；fe(pe)表示邊緣特征點pe到全局地圖線特征的距離；fs(ps)表示平面特征點ps到全局地圖面特征的距離。

14、進一步地，所述邊緣特征點pe到全局地圖線特征的距離的表達式包括：

15、

16、

17、其中，tk表示所述目標物在掃描的第k幀時相較于上一幀的位姿變換矩陣；ne和分別表示邊緣特征點在全局地圖中最近鄰點的協(xié)方差矩陣取極大特征值時邊緣特征點所在直線的法向量和位置；pn為單位向量；

18、所述平面特征點ps到全局地圖面特征的距離的表達式包括：

19、

20、其中，ns和分別表示平面特征點在全局地圖中最近鄰點的協(xié)方差矩陣取極小特征值時平面特征點所在平面的法向量和位置。

21、進一步地，所述匹配評判值的表達式包括：

22、

23、其中，ek表示第k幀的邊緣特征點集合；sk表示第k幀的平面特征點集合；nk(pk)表示pk在局部地圖m中所對應(yīng)的點的集合；當時，表示在第k幀中存在一個特征點可以與相匹配，在連續(xù)多幀中能夠被持續(xù)觀察到的特征點被視為持久特征點；

24、所述特征點最近鄰點強度分布值的表達式包括：

25、

26、其中，ni表示特征點在局部地圖m中進行最近鄰搜索所搜索到的鄰近點的個數(shù)；ηi表示點云中相應(yīng)點的強度值。

27、第二方面，本技術(shù)提供了一種地圖構(gòu)建方法，包括：

28、基于前述任一項所獲取的所述最終位姿估計值，通過掃描幀到局部地圖配準，利用點云的空間關(guān)系，基于高度特征和強度特征的融合，實現(xiàn)對周圍環(huán)境的有效建模和表示，構(gòu)建基于強度信息優(yōu)化回環(huán)的激光雷達slam模型；

29、將目標物移動時所獲取的原始點云數(shù)據(jù)輸入所述基于強度信息優(yōu)化回環(huán)的激光雷達slam模型，輸出最終的點云地圖。

30、進一步地，所述通過掃描幀到局部地圖配準，利用點云的空間關(guān)系，基于高度特征和強度特征的融合，實現(xiàn)對周圍環(huán)境的有效建模和表示包括：

31、將扇環(huán)單元盒內(nèi)點的代表高度值和代表強度均值進行加權(quán)求和，獲取該扇環(huán)單元盒的特征值；

32、基于所述扇環(huán)單元盒的特征值，通過對每個掃描點的信號強度進行提取和整理，獲取nr*ns大小的表征掃描上下文信息的矩陣t；其中，nr和ns分別表示單次掃描幀中的點云在極坐標中的徑向方向等分環(huán)數(shù)和周向方向等分扇區(qū)數(shù)；所述矩陣t的每個元素aij代表在第i行第，列位置的信號強度；

33、將當前幀的掃描上下文信息與歷史幀的掃描上下文信息進行比對，構(gòu)建掃描上下文對；

34、基于當前幀特征描述子與歷史幀特征描述子之間的余弦距離，得到所述掃描上下文對的距離值；

35、根據(jù)所述掃描上下文對的距離值，對當前幀的掃描上下文信息與歷史幀的掃描上下文信息之間的相似度進行評估，獲取相應(yīng)的評估結(jié)果。

36、進一步地，所述扇環(huán)單元盒的特征值的表達式包括：

37、f(pij)＝φ(pij)+ωti′max(p)；

38、其中，pij表示第i個環(huán)和第j個扇區(qū)構(gòu)成的扇環(huán)單元盒中的點的集合；f(pij)表示所述扇環(huán)單元盒的特征值；φ(pij)表示扇環(huán)單元盒pij內(nèi)點的最大高度值，即為所述代表高度值；i′max(p)表示預(yù)設(shè)數(shù)量的最近鄰點的最大強度均值，即為所述代表強度均值；ωt表示i′max(p)的權(quán)重值。

39、進一步地，包括：

40、剔除扇環(huán)單元盒內(nèi)強度值低于預(yù)設(shè)強度閾值的點；

41、對該扇環(huán)單元盒內(nèi)剩余的點進行近鄰點搜索，逐次取其所述預(yù)設(shè)數(shù)量的最近鄰點并分別求解它們相應(yīng)的強度均值；

42、從這些強度均值中選擇最大的作為該扇環(huán)單元盒的強度特征值。

43、進一步地，包括：

44、遍歷當前掃描幀在不同圓環(huán)中的特征點數(shù)量，根據(jù)這些特征點數(shù)量生成特征分布向量；所述特征分布向量的表達式為：

45、

46、

47、其中，||ri||0表示當前掃描幀中半徑為ri的圓環(huán)中非空扇環(huán)單元盒的個數(shù)；

48、對所述特征分布向量中的各個分量做比較，篩選出與當前掃描幀超過預(yù)設(shè)差異閾值的歷史幀；

49、將當前幀的掃描上下文信息與篩選出的歷史幀的掃描上下文信息進行比對，構(gòu)建掃描上下文對；

50、基于當前幀特征描述子與篩選出的歷史幀特征描述子之間的余弦距離，得到所述掃描上下文對的距離值，其表達式包括：

51、

52、其中，tq表示當前幀的特征描述子，tc為篩選出的歷史幀的特征描述子；是tq的第j列，是tc的第j列，和分別為相應(yīng)的范數(shù)；d(tq，tc)表示所述掃描上下文對的距離值。

53、總體而言，通過本技術(shù)所構(gòu)思的以上技術(shù)方案與現(xiàn)有技術(shù)相比，能夠取得下列有益效果：

54、(1)本技術(shù)的定位方法融合了點云數(shù)據(jù)的強度信息以優(yōu)化殘差，考慮了點云的空間分布，將點云的強度屬性納入計算，使得損失函數(shù)更加精細化，從而能夠通過損失函數(shù)的優(yōu)化過程利用變換矩陣來更精確地估計目標物的最終位姿，進而提升了對目標物的定位精度。

55、(2)本技術(shù)的地圖構(gòu)建方法基于所獲取的最終位姿估計值，通過掃描幀到局部地圖配準，利用點云的空間關(guān)系，將強度特征與傳統(tǒng)幾何特征進行了融合，構(gòu)建了融合特征描述符，實現(xiàn)了對周圍環(huán)境的有效建模和表示。這些豐富的融合特征描述符使得算法能夠在復(fù)雜環(huán)境下更加精確地執(zhí)行匹配操作，有效地進行回環(huán)閉合，提高了特征點的判別力，提升了slam系統(tǒng)對環(huán)境的認知能力，從而降低了誤差累積。

56、(3)本技術(shù)的地圖構(gòu)建方法通過計算當前幀特征描述子與歷史幀特征描述子之間的余弦距離，可以得到一個量化指標，用于評估兩者掃描上下文信息之間的相似程度。這種相似性度量方式既考慮了特征的數(shù)量分布，又兼顧了它們之間的相對關(guān)系，從而提高了回環(huán)檢測的準確性和魯棒性。