1. 项目缘起当固态激光雷达遇上“退化”场景这几年固态激光雷达Solid-State LiDAR在机器人、自动驾驶领域火得一塌糊涂。和传统的机械旋转式雷达相比它没有活动部件体积小、成本低、可靠性高简直是量产上车的“梦中情达”。但真把它用在SLAM同步定位与建图里尤其是和IMU惯性测量单元搞在一起做紧耦合的激光雷达惯性里程计LIO时一个老生常谈但又被固态特性放大的问题就冒出来了场景退化。什么叫退化简单说就是传感器收到的信息“不够用”了导致系统没法准确估计自己的运动。比如你开车进了一条又长又直、两边光秃秃的隧道或者在一个空旷的大平面比如停车场里转悠。对于依赖几何特征比如点、线、面的激光雷达来说这些环境提供的有效约束太少了运动方向上的不确定性急剧增加。传统机械雷达好歹有个360°的视场角总能扫到点东西而固态雷达特别是像Livox那种非重复扫描模式的或者视场角有限的在退化场景下更容易“抓瞎”。我最近在折腾一个项目核心就是做一个环境自适应的固态激光雷达惯性里程计。目标很明确让系统能自己“感觉”到是不是进入了退化环境然后自动调整策略利用一切能利用的几何信息哪怕很微弱来优化状态估计防止里程计“飘”得没边。这不仅仅是加几个判断条件那么简单它涉及到对传感器观测质量的实时评估、对优化问题中信息矩阵的智能加权以及对不同几何约束点、线、面的深度融合与可靠性判断。2. 系统架构紧耦合LIO与退化感知模块设计我们的系统整体上是一个紧耦合的激光雷达惯性里程计框架。所谓“紧耦合”就是把激光雷达的原始点云观测和IMU的原始角速度、加速度观测在状态估计的层面比如一个非线性优化问题中直接融合而不是各自先算出一个位姿再去做融合。这样做的好处是能充分利用所有传感器的原始信息理论上精度更高对噪声和延迟的处理也更好。整个系统的数据流可以概括为以下几个核心环节2.1 前端预处理与特征提取固态激光雷达的点云模式五花八门比如Livox的“花瓣式”非重复扫描会导致点云在空间分布上随时间变化。我们的预处理第一步是进行运动畸变补偿。由于雷达扫描一帧需要时间这段时间内车本身在动点云就“糊”了。我们利用IMU预积分得到的短时相对运动对当前帧内的每一个点进行去畸变把它们都校正到同一个时刻通常是帧尾时刻的雷达坐标系下。校正后的点云我们会提取几何特征。不同于LOAM系列主要提取角点和平面点我们根据固态雷达点云密度高但分布不规则的特点设计了一套混合特征提取策略曲面点用于拟合局部小平面提供面约束。我们计算每个点周围邻域的协方差矩阵通过特征值分析来判断该区域是否平坦。特征值满足一定条件例如最小特征值远小于另外两个的区域其中心点被标记为曲面点。边缘点用于提供线约束。在长走廊、墙角等场景边缘特征非常稳定。我们寻找那些在一个主方向上分布很长但在另外两个方向上分布很窄的点簇中心。稀疏角点作为点约束的补充。在特征极其匮乏的区域任何突出的、不易拟合为面或线的孤立点也变得珍贵。2.2 IMU预积分与状态预测这部分是紧耦合的基石。IMU在激光雷达帧间高速提供角速度和加速度。我们采用IMU预积分技术将两帧激光雷达之间的所有IMU观测积分成一个相对运动约束。这个约束只依赖于这两帧之间的IMU测量和零偏与全局状态无关大大提高了计算效率和优化稳定性。这个预积分结果为我们提供了一个高频、短时精度较高的状态预测作为激光雷达匹配的初始值。2.3 核心退化感知与几何约束构建这是本项目区别于普通LIO的核心。我们不是简单地把所有提取的特征拿去和地图匹配而是先评估当前帧的“健康度”。退化检测我们设计了一个基于特征分布矩阵的实时评估器。具体来说对于当前帧提取的所有特征点包括点、线、面特征我们计算它们在雷达局部坐标系下的协方差矩阵并对该矩阵进行特征值分解。假设特征值为 λ1 ≥ λ2 ≥ λ3。如果 λ1 λ2 ≈ λ3说明特征主要分布在一个方向上例如长直走廊系统在垂直于该方向的两个维度上可观性差发生平面退化。如果 λ1 ≈ λ2 λ3说明特征分布在一个平面上例如空旷地面系统在法线方向上的可观性差发生线退化只能约束在平面内运动。如果三个特征值都很大且相近说明特征分布良好无退化。如果三个特征值都很小说明特征数量太少或质量太差发生严重退化。我们设定阈值来实时判断退化类型和程度并输出一个退化因子用于后续优化。自适应几何约束构建无/弱退化时采用标准的点到面、点到线距离约束构建残差。此时所有约束的权重可以设为一致或根据匹配质量简单调整。检测到退化时约束选择强化在退化方向上仍有效的约束。例如在长直走廊平面退化中虽然左右和上下的约束变弱但走廊延伸方向的前后约束以及重力方向由IMU提供的约束依然可靠。我们会更多地利用与走廊方向平行的平面特征两侧的墙和边缘特征墙角的线。权重自适应根据计算出的退化因子动态调整优化问题中各类约束残差的信息矩阵或协方差矩阵的逆。在退化方向对应的状态量位姿中的特定平移或旋转分量上降低来自可能不可靠的激光雷达约束的权重同时相对提升IMU约束的权重防止优化器在缺乏信息的方向上“瞎猜”。历史约束利用引入一个短时的滑动窗口将过去几帧中在非退化方向上的强约束也加入到当前帧的优化中为系统提供额外的历史支撑。2.4 基于滑动窗口的联合优化我们维护一个包含最近N个关键帧状态的滑动窗口。每一帧新数据到来我们构建一个包含以下残差项的非线性最小二乘问题IMU预积分残差约束相邻帧间的相对运动与IMU测量一致。激光雷达几何残差包括自适应加权后的点到面、点到线、点到点残差。边缘化先验残差当滑动窗口满时将最老的帧边缘化将其信息转化为一个先验约束加到后续优化中保持问题规模恒定同时保留历史信息。使用Ceres Solver或g2o等优化库来求解这个优化问题得到窗口内所有关键帧的最优状态估计位置、姿态、速度、IMU零偏等。3. 关键技术深潜退化感知与几何约束的工程实现理论听起来不错但工程上怎么实现这个“自适应”和“优化”呢这里分享几个踩过坑的细节。3.1 特征分布矩阵的计算与陷阱计算特征分布矩阵时直接用所有特征点的坐标去算协方差是不行的。因为不同特征点的质量不同匹配不确定性也不同。一个更鲁棒的做法是加权协方差计算。假设我们提取了M个特征点每个点p_i在匹配到地图后会有一个匹配残差r_i和近似的不确定性比如通过匹配点簇的分布或ICP的Hessian矩阵近似得到。我们可以用残差的倒数或不确定性的逆作为权重w_i。加权协方差矩阵C的计算如下mean Σ(w_i * p_i) / Σ(w_i) C Σ( w_i * (p_i - mean) * (p_i - mean)^T ) / Σ(w_i)这样做的好处是匹配质量差、不确定性大的点对整体分布矩阵的贡献小防止这些“坏点”干扰退化检测的判断。我一开始没加权重在特征点较少且噪声大时退化检测模块非常敏感频繁误报。3.2 信息矩阵的自适应调整策略检测到退化后如何调整信息矩阵Ω假设我们通过特征分布矩阵的特征向量分析确定在单位向量v所指示的方向上发生了退化可观性差。对于激光雷达的每一个残差项r_k其原始的信息矩阵假设为标量ω_k或对角阵需要被调制。一个简单有效的策略是进行方向性衰减ω_k_adapted ω_k * (1 - α * (|J_k * v|^2 / ||J_k||^2))这里J_k是该残差对状态量的雅可比矩阵在退化方向上的投影。α是一个与退化因子正相关的衰减系数0到1之间。这个公式的含义是如果一个残差项对退化方向的状态量变化越敏感|J_k * v|越大那么它在本次优化中的权重就被衰减得越多。注意直接粗暴地将某个方向的信息矩阵元素设为零可能导致优化问题奇异。我们采用平滑衰减并始终保证IMU约束在对应方向上保持一个基础权重因为IMU在短时内特别是加速度计对重力方向的感知能提供退化方向上的必要约束。3.3 多种几何约束的融合与可靠性校验在极度退化场景如白色空旷走廊可能连清晰的平面或线都提取不出来。这时候我们启用了点云分布一致性约束作为补充。它不是基于特征的而是基于整个局部点云块的统计特性。具体来说我们将当前帧的一个局部点云块和地图中预测位置附近的点云块进行体素化。然后比较两者在不同体素内的点密度分布构建一个分布相似性残差。这个约束很弱噪声大但在其他几何约束几乎失效时它能提供一个“有总比没有强”的软约束配合强IMU约束能有效抑制发散。同时对于任何匹配上的几何特征我们都增加一个动态一致性校验。利用IMU预积分提供的运动先验我们可以预测特征点在下一次观测中应该出现的位置。如果实际匹配到的位置与预测位置偏差持续过大则该特征点可能是一个动态物体比如移动的行人或错误匹配我们会降低其权重或直接剔除。4. 实验验证从仿真到实车的数据对比光说不练假把式。我们设计了多组实验来验证系统的有效性。实验环境搭建仿真环境使用Gazebo配合Livox雷达模型和IMU模型构建了典型退化场景长直隧道、空旷广场、带有移动障碍物的走廊。仿真环境提供了绝对真值便于定量分析。实车平台一辆搭载了Livox Avia固态激光雷达和Xsens MTi-670工业级IMU的实验车。数据采集场景包括地下停车场、城市峡谷长街道、园区内开阔地带。对比方法基线方法1经典的紧耦合LIO如LIO-SAM但不带退化感知。基线方法2采用固定权重启发式规则处理退化例如在z轴高度变化小时降低高度约束权重。我们的方法环境自适应退化感知与几何约束优化LIO。评价指标绝对轨迹误差与真值仿真或高精度组合导航结果实车作为参考对比的平移误差。相对位姿误差衡量局部一段轨迹内的漂移情况。方向误差特别是偏航角误差在退化场景下容易发散。运行频率在嵌入式计算平台如NVIDIA Jetson AGX Orin上的平均处理频率。结果分析 在长直隧道仿真中基线方法1的轨迹在垂直于隧道方向出现了明显的“蛇形”漂移ATE绝对轨迹误差在100米轨迹末端达到了2.5米。基线方法2有所改善但因为在隧道入口/出口处退化条件变化固定规则不灵活误差仍有1.2米。我们的方法通过实时感知退化并调整约束将ATE控制在0.3米以内。在实车地下停车场测试中地面平坦立柱稀疏基线方法1在绕场一周后出现了严重的闭合误差轨迹无法闭合。我们的方法虽然也有轻微漂移但通过利用立柱的弱边缘约束和地面平面约束的自适应融合成功实现了轨迹闭合闭合误差小于0.5%。性能方面由于增加了特征分布计算和自适应权重模块我们的方法比基线方法1增加了约15%的单帧计算耗时但平均仍能保持在10Hz以上满足实时性要求。关键在于退化检测和权重调整的计算是轻量级的主要开销仍在点云匹配和优化求解上。5. 实战踩坑与参数调优心得在实际部署和调试这套系统的过程中遇到了不少坑这里总结几个关键的坑1退化检测的抖动问题。一开始退化因子在边界场景如从开阔地进入走廊会剧烈跳变导致优化问题权重频繁突变反而引入抖动。解决方案对退化因子进行低通滤波并引入滞后阈值。例如设定进入退化的阈值是0.7退出退化的阈值是0.4。这样只有当环境特征分布持续、明显地变化时系统才会切换模式避免了频繁振荡。坑2IMU零偏在退化场景下的估计发散。在严重退化场景激光雷达约束很弱优化问题对IMU加速度计和陀螺仪零偏的可观性变差。如果还像正常场景一样让零偏自由优化很容易估计错误进而影响IMU预积分形成恶性循环。解决方案当系统检测到严重退化时我们暂时“冻结”IMU零偏的状态量或者在优化中为其添加一个强先验将其信息矩阵增大约束其仅在历史估计值附近做微小变化。待环境恢复丰富后再放开对零偏的优化。坑3几何约束权重衰减过度。在调整信息矩阵时如果衰减系数α设置过大可能导致某些约束在优化中完全失效使得问题在非退化方向上的约束也变弱了。解决方案采用分方向、分残差类型的精细化衰减。不是所有激光雷达残差对退化方向都同样敏感。通过分析雅可比矩阵我们只对那些确实主要影响退化方向状态量的残差项进行显著衰减。对于同时影响多个方向的残差衰减幅度要谨慎。参数调优经验特征分布矩阵的邻域半径用于计算点云局部特征值。半径太小噪声敏感太大则无法反映局部几何特性。建议从雷达分辨率例如0.1m的5-10倍开始尝试根据场景调整。退化因子滤波时间常数需要与机器人的运动速度相匹配。速度越快环境变化越快滤波窗口应越短。通常设置在0.5秒到2秒之间。滑动窗口大小权衡计算量和历史约束强度。对于地面机器人10-15个关键帧通常足够。对于无人机等高速平台可能需要更小的窗口如5-7帧以保证实时性但同时要确保窗口内包含足够多的非退化帧。6. 未来展望与扩展思考目前这个系统主要解决了已知结构化环境中的退化问题。但现实世界更复杂还有一些方向值得深入动态物体与退化混淆一群密集移动的行人可能会被误判为“特征分布混乱”导致的退化。下一步可以集成简单的动态物体检测如基于点云速度或深度学习在退化判断中排除动态区域的影响。多传感器深度融合在极端退化场景如浓雾、大雨激光雷达和相机都可能失效但毫米波雷达可能仍能工作。如何将毫米波雷达的稀疏点云特征也纳入到这个自适应融合框架中是一个挑战。学习式退化感知目前我们的退化检测基于手工设计的几何特征统计。是否可以端到端地训练一个小型网络直接从前端点云或中间特征图预测退化类型和因子这样可能更鲁棒但需要大量带有真值和退化标签的数据。与全局地图的耦合当前系统主要是里程计侧重局部一致性。当发生长时退化导致累积漂移后如何利用偶尔出现的、具有高区分度的场景片段如一个特殊的门廊、一个独特的交叉口进行全局重定位来修正漂移是提升系统实用性的关键。做这个项目的体会是对于SLAM系统尤其是在追求鲁棒性的实际应用中处理“信息不足”的极端情况往往比处理“信息充足”的正常情况更重要。一个能在退化场景下“苟住”不崩溃的里程计才是真正可靠的里程计。我们的这套环境自适应机制本质上就是给系统加装了一个“情境意识”让它知道什么时候该“自信”什么时候该“谨慎”从而在各种复杂环境下都能交出稳定的答卷。
