《智能驾驶之激光雷达算法详解》激光雷达 +IMU组合定位

激光里程计算法，仅凭激光点云估算激光雷达的运动状态，其精度显著受激光点云质量波动影响。IMU（惯性测量单元），作为广泛应用于机器人及汽车领域的传感器，集成了陀螺仪与加速度计，能精确捕捉并输出被测物体的角速度与加速度信息，进而通过积分运算推算出其在一定时间内的姿态与位置变化。然而，IMU在实际运作中易受多种干扰因素影响，尤其是加速度计的误差会随时间累积，导致导航精度下降。因此，常需借助外部信息融合策略，以强化IMU的定位精度。鉴于激光里程计依赖低频的环境感知进行定位，而IMU则通过高频的自身运动状态积分进行位姿估计，两者在功能上存在天然的互补性。众多学者因此致力于将激光里程计与IMU相结合，以实现高精度、实时性的定位。根据融合方式与原理的不同，这一领域的研究被细化为LiDAR+IMU松耦合与紧耦合两大方向。 激光雷达与IMU的松耦合定位策略，巧妙融合了激光里程计与IMU航位推算技术。两者独立运作，依托卡尔曼滤波、粒子滤波等先进框架，实现精准信息融合，最终精确输出定位结果。2019年，国助科技BXue团队创新性地提出IMU-AHFLO算法，该算法凭借点线特征或点云分布特征匹配的激光里程计，捕捉车辆在两帧点云间的位姿变化，随后，高频IMU数据携手车辆运动学方程，预测上述时刻的位姿变迁，最终，卡尔曼滤波器精准估算车辆新姿态。南昌大学的廖杰华则另辟蹊径，将LOAM算法与自适应粒子滤波技术巧妙结合，专为无人物流小车打造室内定位新方案。而Google的Cartographer算法，更是以分层优化为核心，前端运用无迹卡尔曼滤波器实现2D激光雷达与IMU数据的无缝融合，后端则聚焦于子地图构建与优化，辅以分支定界法，显著加速闭环检测，确保定位系统的高效与精准。 激光雷达与IMU的紧耦合定位技术，相较于松耦合方式，显著减少了信息损失。这一创新方法将激光雷达与IMU数据融合于同一位姿优化问题中，实现了更为精准的位姿估计。紧耦合定位策略可细化为基于滤波器与平滑优化两大流派。基于滤波器的方法，在状态更新中无缝整合多源传感器数据，如H. Sebastian等先驱者利用自适应扩展卡尔曼滤波器，成功将3D激光雷达与GPS/INS融合，赋能无人小车室外精准导航。然而，滤波器固有的线性化近似与递推机制，随时间推移易累积误差，影响长期定位精度。为克服此局限，香港科技大学机器人与多感知实验室的C. Qin团队在ICRA 2020上隆重推出LINS算法，该算法采用迭代误差状态卡尔曼滤波器，深度融合激光雷达与IMU数据，通过持续校正系统状态误差，实现了车辆实时、高精度的定位与建图，为紧耦合定位技术树立了新的里程碑。

MU-AHFLO 算法：聚焦于LDAR+IMU融合策略中极具代表性的IMU辅助高频LDAOdmery（简称MULAHFLO）算法，深入其量测与融合机制。相较于紧密耦合方案中复杂的IMU预积分与因子图等理论框架，基于EKF的长时耦合模型展现了其简洁性。为深化理解，本节将细致剖析wXur等人在MULAHFLO算法中的公式推导精髓。核心目标是融合IMU与激光雷达数据，以精准捕捉车辆的实时位姿信息。为此，首要任务是明确求解流程中不可或缺的三个坐标系定义，具体构建如图12-1清晰展示。此步骤为数据融合奠定了坚实的理论基础，确保后续处理能够精准无误地跨越不同坐标空间，实现高精度的位姿估计。 IML-AHFLO算法：聚焦于IML-AHFLO算法，深入剖析了IMU与LiDAR如何通过卡尔曼滤波框架实现松耦合定位的精妙机制。鉴于IMU与LiDAR在车辆位置计算上的原理迥异，其失效模式亦不相同，因此，二者的融合策略巧妙地弥补了各自的不足。具体而言，鉴于IMU/轮速计数据的高频特性与激光里程计的低频特性，IML-AHFLO算法巧妙地利用IMU/轮速计数据结合车辆运动状态方程，对车辆位置进行前瞻预测，并借助激光里程计的输出作为观测依据，最终通过卡尔曼更新流程，精准地估算出车辆的后验状态。然而，此松耦合策略虽原理简明且易于实现，但在高效利用IMU与LiDAR数据方面略显不足，且IMU测量误差的累积效应可能削弱算法精度，此时，IMU与LiDAR的紧耦合定位策略便显得尤为重要。 WO-SAM算法：由麻省理工学院T.Shan等人在2020年匠心独运的杰作。LIO-SAM旨在依托因子图优化框架，实现激光雷达、IMU与GPS的实时、稳定且高精度的融合定位，其开源代码已在GitHub上开放共享。为深入理解该算法，我们需先掌握因子图优化与IMU预积分技术的基础理论，随后再逐步揭开LIO-SAM算法的神秘面纱。 因子图优化：因子图，这一无向概率图的杰出代表，源自Kschischang等人在信道编码领域对Tanner图、Wiberg图等模型的深刻洞察与创新。因子图以其独特的因式分解能力，将复杂系统的全局函数拆解为多个简洁的局部函数乘积，并通过“和-积”算法清晰地勾勒出系统状态变量间的信息传递脉络。在统计推断、译码编码、实时定位等多个领域，因子图均展现出广泛的应用价值。其结构由变量节点与因子节点构成，二者通过无向边紧密相连，共同编织出系统状态的精密网络。在SLAM领域，因子图更是以其独特的优势，助力我们更高效地解析与定位复杂环境。