日本无码免费高清在线|成人日本在线观看高清|A级片免费视频操逼欧美|全裸美女搞黄色大片网站|免费成人a片视频|久久无码福利成人激情久久|国产视频一二国产在线v|av女主播在线观看|五月激情影音先锋|亚洲一区天堂av

  • 手機站
  • 小程序

    汽車測試網(wǎng)

  • 公眾號

    • 汽車測試網(wǎng)

    • 在線課堂

    • 電車測試

首頁 > 汽車技術 > 正文

LINS：用于魯棒高效導航的激光雷達-慣導狀態(tài)估計器

2020-08-18 20:19:03·  來源：同濟智能汽車研究所  
　
編者按：對于自動駕駛任務來說，高精度的定位信息作為后續(xù)決策規(guī)劃等模塊的輸入，尤為重要。而定位領域中，高頻率的自身運動估計必不可少，尤其是在高速行駛的任務場景下。近年來基于激光雷達或相機等傳感器數(shù)據(jù)的同步定位與建圖技術發(fā)展迅速，但受限于傳感器工作頻率，無法獲得高頻的定位輸出。慣性測量單元(IMU)則可以提供高頻的測量數(shù)據(jù)，因此，基于多傳感器融合的算法極具應用價值。本文基于誤差狀態(tài)卡爾曼濾波，將激光雷達與慣導進行緊耦合，可以實現(xiàn)精確而穩(wěn)定的運動估計。
 
文章譯自：
LINS：A Lidar-Inertial State Estimator for Robust and Efficient Navigation
文章來源：
2020 International Conference on Robotics and Automation (ICRA)
作者：
Qin, Chao and Ye, Haoyang and Pranata, Christian E and Han, Jun and Zhang, Shuyang and Liu, Ming
原文鏈接：
https://arxiv.org/abs/1907.02233
 
摘要：LINS是一種輕量級的激光雷達慣性狀態(tài)估計器，用于實時的自身運動估計。本文所提出的方法通過緊密耦合6軸IMU和3D激光雷達，能夠在挑戰(zhàn)性環(huán)境（如特征稀疏的場景）中為地面車輛進行魯棒而高效的導航。本文設計了迭代的誤差狀態(tài)卡爾曼濾波器（ESKF），以通過在每次迭代中生成新的特征對應關系來遞歸地校正估計狀態(tài)，并使系統(tǒng)在計算上易于處理。此外，為了防止長時間運行時濾波器發(fā)散，我們使用以機器人為中心的公式表示局部運動幀的狀態(tài)。為了驗證魯棒性和通用性，在各種情況下都進行了廣泛的實驗。實驗結果表明，LINS在穩(wěn)定性和準確性方面的性能與最新的激光雷達慣性里程表相當，并在速度上實現(xiàn)了數(shù)量級的提高。
關鍵詞：激光雷達，慣性導航，多傳感器融合，卡爾曼濾波
 
1 前言
自身運動估計是大多數(shù)移動機器人應用的基本先決條件，實時性差和算法失敗會很快破壞硬件及其周圍環(huán)境。為此，提出了諸如激光雷達的有源傳感器來完成該任務，即所謂的同時定位和建圖（SLAM）。典型3D激光雷達的一些關鍵優(yōu)勢包括（i）寬闊的水平視場（FOV）[1]和（ii）對環(huán)境照明條件的不變性[2]。但是，基于激光雷達的導航系統(tǒng)對周圍環(huán)境敏感。此外，運動畸變[3]和點云的稀疏性[4]在某些挑戰(zhàn)性場景（例如寬闊開放區(qū)域）中會使算法的性能下降。
最近的研究表明，可以通過融合IMU來彌補激光雷達的不足。與激光雷達不同，IMU對環(huán)境不敏感。它提供了準確的短期運動約束，并且通常在高頻下（例如100Hz-500Hz）工作。這些功能可以幫助激光雷達導航系統(tǒng)從高度動態(tài)的運動畸變中恢復點云，從而提高準確性。但是，目前最佳的基于圖優(yōu)化的激光雷達慣性里程計（LIO）[5]由于計算量大，無法直接應用于實時導航; 對于單次掃描，要花費超過100毫秒的時間來進行激光雷達慣性里程計的計算，甚至需要更多時間以維護地圖。
 
 圖1 LINS在廣東某港口用Velodyne VLP-16和Xsens MTi-G-710 IMU構建的3D地圖。即使在某些特征稀疏的環(huán)境中，我們也可以觀察到生成的地圖與Google Map的良好對齊，如右圖所示。
 
在本文中，我們提出了LINS，這是一種用于無人地面車輛（UGV）實時導航的輕量級激光雷達慣性狀態(tài)估計器。迭代錯誤狀態(tài)卡爾曼濾波器（ESKF）旨在確保準確性和效率。為了實現(xiàn)長期穩(wěn)定性，我們引入了一種以機器人為中心的狀態(tài)公式，在該狀態(tài)下，局部參考幀依照激光雷達的時間步長進行移動，兩個連續(xù)局部幀之間的相對姿態(tài)估計用于更新全局姿態(tài)估計。我們工作的主要貢獻如下：
1.提出了一種緊密耦合的激光雷達慣性里程計算法，該算法比我們以前的工作[5]快一個數(shù)量級。
 
2.我們提出了一個以機器人為中心的迭代式ESKF，該ESKF在各種挑戰(zhàn)性場景中均得到了驗證，并顯示出優(yōu)于現(xiàn)有技術的性能。
 
3.源代碼可在網(wǎng)上獲得（https://github.com/ChaoqinRobotics）。據(jù)我們所知，LINS是第一個通過迭代卡爾曼濾波解決6自由度自身運動的緊密耦合LIO算法。
 
其余部分的組織方式如下。在第二章節(jié)，討論相關的文獻。在第三章節(jié)我們將概述整體系統(tǒng)的完整框架。在第四章節(jié)說明了實驗結果。在第五章節(jié)得出結論。
 
圖2 LINS系統(tǒng)的框架。激光雷達慣性里程計模塊由狀態(tài)傳播和更新子模塊組成，它使用IMU測量值和從特征提取模塊中提取的點云特征，進行迭代卡爾曼濾波。建圖模塊輸出精確的姿態(tài)估計以及全局3D地圖。精確的姿態(tài)估計值與IMU測量值相結合，以產(chǎn)生高頻率的輸出結果。請注意，這項工作的重點是激光雷達慣性里程計模塊。
2 相關工作
文獻中有數(shù)百篇激光雷達相關的里程計算法。我們將注意力集中在有關6個自由度自身運動估計量和相關融合算法的相關工作上，它們分為松耦合和緊耦合。
A.僅使用激光雷達的方法
許多僅使用激光雷達的方法是眾所周知的基于掃描到掃描配準的迭代最近點（ICP）掃描匹配方法的變體。[6]，[7]調(diào)研了ICP的有效變體算法。對于實時應用，[8]設計了LOAM算法，將提取的邊緣和平面特征注冊到增量構建的全局地圖中。[9]提出了將原始LOAM應用于UGV應用的LeGO算法。通過使用地面提取和點云分割，LeGO濾除了不可靠的特征，并在覆蓋有嘈雜物體（例如草木）的區(qū)域表現(xiàn)出極大的穩(wěn)定性。[10]提供了一種有效的閉環(huán)機制，可以實時進行大規(guī)模建圖。
B.松耦合Lidar-IMU
松耦合方法分別處理兩個傳感器以推斷它們的運動約束，這些約束稍后融合（例如，[11]，[12]）。IMU輔助的LOAM[8]將IMU計算的方向和平移作為優(yōu)化的先決條件。[13]將IMU測量值與姿態(tài)估計值結合起來，后者從基于激光雷達的高斯粒子濾波器和預先構建的地圖獲得。通常，松耦合計算很高效[14]，但是激光雷達和慣性約束的解耦會導致信息丟失[15]。
C.緊耦合Lidar-IMU
緊耦合方法通過聯(lián)合優(yōu)化直接將激光雷達和慣性測量融合在一起，可將其分為基于優(yōu)化的[16]，[17]和基于擴展卡爾曼濾波器（EKF）的[18]，[15]。[19]通過最小化來自IMU和激光雷達的約束來進行局部軌跡優(yōu)化。[20]提出了LIPS，在IMU預積分約束[21]和激光雷達的平面約束的基礎上利用了圖優(yōu)化。[5]提出了LIO建圖算法（為簡便起見，在以下簡稱為LIOM），它也是基于圖優(yōu)化的，但具有新的旋轉(zhuǎn)約束建圖方法來優(yōu)化最終的姿勢和地圖。但是，局部地圖窗口中的約束構造和批次優(yōu)化對于實時應用而言太過耗時。[22]介紹了一種基于2D激光雷達的激光雷達輔助的慣性EKF算法。但是它的應用場景僅限于室內(nèi)環(huán)境，因為它要求所有周圍的平面都呈正交結構。
眾所周知，EKF容易受到線性誤差的影響，線性誤差可能導致性能不佳甚至導致發(fā)散[23]，[24]。當涉及激光雷達觀察到的掃描到掃描約束時，此缺點變得很明顯，如果初始姿勢不正確并導致錯誤的特征匹配結果，則該誤差被認為是高度非線性的。為了消除由錯誤匹配引起的誤差，我們提出了一個迭代卡爾曼濾波[26]，它可以在每次迭代中反復找到更好的匹配。此外，我們采用誤差狀態(tài)表示法來保證線性化的有效性[27]。該特性將我們的方法與迭代擴展卡爾曼濾波器[25]區(qū)別開。
 
3 激光雷達-慣性里程計與建圖
A.系統(tǒng)概況
考慮到配備IMU和3D激光雷達的UGV，我們的目標是估計其6自由度的自身運動并同時建立全局地圖，如圖1所示。系統(tǒng)框架如圖2所示。整個系統(tǒng)包括三個主要模塊：-特征提取，LIO和建圖。（i）特征提取模塊旨在從原始點云中提取穩(wěn)定的特征。（ii）由傳播和更新子模塊組成的LIO模塊執(zhí)行迭代卡爾曼濾波，并輸出初始里程計以及無畸變的特征。（iii）建圖模塊通過全局地圖優(yōu)化初始里程計，然后通過使用特征更新地圖，以輸出新的里程計結果。由于空間問題，我們只專注于里程計模塊。我們請讀者參考[8]，[9]了解特征提取和建圖的詳細過程。
B.特征提取
該模塊輸入原始點云并輸出一組邊緣特征Fe和一組平面特征Fp。讀者可以參見[9]，[8]了解詳細的實現(xiàn)。
C.使用迭代ESKF的激光雷達-慣性里程計
LIO模塊使用IMU測量和兩次連續(xù)掃描中提取的特征來估計車輛的相對位姿變換。我們使用以機器人為中心的公式來構建迭代的ESKF，因為它可以防止由于不確定性不斷增加而導致的線性誤差[28]，[29]。令Fw代表固定的世界坐標系，F(xiàn)bk代表在第k幀的IMU附加幀，而Flk代表在第k幀的激光雷達點云幀。請注意，在我們的工作中，局部幀始終在先前的激光雷達時間步設置為IMU附加幀。
1)狀態(tài)定義
定義為Fw相對于Fbk的位姿，定義為當前狀態(tài)，即描述了從Fbk+1到Fbk的相對位姿：
 
是Fw相對于Fbk的平移，是為Fw相對于Fbk的旋轉(zhuǎn)單位四元數(shù)。和表示了從Fbk+1到Fbk的平移和旋轉(zhuǎn)。是相對于Fbk的速度，ba是加速度偏差，bg是陀螺儀偏差，注意局部重力，gbk也是局部狀態(tài)的一部分。為了在狀態(tài)估計[30]中具有良好的特性，一個誤差狀態(tài)的表示被用來求解它的誤差向量被定義為：
 
 
 
 
 
 
其中是3自由度的角度誤差。
 
根據(jù)ESKF的要求，一旦誤差向量被求解，就可以通過將其插入到的狀態(tài)先驗估計中，以獲得最終的即：
 
 
 
其中表示四元數(shù)乘法，exp將角度向量映射到四元數(shù)旋轉(zhuǎn)中。
2）傳播
在這一步，如果有新的IMU數(shù)據(jù)，則預測誤差狀態(tài)，誤差狀態(tài)的協(xié)方差矩陣Pk以及狀態(tài)先驗估計。IMU誤差狀態(tài)的線性化的連續(xù)時間模型[32]如下：
 
 
其中是高斯噪聲向量（定義與[16]相同）。Ft是誤差狀態(tài)的轉(zhuǎn)換矩陣，Gt是在t時刻的噪聲雅克比矩陣：
 
 
其中將一個三維向量變換為它的反對稱矩陣。是從t時刻IMU附加幀獲得的相對于Fbk的旋轉(zhuǎn)矩陣。和是t時刻的加速度和角速度，并通過原始數(shù)據(jù)，去除偏差和重力影響進行計算得到：
 
 
 
 
 
 
離散方程(5)推導出以下的傳播方程：
 
其中，分別為連續(xù)的IMU時間，Q為w的協(xié)方差矩陣，通過離線的傳感器標定獲得。
 
為了預測，需要建立以機器人為中心狀態(tài)的離散時間傳播模型。讀者可以參照[15]，[27]以查看IMU測量值積分的細節(jié)。
 
3)更新
我們提出了一種迭代更新方案，這是這項工作的主要貢獻。
在迭代的卡爾曼濾波中，考慮到先驗的偏差和從測量模型中得到的殘差函數(shù)f(·)，狀態(tài)更新可以作為一個優(yōu)化問題[7]，[29]：
 
 
其中||·||表示馬氏距離，Jk是f(·)對測量噪聲的雅克比矩陣，Mk為測量噪聲的協(xié)方差矩陣。f(·)的輸出實際上是根據(jù)點-邊或點-平面對計算出的堆疊殘留向量。給定，f(·)中的誤差形式與相關，即Fk+1中的第i個特征點，描述為：
 
 
 
又有
 
其中是從Fk+1變換到Fk的點。和共同表示了激光雷達和IMU之間的外參（通過離線標定計算得出）。
 
 
 
 
公式13的物理解釋如下。對于一個邊緣點，它表示了其到對應邊緣的距離。對于一個平面點，它表示了其到對應平面（由三個點定義）的距離。如何選擇這三個點的細節(jié)可以參考[8]。
公式12通過以下的迭代更新公式進行求解：
 
其中表示第j次迭代的修正向量。Hk,j是對的雅克比矩陣。注意到，對于每次迭代，尋找新的匹配邊和匹配面以最小化誤差，然后計算新的H,J和K矩陣。當?shù)趎次迭代小于一個確定閾值時，通過如下公式更新Pk：
 
 
 
 
 
使用公式4可以獲得最終的原始的畸變特征可以通過估計的相對位姿進行畸變校正。
 
最后，我們進行下一幀狀態(tài)的初始化：
 
 
q0表示單位四元數(shù)，和可以通過和
 
 
 
 
計算得到。注意，關于速度，偏差和局部重力的協(xié)方差保留在協(xié)方差矩陣中，而與相對位姿相對應的協(xié)方差設置為零，即，對于機器人中心坐標系的本身參考不存在不確定性。
4)狀態(tài)組合
在機器人中心的公式中，每次更新完成后，需要更新全局位姿，通過如下的組合步驟：
 
 
5）初始化
如第三章節(jié)-C所述，以機器人為中心的公式可以促進濾波器狀態(tài)的初始化。關于初始參數(shù)設置，在我們的實現(xiàn)中，（i）初始加速度偏差和激光雷達-IMU外參是通過離線標定獲得的，而初始陀螺儀偏差是相應靜態(tài)測量值的平均值，（ii）初始側傾和俯仰是從移動前的無偏加速度測量中獲得的；（iii）通過使用（ii）中的初始側傾和俯仰，將導航幀中表示的重力向量轉(zhuǎn)換到當前局部幀，來獲取初始的局部重力。
4 結果
我們在具有2.4GHz四核和8Gib內(nèi)存的便攜式計算機上評估LINS在不同情況下的性能，并將其與LeGO[9]，LOAM[8]和LIOM[5]進行比較。所有方法都用C++實現(xiàn)，并在Ubuntu Linux中使用機器人操作系統(tǒng)（ROS）[33]執(zhí)行。在以下實驗中，LINS的建圖模塊由LeGO[9]中提出的建圖算法實現(xiàn)。之前的大多數(shù)文獻僅分析了最終軌跡的性能，即地圖模塊已進行優(yōu)化了的里程計。但是，我們發(fā)現(xiàn)初始里程計，即里程計模塊的輸出，對整體性能有很大影響。因此，我們將兩者都考慮在內(nèi)。為了區(qū)分這兩種測距法，我們將地圖優(yōu)化的里程計稱為地圖精制里程計（MRO），初始里程計稱為純里程計（PO）。
A. 室內(nèi)實驗
在室內(nèi)測試中，選擇一個停車場作為實驗區(qū)域，如圖4（a）所示。我們將傳感器套件安裝在大巴上，如圖4（b）所示，其中RS-LiDAR-16安裝在頂部，IMU放置在大巴內(nèi)部。圖5（a），5（b）和5（c）分別提供了LINS，LeGO和LOAM的結果。盡管我們沒有地面真值，但我們?nèi)匀豢梢阅恳暀z查LINS-PO的軌跡是否可以與MRO軌跡精確對齊（通常，MRO在室內(nèi)幾乎沒有漂移，并且比PO更準確），而LeGO-PO和LIOM-PO都在偏航角上有明顯的漂移。
B.大尺度的室外實驗
為了驗證通用性和穩(wěn)定性，在四個室外應用場景中進行了實驗：城市，港口，工業(yè)園區(qū)和森林。圖3展示了LINS生成的一些環(huán)境照片和相應的地圖。我們測量了GPS接收器產(chǎn)生的地面真相與提供的估計位置之間的差距通過每種方法來指示漂移量，然后將其與行進距離進行比較以產(chǎn)生相對漂移。實驗結果列于表I。
 
圖3 （a）寬闊而開放的港口區(qū)域；（b）存在許多建筑物，樹木和汽車的工業(yè)園區(qū)；（c）穿過森林區(qū)域的整潔道路照片和相應的地圖（由LINS制作）。LINS在所有測試環(huán)境中均表現(xiàn)良好。
 
圖4.室內(nèi)測試的傳感器配置。（a）LINS建造的停車場地圖。（b）激光雷達安裝示意，IMU固定在大巴上。
總而言之，LINS在所有測試的場景中均表現(xiàn)良好。特定環(huán)境的詳細分析如下。
1）港口實驗：我們評估了廣東某港口的LINS效果。該傳感器套件包括一個Velodyne VLP-16激光雷達和一個固定在汽車頂部的Xsens MTi-G-710 IMU。地面真實軌跡由GPS模塊提供。我們從一個集裝箱包圍的路徑開始記錄數(shù)據(jù)。汽車駛向碼頭，然后行駛了1264米的距離，回到了原來的地點。值得一提的是，這些集裝箱會不斷地進出，從而改變了全局地圖，這可能會破壞MRO的性能。
表1 運動估計的相對漂移誤差
 
根據(jù)表一，我們發(fā)現(xiàn)LINS和LIOM的漂移最少。LINS-MRO的相對漂移為1.56％，略高于LIOM的1.40％，而LINS-PO的相對漂移僅為2.75％。結果表明，IMU和激光雷達的組合可以有效地提高精度。盡管LOAM和LeGO的相對漂移似乎很小，但它們可能會產(chǎn)生巨大的方向誤差。圖6（a）和6（b）提供了LeGO和LINS的詳細軌跡和地圖。與地面真值（綠線）相比，我們發(fā)現(xiàn)LeGO（包括MRO和PO）的軌跡在第一次轉(zhuǎn)彎時轉(zhuǎn)向了錯誤的方向。我們還可以目視檢查圖6（c）中由LeGO構建的地圖發(fā)生了變形。相比之下，LINS表現(xiàn)出與地面真實軌跡的良好對齊，并且生成的地圖對真實環(huán)境具有很高的保真度。即使在特征不足的第一次轉(zhuǎn)彎中（每次掃描僅可使用約30個邊緣特征），LINS仍然表現(xiàn)出色，這表明我們的算法對特征稀疏的場景更為魯棒。
 
圖5 （a）LINS，（b）LeGO和（c）LOAM在室內(nèi)實驗中得到的軌跡。我們觀察到，所有方法的MRO軌跡看起來都很相似，但是它們的PO軌跡完全不同。與其他方法相比，LINS的PO軌跡與其MRO軌跡對齊的更好。
 
圖6 LeGO和LINS估算的軌跡和地圖。請注意，PO軌跡以藍線繪制，MRO軌跡以紅線繪制，GPS地面真值以綠線繪制。我們看到，LINS的軌跡接近于地面真實情況，因此生成的地圖的保真度高于LeGO。
2）城市實驗：我們使用圖4（b）中相同的傳感器套件進行了城市實驗。GPS接收器產(chǎn)生的位置被用作地面真值。值得一提的是，在此場景中，每次掃描的平均邊緣特征數(shù)僅為56，這是所有測試場景中最低的。
 
圖7 在城市實驗中通過不同方法生成的MRO軌跡，以紅色線繪制。
 
圖8. LINS和LIOM的MRO和PO之間的絕對軌跡誤差比較。
我們首先看一下LeGO和LOAM的輸出，分別在圖7（b）和7（c）中可以看到。我們觀察到幾乎每次轉(zhuǎn)彎都會發(fā)生巨大的方向誤差。圖7（a）展示了在同一數(shù)據(jù)集上運行時LINS的結果。生成的軌跡與現(xiàn)實世界的道路顯示出良好的對齊，這證實了LINS即使在特征稀疏的場景中也可以穩(wěn)定運行。LINS的最終MRO和PO漂移分別為1.79％和4.42％，非常接近LIOM的MRO和PO漂移，分別為1.76％和4.44％。此外，與圖7（d）所示的LIOM軌跡相比，以及圖8（a）和8（b）所示的絕對軌跡誤差（ATE），我們可以看到LINS的性能就準確性而言，接近LIOM的性能。我們分析了LIOM在建圖步驟中受益于旋轉(zhuǎn)約束的細化，從而導致MRO結果的準確性更高。
C.運行時間比較
表2比較了LINS和LIOM中激光雷達慣性里程計模塊的平均運行時間。我們看到LINS的速度比LIOM快得多，LINS處理一次掃描所需的時間少于30毫秒，而LIOM始終需要100毫秒以上。在某些極端情況下，例如停車場，那里的特征非常豐富，LIOM最多需要223毫秒，而LINS僅需要25毫秒。結果表明，LINS的實時性優(yōu)于LIOM。
表2 每幀LIO模塊的運行時間
 
LIOM的LIO模塊中最耗時的部分是局部地圖約束構造和批處理優(yōu)化，其中它在多個激光雷達掃描中維護局部地圖，并通過MAP估計求解所有的相對狀態(tài)。LINS出色的計算速度背后的主要原因是，它使用卡爾曼濾波器而不是批處理MAP，因為卡爾曼濾波器通過按時間順序分解批解決問題，并以遞歸形式求解來隱式減小了優(yōu)化問題的維度[25]。另一個原因是我們僅使用來自先前激光雷達掃描的點云進行匹配。這樣，盡管使用的點云比LIOM中構建的本地地圖稀疏，但我們?nèi)钥梢越柚鶬MU來獲得準確的結果。
5 結論
在本文中，我們開發(fā)了一種用于機器人導航的輕量級激光雷達慣性狀態(tài)估計器。通過使用以機器人為中心的公式化的迭代ESKF，我們的算法能夠在具有挑戰(zhàn)性的環(huán)境下提供實時，長期，可靠和高精度的自身運動估計。該算法在城市，港口，工業(yè)園區(qū)，森林和室內(nèi)停車場等各種場景中得到了驗證。實驗結果表明，LINS優(yōu)于僅使用激光雷達的方法，并與最先進的激光雷達慣性里程計算法的性能相當，而計算成本卻更低。
參考文獻：
 
 
END
 
聯(lián)系人：唐老師  
電話：021-69589116
郵箱：20666028@#edu.cn
 
 
分享到：
 
反對 0 舉報 0 收藏 0 評論 0
滬ICP備11026917號-25