本發(fā)明涉及一種基于偽占據(jù)柵格描述符的激光慣性動態(tài)slam系統(tǒng)，屬于移動機(jī)器人自主導(dǎo)航與建圖。

背景技術(shù)：

1、slam（同時定位和建圖）對于移動機(jī)器人在陌生環(huán)境中安全運動至關(guān)重要。近年來，基于激光慣性的slam解決方案在移動機(jī)器人自主移動和導(dǎo)航領(lǐng)域受到越來越多的關(guān)注。傳統(tǒng)的激光慣性slam系統(tǒng)的數(shù)據(jù)預(yù)處理、特征匹配等環(huán)節(jié)均基于靜態(tài)假設(shè)：周圍環(huán)境中的物體是靜止不動的。在實際場景中，移動機(jī)器人的作業(yè)環(huán)境中存在移動車輛、人員等動態(tài)物體。這些動態(tài)物體會將壞值引入到slam的特征匹配環(huán)節(jié)，從而影響最終位姿估計的準(zhǔn)確度和建圖精度。目前大部分針對動態(tài)場景的slam方法大部分采用基于深度學(xué)習(xí)的方法，這些方法嚴(yán)重依賴于預(yù)先提供的先驗信息，在復(fù)雜場景下面臨計算量大、魯棒性差等問題。此外，還有一些方法僅僅通過對比當(dāng)前掃描點云與局部地圖之間的差異性來去除動態(tài)點云，但是這些方法大部分是針對局部地圖設(shè)計的，對當(dāng)前點云幀中的動態(tài)點云濾除效果較差。

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

1、本發(fā)明旨在解決以上現(xiàn)有的技術(shù)問題，提出一種基于偽占據(jù)柵格描述符的激光慣性動態(tài)slam系統(tǒng)。

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

3、一種基于偽占據(jù)柵格描述符的激光慣性動態(tài)slam系統(tǒng)，該系統(tǒng)包括激光慣性slam子系統(tǒng)、地面約束子系統(tǒng)、動態(tài)點云濾除子系統(tǒng)。

4、所述激光慣性slam子系統(tǒng)由點云預(yù)處理模塊、imu預(yù)積分模塊、特征匹配模塊、后端優(yōu)化模塊、子圖構(gòu)建模塊組成。所述點云預(yù)處理模塊實時獲取激光雷達(dá)點云數(shù)據(jù)，并對點云數(shù)據(jù)進(jìn)行濾波和去除運動畸變；imu預(yù)積分模塊根據(jù)系統(tǒng)優(yōu)化后的位姿和imu數(shù)據(jù)得到下一幀的初始位姿；特征匹配模塊通過匹配當(dāng)前點云數(shù)據(jù)和局部地圖，獲取激光雷達(dá)的位姿數(shù)據(jù)；后端優(yōu)化模塊使用圖優(yōu)化的方式獲取最終優(yōu)化后的雷達(dá)位姿。

5、所述動態(tài)點云濾除模塊由同心圓區(qū)域提取模塊、偽占據(jù)柵格描述符轉(zhuǎn)化模塊、當(dāng)前幀動態(tài)點云濾除模塊、局部地圖動態(tài)點云去除模塊組成。所述同心圓區(qū)域提取模塊根據(jù)激光雷達(dá)點云分布的特性，提取以雷達(dá)中心為中心，設(shè)定距離為半徑的同心圓內(nèi)的點云；偽占據(jù)柵格描述符轉(zhuǎn)化模塊將同心圓區(qū)域提取到的點云根據(jù)極坐標(biāo)系的形式轉(zhuǎn)化到不同的柵格中；當(dāng)前幀動態(tài)點云濾除模塊通過對比當(dāng)前幀激光雷達(dá)點云信息和歷史幀的描述符高度特征差異去除當(dāng)前幀中的動態(tài)點云；局部地圖動態(tài)點云濾除模塊通過對比當(dāng)前幀激光雷達(dá)點云和局部地圖的描述符高度特征差異去除局部地圖中的動態(tài)點云。

6、所述地面約束子系統(tǒng)由地面分割模塊、地面擬合模塊、地面約束構(gòu)建模塊組成。所述地面分割模塊從當(dāng)前幀靜態(tài)點云數(shù)據(jù)和局部地圖靜態(tài)點云數(shù)據(jù)中分割出地面點云和非地面點云；地面擬合模塊根據(jù)地面點云擬合出地平面的參數(shù)；地面約束構(gòu)建模塊根據(jù)地平面參數(shù)得到當(dāng)前地面約束因子。

7、動態(tài)點云濾除模塊中局部地圖動態(tài)點云濾除方法包括以下步驟：

8、s1:提取同心圓區(qū)域內(nèi)的經(jīng)過預(yù)處理的雷達(dá)點云數(shù)據(jù)和激光慣性slam系統(tǒng)實時構(gòu)建的局部地圖點云數(shù)據(jù)，同心圓區(qū)域的半徑和高度如下：

9、

10、s2:將同心圓區(qū)域內(nèi)提取的點云從激光雷達(dá)坐標(biāo)系根據(jù)激光慣性slam子系統(tǒng)中imu預(yù)積分模塊得到的imu里程計位姿轉(zhuǎn)到世界坐標(biāo)系下，具體表示如下：

11、

12、s3:?將雷達(dá)點云數(shù)據(jù)從直角坐標(biāo)系格式轉(zhuǎn)為極坐標(biāo)形式，其中，。同心圓區(qū)域根據(jù)方位角和徑向規(guī)則劃分為一個個扇形區(qū)域，具體表示如下：

13、

14、這里的,表示徑向劃分和方位角劃分的數(shù)量。根據(jù)激光點云極坐標(biāo)將點云投影到扇形區(qū)域中，記錄每個扇形區(qū)域點云值最大值和最小值，每個扇形區(qū)域的偽占用概率為。

15、s4:根據(jù)s3中得到的當(dāng)前幀雷達(dá)點云的和局部地圖的，

16、遍歷每一個扇形區(qū)域，計算當(dāng)前幀點云和局部地圖點云在該扇形區(qū)域內(nèi)的掃描高度比并表示如下：

17、

18、根據(jù)計算得到數(shù)值與設(shè)定閾值進(jìn)行比較，確定出疑似動態(tài)區(qū)域，并對疑似動態(tài)區(qū)域的相鄰柵格區(qū)域進(jìn)行檢測。

19、所述步驟s4，具體包括以下過程：

20、s4.1遍歷同心圓區(qū)域內(nèi)的劃分的柵格區(qū)域，根據(jù)的點云數(shù)量初步剔除掉點云數(shù)量較少的柵格區(qū)域，減少后續(xù)計算量。

21、s4.2對于當(dāng)前掃描點云和局部地圖點云數(shù)量均滿足設(shè)定閾值要求的區(qū)域計算該區(qū)域的掃描高度比。如果當(dāng)前區(qū)域，則認(rèn)為該區(qū)域當(dāng)前掃描點云和局部地圖點云差異較小，判定為待檢查區(qū)域；如果當(dāng)前區(qū)域，則認(rèn)為該區(qū)域當(dāng)前掃描點云和局部地圖點云差異較大，然后將滿足的區(qū)域判定為疑似動態(tài)區(qū)域。

22、s4.3?循環(huán)遍歷疑似動態(tài)區(qū)域，檢查疑似動態(tài)區(qū)域內(nèi)局部地圖的點云高度差是否大于設(shè)定的正常移動物體的最小高度閾值。如果高度差大于閾值，則判定該區(qū)域局部地圖為動態(tài)區(qū)域。

23、s5:對滿足的柵格區(qū)域進(jìn)行鄰域柵格檢測，若該柵格的鄰近柵格均為疑似動態(tài)區(qū)域，則判定該柵格也為疑似動態(tài)區(qū)域。

24、進(jìn)一步的，步驟s5具體實現(xiàn)如下：

25、s5.1對判定為動態(tài)區(qū)域的扇形柵格區(qū)域內(nèi)的點云進(jìn)行地面分割。首先對該區(qū)域內(nèi)的點云進(jìn)行z軸方向高度排序，從低到高將一定數(shù)量的點云作為初始點云進(jìn)行擬合，然后重新計算其他點云到該平面的距離，將距離較小的點云添加到該平面集合中，重復(fù)執(zhí)行這個步驟3次得到最終的地面點云和非地面點云。

26、s5.2?將動態(tài)區(qū)域內(nèi)的地面點云從該區(qū)域點云集合中去除，獲得該區(qū)域的動態(tài)點云。

27、s5.3?使用該區(qū)域當(dāng)前掃描的點云替代局部地圖中該區(qū)域原有點云。

28、s6:對疑似動態(tài)區(qū)域進(jìn)行地面點云分割，該區(qū)域內(nèi)的非地面點云被判定為動態(tài)點云，并用當(dāng)前幀該區(qū)域的點云替代局部地圖中該區(qū)域的點云。

29、進(jìn)一步的，動態(tài)點云濾除模塊中當(dāng)前幀動態(tài)點云濾除方法包括以下步驟：

30、（1）:將經(jīng)過預(yù)處理的當(dāng)前幀激光雷達(dá)點云和對應(yīng)的imu里程計位姿分別存儲到隊列中。

31、（2）:將當(dāng)前幀激光雷達(dá)點云和隊列中存儲的幀激光雷達(dá)點云轉(zhuǎn)換到世界坐標(biāo)系下，具體實現(xiàn)形式如下：

32、

33、其中，為當(dāng)前幀點云位姿，為幀點云位姿。

34、3):將步驟(2)中世界坐標(biāo)系下的點云和通過局部地圖動態(tài)點云去除模塊(這里的意思是和激光雷達(dá)轉(zhuǎn)化為偽占據(jù)柵格描述符的方式一樣，并不是直接通過局部地圖動態(tài)點云去除模塊)得到點云和對應(yīng)的偽占據(jù)柵格描述符。

35、（4）:遍歷每個扇形柵格區(qū)域，根據(jù)掃描高度比，找出當(dāng)前幀點云中疑似動態(tài)區(qū)域。

36、（5）:對疑似動態(tài)區(qū)域中的點云進(jìn)行自由空間檢測；

37、s5-1?遍歷當(dāng)前幀點云中疑似動態(tài)區(qū)域內(nèi)的點云，對附近的點云進(jìn)行平面擬合。

38、s5-2?尋找?guī)袙呙璧揭伤苿討B(tài)點云的那束激光，若該激光穿過了該點云附近的平面，則認(rèn)為該點云是動態(tài)點云；若該激光束打到了該點云附近的平面上，則認(rèn)為該點云是靜態(tài)點云。

39、（6）:對完成自由空間檢測的當(dāng)前幀動態(tài)點云進(jìn)行局部平面約束檢測。

40、s6-1?遍歷經(jīng)過自由空間檢測的動態(tài)點云，使用最近鄰搜索查詢該點云附近的幀中的激光點云。

41、s6-2?使用主成分分析法擬合查詢幀中的激光點云所在的平面，計算當(dāng)前動態(tài)點云距離該平面的距離，若距離小于設(shè)定閾值，則認(rèn)為該點云是誤判點，將其恢復(fù)為靜態(tài)點云到當(dāng)前掃描點云集合中。

42、（7）:將步驟（6）中獲取的動態(tài)點云作為初始點云，通過dbscan聚類算法得到最終的動態(tài)點云。最后將當(dāng)前幀點云減去動態(tài)點云，得到當(dāng)前幀激光雷達(dá)靜態(tài)點云。

43、優(yōu)選的，所述步驟（5）和（6）為對疑似動態(tài)區(qū)域中的進(jìn)行自由空間檢測和局部平面約束檢測，目的都是為了減少錯誤動態(tài)點云的濾除。

44、本發(fā)明中的地面約束子系統(tǒng)的工作流程包括以下步驟：

45、①：將經(jīng)過動態(tài)點云濾除的點云進(jìn)行高度排序，選取最低的n個點云視為初始地面點云。

46、②：對初始地面點云進(jìn)行主成分分析，得到初始地平面模型參數(shù)。

47、③：計算當(dāng)前幀所有點云與平面模型的距離值，進(jìn)一步做距離閾值判定；得到新的地面點云和非地面點云。

48、④：重復(fù)上述步驟②、③直至得到最終的地面點云和地平面模型參數(shù)。

49、⑤：根據(jù)當(dāng)前幀地平面模型參數(shù)和局部地圖地平面模型參數(shù)構(gòu)建地面約束并添加到激光慣性子系統(tǒng)的后端優(yōu)化模塊中。

50、5-1平面通過表示，將步驟④中擬合得到的當(dāng)前局部地圖地面根據(jù)imu預(yù)測位姿從世界坐標(biāo)系轉(zhuǎn)到所在的雷達(dá)坐標(biāo)系下，具體形式如下：

51、

52、其中，為激光雷達(dá)坐標(biāo)系下局部地圖地面的平面法向量，

53、是由、和組成的平面法向量；為激光雷達(dá)坐標(biāo)系下局部地圖地面的截距，表示平面截距；和為世界坐標(biāo)系轉(zhuǎn)到當(dāng)前激光雷達(dá)坐標(biāo)系的變換矩陣的旋轉(zhuǎn)分量和平移分量。

54、5-2根據(jù)當(dāng)前幀激光點云地面和轉(zhuǎn)到雷達(dá)坐標(biāo)系下的局部地圖地面分別構(gòu)建地面約束方程，表示如下：

55、

56、其中，為平面法向量軸方向上的分量，為平面法向量軸方向上的分量；表示定義，即將地面約束方程定義為；

57、5-3根據(jù)地面約束方程構(gòu)建地面約束殘差模型，具體表示如下：

58、

59、其中，、、分別表示的、；

60、5-4由殘差模型計算得到雅可比矩陣，在后端優(yōu)化模塊中添加自定義地面因子。具體的，將殘差模型輸入到gtsam庫的express自動求導(dǎo)獲得雅可比矩陣。

61、本發(fā)明有益效果如下：

62、提出了一種基于偽占據(jù)柵格描述符的激光慣性動態(tài)slam系統(tǒng)，能夠高效精確地去除當(dāng)前幀和局部子圖中的動態(tài)點云，消除復(fù)雜動態(tài)環(huán)境中的動態(tài)物體對slam系統(tǒng)的干擾，提高slam系統(tǒng)定位精度。通過在激光慣性動態(tài)slam系統(tǒng)中添加當(dāng)前幀點云濾除模塊并使用偽占據(jù)柵格描述符來快速判斷當(dāng)前幀動態(tài)區(qū)域，極大地提高了當(dāng)前幀動態(tài)點云濾除速度和最終的動態(tài)點云濾除效果；通過添加地面約束子系統(tǒng)，降低了slam系統(tǒng)在大規(guī)模動態(tài)場景的高程誤差，提高了動態(tài)點云濾除模塊中地面點云濾除部分的穩(wěn)定性，從而提高動態(tài)點云濾除效果和建圖效果。