本發(fā)明涉及定位，尤其涉及一種基于msckf的激光雷達慣性里程計融合方法。

背景技術：

1、多狀態(tài)約束卡爾曼濾波(multi-state?constraint?kalman?filter，msckf)算法最早被提出用于實現(xiàn)實時視覺輔助慣性導航，后被廣泛應用在視覺慣性slam和視覺慣性里程計（visual-inertial?odometry,?vio）中。msckf通過對相機的視覺信息和慣性測量單元（慣性測量）的慣性信息進行融合，在擴展卡爾曼濾波（ekf）的基礎上進行狀態(tài)估計，通過增加多幀的特征點觀測信息來實現(xiàn)狀態(tài)約束，從而完成對位姿估計的優(yōu)化，提高估計的精度。由于msckf在需要高精度實時定位的場景中具有顯著優(yōu)勢，尤其是在多傳感器融合、計算資源有限的嵌入式系統(tǒng)中，其高效的計算性能、對視覺與慣性數(shù)據(jù)的融合能力、良好的實時性和魯棒性，使得它成為無人機、機器人導航、自動駕駛、ar/vr設備等應用的理想選擇。雖然msckf算法在視覺與慣性導航融合領域有著廣泛的應用，但在實際使用中基于視覺導航的方法容易受到光照變化、環(huán)境特征不足、遮擋和視角變化等因素的影響，包括在動態(tài)場景下難以區(qū)分動態(tài)物體和靜態(tài)背景等問題，從而造成最終的定位和定姿精度降低。

技術實現(xiàn)思路

1、為了解決上述技術問題或者至少部分地解決上述技術問題，本發(fā)明提供一種基于msckf的激光雷達慣性里程計融合方法。

2、第一方面，本發(fā)明提供一種基于msckf的激光雷達慣性里程計融合方法，包括：

3、通過慣性測量單元和激光雷達獲取狀態(tài)矢量；狀態(tài)矢量包含：慣性測量單元解算的位姿、速度以及陀螺儀偏置和加速度計偏置，同時包含最近設定數(shù)量個激光雷達幀解算出來的位姿狀態(tài)信息；

4、慣性測量單元的運動模型使用慣性測量單元的加速度計和陀螺儀數(shù)據(jù)來進行狀態(tài)預測，將狀態(tài)向量更新到下一時刻；

5、采集并預處理激光雷達點云數(shù)據(jù)，從激光雷達幀中提取關鍵幀；基于慣性測量單元計算的位姿和根據(jù)各關鍵幀的靜態(tài)特征點匹配結果計算采集關鍵幀時激光雷達處于全局坐標系g的位姿；

6、基于激光雷達相對于全局坐標系 g的位姿估計對所有被檢測靜態(tài)特征點的量測殘差進行建模得到量測模型；

7、利用激光雷達觀測中的重投影誤差，在卡爾曼濾波的更新步驟來校正系統(tǒng)狀態(tài)，量測模型通過最小化重投影誤差的方式來實現(xiàn)優(yōu)化，進而更新整個系統(tǒng)的慣性測量單元和激光雷達的位姿。

8、更進一步地，慣性測量單元的運動模型進行運動狀態(tài)傳播預測狀態(tài)時，使用離散積分的方法將狀態(tài)向量更新到下一時刻。

9、更進一步地，利用曲率和預設的曲率閾值對預處理好的點云數(shù)據(jù)進行特征點提取，獲取符合要求的靜態(tài)特征點；

10、對靜態(tài)特征點進行特征匹配和跟蹤，對靜態(tài)特征點進行特征匹配和跟蹤，通過特征匹配和跟蹤得到當前激光雷達幀相對于前一關鍵幀的靜態(tài)特征點保有率，根據(jù)當前激光雷達幀相對于前一關鍵幀的靜態(tài)特征點保有率確定當前激光雷達幀是否為關鍵幀。

11、更進一步地，靜態(tài)特征點的獲取過程包括：

12、計算點云數(shù)據(jù)中點的曲率：

13、；

14、其中表示點的領域內(nèi)的點集，表示點的領域內(nèi)的點；

15、通過曲率值對點進行分類，當點曲率值高于給定的曲率閾值，則點為角點，當點的曲率值低于曲率閾值，則點為平面點；

16、將屬于角點的點進行時序配準，利用icp算法將連續(xù)兩幀點云進行配準，將當前激光雷達幀點云變換到上一幀坐標系中；

17、判斷角點所在區(qū)域的點在多幀中是否出現(xiàn)超出預設空間位移閾值的空間位移；

18、如果沒有，則表示該角點屬于靜態(tài)特征；將符合靜態(tài)特征的角點作為靜態(tài)特征點進行保留，所組成的集合定義為，其中表示當前激光雷達幀點云中提取到個靜態(tài)特征點。

19、更進一步地，統(tǒng)計上一個關鍵幀中的靜態(tài)特征點在當前激光雷達幀被觀測跟蹤的數(shù)量，計算當前激光雷達幀相對前一關鍵幀的靜態(tài)特征點保有率：

20、；

21、其中表示當前激光雷達幀相對前一關鍵幀的靜態(tài)特征點保有率，表示上一個關鍵幀中靜態(tài)特征點的總數(shù)，表示當前激光雷達幀中靜態(tài)特征點的總數(shù)，表示在上一個關鍵幀中的靜態(tài)特征點在當前激光雷達幀被觀測跟蹤的數(shù)量，當小于預先設定的保有率閾值時，說明激光雷達幀狀態(tài)變化比較大，將當前激光雷達幀作為新的關鍵幀將入到關鍵幀數(shù)組，而前一關鍵幀和當前關鍵幀之間的激光雷達幀被丟棄。

22、更進一步地，基于慣性測量單元計算的位姿和根據(jù)各關鍵幀的靜態(tài)特征點匹配結果計算采集關鍵幀時激光雷達處于全局坐標系g的位姿：

23、根據(jù)各關鍵幀的靜態(tài)特征點匹配結果，利用最小二乘法計算各關鍵幀激光雷達設備的位姿：

24、；

25、其中，和分別為旋轉矩陣和平移向量，和分別是源關鍵幀和目標關鍵幀中的匹配靜態(tài)特征點，表示靜態(tài)特征點總數(shù)，則采集當前激光雷達幀時，激光雷達相對于全局坐標系 g的位姿估計為：

26、；

27、；

28、其中，表示激光雷達相對于全局坐標系 g的姿態(tài)估計，表示激光雷達相對于全局坐標系 g的位置估計，表示從慣性測量單元坐標系 i到全局參考系 g的姿態(tài)信息，表示慣性測量單元坐標系i在全局參考系g的位置信息。

29、更進一步地，對所有被檢測靜態(tài)特征點的量測殘差進行建模得到量測模型：

30、；

31、其中，表示量測信息在量測矩陣左零空間內(nèi)的投影，表示量測噪聲在量測矩陣左零空間內(nèi)的投影，量測矩陣，表示狀態(tài)誤差矢量，其中是由塊向量組成，表示如下：

32、；

33、其中表示當前一共檢測到的靜態(tài)特征點總數(shù)，塊向量為第個靜態(tài)特征點在左零空間內(nèi)的投影，表示為：

34、，其中，表示量測矩陣的左零空間轉換矩陣，表示第個靜態(tài)特征點的測量矢量，表示為：

35、；

36、其中，表示第個靜態(tài)特征點被檢測到的激光雷達幀數(shù)，其中表示第個靜態(tài)特征點在第個激光雷達幀的量測殘差，表示為：

37、；

38、其中，表示第個靜態(tài)特征點在第個激光雷達幀的觀測值，表示第個靜態(tài)特征點在第個激光雷達幀的預測值，表示為：

39、；

40、其中、和表示第個靜態(tài)特征點在第個激光雷達幀的三維坐標預測值，和分別表示在第個激光雷達幀的位姿估計結果，表示第個靜態(tài)特征點在全局坐標系 g中的位置信息。

41、更進一步地，選擇地心地固坐標系作為全局參考系 g。

42、第二方面，本發(fā)明提供一種msckf的激光雷達慣性里程計融合裝置，包括：至少一處理單元，通過總線單元將處理單元、存儲單元，慣性測量單元和激光雷達單元互聯(lián)，所述存儲單元存儲計算機程序和慣性測量單元和激光雷達單元采集的數(shù)據(jù)，所述計算機程序被所述處理單元執(zhí)行時，實現(xiàn)所述的基于msckf的激光雷達慣性里程計融合方法。

43、第三方面，本發(fā)明提供一種計算機可讀存儲介質(zhì)，所述計算機可讀存儲介質(zhì)存儲計算機程序，所述計算機程序被處理器執(zhí)行時，實現(xiàn)所述的基于msckf的激光雷達慣性里程計融合方法。

44、本發(fā)明實施例提供的上述技術方案與現(xiàn)有技術相比具有如下優(yōu)點：

45、傳統(tǒng)的msckf算法是一種基于視覺輔助慣性導航定位的算法，通過結合視覺信息和慣性測量信息，基于擴展卡爾曼濾波器算法來實現(xiàn)實時的狀態(tài)估計，完成高精度的實時運動跟蹤功能。msckf在保持系統(tǒng)計算效率和精度的平衡上表現(xiàn)出色，是經(jīng)典的視覺慣性融合方法之一。傳統(tǒng)msckf的優(yōu)勢在于通過邊緣化舊的相機位姿，避免直接估計3d特征點，降低了計算復雜度，滿足實時應用的要求。但是由于基于視覺的方案對光線條件和環(huán)境紋理較為敏感，在一些特殊場景中容易出現(xiàn)導致算法性能下降的問題。此外，msckf對初始狀態(tài)比較敏感，如果初始化不準確，估計結果可能偏差較大，導致算法無法收斂。

46、而基于激光雷達的方案對光線條件和環(huán)境紋理不敏感，適應性較強，尤其在復雜地形、狹窄空間、無紋理區(qū)域等場景中表現(xiàn)出色，能夠有效地克服視覺方案無法避免的問題。通過將激光雷達信息作為msckf的一個信息源，替代視覺信息，這樣就可以解決視覺方案中無法解決的問題，同時由于激光雷達數(shù)據(jù)信息相對比較豐富，能夠降低算法中由于非線性化問題引入的誤差，避免了msckf算法在非線性特征情況下精度下降的問題。通過將激光雷達與慣導進行融合，不僅提升了系統(tǒng)的工程實用性，也為后續(xù)的基于視覺、激光雷達和慣性測量的多源導航數(shù)據(jù)融合提供了理論基礎和有效地驗證。

47、本發(fā)明提出一種基于msckf的激光雷達慣性里程計融合算法及方案，由于傳統(tǒng)的msckf算法方案中，是以基于視覺為主要的信息源作為算法融合解算的，但是基于視覺方案在某些特殊環(huán)境下容易受到環(huán)境因素影響，導致性能顯著下降的問題。因此本技術方案通過將激光雷達與慣導進行融合，解決了視覺方案對光線條件和環(huán)境紋理較為敏感的問題，同時提升了系統(tǒng)的工程實用性，也為后續(xù)的基于視覺、激光雷達和慣性測量的多源導航數(shù)據(jù)融合提供了理論基礎和有效地驗證。