广告
FAST-LIO2: 快速直接的激光雷达-惯性里程计
2021-09-12 21:53:49· 来源：同济智能汽车研究所
k表示激光雷达扫描的索引，
j=1,... ,m} 表示在扫描结束时间时，在局部激光雷达坐标系L中第k次扫描采到的点。由于激光雷达测量噪声，每个测量点通常都会受到由测距和波束定向噪声组成的噪声
的污染。去除此噪声可得到在局部激光雷达坐标系中的真实点位置
在使用相应的激光雷达姿态
和外参
将真实点投影到全局坐标系之后，这个真实点应该正好位于地图中的一个局部小平面块上，即：
其中
是对应平面的法向量，
是平面上的一个点（见图2）。需要注意的是，
和
都包含在状态向量xk中。因此，第 j个点测量值
所贡献的测量值可以从（4）总结为更紧凑的形式，如下所示
这定义了状态向量的隐式测量模型.
图2 测量模型
B.迭代卡尔曼滤波
基于在流形
上制定的状态模型(2)和测量模型(5)，我们采用迭代卡尔曼滤波器，按照[55]和[22]中的过程直接在流形M上操作。它由两个关键步骤组成：每此IMU测量值的传播和每此激光雷达扫描的迭代更新，这两个步骤都自然地估计流形上的状态，从而避免任何重整化。由于IMU测量的频率通常高于激光雷达扫描的频率（例如，IMU测量为200Hz，激光雷达扫描为10Hz~100Hz），因此滤波器在更新之前通常会执行多个传播步骤。
1）传播：假设在融合最后一次（即第k-1次）激光雷达扫描后的最佳状态估计是为
，协方差矩阵为
。前向传播是在IMU测量值到达时执行的。更具体地说，通过将过程噪声wi设置为零，状态和协方差按照(2)进行传播：
其中Qi是噪声wi的协方差，矩阵
和
的计算如下（参见[55]中更抽象的推导和[22]中更具体的推导）：
前向传播一直持续到新的（即第k次）扫描结束，新扫描中的传播状态和协方差表示为
。
2）残差计算：假设在当前迭代更新时状态xk的估计值（见第IV-B3节）是
，根据（6）中的传播预测出的状态，当k=0时（即在第一次迭代前），
。然后，我们将每个测量的激光雷达点投影到全局坐标系
，并在由ikd-Tree表示的地图中搜索其最近的5个点（参见第V-A节）。然后使用找到的最近相邻点来拟合局部小平面块，该小平面块具有测量模型中使用的法向量
和质心
编辑推荐
最新资讯
-
推荐性国家标准《乘/商用车电子机械制动卡
2025-04-30 11:13
-
载荷分解
2025-04-30 10:46
-
布雷博在上海开设亚洲首个灵感实验室
2025-04-30 10:25
-
组分性能对锂离子电池卷芯挤压力学响应的影
2025-04-30 09:00
-
美国发布自动驾驶新框架，放宽报告要求+扩
2025-04-30 08:59
