本發(fā)明涉及激光傳感器定位，尤其涉及一種用于挑戰(zhàn)性場景的可定位性感知的激光雷達慣性里程計。

背景技術：

1、很多優(yōu)秀的lio系統(tǒng)只討論了自身運動相對比較緩慢的場景，像fast-lio2對原始激光點進行下采樣，通過imu來進行狀態(tài)預測，并補償點云畸變，然后使用ikd-tree構建點到平面作為觀測，直接在迭代誤差狀態(tài)卡爾曼濾波器框架中進行狀態(tài)更新。lio-sam提出了一種基于因子圖優(yōu)化的緊耦合激光-慣性里程計方法，通過優(yōu)化包含lidar里程計因子，imu預積分因子，gps因子和回環(huán)因子來得到機器人全局一致的位姿。以上方法都需要在點云累計期間，使用imu來恢復運動，imu的測量中存在大量噪聲，較長時間的積分帶來的誤差將不可避免的影響到里程計的精度和魯棒性，并且也限制著激光里程計的輸出頻率。導致他們不能在過于激進運動的場景下進行可靠的定位。clins等連續(xù)時間方法能夠通過非剛性配準策略忽略攻擊性運動引起的運動補償誤差。然而這需要更多的控制點來擬合劇烈變化的軌跡，這極大地影響了系統(tǒng)的效率。

2、為了降低imu長時間積分帶來的累積誤差，也有不少優(yōu)秀的工作使用分割激光點云數(shù)據的方式來提高魯棒性，llol通過劃分激光雷達點將旋轉激光雷達視為流傳感器。sr-lio通過將完整激光幀分為3段，然后再進行組合得到新幀，減少了imu預積分誤差。這兩個系統(tǒng)雖然是進行分割但是依舊是完整點云幀進行位姿更新，無法處理更大程度的激進運動。最新point-lio提出一種逐點更新位姿的方式，從根本上解決了點云畸變問題，但是由于每次更新的約束過少這種方法對退化場景比較敏感。

3、關于退化場景的可定位性，沿海導航是對環(huán)境中的信息內容(即信息增益)建模的比較早的一個嘗試?！癱ensi,andrea."on?achievable?accuracy?for?range-finderlocalization."proceedings?2007ieee?international?conference?on?robotics?andautomation.ieee,2007.”提出一種信息矩陣的方法，通過對信息矩陣進行特征值分解，來分析每個方向的約束程度，從而來評估可定位性。“l(fā)iu,zhe,et?al."localizabilityestimation?for?mobile?robots?based?on?probabilistic?grid?map?and?itsapplications?to?localization."2012ieee?international?conference?onmultisensor?fusion?and?integration?for?intelligent?systems(mfi).ieee,2012.”將這種方法應用到2d場景中，旨在最大化信息矩陣的行列式，來估計可定位性。

4、在激光雷達退化檢測方面，“l(fā)iu,zhe,et?al."localizability?estimation?formobile?robots?based?on?probabilistic?grid?map?and?its?applications?tolocalization."2012ieee?international?conference?on?multisensor?fusion?andintegration?for?intelligent?systems(mfi).ieee,2012.”通過分析狀態(tài)估計的優(yōu)化問題，提出一種新的退化因子來表征幾何退化，旨在判斷激光退化的方向，并且在退化方向不使用雷達進行更新，這個方法只能應用于基于優(yōu)化的狀態(tài)估計問題?！皕hang,ji,et?al.“ondegeneracy?of?optimization-based?state?estimation?problems.”2016ieeeinternational?conference?on?robotics?and?automation(icra),2016.”通過對法向量進行奇異值分解，提出一種新的方法來判斷機器人處于當前位置是否能持續(xù)的產生準確的定位結果。之后“zhen,weikun,et?al.“robust?localization?and?localizabilityestimation?with?a?rotating?laser?scanner.”2017ieee?international?conferenceon?robotics?and?automation(icra),2017.”又提出擾動約束的思想來對激光退化進行判斷。但是這些方法計算出的退化判斷閾值沒有一個明確的物理含義，不便于在不同數(shù)據集上應用。

5、在判斷出退化程度后，有很多處理方法例如“shan,tixiao,et?al.“l(fā)vi-sam:tightly-coupled?lidar-visual-inertial?odometry?via?smoothing?and?mapping.”2021ieee?international?conference?on?robotics?and?automation(icra),2021.”提出一種視覺系統(tǒng)和激光系統(tǒng)松耦合的系統(tǒng)，系統(tǒng)會對視覺和激光進行退化檢測，當某個系統(tǒng)出現(xiàn)問題，系統(tǒng)會自動切換到第另一個系統(tǒng)保證系統(tǒng)的穩(wěn)定性?！皕hen,weikun,et?al.“robust?localization?and?localizability?estimation?with?a?rotating?laserscanner.”2017ieee?international?conference?on?robotics?and?automation(icra),2017.”提出利用uwb傳感器在激光退化的過程中提供另一種有效的定位信息?，F(xiàn)在很多多傳感器融合定位的slam方案都有對單個傳感器的退化判斷，但是這些方法大多都是僅僅對傳感器退化進行判斷，然后放棄該傳感器的信息，但是其中一些有效信息也被丟棄了。

技術實現(xiàn)思路

1、本發(fā)明的目的是提出一種具有退化感知的激光慣性里程計，這種方法對退化和劇烈運動等極端場景下具有很強的魯棒性。

2、本發(fā)明的技術方案如下：一種用于挑戰(zhàn)性場景的可定位性感知的激光雷達慣性里程計，通過退化檢測器判斷整體里程計系統(tǒng)當前的約束狀態(tài)，根據約束狀態(tài)將整體里程計系統(tǒng)分為兩個子系統(tǒng)，分別為退化處理子系統(tǒng)和高頻里程計子系統(tǒng)；在檢測到約束不足發(fā)生退化時進入退化處理子系統(tǒng)，當約束良好則進入高頻里程計子系統(tǒng)；所述退化處理子系統(tǒng)中形成自適應權重對激光雷達頻率狀態(tài)進行更新；所述高頻里程計子系統(tǒng)中將時間幀分成子幀后，對子幀進行imu頻率狀態(tài)更新。

3、所述退化檢測器具體為：

4、提取當前幀每個點對應局部地圖的z個點，將對應局部地圖的z個點擬合為一個平面，每個平面有一個法向量，將本幀每個點對應局部地圖平面的法向量進行歸一化處理后，將本幀對應的所有平面法向量的起點均移動到世界坐標系的原點下，構建一個約束球矩陣，如下公式：

5、npre＝[n1,n2,n3,...,nm]

6、其中，npre表示當前幀點對應的平面法向量組成的約束球矩陣，ni表示第i個點對應的平面法向量，m表示平面法向量的數(shù)量；對于每個平面法向量約束，將其取負數(shù)得到一個新的法向量；對于一個平面來說，有兩個法向量；最終得到的約束矩陣n如下：

7、n＝[npre,-npre]

8、所述約束球矩陣反映本幀的所有約束，當所述約束球越均勻時，則表明這幀的約束越好；將每個歸一化的平面法向量作為一個平面點，使用主成分分析pca對這些平面點進行約束分析；首先，對這些平面點減去均值，由于之前將每個平面點取負數(shù)并添加至約束球矩陣中，所以均值恒為零；然后，計算這些平面點的斜方差矩陣對這個協(xié)方差矩陣進行特征值分解：

9、

10、得到三個特征值{λ1，λ2，λ3}，對特征值進行排序，最大特征值對應的特征向量代表這些平面點的主要方向；定義退化條件數(shù)為λmin為最小特征值，λmax為最大特征值，退化條件數(shù)越大表示約束越好，越小表示約束越差。

11、所述約束狀態(tài)通過退化條件數(shù)和閾值比較確定；

12、定義兩個閾值，退化條件數(shù)小于閾值t1表示整體里程計系統(tǒng)從高頻里程計子系統(tǒng)進入退化處理子系統(tǒng)，當退化條件數(shù)大于閾值t2表示整體里程計系統(tǒng)將從退化處理子系統(tǒng)進入高頻里程計子系統(tǒng)；t1小于t2；退化條件數(shù)處于閾值t1與閾值t2之間時，依舊維持上一個時刻的系統(tǒng)狀態(tài)，以免整體系統(tǒng)頻繁的在兩個子系統(tǒng)中切換。

13、所述z取5。

14、所述高頻里程計子系統(tǒng)具體為：

15、根據imu數(shù)據分割激光雷達點云數(shù)據，獲取子數(shù)據幀；得到子數(shù)據幀后使用eskf算法進行狀態(tài)更新，首先定義系統(tǒng)狀態(tài)量如下，

16、

17、其中表示世界系下自身的位置，表示世界系下自身的速度，表示世界坐標系下的旋轉，表示imu的加速度，表示角速度計的偏移量，ggt表示重力加速度；

18、使用誤差狀態(tài)卡爾曼濾波進行狀態(tài)估計，新的狀態(tài)量為

19、

20、首先離散時間下系統(tǒng)誤差狀態(tài)的狀態(tài)轉移方程為：

21、δxk+1＝fxδxk+fin

22、其中fx表示線性化后系統(tǒng)的狀態(tài)轉移矩陣、fi表示線性化后系統(tǒng)的控制矩陣。

23、然后計算先驗誤差狀態(tài)協(xié)方差矩陣為：

24、pk+1＝fxpkfxt+fiqkfit

25、其中pk表示上一個時刻的后驗誤差協(xié)方差矩陣，qk為上一時刻系統(tǒng)噪聲的協(xié)方差矩陣。

26、使用上述點到平面距離作為觀測，進行迭代的狀態(tài)更新，在每輪狀態(tài)更新中均重新進行點到平面的關聯(lián)，并通過下述公式進行狀態(tài)的更新：

27、k＝(htr-1h+pk-1)-1htr-1

28、

29、其中h為觀測方程的映射矩陣、r為觀測噪聲的協(xié)方差矩陣、為后驗估計，也就是當前的最優(yōu)估計。

30、最后對系統(tǒng)狀態(tài)的協(xié)方差矩陣進行更新：

31、pk＝(i-kh)pk

32、將每次狀態(tài)更新后的子幀的點云進行累積，當累積到完整一幀后將該完整激光幀的點云送入退化檢測器中。

33、為了減少重復的點云去畸變操作，將當前點云幀去畸變后的點一邊用于退化處理子系統(tǒng)中自適應權重分配一邊送入退化檢測器進行退化檢測。

34、所述退化處理子系統(tǒng)具體為：

35、按照地球經緯度劃分所述約束球矩陣分割為q個曲面片，以每個曲面片的面積作為這個曲面片的權重值；每個曲面片的面積和每個曲面片內的法向量個數(shù)如下：

36、s＝{s1,s2,s3,...,sq}

37、p＝{p1,p2,p3,...,pq}

38、曲面片內的點平分所述曲面片的權重；如下計算得到每個點的權重，得到其他點對應該點的重要性：

39、

40、同樣按照eskf算法進行狀態(tài)估計，通過得到的每個點的權重來自適應的分配每個點的協(xié)方差，從而得到最終的協(xié)方差矩陣；將計算得到的權重和實際的協(xié)方差統(tǒng)一到同一個尺度下；使用如下公式來得到每個點的方差：

41、

42、其中，a表示普遍使用的每個點的協(xié)方差的逆，2m表示約束個數(shù)；由于每個點之間為相互獨立，所以每兩個點之間的協(xié)方差是零，最終得到r矩陣為對角線為每個點的方差的對角矩陣；同樣按照eskf算法來進行狀態(tài)估計，在計算卡爾曼增益的時候將觀測協(xié)方差矩陣替換為所計算的新的協(xié)方差矩陣r。

43、本發(fā)明的有益效果：通過退化檢測器判斷系統(tǒng)的當前狀態(tài)，使得整體系統(tǒng)可以在退化處理子系統(tǒng)和高頻里程計子系統(tǒng)中進行切換。最終系統(tǒng)可以在劇烈運動和退化場景實現(xiàn)魯棒的定位和建圖。