本技術(shù)涉及自動駕駛，更具體地，涉及一種基于激光slam的地圖定位與構(gòu)建方法。

背景技術(shù)：

1、隨著自動駕駛領(lǐng)域相關(guān)技術(shù)的不斷發(fā)展，擁有自主駕駛能力的車輛有望在未來成為交通運輸領(lǐng)域的重要組成部分，為我們的生活方式帶來了巨大的改變。其中，實現(xiàn)自身的精準定位是支撐自動駕駛下游應(yīng)用的前提，越高等級的自動駕駛對定位精度的要求越高。然而，傳統(tǒng)的基于高精度差分（differential?global?positioning?system，dgps）和慣性導(dǎo)航系統(tǒng)（inertial?navigation?system，ins）的定位方案在高架橋下、密集的高大建筑群內(nèi)和地下停車場等場景會出現(xiàn)精度差和定位漂移等現(xiàn)象，無法滿足自動駕駛持續(xù)精準定位的需求。slam（simultaneous?localization?and?mapping，slam，即：同步定位與建圖）憑借其搭載感知環(huán)境信息的傳感器實現(xiàn)了穩(wěn)定精準的自身定位且不受信號遮擋干擾的特點以及能夠輔助構(gòu)建高精地圖的特點，使得其在自動駕駛領(lǐng)域的應(yīng)用較廣泛。

2、然而，傳統(tǒng)的slam算法仍存在一些問題。首先，傳統(tǒng)slam算法僅依賴點云的幾何信息進行配準，容易出現(xiàn)誤匹配；其次，僅利用點云的幾何信息進行回環(huán)檢測的方法，在面臨相似場景時，容易出現(xiàn)誤檢測；此外，構(gòu)建出來的點云地圖表達能力較差，不利于后期的開發(fā)維護；最后，點云誤匹配和回環(huán)誤檢測會在slam系統(tǒng)中引入錯誤的約束信息，進而影響slam系統(tǒng)的整體定位精度和建圖質(zhì)量。

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

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

2、為實現(xiàn)上述目的，本技術(shù)提供了一種基于激光slam的地圖定位與構(gòu)建方法，包括：

3、獲取原始點云數(shù)據(jù)；

4、對所述原始點云數(shù)據(jù)進行語義分割預(yù)處理，提取每一幀點云中每個點的語義標簽信息，得到語義點云；

5、對所述語義點云的相鄰掃描幀通過融合語義和分布信息的點云配準算法進行配準，獲取相鄰幀間點云的相對位姿；

6、基于所述相鄰幀間點云的相對位姿，提取出局部地圖；

7、基于所述融合語義和分布信息的點云配準算法對所述局部地圖進行掃描幀到局部地圖的配準，以篩選出關(guān)鍵幀；

8、對所述關(guān)鍵幀進行回環(huán)檢測以獲取回環(huán)幀；

9、基于所述融合語義和分布信息的點云配準算法對所述回環(huán)幀進行配準，以獲取所述回環(huán)幀的幀間相對位姿；

10、將所述回環(huán)幀的幀間相對位姿以及所述關(guān)鍵幀和局部地圖的位姿進行聯(lián)合優(yōu)化，得到最終的語義點云地圖。

11、進一步地，所述融合語義和分布信息的點云配準算法包括：

12、分別對所述語義點云的相鄰掃描幀進行語義下采樣處理；

13、根據(jù)源點云的位姿構(gòu)建源點云和目標點云之間的語義關(guān)聯(lián)損失函數(shù)；

14、通過列文伯格-馬夸爾特非線性優(yōu)化算法對所述語義關(guān)聯(lián)損失函數(shù)進行循環(huán)求解，獲取源點云到目標點云的相對位姿變換矩陣。

15、進一步地，所述語義下采樣處理包括：

16、將所述語義點云的相鄰掃描幀輸入至語義下采樣器中，不同類別的點云使用不同的標志進行區(qū)分；

17、根據(jù)所述語義點云的所述語義標簽信息將不同類別的點云分別提取出來，構(gòu)成子點云；

18、所述語義下采樣器中不同標志的塊代表不同類別的點云數(shù)據(jù)，每一個類別的點云后面都跟隨著一個相應(yīng)的體素下采樣器。

19、進一步地，構(gòu)建所述語義關(guān)聯(lián)損失函數(shù)的方法包括：

20、采用最近鄰搜索算法在目標點云中找到源點云中每個點的最近鄰點并構(gòu)成最近鄰點集，基于所述最近鄰點集，在目標點云中找出每個點的歐氏最近鄰點集，則在該歐氏最近鄰點集中每個點的協(xié)方差矩陣的計算公式為：

21、

22、其中，表示所述歐氏最近鄰點集中的編號為的點；表示所述歐氏最近鄰點集的均值；表示的協(xié)方差矩陣；

23、引入所述語義標簽信息對點云的關(guān)聯(lián)過程進行優(yōu)化，令和分別為源點云和目標點云中的單個點，且是通過所述最近鄰搜索算法找到的對應(yīng)點，則該兩個點之間的歐式距離為，定義一個語義相關(guān)系數(shù)用于評估關(guān)聯(lián)的有效性，可以表示為：

24、；

25、其中，是一個小于1的常量，用于衡量語義標簽信息不匹配的相關(guān)系數(shù)；

26、所述語義關(guān)聯(lián)損失函數(shù)的表達式為：

27、；

28、其中，表示待求解的最優(yōu)的相對位姿；表示源點云中的的協(xié)方差矩陣；表示對應(yīng)目標點云中最近鄰點的協(xié)方差矩陣；表示源點云中的點和點對應(yīng)目標點云中的最近鄰點之間的歐氏距離。

29、進一步地，所述基于所述相鄰幀間點云的相對位姿，提取出局部地圖包括：

30、使用ikd-tree進行局部地圖的維持；

31、所述源點云到目標點云的相對位姿變換矩陣為局部地圖的維持提供初始位姿；

32、從所述初始位姿的區(qū)域提取所述局部地圖。

33、進一步地，所述基于所述融合語義和分布信息的點云配準算法對所述局部地圖進行掃描幀到局部地圖的配準，以篩選出關(guān)鍵幀包括：

34、基于所述融合語義和分布信息的點云配準算法對所述局部地圖進行掃描幀到局部地圖的配準，以獲取語義點云當(dāng)前幀的位姿估計；

35、若所述語義點云的當(dāng)前幀與前一幀的位移和/或旋轉(zhuǎn)超過第一預(yù)設(shè)閾值，則將該當(dāng)前幀標記為關(guān)鍵幀并提取出來。

36、進一步地，所述對所述關(guān)鍵幀進行回環(huán)檢測以獲取回環(huán)幀包括：

37、根據(jù)所述語義點云的所述關(guān)鍵幀的所述語義標簽信息，提取出具有持續(xù)不變性的點云；

38、基于融合語義與幾何信息的回環(huán)描述子對所述具有持續(xù)不變性的點云進行編碼，得到編碼矩陣；

39、基于評價編碼矩陣相似性算法對所述編碼矩陣進行相似度計算，獲取當(dāng)前關(guān)鍵幀的編碼矩陣與歷史關(guān)鍵幀的編碼矩陣的最高相似度；

40、若所述最高相似度小于第二預(yù)設(shè)閾值，則所述最高相似度對應(yīng)的當(dāng)前關(guān)鍵幀和歷史關(guān)鍵幀相應(yīng)的點云互為所述回環(huán)幀。

41、進一步地，所述融合語義與幾何信息的回環(huán)描述子的構(gòu)建方法包括：

42、使用scan-context的劃分方法，將所述具有持續(xù)不變性的點云中的每個點進行笛卡爾坐標到極坐標的轉(zhuǎn)換，得到；其中，表示點到激光雷達的水平距離，表示水平方向的方位角，表示垂直方向的俯仰角；

43、根據(jù)和將點云進行環(huán)扇塊劃分，得到共計個環(huán)扇塊；每一個環(huán)扇塊的表達式為：

44、

45、其中，和分別表示每個環(huán)扇塊的編號；下角標表示點云的序號，表示序號為的點的高度信息，表示序號為的點的語義標簽信息，表示序號為的點到激光雷達的水平距離，表示序號為的點在水平方向的方位角；表示激光雷達的最大掃描半徑；

46、遍歷每個環(huán)扇塊中的點云的高度值，找到點云中最大的高度值，并按照高度編碼矩陣進行賦值，如果沒有點落入當(dāng)前環(huán)扇塊，則令高度編碼矩陣，按照這種方法得到高度編碼矩陣；

47、遍歷每個環(huán)扇塊中的點云的語義標簽信息，找出優(yōu)先級最高的標簽，并按照高度編碼矩陣進行賦值，如果沒有點落入當(dāng)前環(huán)扇塊，則令語義編碼矩陣，按照這種方法得到語義編碼矩陣；

48、創(chuàng)建一個尺寸為的三維矩陣，在的其中一層存入高度編碼矩陣，在另一層存入語義編碼矩陣，最終得到融合高度信息和語義信息的聯(lián)合編碼矩陣，其表達式為：

49、。

50、進一步地，所述評價編碼矩陣相似性算法包括：

51、；

52、其中，表示兩個聯(lián)合編碼矩陣的相似度；和分別表示源點云和目標點云對應(yīng)的聯(lián)合編碼矩陣；和分別表示源點云和目標點云對應(yīng)的高度編碼矩陣；和分別表示源點云和目標點云對應(yīng)的語義編碼矩陣；和分別表示高度編碼矩陣間的相似度和語義編碼矩陣間的相似度；表示相似度權(quán)重系數(shù)；

53、

54、其中，為語義相關(guān)性函數(shù)，用于評價兩個環(huán)扇塊的關(guān)系，其表達式為：

55、。

56、進一步地，所述將所述回環(huán)幀的幀間相對位姿以及所述關(guān)鍵幀和局部地圖的位姿進行聯(lián)合優(yōu)化，得到最終的語義點云地圖包括：

57、使用?gtsam構(gòu)建因子圖；

58、基于所述因子圖，將所述回環(huán)幀的幀間相對位姿約束信息以及所述關(guān)鍵幀和局部地圖的位姿約束信息進行位姿聯(lián)合優(yōu)化，得到最終的經(jīng)過優(yōu)化后的位姿；

59、利用所述最終的經(jīng)過優(yōu)化后的位姿對點云進行坐標變換，得到所述最終的語義點云地圖。

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

61、（1）本技術(shù)提出了一種基于激光slam的地圖定位與構(gòu)建方法，該方法提出了融合語義和分布信息的點云配準算法，該算法可利用點云的語義標簽信息來優(yōu)化點云的關(guān)聯(lián)過程，可通過語義標簽信息對點云分配不同的下采樣參數(shù)，從而減小了點云的信息損失，降低了配準誤匹配的概率。本技術(shù)從slam方法的點云配準環(huán)節(jié)入手進行了優(yōu)化，可改善slam方法的定位精度和建圖質(zhì)量。

62、（2）本技術(shù)設(shè)計了一個語義下采樣方法對點云進行自定義下采樣，通過語義標簽信息對點云分配不同的下采樣參數(shù)，從而減小了點云的信息損失，并定義了一套新的語義配準損失函數(shù)，從而降低了誤匹配概率并優(yōu)化了損失函數(shù)的構(gòu)建。

63、（3）本技術(shù)提出了融合語義與幾何信息的回環(huán)描述子，通過結(jié)合點云高維語義信息以及點云的幾何信息進行聯(lián)合編碼，從而提高了編碼矩陣的代表性，提升了構(gòu)建出來的點云地圖的表達能力，更有利于后期的開發(fā)維護。同時，由于融合了點云的高維語義信息以及點云的幾何信息進行回環(huán)檢測，從而降低了回環(huán)檢測時出現(xiàn)誤檢測的概率。本技術(shù)提出了一種用于評價編碼矩陣相似性的計算方法，用于檢測具有相似編碼矩陣對應(yīng)的點云是否存在回環(huán)關(guān)系，從而進一步降低了回環(huán)檢測時出現(xiàn)誤檢測的概率。

64、（4）本技術(shù)將上述改進融入進slam框架中，構(gòu)成了一套完整的新型語義slam方法。本技術(shù)從slam方法的點云配準環(huán)節(jié)和回環(huán)檢測環(huán)節(jié)入手，提出了上述針對性的優(yōu)化方案，從而在整體層面改善了slam方法的定位精度和建圖質(zhì)量。