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

背景技術(shù)：

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

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

3、關(guān)于退化場(chǎng)景的可定位性，沿海導(dǎo)航是對(duì)環(huán)境中的信息內(nèi)容(即信息增益)建模的比較早的一個(gè)嘗試?！癱ensi,andrea."on?achievable?accuracy?for?range-finderlocalization."proceedings?2007ieee?international?conference?on?robotics?andautomation.ieee,2007.”提出一種信息矩陣的方法，通過(guò)對(duì)信息矩陣進(jìn)行特征值分解，來(lái)分析每個(gè)方向的約束程度，從而來(lái)評(píng)估可定位性?！發(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.”將這種方法應(yīng)用到2d場(chǎng)景中，旨在最大化信息矩陣的行列式，來(lái)估計(jì)可定位性。

4、在激光雷達(dá)退化檢測(cè)方面，“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.”通過(guò)分析狀態(tài)估計(jì)的優(yōu)化問(wèn)題，提出一種新的退化因子來(lái)表征幾何退化，旨在判斷激光退化的方向，并且在退化方向不使用雷達(dá)進(jìn)行更新，這個(gè)方法只能應(yīng)用于基于優(yōu)化的狀態(tài)估計(jì)問(wèn)題?！皕hang,ji,et?al.“ondegeneracy?of?optimization-based?state?estimation?problems.”2016ieeeinternational?conference?on?robotics?and?automation(icra),2016.”通過(guò)對(duì)法向量進(jìn)行奇異值分解，提出一種新的方法來(lái)判斷機(jī)器人處于當(dāng)前位置是否能持續(xù)的產(chǎn)生準(zhǔn)確的定位結(jié)果。之后“zhen,weikun,et?al.“robust?localization?and?localizabilityestimation?with?a?rotating?laser?scanner.”2017ieee?international?conferenceon?robotics?and?automation(icra),2017.”又提出擾動(dòng)約束的思想來(lái)對(duì)激光退化進(jìn)行判斷。但是這些方法計(jì)算出的退化判斷閾值沒(méi)有一個(gè)明確的物理含義，不便于在不同數(shù)據(jù)集上應(yīng)用。

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.”提出一種視覺(jué)系統(tǒng)和激光系統(tǒng)松耦合的系統(tǒng)，系統(tǒng)會(huì)對(duì)視覺(jué)和激光進(jìn)行退化檢測(cè)，當(dāng)某個(gè)系統(tǒng)出現(xiàn)問(wèn)題，系統(tǒng)會(huì)自動(dòng)切換到第另一個(gè)系統(tǒng)保證系統(tǒng)的穩(wěn)定性。“zhen,weikun,et?al.“robust?localization?and?localizability?estimation?with?a?rotating?laserscanner.”2017ieee?international?conference?on?robotics?and?automation(icra),2017.”提出利用uwb傳感器在激光退化的過(guò)程中提供另一種有效的定位信息?，F(xiàn)在很多多傳感器融合定位的slam方案都有對(duì)單個(gè)傳感器的退化判斷，但是這些方法大多都是僅僅對(duì)傳感器退化進(jìn)行判斷，然后放棄該傳感器的信息，但是其中一些有效信息也被丟棄了。

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

1、本發(fā)明的目的是提出一種具有退化感知的激光慣性里程計(jì)，這種方法對(duì)退化和劇烈運(yùn)動(dòng)等極端場(chǎng)景下具有很強(qiáng)的魯棒性。

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

3、所述退化檢測(cè)器具體為：

4、提取當(dāng)前幀每個(gè)點(diǎn)對(duì)應(yīng)局部地圖的z個(gè)點(diǎn)，將對(duì)應(yīng)局部地圖的z個(gè)點(diǎn)擬合為一個(gè)平面，每個(gè)平面有一個(gè)法向量，將本幀每個(gè)點(diǎn)對(duì)應(yīng)局部地圖平面的法向量進(jìn)行歸一化處理后，將本幀對(duì)應(yīng)的所有平面法向量的起點(diǎn)均移動(dòng)到世界坐標(biāo)系的原點(diǎn)下，構(gòu)建一個(gè)約束球矩陣，如下公式：

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

6、其中，npre表示當(dāng)前幀點(diǎn)對(duì)應(yīng)的平面法向量組成的約束球矩陣，ni表示第i個(gè)點(diǎn)對(duì)應(yīng)的平面法向量，m表示平面法向量的數(shù)量；對(duì)于每個(gè)平面法向量約束，將其取負(fù)數(shù)得到一個(gè)新的法向量；對(duì)于一個(gè)平面來(lái)說(shuō)，有兩個(gè)法向量；最終得到的約束矩陣n如下：

7、n＝[npre,-npre]

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

9、

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

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

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

13、所述z取5。

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

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

16、

17、其中表示世界系下自身的位置，表示世界系下自身的速度，表示世界坐標(biāo)系下的旋轉(zhuǎn)，表示imu的加速度，表示角速度計(jì)的偏移量，ggt表示重力加速度；

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

19、

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

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

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

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

24、pk+1＝fxpkfxt+fiqkfit

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

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

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

28、

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

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

31、pk＝(i-kh)pk

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

33、為了減少重復(fù)的點(diǎn)云去畸變操作，將當(dāng)前點(diǎn)云幀去畸變后的點(diǎn)一邊用于退化處理子系統(tǒng)中自適應(yīng)權(quán)重分配一邊送入退化檢測(cè)器進(jìn)行退化檢測(cè)。

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

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

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

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

38、曲面片內(nèi)的點(diǎn)平分所述曲面片的權(quán)重；如下計(jì)算得到每個(gè)點(diǎn)的權(quán)重，得到其他點(diǎn)對(duì)應(yīng)該點(diǎn)的重要性：

39、

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

41、

42、其中，a表示普遍使用的每個(gè)點(diǎn)的協(xié)方差的逆，2m表示約束個(gè)數(shù)；由于每個(gè)點(diǎn)之間為相互獨(dú)立，所以每?jī)蓚€(gè)點(diǎn)之間的協(xié)方差是零，最終得到r矩陣為對(duì)角線為每個(gè)點(diǎn)的方差的對(duì)角矩陣；同樣按照eskf算法來(lái)進(jìn)行狀態(tài)估計(jì)，在計(jì)算卡爾曼增益的時(shí)候?qū)⒂^測(cè)協(xié)方差矩陣替換為所計(jì)算的新的協(xié)方差矩陣r。

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