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

背景技術(shù)：

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

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

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

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

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

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

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

4、獲取激光雷達(dá)點(diǎn)云數(shù)據(jù)；

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

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

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

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

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

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

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

12、計(jì)算像素塊在其中心像素點(diǎn)處的二階矩，用二階矩的最大特征值對(duì)應(yīng)的特征向量表示像素塊的像素梯度；

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

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

15、進(jìn)一步地，基于激光雷達(dá)點(diǎn)云數(shù)據(jù)生成強(qiáng)度圖像時(shí)，通過(guò)將激光雷達(dá)點(diǎn)云數(shù)據(jù)按照球面投影模型投影生成強(qiáng)度圖像。

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

17、獲取計(jì)算得到的系統(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、通過(guò)拉格朗日乘子法，計(jì)算在引入約束方程條件下重新得到的系統(tǒng)狀態(tài)增量。

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

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

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

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

24、計(jì)算點(diǎn)面匹配的殘差和強(qiáng)度圖像的匹配殘差；

25、基于計(jì)算得到的強(qiáng)度圖像的匹配殘差更新雅可比矩陣；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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