論文閱讀：Rail Vehicle Localization and Mapping With LiDAR-Vision-Inertial-GNSS Fusion(RAL 2022)

Motivation

監(jiān)測(cè)鐵路安全運(yùn)營(yíng)需要精確的軌道車(chē)輛定位和長(zhǎng)期的鐵路環(huán)境。國(guó)內(nèi)現(xiàn)行鐵道系統(tǒng)的定位策略仍以軌旁系統(tǒng)為主，不僅實(shí)時(shí)性和準(zhǔn)確率都差，而且需要大量的前期投資和后續(xù)維護(hù)。雖然已經(jīng)有基于GNSS和odometry的研究，但是這類方法缺失了對(duì)于環(huán)境的感知信息，并且會(huì)受到GNSS環(huán)境的限制。所以加入諸如雷達(dá)和相機(jī)的感知進(jìn)行輔助融合是一個(gè)可行的方向，但是在鐵路系統(tǒng)加入lidar和相機(jī)存在幾個(gè)問(wèn)題：

1. 一是運(yùn)動(dòng)長(zhǎng)時(shí)間受到約束，軌道車(chē)輛被限制沿平面軌跡移動(dòng)，導(dǎo)致 IMU存在非觀測(cè)方向的虛假信息增益。這種問(wèn)題會(huì)導(dǎo)致許多VIO的大規(guī)模漂移。

2. 二是鐵路系統(tǒng)信息重復(fù)，主要可觀察的特征是重復(fù)的鐵軌和懸架夾具，這對(duì)于基于特征跟蹤的方法具有挑戰(zhàn)性。

3. 三是鐵路系統(tǒng)基本無(wú)回環(huán)，現(xiàn)有的SLAM 通常使用地標(biāo)描述符來(lái)檢測(cè)重訪問(wèn)的地點(diǎn)，并在檢測(cè)到的循環(huán)上糾正累積的漂移。鐵路系統(tǒng)沒(méi)有回環(huán)，所以需要更加低漂移的位姿估計(jì)。

作者 ：晃晃悠悠的虛無(wú)周， 來(lái)源：微信公眾號(hào)「3D視覺(jué)工坊」

Contribution

1. 這篇文章提出了一個(gè)框架，基于滑窗因子圖將 LiDAR、IMU、軌道車(chē)輛車(chē)輪里程計(jì)、相機(jī)和 GNSS 緊密融合。實(shí)驗(yàn)結(jié)果表明，融合后的系統(tǒng)對(duì)于大規(guī)模鐵路環(huán)境定位足夠準(zhǔn)確，并且對(duì)鐵路上的長(zhǎng)時(shí)間倒退具有魯棒性。

2. 充分利用傳感器測(cè)量的幾何信息，利用提取的鐵軌和消失點(diǎn)的平面約束來(lái)提高系統(tǒng)精度和魯棒性。實(shí)驗(yàn)證明這些特征有效地限制了具有重復(fù)結(jié)構(gòu)的區(qū)域的高度和旋轉(zhuǎn)誤差。

3. 實(shí)驗(yàn)中所使用的數(shù)據(jù)集時(shí)長(zhǎng)跨度一年多，涵蓋各種規(guī)模、天氣和鐵路狀況。

Content

1. 問(wèn)題描述

多傳感器融合的狀態(tài)估計(jì)問(wèn)題本質(zhì)上是MAP問(wèn)題，定義當(dāng)前時(shí)刻列車(chē)狀態(tài)如下：

為了保證實(shí)時(shí)性，滑窗基于關(guān)鍵幀建立，對(duì)于滑窗內(nèi)的關(guān)鍵幀，優(yōu)化問(wèn)題表述如下：

其中是通過(guò)舒爾補(bǔ)給出的先驗(yàn)運(yùn)動(dòng)因子，是通過(guò)IMU/輪式里程計(jì)給出的預(yù)積分，是lidar約束，是平面約束，是重投影約束，是線段約束，是滅點(diǎn)約束，是GNSS約束。2.系統(tǒng)框圖

虛線框表示因子圖結(jié)果。給定輸入的 LiDAR 點(diǎn)云和單目圖像，提取每張圖像的點(diǎn)、線和滅點(diǎn)，并跟蹤圖像幀，然后通過(guò)分配的深度信息校正比例。此外，以鐵軌平面作為平面約束，檢測(cè)提取鐵軌。所有測(cè)量都與滑窗因子圖進(jìn)行后續(xù)的聯(lián)合優(yōu)化。關(guān)于滑窗因子圖中的各類因子介紹如下：A、IMU/輪式里程計(jì)預(yù)積分因子鐵軌的輪式里程計(jì)模型可以表述為：

其中，表示為通過(guò)隨機(jī)游走建模的輪式里程計(jì)的尺度因子。然后通過(guò)同步采集的陀螺儀和里程表輸出來(lái)實(shí)現(xiàn)姿態(tài)估計(jì)，連續(xù)兩幀之間的位移可以表述為：

(是0均值高斯噪聲)因此，在幀K和幀K+1之間的離散形式的IMU/輪式里程計(jì)信息

可以表示為：

那么，最終可以得到關(guān)于IMU/輪式里程計(jì)的預(yù)積分殘差

：

其中，

表示IMU和輪式里程計(jì)之間的位移，用于表示從四元數(shù)中取最后四個(gè)元素。B、lidar殘差因?yàn)殍F路速度過(guò)快，所以首先通過(guò)IMU/輪式里程計(jì)的增量運(yùn)動(dòng)模型來(lái)線性矯正lidar的點(diǎn)云畸變，并且去除距離過(guò)近的點(diǎn)云點(diǎn)。然后基于LOAM的特征提取方式來(lái)基于曲率提取角點(diǎn)和面點(diǎn)，并且基于這兩類點(diǎn)進(jìn)行幀間的掃描配準(zhǔn)。那么lidar殘差可以表述為：

(LOAM的兩個(gè)殘差，不贅述)另外，作者注意到，具有有限 FoV 的僅 LiDAR 里程計(jì)對(duì)由軌道和軌道道岔的接頭引起的振動(dòng)過(guò)度敏感，可能會(huì)出現(xiàn)pitch方向的誤差。因?yàn)閮蓷l鐵軌在轉(zhuǎn)彎處高度不同，在隨后的直線鐵路中，僅 LiDAR 的里程計(jì)也會(huì)保持roll的發(fā)散。針對(duì)這個(gè)問(wèn)題，利用LEGO-LOAM的分段地面約束方法，可以有效地限制roll和pitch角度的漂移，但是LEGO-LOAM的基于角度地面提取方法并不適用于鐵軌系統(tǒng)，所以可以通過(guò)鐵軌平面提取方法來(lái)確定地面。C、和視覺(jué)相關(guān)的殘差與具有任意運(yùn)動(dòng)的 UAV 或 UGV 不同，軌道車(chē)輛僅限于高度受限的運(yùn)動(dòng)，比如速度長(zhǎng)時(shí)間恒定，加速度恒定，以及沒(méi)有旋轉(zhuǎn)。在恒定加速度或不旋轉(zhuǎn)的情況下，會(huì)引入局部不可觀察的 IMU 偏差，生成病態(tài)甚至秩不足的信息矩陣，并導(dǎo)致 VIO 的顯著尺度漂移。因此，在初始化階段利用LIO的 IMU 偏差，并將深度信息與圖像關(guān)鍵幀上提取的特征點(diǎn)相關(guān)聯(lián)從而進(jìn)一步矯正漂移。在尺度矯正的前提下，平移精度可以顯著提高。但是旋轉(zhuǎn)誤差仍然是不可避免的，因此需要引入兩個(gè)額外的結(jié)構(gòu)約束來(lái)進(jìn)一步優(yōu)化。首先基于LSD進(jìn)行線段分割，

是投影到圖像平面的線特征，是線特征的中點(diǎn)，那么線特征的重投影殘差可以表述為：

但是LSD提取的大多是大量的短線特征（灌木樹(shù)葉、陽(yáng)光或樹(shù)木的早午餐）。這些特征很難跟蹤，因?yàn)槠渲幸恍┛赡軙?huì)在下一幀中消失。因此將圖像分辨率調(diào)整為 720 × 480，并設(shè)置最小線特征長(zhǎng)度和寬度閾值以提取主要特征。

旋轉(zhuǎn)漂移可以通過(guò)滅點(diǎn)進(jìn)行有效的約束，因此可以利用滅點(diǎn)來(lái)約束相機(jī)方向。滅點(diǎn)基于平行線的交點(diǎn)進(jìn)行檢測(cè)。也就是說(shuō)，平行線的方向決定了滅點(diǎn)。與一般的室內(nèi)或室外環(huán)境不同，在單個(gè)幀中可以提取多個(gè)滅點(diǎn)，對(duì)于大多數(shù)鐵路場(chǎng)景只能找到一個(gè)滅點(diǎn)，如下圖。

假設(shè)是檢測(cè)到的滅點(diǎn)在第k幀的角度，那么對(duì)應(yīng)的滅點(diǎn)殘差可以表述為：

D、GNSS殘差當(dāng)估計(jì)的位姿協(xié)方差大于GNSS 協(xié)方差時(shí)，添加 GNSS 因子。僅依賴于 GNSS 協(xié)方差進(jìn)行判斷有時(shí)不可信，并且可能會(huì)產(chǎn)生模糊或不連續(xù)的建圖結(jié)果。因此使用增量噪聲對(duì) GNSS 測(cè)量進(jìn)行建模，GNSS殘差可以定義為：

其中，

表示從GNSS接收天線到IMU的位姿，因?yàn)殍F路的 4G 通信質(zhì)量不一致，僅將單點(diǎn)定位 (SPP) 結(jié)果視為輸入。3、地圖管理

為了減少頻繁的 GNSS 輔助優(yōu)化導(dǎo)致的建圖模糊，提出了一種基于子圖的兩階段map2map配準(zhǔn)，首先基于局部?jī)?yōu)化創(chuàng)建子圖，然后利用 GNSS 信息進(jìn)行子圖到子圖的配準(zhǔn)正態(tài)分布變換（NDT）。在實(shí)際操作過(guò)程中，每個(gè)子圖維護(hù)30個(gè)關(guān)鍵幀。4、實(shí)驗(yàn)

A、硬件在兩條鐵路上對(duì)各種維修車(chē)輛進(jìn)行了一系列實(shí)驗(yàn)：一個(gè)是普通高速列車(chē)的貨運(yùn)鐵路，另一個(gè)是高速列車(chē)的載人運(yùn)輸鐵路。根據(jù)鐵路上的安全原則，前者只能在白天進(jìn)行，后者只能在午夜進(jìn)行。

數(shù)據(jù)集的詳細(xì)情況如下表，真值由 MPSTNAV M39 GNSS/INS 組合導(dǎo)航系統(tǒng)的后處理結(jié)果保存（帶有來(lái)自千尋 SI 的 RTK 校正）。

Baseline采用R2LIVE、FAST-LIO2、LiLi-OM、Lio-Livox 3 和 VINS-Mono。對(duì)于消融研究，將 RailLoMer-V w/o GNSS、RailLoMer-V w/o ODO 和 RM-LVI 定義為無(wú) GNSS 因素、無(wú)里程計(jì)因素和僅 LiDAR 視覺(jué)慣性部分。B、評(píng)測(cè)1）Benchmark指標(biāo)所提出的系統(tǒng)可以實(shí)現(xiàn)所有序列的分米級(jí)精度。其他方法的性能并不理想，特別是對(duì)于長(zhǎng)時(shí)間的任務(wù)。對(duì)于小規(guī)模測(cè)試，不正確初始化的重力矢量加速了垂直誤差的累積。由于兩條軌道在轉(zhuǎn)彎時(shí)高度不同，因此無(wú)法正確初始化重力矢量，并且所選方法在沒(méi)有額外約束的情況下都無(wú)法正常工作。對(duì)于大規(guī)模測(cè)試，僅視覺(jué)方法在恒定速度下表現(xiàn)出糟糕的性能。

2）關(guān)于運(yùn)動(dòng)方向的魯棒性與無(wú)人地面車(chē)輛（UGV）或自動(dòng)車(chē)輛在收集數(shù)據(jù)時(shí)始終向前移動(dòng)不同，軌道車(chē)輛還包括長(zhǎng)時(shí)間的向后運(yùn)動(dòng)。HQ-Long，一個(gè)僅向后運(yùn)動(dòng)的序列，可以用來(lái)研究運(yùn)動(dòng)方向的影響。如上表和下圖所示，基于濾波器的算法 R2LIVE 和 FAST-LIO2 在這個(gè)序列上的 6-DoF 中展現(xiàn)了巨大的誤差。然而，基于優(yōu)化的算法對(duì)這種影響不敏感

3）關(guān)于退化情況的魯棒性高速鐵路隧道采用光滑的人造墻、重復(fù)的鐵軌和懸垂線夾。這些地區(qū)是 SLAM 最困難的場(chǎng)景之一，數(shù)據(jù)集中的隧道由三個(gè)連續(xù)的隧道組成（最長(zhǎng)的 1.7 公里）。在里程表和 GNSS 的幫助下，可以很好地消除縱向發(fā)散和累積誤差，所提出的系統(tǒng)可以保持準(zhǔn)確的軌跡。相反，其他方法要么“停止”，要么“倒退”到嚴(yán)重退化的場(chǎng)景。量化結(jié)果見(jiàn)上表的CH-Tunnel。

4）天氣的影響常速鐵路數(shù)據(jù)集在夏季收集，而高速鐵路數(shù)據(jù)集在冬季收集。由于鐵軌在冬季午夜變得濕滑，車(chē)輪打滑是不可避免的，所提出的 RailLoMer-V 系統(tǒng)在冬季的性能降低了 30%。5）消融實(shí)驗(yàn)可以看出，里程計(jì)可以很好地約束退化時(shí)的縱向位移。在 CH-Tunnel 中，不帶 GNSS 的 RailLoMer-V 比不帶 ODO 的 RailLoMer-V 具有更好的性能。然而，對(duì)于特征豐富的地區(qū)，里程計(jì)的貢獻(xiàn)并不明顯，并且 RM-LVI 可以得到與 RailLoMer-V w/o GNSS 的其他序列相似的精度。量化結(jié)果在上面的表格。6）高精度地圖構(gòu)建所提出的方法可以構(gòu)建鐵路環(huán)境的大比例尺地圖。實(shí)時(shí)建圖如下圖所示，其中與衛(wèi)星圖像清晰且匹配良好的結(jié)果表明提出的方法具有較高的精度。

7）運(yùn)行時(shí)間分析如下表，所提出的系統(tǒng)能夠在所有條件下實(shí)時(shí)操作。此外，時(shí)間并沒(méi)有隨著距離的增加而大幅增長(zhǎng)。

Conclusion

這篇文章提出的RailLoMer-V以緊耦合的方式融合了來(lái)自 LiDAR、相機(jī)、IMU、火車(chē)?yán)锍逃?jì)和 GNSS 的測(cè)量結(jié)果，并且利用額外的幾何結(jié)構(gòu)約束來(lái)應(yīng)對(duì)高度重復(fù)的環(huán)境。在各種光照條件、不同尺度和退化區(qū)域的評(píng)估顯示了分米級(jí)的定位精度。首先這種方法相較于傳統(tǒng)的火車(chē)定位方法，精度是明顯提高的，因?yàn)椴捎昧朔浅６嗟娜哂鄠鞲衅鱽?lái)保證定位的準(zhǔn)確性，另外，定位其實(shí)是一個(gè)輔助的功能，傳統(tǒng)的火車(chē)定位無(wú)法建模周?chē)母呔拳h(huán)境，這個(gè)方法對(duì)于火車(chē)感知環(huán)境能力的提升非常明顯。目前方法里關(guān)于視覺(jué)激光的融合相對(duì)比較粗糙，可以考慮后續(xù)數(shù)據(jù)級(jí)別的融合，進(jìn)一步提升環(huán)境感知能力。

本文僅做學(xué)術(shù)分享，如有侵權(quán)，請(qǐng)聯(lián)系刪文。

3D視覺(jué)工坊精品課程官網(wǎng)：3dcver.com
1.面向自動(dòng)駕駛領(lǐng)域的多傳感器數(shù)據(jù)融合技術(shù)
2.面向自動(dòng)駕駛領(lǐng)域的3D點(diǎn)云目標(biāo)檢測(cè)全棧學(xué)習(xí)路線！(單模態(tài)+多模態(tài)/數(shù)據(jù)+代碼)
3.徹底搞透視覺(jué)三維重建：原理剖析、代碼講解、及優(yōu)化改進(jìn)
4.國(guó)內(nèi)首個(gè)面向工業(yè)級(jí)實(shí)戰(zhàn)的點(diǎn)云處理課程
5.激光-視覺(jué)-IMU-GPS融合SLAM算法梳理和代碼講解
6.徹底搞懂視覺(jué)-慣性SLAM：基于VINS-Fusion正式開(kāi)課啦
7.徹底搞懂基于LOAM框架的3D激光SLAM: 源碼剖析到算法優(yōu)化
8.徹底剖析室內(nèi)、室外激光SLAM關(guān)鍵算法原理、代碼和實(shí)戰(zhàn)(cartographer+LOAM +LIO-SAM)
9.從零搭建一套結(jié)構(gòu)光3D重建系統(tǒng)[理論+源碼+實(shí)踐]
10.單目深度估計(jì)方法：算法梳理與代碼實(shí)現(xiàn)
11.自動(dòng)駕駛中的深度學(xué)習(xí)模型部署實(shí)戰(zhàn)
12.相機(jī)模型與標(biāo)定(單目+雙目+魚(yú)眼）
13.重磅！四旋翼飛行器：算法與實(shí)戰(zhàn)
14.ROS2從入門(mén)到精通：理論與實(shí)戰(zhàn)
15.國(guó)內(nèi)首個(gè)3D缺陷檢測(cè)教程：理論、源碼與實(shí)戰(zhàn)
16.基于Open3D的點(diǎn)云處理入門(mén)與實(shí)戰(zhàn)教程

重磅！3DCVer-學(xué)術(shù)論文寫(xiě)作投稿 交流群已成立

掃碼添加小助手微信：dddvision，可申請(qǐng)加入3D視覺(jué)工坊-學(xué)術(shù)論文寫(xiě)作與投稿 微信交流群，旨在交流頂會(huì)、頂刊、SCI、EI等寫(xiě)作與投稿事宜。

同時(shí)也可申請(qǐng)加入我們的細(xì)分方向交流群，目前主要有3D視覺(jué)、CV&深度學(xué)習(xí)、SLAM、三維重建、點(diǎn)云后處理、自動(dòng)駕駛、多傳感器融合、CV入門(mén)、三維測(cè)量、VR/AR、3D人臉識(shí)別、醫(yī)療影像、缺陷檢測(cè)、行人重識(shí)別、目標(biāo)跟蹤、視覺(jué)產(chǎn)品落地、視覺(jué)競(jìng)賽、車(chē)牌識(shí)別、硬件選型、學(xué)術(shù)交流、求職交流、ORB-SLAM系列源碼交流、深度估計(jì)等微信群。

一定要備注：研究方向+學(xué)校/公司+昵稱，例如：”3D視覺(jué) + 上海交大 + 靜靜“。請(qǐng)按照格式備注，可快速被通過(guò)且邀請(qǐng)進(jìn)群。原創(chuàng)投稿也請(qǐng)聯(lián)系。