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

背景技術(shù)：

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

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

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

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

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

3、獲取目標(biāo)物移動(dòng)時(shí)的原始點(diǎn)云數(shù)據(jù)，提取特征指標(biāo)；

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

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

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

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

8、

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

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

11、進(jìn)一步地，所述目標(biāo)物的最終位姿估計(jì)值的表達(dá)式包括：

12、

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

14、進(jìn)一步地，所述邊緣特征點(diǎn)pe到全局地圖線特征的距離的表達(dá)式包括：

15、

16、

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

18、所述平面特征點(diǎn)ps到全局地圖面特征的距離的表達(dá)式包括：

19、

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

21、進(jìn)一步地，所述匹配評判值的表達(dá)式包括：

22、

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

24、所述特征點(diǎn)最近鄰點(diǎn)強(qiáng)度分布值的表達(dá)式包括：

25、

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

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

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

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

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

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

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

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

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

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

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

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

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

39、進(jìn)一步地，包括：

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

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

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

43、進(jìn)一步地，包括：

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

45、

46、

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

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

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

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

51、

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

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

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

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

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