1.技术背景
SLAM（Simultaneous Localization and Mapping）是一种建图及定位技术，应用于机器人、自动驾驶和增强现实等领域，其核心是通过相机、激光雷达、惯性测量单元等设备获取环境几何信息，实时构建地图并估计载体运动。组合导航系统是一种常规的提供实时导航服务的装备，其核心是基于GNSS（全球卫星导航定位系统)定位的技术，它在特定环境下存在难以提供准确导航服务的问题：如隧道、地下空间等卫星信号难以覆盖的场景，如城市高楼、玻璃墙面等多路径效应导致定位误差大的场景，如大型水体、高压线、极端天气等电磁干扰导致定位误差大的场景。
为解决组合导航系统存在的问题，结合棋骥无人车软硬件系统的特点，探索出一种可满足低速行驶、可替代组合导航系统的SLAM技术。在硬件层，棋骥各类型无人车均搭载一至四个集成IMU(惯性测量单元)传感器的固态激光雷达，要求SLAM算法能融合激光雷达、IMU传感器；在软件层，自动驾驶系统要求SLAM算法能输出频率不低于10Hz、位置精度优于250px的定位信息。
2.激光雷达SLAM技术现状
如表1所示，汇总了一部分支持激光雷达的SLAM算法：
表1 激光雷达SLAM算法汇总表
先后在棋骥无人车上测试了其中的Cartographer、hdl_graph_slam、LeGO-LOAM、FAST-LIO2等算法。综合测试结果，融合了固态激光雷达和IMU的SLAM算法在位姿估算精度、鲁棒性、实时性效果上均表现更好。
如图1所示，对比了LeGO-LOAM与FAST-LIO2在测试场地内环形路线的定位结果。假设GNSS的定位结果作为真实值，SLAM定位结果作为估算值，那么两者差值为SLAM估算误差。其中，FAST-LIO2算法总体精度更高、能兼容棋骥不同车型，是棋骥优先采用的算法。
图1 FAST-LIO2与LeGO-LOAM算法定位结果
3.棋骥无人车使用的FAST-LIO2算法
3.1算法简介
FAST-LIO（Fast LiDAR-Inertial Odometry）是一种高效的基于LiDAR和IMU的紧耦合里程计算法。在FAST-LIO之前，已经出现了许多经典的激光SLAM算法，但这些算法存在以下局限性：
激光雷达测量中的特征点通常是环境中的几何结构(例如，边缘和平面)。当无人车在杂乱的环境中工作时，由于没有强有力的特征，基于激光雷达的解决方案很容易发生退化。在激光雷达只具有小视场的时候，这个问题更加明显。
由于沿着扫描方向的高分辨率，激光雷达扫描通常包含许多特征点，将这些大量的特征点融合到IMU测量中依旧需要巨大的计算资源。
由于激光雷达通过多个激光器/接收器进行采样点，扫描中的激光点无法保证在同一时间采样，这会导致出现运动失真，也将显著降低扫描配准。
为了解决以上问题，FAST-LIO采取了以下方法：
为了应对发生退化的快速运动、噪声或杂乱环境，采用紧耦合迭代卡尔曼滤波器来融合激光雷达特征点和IMU测量。
为了降低大量激光雷达特征点带来的巨大计算量，提出了一种新的卡尔曼增益计算公式，并证明了它与传统卡尔曼增益公式的等价性。
提出了一个反向传播过程来补偿激光雷达采样时带来的运动失真。
FAST-LIO2 是FAST-LIO的升级版本，它以更低的计算负载持续实现更高的精度，还进行了关于具有小FOV的固态激光雷达的各种真实实验，能够适应具有不同扫描模式的新兴雷达，增强了其设备兼容性和实用性。
总的来说，FAST-LiO2的建图结果是有效的（例如，高达100 Hz的里程计和大型环境中的建图），强鲁棒性（例如，杂乱的室内环境中具有旋转的杂乱的室内环境的可靠姿态估计，强兼容性（适用于多线和固态LIDARS，UAV和手持平台，英特尔和基于ARM的处理器），同时仍然比现有方法实现更高的准确性。
3.2.算法实验
1）数据采集：确定并记录采集起点，尽量慢速，保证起点和终点重合，需要采集/livox/front 、/livox/imu_192_168_8_177、/qiji/pose数据。算法接收的livox点云是livox_ros_driver2/CustomMsg格式的，需要修改msg_MID360.launch的参数，将“xfer_format”改为1。
2）IMU转换：车端固态激光雷达mid360的发的IMU数据(/livox/imu_192_168_8_177)，不是水平放置的，因此开发一个转换节点，根据雷达-IMU的外参，得到水平坐标下的IMU。
具体的外参如下：
3）SLAM建图：如图2所示，是测试场地SLAM建图结果，保存地图文件到：~/fast_lio2_ws/src/FAST_LIO/PCD/scans.pcd。
图2 测试场地SLAM建图结果
4）SLAM定位：采用hdl_localization实时三维定位算法进行重定位。它利用实时获取的激光雷达数据，通过对FAST-LIO2的建图结果进行特征提取、地图匹配和卡尔曼滤波优化，计算出高精度、实时的定位信息。
4.无人车项目上的SLAM应用
4.1.项目概况
项目名称：天津泰达街无人智能城市管理巡查车。
图3 棋骥35号车在泰达街道执行巡查任务
项目中的组合导航系统惯导状态异常：巡查路线中，在图4路段有约1km路段组合导航系统不能提供有效的定位服务。既使道路两侧以中低层建筑物为主，但大面积玻璃墙面导致GNSS接收到的卫星信号多路径效应明显，组合导航系统无法进入组合惯导状态，平面最大误差甚至超过６米。
图4 组合导航系统惯导状态异常路段
4.2.无人车硬件配置
4.2.1车载激光雷达
表2 棋骥35号车的激光雷达配置
无人车在项目区内平均以7km/h的速度行驶。搭载四个大疆Mid-70固态激光雷达，输出50Hz的IMU数据，10Hz的点云数据，符合激光雷达SLAM算法在低速无人车的应用场景，作为FAST-LIO2算法使用的设备。
4.2.2车载计算单元
部署了主机和从机，都搭载NVIDIA的jetson xavier nx边缘计算平台。其规格参数如下：
AI性能：21 TOPS (INT8)
CPU： 6核NVIDIA Carmel ARMv8.2 64位CPU 6 MB L2缓存 + 4 MB L3缓存
GPU： NVIDIA VoltaTM架构384 NVIDIA CUDA cores和48 Tensor cores
深度学习加速器：2个NVDLA引擎
内存：8 GB 128-bit LPDDR4x，传输速率为51.2 GB/s
因为传感器和多媒体设备的驱动、自动驾驶系统的多个模块都部署在主机中，主机算力资源紧张，所以把SLAM定位模块部署在从机中。
4.3.SLAM定位流程及结果
1)设置起点：在惯导异常路段前100米左右惯导状态正常的位置，设置为起点。车辆到达起点后，读取SLAM定位模块提供的位姿信息。
2)设置终点：在惯导异常路段后100米左右惯导状态正常的位置，设置为终点。车辆到达终点后，切换为读取组合导航系统提供的位姿信息。
3)SLAM定位：车辆行驶在在起点和终点之间时，hdl_localization算法根据实时采集的点云数据，计算车辆的位姿，以10hz频率发布定位数据。
.jpg)
