本發(fā)明屬于機器人同時定位和建圖，尤其涉及基于全約束的3d激光slam方法、系統(tǒng)、介質(zhì)及設備，可以實現(xiàn)在特征稀疏的退化環(huán)境下高精度、高穩(wěn)健性的定位和地圖構(gòu)建。

背景技術(shù)：

1、本部分的陳述僅僅是提供了與本發(fā)明相關(guān)的背景技術(shù)信息，不必然構(gòu)成在先技術(shù)。

2、近年來，自動駕駛汽車、智能巡檢機器人等一系列自動化產(chǎn)品的發(fā)展，推動了機器人自主移動技術(shù)的發(fā)展。同時定位和建圖（slam）作為其中的關(guān)鍵技術(shù)，受到廣泛關(guān)注。而3d激光slam以其精度高的特點，成為slam發(fā)展的重要方向之一?，F(xiàn)有的3d激光slam通常會融合慣性測量單元（imu），為系統(tǒng)提供高頻運動數(shù)據(jù)。但是在長走廊、隧道、室外空曠地區(qū)等場景下，由于周圍環(huán)境特征稀疏，激光雷達在某些方向會發(fā)生退化，這會降低系統(tǒng)定位和建圖精度甚至會導致slam系統(tǒng)發(fā)生錯誤。

3、目前解決3d激光slam退化的主要方法是引入額外的傳感器，如相機、輪速計、雷達等。許多3d激光slam系統(tǒng)會融合imu，為系統(tǒng)提供預測值。融合輪速計能提供額外的速度觀測，為系統(tǒng)提供更精確的預測值，能提高slam系統(tǒng)的性能。融合相機能提高slam系統(tǒng)對外界環(huán)境的信息獲取，但引入更多的傳感器意味著引入更多的噪聲，并且需要對相機標定，增加了工作量，同時，在光線昏暗的場合相機難以正常工作，不利于slam系統(tǒng)對周圍環(huán)境信息的獲取。

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

1、為了解決上述背景技術(shù)中存在的至少一項技術(shù)問題，本發(fā)明提供基于全約束的3d激光slam方法、系統(tǒng)、介質(zhì)及設備，在特征稀疏的場合，設計了一種融合輪速計以及利用激光雷達的反射強度信息的slam系統(tǒng)，能顯著提高slam系統(tǒng)對周圍環(huán)境信息的獲取，提高slam系統(tǒng)的穩(wěn)健性。

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

3、本發(fā)明的第一方面提供基于全約束的3d激光slam方法，包括如下步驟：

4、獲取激光雷達點云數(shù)據(jù)；

5、基于激光雷達點云數(shù)據(jù)得到點面匹配的信息矩陣，對點面匹配的信息矩陣中僅與旋轉(zhuǎn)變量和位移變量相關(guān)的子塊進行特征分解，得到多個特征向量，將每個特征向量作為一個優(yōu)化方向，計算點面匹配在各優(yōu)化方向上的約束貢獻值；

6、基于激光雷達點云數(shù)據(jù)生成強度圖像，通過強度圖像幀間匹配計算得到強度圖像在各優(yōu)化方向上的約束貢獻值；

7、比較點面匹配和強度圖像幀間匹配在各優(yōu)化方向上的約束貢獻值大小，選擇貢獻值大的一方，構(gòu)建得到全約束觀測；

8、對全約束觀測進行退化檢測，若發(fā)生退化，在退化方向上添加額外約束后進行全約束狀態(tài)更新，否則直接進行全約束狀態(tài)更新，得到全約束觀測系統(tǒng)增量并更新系統(tǒng)狀態(tài)。

9、進一步地，基于激光雷達點云數(shù)據(jù)得到點面匹配的信息矩陣之前，利用系統(tǒng)狀態(tài)的預測值對每幀點云數(shù)據(jù)進行去畸變處理，其中，系統(tǒng)狀態(tài)的預測值的計算方法為：當僅有imu數(shù)據(jù)時，根據(jù)imu預測模型進行向前傳播得到預測值，當接收到輪速計數(shù)據(jù)時，通過擴展卡爾曼濾波器融合imu預測數(shù)據(jù)和輪速計數(shù)據(jù)，得到預測值。

10、進一步地，所述通過強度圖像幀間匹配計算得到強度圖像在各優(yōu)化方向上的約束貢獻值，包括：

11、在強度圖像上選擇像素塊后，根據(jù)給予激光雷達位姿的擾動，計算像素塊中心像素對應的激光點投影到強度圖像上坐標對該擾動的雅可比矩陣，即為投影梯度，投影梯度在各優(yōu)化方向上投影得到各優(yōu)化方向上的投影梯度；

12、計算像素塊在其中心像素點處的二階矩，用二階矩的最大特征值對應的特征向量表示像素塊的像素梯度；

13、計算各優(yōu)化方向上的投影梯度在像素塊的像素梯度方向上的投影，即為該像素塊對優(yōu)化方向上的約束貢獻值；

14、計算所有像素塊的約束貢獻值的和，得到強度圖像在各優(yōu)化方向上的約束貢獻值。

15、進一步地，基于激光雷達點云數(shù)據(jù)生成強度圖像時，通過將激光雷達點云數(shù)據(jù)按照球面投影模型投影生成強度圖像。

16、進一步地，所述在退化方向上添加額外約束后進行全約束狀態(tài)更新包括：

17、獲取計算得到的系統(tǒng)狀態(tài)變量增量中與系統(tǒng)姿態(tài)和位置有關(guān)的部分；

18、構(gòu)建在退化方向上的更新約束，結(jié)合系統(tǒng)狀態(tài)變量增量中與系統(tǒng)姿態(tài)和位置有關(guān)的部分，得到約束方程；

19、通過拉格朗日乘子法，計算在引入約束方程條件下重新得到的系統(tǒng)狀態(tài)增量。

20、進一步地，在構(gòu)建全約束觀測時，包括計算強度圖像的光度誤差和點云數(shù)據(jù)的幾何殘差；

21、其中，采用直接法得到兩幀強度圖像之間的光度誤差，然后通過強度圖像幀間匹配計算得到強度圖像在各優(yōu)化方向上的約束貢獻值；

22、其中，對于該幀激光雷達點云中的點，在局部地圖中搜索與之匹配的面，構(gòu)建點面之間的距離殘差，得到點云數(shù)據(jù)的幾何殘差。

23、進一步地，全約束狀態(tài)更新，包括如下步驟：

24、計算點面匹配的殘差和強度圖像的匹配殘差；

25、基于計算得到的強度圖像的匹配殘差更新雅可比矩陣；

26、融合點面匹配的殘差和更新后的雅可比矩陣，得到全約束觀測的殘差和雅可比矩陣；

27、基于全約束觀測的殘差和雅可比矩陣得到幾何約束的系統(tǒng)增量和強度約束的系統(tǒng)增量，求和后得到全約束觀測系統(tǒng)增量。

28、本發(fā)明的第二方面提供基于全約束的3d激光slam系統(tǒng)，包括：

29、數(shù)據(jù)獲取模塊，其用于獲取激光雷達點云數(shù)據(jù)；

30、幾何約束模塊，其用于基于激光雷達點云數(shù)據(jù)得到點面匹配的信息矩陣，對點面匹配的信息矩陣中僅與旋轉(zhuǎn)變量和位移變量相關(guān)的子塊進行特征分解，得到多個特征向量，將每個特征向量作為一個優(yōu)化方向，計算點面匹配在各優(yōu)化方向上的約束貢獻值；

31、強度約束模塊，其用于基于激光雷達點云數(shù)據(jù)生成強度圖像，通過強度圖像幀間匹配計算得到強度圖像在各優(yōu)化方向上的約束貢獻值；

32、全約束構(gòu)建模塊，其用于比較點面匹配和強度圖像幀間匹配在各優(yōu)化方向上的約束貢獻值大小，選擇貢獻值大的一方，構(gòu)建得到全約束觀測；

33、狀態(tài)更新模塊，其用于對全約束觀測進行退化檢測，若發(fā)生退化，在退化方向上添加額外約束后進行全約束狀態(tài)更新，否則直接進行全約束狀態(tài)更新，得到全約束觀測系統(tǒng)增量并更新系統(tǒng)狀態(tài)。

34、本發(fā)明的第三方面提供一種計算機可讀存儲介質(zhì)。

35、一種計算機可讀存儲介質(zhì)，其上存儲有計算機程序，該程序被處理器執(zhí)行時實現(xiàn)如上述所述的基于全約束的3d激光slam方法中的步驟。

36、本發(fā)明的第四方面提供一種計算機設備。

37、一種計算機設備，包括存儲器、處理器及存儲在存儲器上并可在處理器上運行的計算機程序，所述處理器執(zhí)行所述程序時實現(xiàn)如上述所述的基于全約束的3d激光slam方法中的步驟。

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

39、1、本發(fā)明通過構(gòu)建幾何約束和強度約束，通過選擇點面匹配和強度圖像幀間匹配在優(yōu)化方向上貢獻值更大的一方，構(gòu)建全約束觀測，并對全約束觀測進行退化檢測和退化處理，實現(xiàn)各方向均得到有效約束的salm系統(tǒng)。

40、2、當檢測到系統(tǒng)某些方向發(fā)生退化時，在退化方向上額外引入約束，使用拉格朗日乘子法，修正系統(tǒng)的全約束狀態(tài)更新，提高了slam系統(tǒng)的穩(wěn)健性。

41、3、在系統(tǒng)更新時，通過推導強度圖像幀間匹配的殘差及相應的雅可比矩陣，結(jié)合點云點面匹配的殘差和雅可比矩陣，共同實現(xiàn)對系統(tǒng)的全約束狀態(tài)更新，進一步提高了slam系統(tǒng)的穩(wěn)健性。

42、本發(fā)明附加方面的優(yōu)點將在下面的描述中部分給出，部分將從下面的描述中變得明顯，或通過本發(fā)明的實踐了解到。