標(biāo)題：R2LIVE: A Robust, Rea-time, LiDAR-Inertial-Visual tightly-coupled state Estimator and mapping.

作者：Jiarong Lin, Chunran Zheng, Wei Xu, and Fu Zhang

來(lái)源：arXiv:2102.12400v1

編譯：方川

審核：段逸凡、Yuri

這是泡泡圖靈智庫(kù)推送的第663篇文章，歡迎個(gè)人轉(zhuǎn)發(fā)朋友圈；其他機(jī)構(gòu)或自媒體如需轉(zhuǎn)載，后臺(tái)留言申請(qǐng)授權(quán)。

摘要

本文提出一個(gè)魯棒實(shí)時(shí)的多傳感器緊耦合框架，可以融合激光雷達(dá)、慣導(dǎo)器件和相機(jī)的數(shù)據(jù)完成精確的狀態(tài)估計(jì)。本文提出的框架由基于濾波方法的里程計(jì)模塊和基于因子圖的優(yōu)化模塊組成。為了保證實(shí)時(shí)性，我們只估計(jì)Kalman濾波器中的當(dāng)前幀狀態(tài)，然后在因子圖優(yōu)化環(huán)節(jié)進(jìn)一步提高滑動(dòng)窗口中的所有傳感器位姿。得益于多模態(tài)傳感器數(shù)據(jù)，我們的算法可以應(yīng)對(duì)視覺(jué)退化、激光退化的場(chǎng)景，并且可以在端上計(jì)算平臺(tái)實(shí)時(shí)運(yùn)行。在我們采集的大量室內(nèi)外數(shù)據(jù)上驗(yàn)證后，證明我們的框架得到的結(jié)果相對(duì)于其他sota的激光-慣導(dǎo)里程計(jì)、視覺(jué)-慣導(dǎo)里程計(jì)方法提高了精度。

主要貢獻(xiàn)

• 我們以緊耦合的方式融合了激光雷達(dá)、慣導(dǎo)、視覺(jué)的數(shù)據(jù)，實(shí)驗(yàn)證明我們的方法可以魯邦處理包括劇烈運(yùn)動(dòng)、傳感器退化、甚至是有很多移動(dòng)物體的隧道場(chǎng)景，包括激光雷達(dá)只有很小FoV的場(chǎng)景。
• 我們提出的框架包括一個(gè)高頻運(yùn)行的基于濾波方法的里程計(jì)模塊，和一個(gè)低頻運(yùn)行的基于因子圖的優(yōu)化模塊。里程計(jì)模塊以一個(gè)kalman濾波器迭代融合雷達(dá)、慣導(dǎo)、視覺(jué)傳感器，取得了實(shí)時(shí)的狀態(tài)估計(jì)。因子圖優(yōu)化模塊進(jìn)一步優(yōu)化滑窗內(nèi)的各傳感器位姿和視覺(jué)地圖點(diǎn)位置。
• 基于這種多模態(tài)傳感器緊耦合的方式，我們的方法得到的很高精度的位姿。大量實(shí)驗(yàn)證明我們的方法是可以構(gòu)建大規(guī)模室內(nèi)外建筑的稠密點(diǎn)云地圖的。

算法流程

整體框架

算法步驟

本文選取IMU坐標(biāo)系為body系，并假設(shè)LiDAR,IMU, Camera之間的外參和時(shí)間偏移都已知，但為了支持camera-imu外參在線標(biāo)定. 令第幀IMU數(shù)據(jù)的狀態(tài)向量為:

通過(guò)流行在幺元處的向量空間的'加減'運(yùn)算我們可以得到離散化的狀態(tài)向量更新方程：

記真值為， 估計(jì)值為，我們可以得到每次狀態(tài)估計(jì)的誤差:

綜上，基于IMU數(shù)據(jù)的狀態(tài)更新由狀態(tài)向量的更新和狀態(tài)誤差向量的更新組成：

到這一步，目前還是一個(gè)kalman濾波器問(wèn)題，那么如何把LiDAR、camera數(shù)據(jù)融合到這個(gè)濾波器中呢？作者在kalman濾波器中提出了一個(gè)最大后驗(yàn)估計(jì)量以及它的狀態(tài)誤差，于是公式(6)可被重新寫成：

這樣，狀態(tài)估計(jì)問(wèn)題就可以轉(zhuǎn)變?yōu)閮?yōu)化IMU提供的位子先驗(yàn)和根據(jù)LiDAR/Camera數(shù)據(jù)估計(jì)的 的最大后驗(yàn)估計(jì)問(wèn)題。

LiDAR數(shù)據(jù)融合

LiDAR數(shù)據(jù)的融合，使用的是plane-plane的誤差來(lái)構(gòu)造LiDAR的參差模塊：

然后把這個(gè)參差模塊寫成與相關(guān)的后驗(yàn)估計(jì)問(wèn)題的形式：

Camera數(shù)據(jù)融合

對(duì)于Camera數(shù)據(jù)，理論上與LiDAR數(shù)據(jù)相同：

因子圖優(yōu)化

基于因子圖的優(yōu)化模塊會(huì)去優(yōu)化滑動(dòng)窗口中的LiDAR 位姿、Camera位姿，以及視覺(jué)特征三角化出來(lái)的稀疏地圖點(diǎn)：

我們使用自己搭建的手持式設(shè)備采集了各種劇烈運(yùn)動(dòng)、傳感器退化、存在大量移動(dòng)物體的長(zhǎng)隧道場(chǎng)景，以及較大規(guī)模的室內(nèi)外建筑物場(chǎng)景的數(shù)據(jù)。在魯棒性、軌跡精度、建圖精度幾個(gè)方面與VINS-Mono, Fast-lio對(duì)比，都取得了較好的結(jié)果，證明了本文提出方法可以取得高精度、低漂移的里程計(jì)和建圖結(jié)果。