【LOAM系列】四：LIO-SAM论文代码阅读笔记

LIO-SAM

IROS 2020 MIT Tixiao Shan

1、论文

1.1 概述

IMU预积分的运动估计对点云进行去畸变并产生激光里程计优化的初始估计。
激光雷达里程计用于估计IMU的零偏。
滑窗优化，在姿态优化的时候边缘化掉旧的激光雷达帧

LOAM是扫描到全局地图的匹配，松耦合方法

1.2 IMU预积分因子

系统状态
$$\begin{array}{c}\mathbf{x}={\left[{\mathbf{R}}^{\mathbf{T}},{\mathbf{p}}^{\mathbf{T}},{\mathbf{v}}^{\mathbf{T}},{\mathbf{b}}^{\mathbf{T}}\right]}^{\mathbf{T}}\end{array}$$
因子图优化：IMU预积分因子、激光雷达里程计因子、GPS因子、回环因子。当机器人姿态的变化超过用户定义的阈值时，图中会添加一个新的机器人状态节点x

IMU测量值
$$\begin{array}{rl}& {\hat{\omega }}_t={\omega }_t+{\mathbf{b}}_t^{\omega }+{\mathbf{n}}_t^{\omega }\\ & {\hat{\mathbf{a}}}_t={\mathbf{R}}_t^{\text{BW}}\left({\mathbf{a}}_t-\text{g}\right)+{\mathbf{b}}_t^{\text{a}}+{\mathbf{n}}_t^{\text{a}}\end{array}$$
欧拉法IMU预积分
v t + Δ t = v t + g Δ t + R t ( a ^ t − b t a − n t a ) Δ t p t + Δ t = p t + v t Δ t + 1 2 g Δ t 2   + 1 2 R t ( a ^ t − b t a − n t a ) Δ t 2 R t + Δ t = R t exp ⁡ ( ( ω ^ t − b t ω − n t ω ) Δ t ) \begin{align*} & { {\mathbf{v}}_{t + \Delta t}} = { {\mathbf{v}}_t} + {\mathbf{g}}\Delta t + { {\mathbf{R}}_t}\left( { { {\widehat {\mathbf{a}}}_t} - {\mathbf{b}}_t^{\text{a}} - {\mathbf{n}}_t^{\mathbf{a}}} \right)\Delta t\\ & \begin{array}{c} { {\mathbf{p}}_{t + \Delta t}} = { {\mathbf{p}}_t} + { {\mathbf{v}}_t}\Delta t + \frac{1}{2}{\mathbf{g}}\Delta {t^2} \ + \frac{1}{2}{ {\mathbf{R}}_t}\left( { { {\widehat {\mathbf{a}}}_t} - {\mathbf{b}}_t^{\mathbf{a}} - {\mathbf{n}}_t^{\mathbf{a}}} \right)\Delta {t^2}\end{array} \\ & { {\mathbf{R}}_{t + \Delta t}} = { {\mathbf{R}}_t}\exp \left( {\left( { { {\widehat {\mathbf{\omega }}}_t} - {\mathbf{b}}_t^\omega - {\mathbf{n}}_t^{\mathbf{\omega }}} \right)\Delta t} \right)\end{align*} ​vt+Δt​=vt​+gΔt+Rt​(a t​−bta​−nta​)Δtpt+Δt​=pt​+v