0. 简介
为了实现精确定位，自动驾驶车辆通常依靠围绕移动平台的多传感器感知系统。校准是一个耗时的过程，机械畸变会导致外部校准误差。因此，《Lidar-Visual-Inertial Odometry with Online Extrinsic Calibration》提出了一种激光雷达-视觉-惯性里程计，结合了适应性滑动窗口机制，允许在线非线性优化和外部校准。在适应性滑动窗口机制中，进行空间-时间对齐以管理以不同频率到达的测量。在在线校准的非线性优化中，使用视觉特征、云特征和惯性测量单元（IMU）测量来估计自我运动并进行外部校准。
1. 主要贡献
本文提出了一种基于非线性优化的激光雷达-视觉-惯性系统，用于鲁棒地和准确地估计姿态并在线进行外部标定。本研究的主要贡献包括：
- 提出了一种适应性滑动窗口机制，用于管理来自异构传感器，包括3D激光雷达、相机和IMU的不同频率的测量。
- 提出了一种非线性优化公式，使用视觉特征、点云特征和IMU测量同时进行在线标定和运动估计。
- 进行了一系列广泛的实验，以验证所提出系统的性能。结果表明，我们的方法优于现有技术水平方法
2. 优化基于的激光雷达-视觉-惯性系统
2.1 整体系统（略看即可）
所提出系统的框架如图1所示，其中T^{B_{tc}}_{B_{tl}}是传播变换，而b_a和b_ω分别是IMU加速度计和陀螺仪偏差。在测量预处理步骤中，基于Kanade-Lucas-Tomasi（KLT）算法[21]，在连续图像之间检测和跟踪视觉特征。基于曲率从点云中提取云特征，并由3D激光雷达在本地地图内跟踪，类似于LIO-SAM[8]。使用连续视觉关键帧之间的所有IMU测量计算IMU预积分项。
构建一个涉及所有系统状态和测量的非线性优化问题，在滑动窗口内对系统状态（运动和外参）进行优化。如果将W、B、C和L分别定义为世界、IMU、相机和三维激光雷达的框架，则要优化的状态变量定义为在滑动窗口范围N+1内的运动状态x^W_{B_{t_i}}(i=0,1,…,N)，相机外参x^B_C，3D激光雷达外参x^C_L和观察到的视觉特征的逆深度d_j(j=1,2,…,m)。状态变量的完整定义为：
由于其较高的频率，视觉特征可能并不总能找到匹配的云特征（如图2中虚线圆圈所示的“未找到云”）。接下来，我们将详细描述时空对准过程。
当新的云特征帧P^L_{tl}在时间t_i到达时，首先将其空间对准到本体坐标系，即
经过空间对准之后，P^B_{tl} 与特定的 x^W_{B_{t_i}}、x^B_C 和 x^C_L 在以下形式上进行时间对准：
其中，R^W_{B_{t_i}}和p^W_{B_{t_i}}代表物体框架在时间t_i的方向（由旋转矩阵描述）和位置（由3D向量描述）；v^W_{B_{t_i}}是速度向量；b_{a,t_i}和b_{ω,t_i}是IMU加速度计和陀螺仪偏差，也在时间t_i；R^B_C和 p^B_C包含相机和IMU之间的外在关系；R^C_L和 p^C_L是3D激光雷达和相机之间的外在关系。最后，应注意VINS-Mono的视觉回路闭合模块被直接用于消除累积的定位误差[6]。
2.2 自适应滑动窗口（和vins类似）
我们系统的滑动窗口机制改编自VINS-Mono[6]。主要区别在于特征管理，如图2所示。考虑到最低频率（点云特征）的测量作为参考，它总是与匹配的视觉特征进行空间-时间对齐。然而，最近的视觉特征时间为t_c：
通过将T^Y_X表示为从框架Y到框架X的变换矩阵，传播的变换可以得到
在IMU测量在时间间隔[t_c, t_l]内计算。IMU测量在图2中用红色括号表示。然后，时间对齐被表述为：
完成这两个步骤后，初始的时空对齐就完成了。然而，在非线性优化后，b_a和b_ω可能会发生变化；因此，如果任何一个偏置的变化超过一定阈值，就会进行IMU重新传播，以更新T Btc Btl并重新对齐点云特征。
2.3 使用在线校准的非线性优化（重点内容）
使用公式（1）中定义的状态变量和传感器测量值，我们构建一个非线性最小二乘问题，以共同执行运动估计和在线校准。
其中，ρ(·)是Huber函数[22]；∥r(·)∥_P是残差r的协方差矩阵；r_p - H_px表示被边缘化的先验；r_B(\hat{z}^{B_i}_{B_{i+1}}, x)和r_C(\hat{z}^{C_j}, x)分别是IMU预积分残差和视觉重投影残差。r_B(\hat{z}^{B_i}_{B_{i+1}}, x)和r_C(\hat{z}^{C_j}, x)的推导在涉及VINS-Mono[6]的讨论中给出。此外，r_E(M^E_l, \hat{z}^E_m, x)和r_P(M^P_l, \hat{z}^P_n, x)分别是激光雷达边缘和平面特征的残差，下面将对其进行讨论；r_{ex}(x, x_{ini})是外部先验的残差项。公式（6）形成了最大后验（MAP）估计，最小化边缘化先验和所有测量残差的马氏范数之和，使用C++库Ceres[23]进行非线性优化实现。
激光雷达边缘残差r_E(M^E_l, \hat{z}^E_m, x)被定义为边缘特征点\hat{z}^E_m（描述为3D矢量P^L_{m,E}）和本地边缘特征地图M^E_l中匹配的边缘之间的误差，在帧l的状态x下定义M^E_l为世界坐标系中N个边缘云帧P^{W,E}_{l-i} (i = 1,2,…,N)的并集，这些信息将在下面讨论。
边缘残差项被定义为点到线的误差：
其中，P_A和P_B是在局部地图M^E_l中匹配的边缘点，P^W_{m,E} ∈ P^{W,E}_l表示从当前激光雷达扫描中提取的边缘特征的世界坐标。
类似地，我们定义r_P(M^P_l,\hat{z}^P_n,x)为平面特征\hat{z}^P_n（描述为3D向量P^{L}_{n,P}）与局部平面特征图M^P_l中匹配的平面surfel（表面元素）之间的残差，该图由N个平面点云帧P^{W,P}_{l−i}(i=1,2，···，N)组成，这样
激光雷达平面残差r_P(M^P_l,\hat{z}^P_n,x)被定义为点到平面误差
其中，n和d分别是匹配的surfel的范数向量和常数项；P^W_{n,P ∈ P^{W,P}_l 表示当前帧平面特征的世界坐标。
外部先验残差r_{ex}(x，x_{ini})定义为初始状态x_{ini}与当前状态x之间的误差：
其中\boxminus是SE(3)中的三维平移和旋转操作符。此外
其中 ln(·) 是定义在 SO(3) 上的对数映射（logarithm map），而 (·)^∨ 是将一个反对称矩阵映射到其相应向量的运算符。SO(3) 是指三维旋转群（3D rotation group）。
评论（0）
您还未登录，请登录后发表或查看评论