2. 激光SLAM基础
激光SLAM的量产落地，第一步不是调参，而是把“传感器—数据—坐标—问题—算法”这条链路彻底讲清楚。激光雷达给出的是离散测距点，点云是这些点在某一坐标系下的集合，SLAM则是在未知环境中同时估计机器人位姿与地图的后端问题。只有把四者串起来，后续的鲁棒验证才有明确的物理与数学对象。
一、激光雷达测距原理与点云生成
主流车载与机器人激光雷达以飞行时间法（ToF）为主：发射激光脉冲，测量回波到达时间 Δt，则距离 r = c·Δt/2。机械旋转式雷达通过旋转扫描获得水平角 θ，通过多线束获得垂直角 φ；固态雷达则通过光学相控阵或MEMS微振镜改变光束方向。每个回波点通常携带距离、水平角、垂直角、反射强度，部分雷达还输出时间戳与线号。
点云在雷达自身坐标系下可表示为球坐标到笛卡尔坐标的转换：
x = r · cos(φ) · cos(θ)
y = r · cos(φ) · sin(θ)
z = r · sin(φ)
intensity = 回波强度量产中必须关注三类非理想因素：一是测距噪声与角度分辨率，直接影响点云密度与地图一致性；二是运动畸变，机械旋转一圈约100ms，车辆高速运动时同一帧内各点实际对应不同位姿；三是反射率与天气影响，低反射目标、雨雾、扬尘会造成丢点或虚假点。因此原始点云进入SLAM前通常要做去畸变、地面分割、离群点滤除与降采样。
二、点云表示与常用数据结构
点云本质是三维空间中的无序点集合，但工程实现必须解决“快速找邻居”的问题。常见表示包括：原始点列表、体素栅格、八叉树、KD-Tree、以及用于配准的特征集合（平面、边缘、法向量）。体素栅格把空间划分为固定边长的小立方体，适合降采样与占据栅格建图；KD-Tree适合最近邻搜索，是ICP类算法的核心加速结构；八叉树适合多分辨率地图与压缩存储。
| 表示方式 | 典型用途 | 优点 | 量产注意点 |
|---|---|---|---|
| 原始点列表 | 数据采集、回放 | 无损、简单 | 数据量大，需压缩 |
| 体素栅格 | 降采样、占据地图 | 检索快、内存可控 | 体素边长影响精度 |
| KD-Tree | 最近邻、ICP配准 | 查询效率高 | 动态更新代价高 |
| 八叉树 | 多分辨率地图 | 层次化、可压缩 | 实现复杂度较高 |
| 特征集合 | LOAM类里程计 | 匹配快、鲁棒 | 特征提取需调参 |
三、坐标系变换：SLAM的“空间语法”
激光SLAM中至少涉及四类坐标系：雷达坐标系 L、车体/基座坐标系 B、世界坐标系 W、以及相机或IMU等外参坐标系。点从雷达系到世界系的变换为：
p_W = T_WB · T_BL · p_L
T = [ R t ]
[ 0 1 ]其中 T_BL 是雷达相对车体的外参，标定误差会直接造成地图重影；T_WB 是待估计的机器人位姿。工程上常用齐次变换矩阵、四元数加平移、李群SE(3)表示。需要注意：旋转顺序、单位、时间戳对齐三者任一出错，都会表现为“点云整体偏移”或“转弯时地图撕裂”。
四、SLAM问题定义与经典算法概述
SLAM可形式化为：在控制输入 u1:t 与观测 z1:t 条件下，联合估计机器人轨迹 x1:t 与地图 m，即求后验概率 P(x1:t, m | z1:t, u1:t)。激光SLAM通常拆成前端与后端：前端做帧间配准与位姿初值，后端做位姿图优化或因子图优化，并配合回环检测消除累积漂移。
经典算法可按“前端匹配方式”和“地图表示”两条线理解：
- 滤波类：EKF-SLAM、FastSLAM，早期用于小规模环境，计算量随路标增长。
- 扫描匹配类：ICP、GICP、NDT，直接对齐两帧点云，是激光里程计的基础。
- 特征类：LOAM及其变体，提取边缘与平面特征，分别匹配，实时性好。
- 图优化类：Cartographer、LIO-SAM、LVI-SAM，用位姿图或因子图融合激光、IMU、GPS。
- 紧耦合类：FAST-LIO系列，将IMU预积分与激光残差联合优化，适合高动态场景。
五、典型场景与复盘要点
以园区物流车为例：16线雷达+IMU，车速0-20km/h。前端用GICP做帧间匹配，后端用位姿图优化，回环用Scan Context。若出现“直行正常、转弯漂移”，优先检查时间同步与外参；若出现“长走廊沿轴向漂移”，说明几何退化，需要引入IMU或轮速约束；若出现“回环后地图错层”，检查回环约束的协方差与鲁棒核函数。
复盘时建议固定一张“问题—现象—根因—验证”清单：点云重影对应外参或时间戳；地图膨胀对应动态物体未滤除；轨迹跳变对应优化权重异常；建图断裂对应回环漏检。把每次故障映射回坐标系、点云表示或算法环节，才能形成可复用的量产验证资产。
把上述链路固化为一套可检查的工程流程：先确认雷达数据格式与时间戳，再验证点云表示与降采样策略，然后检查外参标定与坐标变换，最后评估前端匹配、后端优化与回环策略。每一步都对应可量化的指标，例如点云密度、配准残差、轨迹RMSE、回环召回率。只有基础链路正确，后续的鲁棒验证才有意义。