Tightly-Coupled Multi-Sensor Fusion for Localization with LiDAR Feature Maps

最新推荐文章于 2023-04-14 19:01:34 发布

xiaoma_bk

最新推荐文章于 2023-04-14 19:01:34 发布

阅读量966

点赞数 2

分类专栏： # Paper Slam 文章标签：自动驾驶深度学习机器学习

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/120345697

版权

Paper Slam 专栏收录该内容

6 篇文章 0 订阅

订阅专栏

文章目录

介绍

介绍

摘要：

定位中长期稳定而准确的姿态估计对于自动驾驶至关重要。在本文中，我们使用 LiDAR 特征图和多传感器测量来处理绝对定位。我们提出了一种具有固定滞后平滑的紧耦合融合方法。通过最小化联合成本函数来估计最近滑动窗口的状态。该成本函数包括全局 LiDAR 配准的残差以及来自 IMU 和车轮编码器的相对运动学约束。此外，我们通过改进 LiDAR 注册来增强我们方法的稳健性。为了实现这一目标，构建和开发了具有几何和正态分布特征混合的 LiDAR 特征图。在超过 200 公里的几个具有挑战性的测试序列中验证了所提出方法的有效性。实验结果表明，即使在 LiDAR 观测下降的情况下，所提出的方法也能在具有挑战性的场景中实现准确定位和高鲁棒性。

紧密耦合的传感器融合框架

紧耦合多传感器融合精度
A. 系统回顾

所提出方法的流程图如图 2 所示。我们假设传感器的所有内在参数都是已知的，并且激光雷达、IMU 和车轮编码器之间的外在参数已经预先校准。一个例外是 LiDAR 和 IMU 的外部参数是在线细化的。所有传感器均由硬件触发以进行时钟同步。为方便起见，我们还假设 IMU 框架与车身框架重合。因此，所有状态变量都在 IMU 框架中表示和优化。本文中的符号总结在表 I 中。我们使用固定滞后平滑优化器。滑动窗口中维护的状态变量总结为：
$\left\{\begin{matrix} X_{k-m+1:k} = [X_{k-m+1},X_{k-m+2},...,X_{k}] \\ X_k = [p_k^T \ \ v_k^T\ \ q_k^T\ \ b_{ak}^T\ \ b_{gk}^T\ \ b_{lb}^T\ \ q_{lb}^T]] \end{matrix}\right.$
这儿， $p_k$ 和 $v_k$ 是相关帧 ${F_W}$ IMU位置和速度。 $q_k$ 是自身坐标系到世界坐标系的旋转矩阵的四元素表示， $b_{ak}^T\ \ b_{gk}^T$ 代表陀螺仪在时间k的加速度和偏置。另外，我们估计激光到base的外参 $b_{lb}^T\ \ q_{lb}^T$ ，

B.激光特征地图生成

要进行在线定位，应提前构建可扩展的高精度地图。我们的地图包含三种特征：边缘特征、表面特征和 3D 分布。与稀疏 3D 点云或 NDT 地图相比，我们的地图同时包含几何特征（边缘和表面）和 3D 正态分布特征，使我们的方法能够获得稳健而准确的特征关联。地图生成包含以下三个步骤：
LiDAR 姿态生成：我们使用基于图优化的离线 SLAM 框架来完全融合来自各种传感器的观察结果，包括具有后处理功能的高精度 GNSS 数据、IMU、车轮编码器、和两个 LiDAR，以生成准确的 LiDAR 姿势。离散激光扫描与 LiDAR 姿态对齐，以生成具有全局一致性的点云图。由于 LiDAR 姿态的生成超出了这项工作的范围，我们将忽略具体细节。
点云不失真：受相关工作[23]的启发，在连续时间戳之间均匀运动的假设下，我们使用 IMU 和车轮编码器提供的运动预测作为运动模型来不失真点云。
特征图的参数化：在获得全局一致的点云图后，我们从密集点云图中提取显着的 LiDAR 特征。
- 首先我们将点云图划分为体素，并计算每个体素的均值和协方差矩阵。
- 然后我们通过计算协方差矩阵的特征值将体素分类为边缘、表面或正态分布特征，具体来说，具有两个小特征值和一个大特征值表示边缘特征；有一个小两个大特征值表示表面特征；否则表示正态分布特征。
- 最后，直线方程特征、表面特征的法向量、均值和计算正态分布特征的协方差矩阵分别参数化相应的体素。

C IMU 预积分
在这里插入图片描述

D. 激光特征因素

原始点云 P 由车辆的快速移动引起的失真。然后我们在激光雷达帧中相对于激光扫描的开始获得一组未失真的点云 P。
传统的几何特征提取方法[8][9][11]计算原始点云中数十万个点的曲率，通过曲率选择当前帧的边缘和表面特征。相比之下，我们从密集点云中均匀采样有限点 P S 作为候选特征点，然后计算每个采样点的平滑度 [9]，用于后续特征关联的几何验证。- 我们的采样策略可以节省大量处理时间，同时特征关联的准确性和鲁棒性不受影响。
在对一组候选特征点 P S 进行采样后，首先我们通过运动预测的初始猜测将它们转换为全局帧。然后我们将这些点与预构建地图相关联。关联会在后续优化中修复。如果变换后的查询点位于特定特征（边缘、表面或正态分布）的体素上，并且其曲率满足相应特征的阈值范围，则通过几何验证。对于通过几何验证的点，使用相应的观测模型构造观测约束。三种 LiDAR 特征的残差公式如下：

E.车轮编码器的速度系数
里程计在观测退化时至关重要，自身坐标系 ${v_x,v_y}$ ，和yaw角度 $\dot \theta$ ，残差为： ${r_\nu (R_k;\hat{v}_k) = v_k -R_k \hat v_k }$ 这儿 ${\hat v_k = [v_x,v_y,0]^T}$ 是轮子在base坐标系的观测。

F.状态初始化

需要初始化的状态变量包括： IMU 速度 $V_{1:m}$ 、重力矢量 g 、陀螺仪偏置 $b_g$ 和加速度偏置 $b_a$ 。我们在滑动窗口中对 LiDAR 和车轮编码器观察进行联合优化。
首先，我们从 IMU 获得旋转和车轮编码器的运动预测。然后将滑动窗口中每个 LiDAR 帧的 LiDAR 特征（包括边缘、表面、正态分布特征）与第 III-C 节中提到的运动预测的特征图相关联。最后，我们制定了 LiDAR 特征的约束条件和运动约束条件帧之间转化为非线性优化问题，并通过以下方式求解每帧的 LiDAR 位姿:
G. 多帧紧耦合
提出了一个基于固定滞后平滑器的联合优化框架。固定滞后平滑器是一种迭代算法，它交替更新状态变量并边缘化滑动窗口中的旧约束 [20]。它递归地维护对最后 m 个时间戳的状态变量的总概率密度的估计，这是一种基于图优化的增量状态估计方法 [27]。