【LOAM系列】二：LIO-Mapping论文笔记

LIO-Mapping

ICRA 2019 叶浩阳 香港科技大学 电子与计算机工程系 LIOM

1、论文

Tightly Coupled 3D Lidar Inertial Odometry and Mapping

Mapping源码解析 https://blog.csdn.net/qq_17693963/article/details/94406205?spm=1001.2014.3001.5501

1.1 整体框架

一种紧耦合激光雷达IMU融合方法，一种基于旋转约束的细化算法（LIO映射）以进一步将激光雷达姿态与全局地图对齐

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-MfX1b6Hx-1686290009356)(/home/jeet/图片/lio-mapping-pipeline.jpg)]

1.2 IMU预积分

IMU与激光雷达的松耦合：将激光雷达和惯性测量单元的测量解耦，分别考虑激光雷达的估计和惯性测量组合的估计。激光雷达里程计依赖惯性测量单元辅助，由IMU计算的旋转与平移作为整个系统的先验项，无法利用IMU的测量进一步优化，也不能估计IMU的相关参数。

符号表示：

符号	表示内容
scan C	一条扫描线
sweep S	一帧点云
T_b^a∈SE(3)	将坐标系b 中的点旋转到a中
a^(k) w^(k)	IMU的原始测量值
Fa	在a坐标下提取到的特征

状态变量

$$\begin{array}{rl}& {\mathbf{X}}_{B_i}^W={\left[\begin{array}{ccccc}{\mathbf{p}}_{B_i}^{W^T}& {\mathbf{v}}_{B_i}^{W^T}& {\mathbf{q}}_{B_i}^{W^T}& {\mathbf{b}}_{a_i}^T& {\mathbf{b}}_{g_i}^T\end{array}\right]}^T\\ & {\mathbf{T}}_B^L={\left[\begin{array}{cc}{\mathbf{p}}_B^{L^T}& {\mathbf{q}}_B^{L^T}\end{array}\right]}^T,\end{array}$$

所有的优化量： 滑窗内的状态 + 外参
$$\begin{array}{c}\mathrm{X}=[{\mathrm{X}}_{B_p}^W,\cdots ,{\mathrm{X}}_{B_j}^W,{\mathrm{T}}_B^L].\end{array}\text{\hspace{1em}(5)}$$

两帧之间的IMU积分

$$\begin{array}{rl}& {\mathbf{p}}_j={\mathbf{p}}_i+\sum _{k=i}^{j-1}\left[{\mathbf{v}}_k\Delta t+\frac{1}{2}{\mathbf{g}}^W\Delta t^2+\frac{1}{2}{\mathbf{R}}_k\left({\hat{\mathbf{a}}}_k-{\mathbf{b}}_{a_k}\right)\Delta t^2\right]\\ & {\mathbf{v}}_j={\mathbf{v}}_i+{\mathbf{g}}^W\Delta t_{ij}+\sum _{k=i}^{j-1}{\mathbf{R}}_k\left({\hat{\mathbf{a}}}_k-{\mathbf{b}}_{a_k}\right)\Delta t\\ & {\mathbf{q}}_j={\mathbf{q}}_i\otimes \prod _{k=i}^{j-1}\delta {\mathbf{q}}_k={\mathbf{q}}_i\otimes \prod _{k=i}^{j-1}\left[\begin{array}{c}\frac{1}{2}\Delta t\left({\hat{\mathbf{\omega }}}_k-{\mathbf{b}}_{g_k}\right)\\ 1\end{array}\right]\end{array}\text{\hspace{1em}(2)}$$

积分使用欧拉法，预积分公式和VINS-mono一样，基于状态的协方差传播

IMU预积分：

IMU的预积分残差：

**基于状态的卡尔曼滤波：**使用小角度近似，并且舍去Δt^2的二阶小量，注意：最后的F矩阵是I + FΔt

1.3 IMU-Lidar紧耦合

两个并行的线程，一个是紧耦合的雷达-惯性测量单元里程计，在一个局部窗口内优化所有位姿。第二个是旋转约束细化(导致全局一致的建图过程)，使用来自优化后的位姿和重力约束的信息将激光雷达sweep与全局地图对齐

1. 在激光雷达数据$ Sj$到来之前，IMU原始输入$I_{ij}$使用公式（2）进行积分（Stateprediction）和预积分（Pre-intergration），积分的目的是为了获得我们需要估计的状态变量$T_{Bi}^W$的初值，而预积分的目的是为了获得用于联合优化的$Δpij 、Δvij$和 $\Delta qij$；
2. 当系统收到激光雷达数据$Sj$后，对激光雷达进行去畸变操作（De-skewing）获得去畸变点云$\bar{S}j$，然后在此基础上对去畸变点云提取激光雷达特征点$F_{Lj} $,（Feature extraction）；
3. 在滑动窗口内的激光雷达特征$ F_{Lo,i}$根据优化后的位姿$T_{Bo,i}^{W$和外参$T_{B}}L$‘组成局部地图$ M_{Lo, i}^{L_p}$（Local map management），根据预测的位姿实现当前帧激光特征点和局部地图的匹配（FInd relative lidar measurements）；
4. 最后就第j帧激光雷达观测$m_{Lp+1,j}$以及预积分结果$\Delta pij，\Delta vij$和$ Δqij$实现联合优化，优化后的结果用于更新IMU积分获得的状态变量，避免IMU漂移；

**雷达畸变矫正与特征提取：**畸变矫正使用IMU的预测位姿进行插值，计算曲率提取平面点与边缘点，

1.4 滑动窗口优化

o——是滑窗中的第一帧Lidar Sweep，
i ——是滑窗中最后一帧Lidar Sweep，
j——是当前帧的Lidar Sweep，
p——是滑窗中的Pivot Lidar Sweep，所谓枢纽帧指的就是整个滑窗内帧的位姿都是基于该帧坐标系建立的，随着滑窗移动，Pivot Lidar Sweep也不断变化。

局部地图投影到枢纽帧

蓝色的大窗口即是论文中说的sliding window，在源码中该window设置为包含了15个激光雷达帧大小的窗口，其中从o 到p为之前优化过的帧，而从p 到i 对应的帧为待优化的帧；
窗口内待估计的状态是在Ns个时间戳{p+1，…，i，j}处的状态，其中p+1和j是在枢轴帧下一帧的激光雷达扫描，以及窗口中的当前激光雷达扫描。

KNN最近邻搜索对每个变换后（把p帧前面的帧的点投影到p帧下局部地图也投影到枢纽帧）的平面特征点$X$在 M_{L_{o,i}}^{L_p}中找到K 个最邻的点
$$\pi (x^{L_p})$$
每个平面点计算他拟合平面的法向量和d，平面点的系数用
$${\omega }^T{x}^{{}^{\prime }}+d=0,x^{{}^{\prime }}\in \pi (x^{L_p})$$

点的匹配残差

把p后面的帧的点投影到p帧i 时刻和p时刻IMU坐标系到世界坐标系的位姿变化关系，且已知激光雷达到IMU坐标系之间的变换关系,可以得到i 时刻激光雷达坐标系到p 时刻激光雷达坐标系之间的变换关系为：
$$\begin{array}{c}{\mathrm{T}}_{L_{\alpha }}^{L_p}={\mathrm{T}}_B^L{\mathrm{T}}_{B_p}^{W-1}{\mathrm{T}}_{B_{\alpha }}^W{\mathrm{T}}_B^{L-1}=\left[\begin{array}{cc}{\mathrm{R}}_{L_{\alpha }}^{L_p}& {\mathrm{p}}_{L_{\alpha }}^{L_p}\\ 0& 1\end{array}\right].\end{array}\text{\hspace{1em}(3)}$$
定义的线性方程求解，其中ω 是平面法线方向，d 是到平面FLp局部地图原点的距离。每个平面特征点$x ∈ F_{Lα} $对应$m = [ x , ω , d ] ∈ m_l $

作为相对雷达测量之一。点的坐标是 x和平面参数是定义在局部坐标系下的。

激光雷达代价函数中的每一项都包含两个激光sweep的位姿即$T_{LP}^W$ 和 $T_{La}^W $

每个相对激光雷达测量$m=[x，\omega ，d]\in m_{L\alpha }，\alpha \in [p+1，...，j]$的残差可以表示为点到平面的距离

$$\begin{array}{c}{\mathrm{r}}_{\mathcal{L}}(m,{\mathrm{T}}_{L_p}^W,{\mathrm{T}}_{L_{\alpha }}^W,{\mathrm{T}}_B^L)={\omega }^T({\mathrm{R}}_{L_{\alpha }}^{L_p}x+{\mathrm{p}}_{L_{\alpha }}^{L_p})+d.\end{array}\text{\hspace{1em}(4)}$$

优化函数

min ⁡ X 1 2 { ∥ r P ( X ) ∥ 2 + ∑ m ∈ m L α ∈ { p + 1 , ⋯   , j } ∥ r L ( m , X ) ∥ C L α m 2 + ∑ β ∈ { p , ⋯   , j − 1 } ∥ r B ( z β + 1 β , X ) ∥ C B β + 1 B β + 1 } , \begin{align*}\min _{\mathbf{X}} \frac{1}{2}\bigg\{\left\|\mathbf{r}_{\mathcal{P}}(\mathbf{X})\right\|^{2}+\sum_{{m \in \mathbf{m}_{L} \alpha \in\{p+1, \cdots, j\}}}\left\|\mathbf{r}_{\mathcal{L}}(m, \mathbf{X})\right\|_{\mathbf{C}_{L_{\alpha}}^{m}}^{2}+\sum_{\beta \in\{p, \cdots, j-1\}}\left\|\mathbf{r}_{\mathcal{B}}\left(z_{\beta+1}^{\beta}, \mathbf{X}\right)\right\|_{\mathbf{C}_{B_{\beta+1}}^{B_{\beta+1}}}\bigg \},\tag{6}\end{align*} Xmin​21​{∥rP​(X)∥2+m∈mL​α∈{p+1,⋯,j}∑​∥rL​(m,X)∥CLα​m​2​+β∈{p,⋯,j−1}∑​ ​rB​(zβ+1β​,X) ​CBβ+1​Bβ+1​​​},​(6)​

其中$r_{\mathcal{P}}(X)$是边缘化后的先验约束，$r_{\mathcal{L}}(m,X)$是相对激光雷达约束的残差$r_{\mathcal{B}}(z_{\beta +1}^{\beta })$是IMU预积分约束的残差。使用ceres优化求解

1.5 四自由度位姿优化位姿

将最新激光雷达特征点与全局地图对齐，代价函数：
$$\begin{array}{r}{\mathrm{C}}_{\mathcal{M}}=\sum _{m\in {\mathrm{m}}_L}\Vert {\mathrm{r}}_{\mathcal{M}}(m,{\mathrm{T}}_L^W){\Vert }^2,\\ {\mathrm{r}}_{\mathcal{M}}(m,\mathrm{T})={\omega }^T(\mathrm{Rx}+\mathrm{p})+d\end{array}\text{\hspace{1em}(7)}$$
$T_L^W$是最新的待估计激光雷达位姿，$x$是雷达帧下的特征点，$m$是全局地图中的特征点平面拟合参数

未解决旋转的累计误差导致全局地图合并不能与重力对齐，利用$SE(2)$约束优化$SE(3)$，四自由度位姿优化，只考虑(x, y,z)和关于yaw轴的旋转 全局匹配的残差函数关于yaw轴的旋转的雅克比

$$\begin{array}{r}{\mathrm{J}}_{{\theta }_{\mathrm{z}}}^{\mathrm{C}}={\mathrm{J}}_{\theta }^{\mathrm{C}}\cdot (\breve{\mathrm{R}}{)}^T\cdot {\breve{\Omega }}_{\mathrm{z}}\\ {\breve{\Omega }}_{\mathrm{z}}=\left[\begin{array}{ccc}{ϵ}_x& 0& 0\\ 0& {ϵ}_y& 0\\ 0& 0& 1\end{array}\right],\end{array}\text{\hspace{1em}(8)}$$
其中$(\breve{\cdot })$表示在最后一次迭代中的状态估计，而$ \breve{\Omega}_z $ 表示相对于${\mathcal{F}}_w$旋转的信息矩阵的近似。${ϵ}_x$和${ϵ}_y$可以通过$x$轴和$y $轴旋转相对于$z $轴旋转的信息比来获得。在最后对p 和 q 更新
$$\begin{array}{rl}& \tilde{\mathrm{p}}=\breve{\mathrm{p}}+\delta \mathrm{p}\\ & \tilde{\mathrm{q}}=\left[\begin{array}{c}\frac{1}{2}\delta {\theta }_z\\ 1\end{array}\right]\otimes \breve{\mathrm{q}}\cdot \end{array}\text{\hspace{1em}(9)}$$

---