本發(fā)明屬于自動駕駛車輛定位，特別涉及一種用于井工礦動態(tài)環(huán)境的融合定位方法和系統(tǒng)。

背景技術：

1、在礦山環(huán)境中，自動駕駛技術的應用可以大大提高礦山作業(yè)的效率和安全性。然而，礦山環(huán)境復雜，包括長走廊、岔路口等各種復雜的地形，這對自動駕駛技術提出了更高的要求。同時，定位技術也是自動駕駛技術的重要組成部分，它決定了自動駕駛車輛在環(huán)境中的位置和姿態(tài)，從而影響到自動駕駛的安全性和準確性。

2、在現(xiàn)有的技術中，通常采用激光slam（simultaneous?localization?andmapping）進行建圖和定位。激光slam是一種通過激光雷達獲取環(huán)境信息，并同時進行定位和地圖構建的技術，但是井下環(huán)境復雜，長走廊效應十分明顯，動態(tài)點對定位的影響極大。其次，還會使用odom（里程計）、imu（慣性測量單元）、lidar（激光雷達）以及uwb的融合定位。里程計信息過解析can報文數(shù)據(jù)，讀取左右車輪的速度信息，簡化模型，構建后輪差速模型，從而推導出以后輪軸中點為中心的里程計模型。但是，現(xiàn)有的定位技術一般使用imu、odom、lidar、uwb的多傳感器融合定位，odom來自于底盤信息，累積誤差較大。因此，如何在井工礦環(huán)境下實現(xiàn)自動駕駛的動態(tài)點剔除，降低輸入的累積誤差，提高定位的精度和穩(wěn)定性，是當前亟待解決的問題。

技術實現(xiàn)思路

1、為了解決上述技術問題，本發(fā)明提出了一種用于井工礦動態(tài)環(huán)境的融合定位方法和系統(tǒng)，剔除環(huán)境中動態(tài)障礙物對長走廊環(huán)境下定位的影響，提高定位的準確性和魯棒性。

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

3、一種用于井工礦動態(tài)環(huán)境的融合定位方法，包括以下步驟：

4、將獲取的目標車輛的歷史激光雷達數(shù)據(jù)、歷史imu數(shù)據(jù)和歷史里程計數(shù)據(jù)分別進行時間同步和空間標定后，進行slam建圖得到點云地圖；對所述點云地圖預處理后得到用于定位的先驗地圖；

5、獲取目標車輛的第一實時點云數(shù)據(jù)，并對第一實時點云數(shù)據(jù)中不在同一坐標系下的點云進行配準；然后采用聚類算法判斷第一實時點云數(shù)據(jù)的動態(tài)性，對第一實時點云數(shù)據(jù)進行動態(tài)點剔除得到第二實時點云數(shù)據(jù)；

6、獲取目標車輛的第一實時imu數(shù)據(jù)、第一底盤車速信息和第一slam位姿；從第一實時imu數(shù)據(jù)獲取航向角，從第一slam位姿中獲取初始位姿；利用初始姿態(tài)和單位時間位姿變換不斷疊加得到更新后里程計信息；

7、將更新后里程計信息、第二實時點云數(shù)據(jù)和第一實時imu數(shù)據(jù)進行卡爾曼融合定位輸出最終融合定位信息。

8、進一步的，時間同步的過程包括：利用ptp時間同步策略對歷史激光雷達數(shù)據(jù)和歷史imu數(shù)據(jù)進行時間同步。

9、進一步的，所述進行slam建圖得到點云地圖的過程包括：

10、將標定后的歷史激光雷達數(shù)據(jù)、歷史imu數(shù)據(jù)和歷史里程計數(shù)據(jù)均輸入至即時定位與地圖構建算法中得到點云地圖。

11、進一步的，獲取目標車輛的第一實時點云數(shù)據(jù)，并對第一實時點云數(shù)據(jù)中不在同一坐標系下的點云進行配準的過程包括：

12、獲取激光雷達的連續(xù)掃描幀，且每一幀包括相同數(shù)量的點；

13、對不在同一坐標系下的點云數(shù)據(jù)采用迭代最近點算法進行配準，用于確保點云數(shù)據(jù)之間的準確對齊。

14、進一步的，對不在同一坐標系下的點云數(shù)據(jù)采用迭代最近點算法進行配準的具體過程包括：

15、計算相鄰點之間的歐式距離：

16、；

17、其中，為第次掃描點云的位置；為第次掃描點云的位置；

18、如果在一個時間窗口內的總掃描次數(shù)為，點出現(xiàn)的次數(shù)為，且每次出現(xiàn)時的位置變化低于位置閾值，則計算一個時間窗口的一致性評分為：

19、；

20、如果一致性評分小于評分閾值，計算其出現(xiàn)位置的標準差：

21、；

22、其中，是點的次出現(xiàn)的平均位置，為標準差；

23、利用決策公式確定點的動態(tài)性：

24、；

25、其中，為標準差最小閾值；為一致性評分最大閾值。

26、進一步的，采用聚類算法判斷第一實時點云數(shù)據(jù)的動態(tài)性的具體過程包括：

27、；

28、其中，為實時點云數(shù)據(jù)在時間的聚類質心；為實時點云數(shù)據(jù)在時間的聚類質心；為相鄰時刻點云數(shù)據(jù)的質心偏移；

29、；

30、其中，為實時點云數(shù)據(jù)在時間聚類的形狀描述符；為實時點云數(shù)據(jù)在時間聚類的形狀描述符；為實時點云數(shù)據(jù)在連續(xù)時間幀的差異。

31、進一步的，對第一實時點云數(shù)據(jù)進行動態(tài)點剔除的過程包括：

32、；

33、其中，為質心偏移的預設閾值；為形狀變化的預設閾值；

34、如果滿足質心偏移大于質心偏移的預設閾值，或者形狀變化大于形狀變化的預設閾值之一，則認定為動態(tài)點，標記為1，否則為非動態(tài)點，標記為0；

35、采用統(tǒng)計離群值移除算法，對動態(tài)點進行剔除。

36、進一步的，利用初始姿態(tài)和單位時間位姿變換不斷疊加得到更新后里程計信息的過程包括：初始姿態(tài)與單位時間位姿變換相乘，得到完整的位態(tài)信息，并發(fā)布里程計到徹底的坐標位置關系得到更新后里程計信息。

37、進一步的，將更新后里程計信息、第二實時點云數(shù)據(jù)和第一實時imu數(shù)據(jù)進行卡爾曼融合定位的過程包括：

38、將第一實時imu數(shù)據(jù)或者更新后里程計信息作為預測值，將第二實時點云數(shù)據(jù)和將先驗地圖輸出的點云地圖做ndt掃描匹配得到的結果作為觀測值，融合定位輸出最終融合定位信息。

39、本發(fā)明還提出了一種用于井工礦動態(tài)環(huán)境的融合定位系統(tǒng)，包括預處理模塊、配準剔除模塊、更新模塊和融合定位模塊；

40、所述預處理模塊用于將獲取的目標車輛的歷史激光雷達數(shù)據(jù)、歷史imu數(shù)據(jù)和歷史里程計數(shù)據(jù)分別進行時間同步和空間標定后，進行slam建圖得到點云地圖；對所述點云地圖預處理后得到用于定位的先驗地圖；

41、所述配準剔除模塊用于獲取目標車輛的第一實時點云數(shù)據(jù)，并對第一實時點云數(shù)據(jù)中不在同一坐標系下的點云進行配準；然后采用聚類算法判斷第一實時點云數(shù)據(jù)的動態(tài)性，對第一實時點云數(shù)據(jù)進行動態(tài)點剔除得到第二實時點云數(shù)據(jù)；

42、所述更新模塊用于獲取目標車輛的第一實時imu數(shù)據(jù)、第一底盤車速信息和第一slam位姿；從第一實時imu數(shù)據(jù)獲取航向角，從第一slam位姿中獲取初始位姿；利用初始姿態(tài)和單位時間位姿變換不斷疊加得到更新后里程計信息；

43、所述融合定位模塊用于將更新后里程計信息、第二實時點云數(shù)據(jù)和第一實時imu數(shù)據(jù)進行卡爾曼融合定位輸出最終融合定位信息。

44、
技術實現(xiàn)要素：
中提供的效果僅僅是實施例的效果，而不是發(fā)明所有的全部效果，上述技術方案中的一個技術方案具有如下優(yōu)點或有益效果：

45、本發(fā)明提出了一種用于井工礦動態(tài)環(huán)境的融合定位方法和系統(tǒng)，屬于自動駕駛車輛定位技術領域，該方法包括以下步驟：將獲取的目標車輛的歷史激光雷達數(shù)據(jù)、歷史imu數(shù)據(jù)和歷史里程計數(shù)據(jù)分別進行時間同步和空間標定后，進行slam建圖得到點云地圖；對所述點云地圖預處理后得到用于定位的先驗地圖；獲取目標車輛的第一實時點云數(shù)據(jù)，并對第一實時點云數(shù)據(jù)中不在同一坐標系下的點云進行配準；然后采用聚類算法判斷第一實時點云數(shù)據(jù)的動態(tài)性，對第一實時點云數(shù)據(jù)進行動態(tài)點剔除得到第二實時點云數(shù)據(jù)；獲取目標車輛的第一實時imu數(shù)據(jù)、第一底盤車速信息和第一slam位姿；從第一實時imu數(shù)據(jù)獲取航向角，從第一slam位姿中獲取初始位姿；利用初始姿態(tài)和單位時間位姿變換不斷疊加得到更新后里程計信息；將更新后里程計信息、第二實時點云數(shù)據(jù)和第一實時imu數(shù)據(jù)進行卡爾曼融合定位輸出最終融合定位信息?；谝环N用于井工礦動態(tài)環(huán)境的融合定位方法，還提出了一種用于井工礦動態(tài)環(huán)境的融合定位系統(tǒng)。本發(fā)明剔除環(huán)境中動態(tài)障礙物對長走廊環(huán)境下定位的影響，提高定位的準確性和魯棒性。

46、本發(fā)明將imu數(shù)據(jù)與車速信息進行融合發(fā)布為里程計，避免傳統(tǒng)的車速里程計模型帶來的累積誤差，且里程計位姿可根據(jù)實際情況進行調整。