Tightly-Coupled Multi-Sensor Fusion for Localization with LiDAR Feature Maps
2024-06-16 09:55:59
文章目录
- 介绍
- 摘要：
- 相关工作
- 紧密耦合的传感器融合框架
- 实验验证：
介绍
摘要：
- 定位中长期稳定而准确的姿态估计对于自动驾驶至关重要。在本文中，我们使用 LiDAR 特征图和多传感器测量来处理绝对定位。我们提出了一种具有固定滞后平滑的紧耦合融合方法。通过最小化联合成本函数来估计最近滑动窗口的状态。该成本函数包括全局 LiDAR 配准的残差以及来自 IMU 和车轮编码器的相对运动学约束。此外，我们通过改进 LiDAR 注册来增强我们方法的稳健性。为了实现这一目标，构建和开发了具有几何和正态分布特征混合的 LiDAR 特征图。在超过 200 公里的几个具有挑战性的测试序列中验证了所提出方法的有效性。实验结果表明，即使在 LiDAR 观测下降的情况下，所提出的方法也能在具有挑战性的场景中实现准确定位和高鲁棒性。
相关工作
A. 基于视觉的定位
- 基于视觉的定位方案[1]、[2]、[3]广泛应用于具有丰富语义约束的结构化场景。它主要将图像中的语义信息（如道路车道、交通灯、路牌等）与高清地图对齐，用于车辆姿态估计。Ma. 等人通过融合 GPS、IMU、车轮编码器和相机的测量结果，提出了一种基于视觉的高清语义定位方法 [3]。
B. 基于 LiDAR 的定位
- 基于 LiDAR 的定位方法将当前 LiDAR 帧的特征与全局地图相关联，并使用特征对应来优化姿势。主要分为三类。
- 基于强度的方法：Levinson 等人。首先提出了一种基于强度的定位方法 [4]，它集成了来自 GPS、IMU 和车轮编码器的测量值。后来由 [5] 改进，其中地图从 2D 网格的反射率升级到反射率的高斯分布。 Wolcott 和 Eustice 提出了一种方法，将地图表示为包含点云强度和高度的混合高斯模型 [14]。最近的一项工作 [13] 将混合高斯分布的地图表示分成两个单一的高斯分布，分别表示强度和高度属性。
- 基于分布的方法：正态分布变换（NDT）是具有代表性的方法，已广泛用于点云配准和车辆定位[6]、[7]、[15]、[16]。它首先将点云划分为规则体素，每个体素中的点通过 3D 高斯分布近似。然后通过最大化所有体素的联合高斯概率密度来优化两个点云之间的姿态。
- 基于几何特征的方法：Zhang 和 Singh 为 LiDAR 里程计提出了一种称为 LOAM [8]、[9] 的方法。首先，它提取独特的几何特征，然后使用迭代最近点 (ICP) [17] 将当前几何特征与 LiDAR 特征图对齐。 LOAM中的特征包括边缘和表面特征，通过计算原始点云的曲率来提取。在[18]中提出了一种基于低级语义分割的激光雷达定位方法。它依赖于包括地面、路缘石、边缘和表面在内的特征，并将这些特征与预先构建的特征图相匹配。尽管这些方法取得了成功，但所有上述方法都依赖于显着几何特征的稳定关联，并且在具有稀疏结构的场景中容易退化。
C. 传感器融合定位框架
- 传感器融合方法主要包括基于过滤和基于优化的方法。流行的滤波器包括扩展卡尔曼滤波器 (EKF) 和粒子滤波器 (PF)。基于EKF的定位方法广泛应用于机器人[16][18][19]，而其预测阶段的线性化会损失一定的精度。基于PF的定位方法[4]、[13] 传感器融合方法主要包括基于过滤和基于优化的方法。流行的滤波器包括扩展卡尔曼滤波器 (EKF) 和粒子滤波器 (PF)。基于EKF的定位方法广泛应用于机器人[16][18][19]，而其预测阶段的线性化会损失一定的精度。基于 PF 的定位方法 [4]、[13] [15] 比基于 EKF 的方法更稳健。虽然 PF 中的随机采样是耗时的，并且 PF 通常仅保持三维状态（水平位置和偏航）。批量优化方法 [20] 维护一组历史观测值，并对过去的几个状态执行非线性优化，以产生更准确的结果。在 [11] 中，它集成了本地 LiDAR 约束和 IMU 以进行车辆定位。然而，这种方法缺乏绝对的观测约束，并且存在累积误差，不适合长期车辆定位。在[21]中，提出了一种松耦合方法，其中相对姿态由独立于全局观察的 LIO 模块估计，并通过姿态图优化与全局框架对齐.
紧密耦合的传感器融合框架
A. 系统回顾
- 所提出方法的流程图如图 2 所示。我们假设传感器的所有内在参数都是已知的，并且激光雷达、IMU 和车轮编码器之间的外在参数已经预先校准。 一个例外是 LiDAR 和 IMU 的外部参数是在线细化的。 所有传感器均由硬件触发以进行时钟同步。 为方便起见，我们还假设 IMU 框架与车身框架重合。 因此，所有状态变量都在 IMU 框架中表示和优化。 本文中的符号总结在表 I 中。我们使用固定滞后平滑优化器。 滑动窗口中维护的状态变量总结为：
{ 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 a k T b g k T b l b T q l b T ] ] \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. {Xk−m+1:k=[Xk−m+1,Xk−m+2,...,Xk]Xk=[pkT vkT qkT bakT bgkT blbT qlbT]]
这儿， p k p_k pk 和 v k v_k vk 是相关帧 F W {F_W} FWIMU位置和速度。 q k q_k qk是自身坐标系到世界坐标系的旋转矩阵的四元素表示， b a k T b g k T b_{ak}^T\ \ b_{gk}^T bakT bgkT代表陀螺仪在时间k的加速度和偏置。另外，我们估计激光到base的外参 b l b T q l b T b_{lb}^T\ \ q_{lb}^T blbT qlbT，
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 {v_x,v_y} vx,vy，和yaw角度 θ ˙ \dot \theta θ˙，残差为： r ν ( R k ; v ^ k ) = v k − R k v ^ k {r_\nu (R_k;\hat{v}_k) = v_k -R_k \hat v_k } rν(Rk;v^k)=vk−Rkv^k 这儿 v ^ k = [ v x , v y , 0 ] T {\hat v_k = [v_x,v_y,0]^T} v^k=[vx,vy,0]T是轮子在base坐标系的观测。
F.状态初始化
- 需要初始化的状态变量包括： IMU 速度 V 1 : m V_{1:m} V1:m 、重力矢量 g 、陀螺仪偏置 b g b_g bg 和加速度偏置 b a b_a ba 。 我们在滑动窗口中对 LiDAR 和车轮编码器观察进行联合优化。
- 首先，我们从 IMU 获得旋转和车轮编码器的运动预测。 然后将滑动窗口中每个 LiDAR 帧的 LiDAR 特征（包括边缘、表面、正态分布特征）与第 III-C 节中提到的运动预测的特征图相关联。 最后，我们制定了 LiDAR 特征的约束条件和运动约束条件 帧之间转化为非线性优化问题，并通过以下方式求解每帧的 LiDAR 位姿:
G. 多帧紧耦合 - 提出了一个基于固定滞后平滑器的联合优化框架。固定滞后平滑器是一种迭代算法，它交替更新状态变量并边缘化滑动窗口中的旧约束 [20]。 它递归地维护对最后 m 个时间戳的状态变量的总概率密度的估计，这是一种基于图优化的增量状态估计方法 [27]。
实验验证：
优化由ceres求解。
- 精度
- 鲁棒性
- 运行时内存使用
Tightly-Coupled Multi-Sensor Fusion for Localization with LiDAR Feature Maps相关推荐
- 经典文献阅读之--Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping（紧耦合3D激光雷达）
0. 简介 传统的地图生成方法一般是依靠Lidar和IMU结合的,但是问题在于,目前Lidar和IMU的紧耦合主要集中在前端里程计,基本没有涉及到后端全局优化以及建图的融合.为此文章<Globa ...
- 深度学习论文: Multi-modal Sensor Fusion for Auto Driving Perception: A Survey
深度学习论文: Multi-modal Sensor Fusion for Auto Driving Perception: A Survey Multi-modal Sensor Fusion fo ...
- Sensor Fusion
1. 概述 首先明确几个概念和名词: Sensor Fusion - 传感器融合 IMU (Inertial Measurement Unit) - 惯性测量单元 AHRS (Attitude and ...
- 赶工读文章（2）— Radar-Camera Sensor Fusion for Joint Object Detection and Distance Estimation in AV
题目是AV是Autonomous Vehicles,太长写不下了.如有错漏,还望指正. Radar-Camera Sensor Fusion for Joint Object Detection an ...
- Seeing Through Fog Without Seeing Fog:Deep Multimodal Sensor Fusion in Unseen Adverse Weather （翻）
Title:Seeing Through Fog Without Seeing Fog:Deep Multimodal Sensor Fusion in Unseen Adverse Weather ...
- 【文献阅读】Sensor Fusion Algorithm Design in Detecting Vehicles Using Laser Scanner and Stereo Vision
利用激光扫描仪和立体视觉检测车辆的传感器融合算法设计 摘要 众所周知,激光扫描仪在探测障碍物的距离和速度方面比立体视觉具有更好的精度,而立体视觉比激光扫描仪更能分辨物体. 通过传感器融合方法,可以最大 ...
- Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（一）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(一) 1. LiDAR inertial odometry and mapping简介 ...
- Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（四）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(四) 3. Joint optimization 3.3 IMU preintegrat ...
- Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（二）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(二) 3. Joint optimization 3.1 Marginalization ...
最新文章
- linux telnet 常见问题配置
- java float 加法_Java-杂项：Float 加减精度问题
- postgres 备份 恢复
- C/C++ 语言中的表达式求值
- spring mvc学习（25）:Eclipse设置代码自动提示
- 使用libjpeg进行JPEG图像解码
- 判断是否为IE浏览器（方法一）
- Android中添加自己的模块 【转】
- javajs ---- 判断字符串中是否包含子串
- 代理模式（动态代理模式）
- android平台开发的安装
- 数字电视智能卡的定义
- 海康威视 摄像头 RTMP 转 FLV
- STM32F100X RCC_APB2Periph_AFIO--复用IO时钟的使用
- C语言伽罗华域乘法,伽罗瓦域上的乘法
- 3分钟教会你解决SVN文件夹没有绿勾和红色感叹号的问题
- TikTok搬运视频怎么做，搬运怎样的视频最好
- Hugo作者、Go核心团队成员Steve Francia谈诞生13年的Go语言：生态系统、演化与未来[译]...
- Google Chrome浏览器字体模糊不清
- Android 开发环境搭建