m0_49384824的博客

本章，我们来关注实时的激光雷达定位系统。在点云地图基础之上，我们可以把当前激光扫描数据与地图进行匹配，从而获得车辆自身的位置，再与 IMU 等传感器进行滤波器融合。然而，。因此，点云定位在实际使用时，会遇到一些特有逻辑问题。本章将使用第 9 章构建的点云地图，展示点云定位的使用方法，并演示一个基于 ESKF 的实时定位方案。

完整的点云建图可以看成是一个的综合优化问题。大部分 L4 级别的自动驾驶任务都需要一张来进行地图标注、高精定位等任务。

和组合导航一样，也可以通过加上来实现。一些现代的 Lidar SLAM 系统也采用了这种方式。相比滤波器方法来说，预积分因子可以更方便地整合到现有的优化框架中，从开发到实现都更为便捷。

紧耦合系统，就是把点云的残差方程直接作为观测方程，写入观测模型中。这种做法相当于在滤波器或者优化算法内置了一个 ICP 或 NDT。因为 ICP 和 NDT 需要迭代来更新它们的最近邻，所以相应的滤波器也应该使用可以迭代的版本，ESKF 对应的可迭代版本的滤波器即为 IESKF。 基于 IESKF 的紧耦合 LIO 系统的流程图如下所示： IESKF 的状态变量及运动过程 和 前文介绍过的 ESKF 的状态变量及运动过程完全相同，包括：① 对名义状态变量的预测 ②对误差状态变量的

处理同步之后的点云和 IMU。初始化完毕后，先使用 IMU 数据进行预测，再用预测数据对点云去畸变，最后对去畸变的点云做配准。前文已经得到了去畸变的点云，这里只需将其传递给增量 NDT 里程计，并使用滤波器预测得到的先验位姿作为增量 NDT 里程计的初始位姿，经过迭代计算后得到优化后的位姿后再返回给滤波器，滤波器进行观测过程。），其存储上一 IMU 时刻 ESKF 的名义状态变量和当前 MeasureGroup 中每一个 IMU 数据预测后的 ESKF 的名义状态变量，用来插值进行点云的去畸变。

这里使用《自动驾驶与机器人中的SLAM技术》ch4：预积分学中提到的散装的形式来实现预积分的顶点部分，所以每个状态被分为位姿（）、速度、陀螺零偏、加计零偏四种顶点（共 15 维）。后三者实际上都是的变量，可以直接使用继承来实现。g2o 部分可参考。1. 相比 ESKF，基于预积分的图优化方案可以累积 IMU 读数。累积多少时间，或者每次迭代优化取多少次，都可以人为选择。而 ESKF 默认只能迭代一次，预测也只依据单个时刻的IMU 数据。

在一个 IMU 系统里，我们考虑它的五个变量：旋转 R、平移 p、角速度 ω、线速度 v 与加速度 a。根据第 2 章介绍的运动学，这些变量的运动学关系可以写成如下运动学方程：在到时间内对运动学方程进行欧拉积分得：IMU 测量方程（其中为 IMU 测量的高斯噪声 ）如下：IMU 测量方程带入积分后的运动学方程（其中为离散化后的 IMU 测量噪声 ）如下：其中噪声项满足：以上过程与我们在 IMU 测量方程和噪声方程（第 3 章）中已有描述。

典型的六轴IMU由陀螺仪（gyroscope）和加速度计（accelerometer）组成。

在实际的处理中，由于测量传感器只能测量离散化的值，在精度不高的应用场景中，我们通常会选择忽略后面三项，只保留最简单的转换关系。注意：能被各种传感器（车速传感器，轮速计）测量到的速度是车体系速度，进行线性化得到的雅可比矩阵，即运动方程对状态。进行线性化得到的雅可比矩阵，即观测方程对状态。，后续噪声的符号会变化，但表示的意义不变。其中 t 为平移向量。为观测方程在当前时刻预测状态。为运动方程在上一时刻状态。或者没有括号，表示省略。

高精地图本质上是结构化的矢量数据。常见的高精地图标准包括 OpenDrive，LaneLet2，Apollo OpenDrive 等。计算机上的高精地图通常由一些专用的绘制软件生成（例如 Arcgis，Autoware map tool等)。L2 在技术实现上会更倾向于实时感知，乃至可以使用感知结果直接构建。（bird eye view, BEV），而 L4 则依赖。