3、环境感知与地图表示：局部栅格地图、代价地图、障碍物表示、传感器数据融合在局部规划中的作用
局部路径规划的核心挑战在于：无人车必须在动态、不确定的环境中，基于有限且带噪声的传感器观测，实时计算出一条安全、平滑且可执行的轨迹。而这一切的前提，是构建一个能够准确反映车辆周围局部环境状态的“数字镜像”——即局部地图。本章将深入剖析局部栅格地图的构建原理、代价地图的生成机制、障碍物的多层级表示方法，以及多传感器数据融合在局部规划中的关键作用与工程实践。
3.1 局部栅格地图：环境离散化的基础模型
3.1.1 核心原理：从连续世界到离散栅格
局部栅格地图（Local Grid Map）将车辆周围的连续空间划分为均匀的二维网格单元（Cell），每个栅格记录该位置被障碍物占据的概率或状态。这种离散化表示将复杂的几何环境转化为规则的数据结构，使得路径搜索算法（如A*、Dijkstra）和代价评估能够以像素级精度高效运行。栅格地图的核心参数包括：分辨率（决定地图精细度与内存开销的平衡）、地图尺寸（需覆盖车辆动态性能所需的安全距离）、坐标系（通常以车辆后轴中心为原点，x轴指向车头前方）。
栅格状态通常采用三值逻辑或概率值表示：占据（Occupied）、空闲（Free）、未知（Unknown）。概率栅格则进一步引入对数几率（Log-odds）表示法，通过贝叶斯更新公式融合多次观测，有效降低单次传感器噪声的影响。例如，激光雷达击中点对应的栅格占据概率增加，而射线穿过的栅格空闲概率增加。
3.1.2 栅格地图构建流程
- 传感器数据预处理：去除无效点、地面点滤波（针对LiDAR）、运动畸变校正。
- 坐标变换：将传感器坐标系下的观测转换到车体坐标系，再转换到全局/局部地图坐标系。
- 栅格更新：采用射线投射（Ray Casting）算法，对每条激光射线经过的栅格执行空闲更新，对命中点栅格执行占据更新。
- 状态判定：根据更新后的概率值（或计数）判定栅格最终状态，并生成时间戳标记。
3.1.3 案例拆解：激光雷达栅格地图构建
某L4级自动驾驶测试车搭载64线激光雷达，在园区道路进行栅格地图构建。初始参数：分辨率0.1m，地图范围80m×40m，更新频率10Hz。原始点云每帧约12万点，经地面滤波后剩余约3.5万有效点。采用射线投射法，单帧更新耗时约8ms，满足实时性要求。实测发现，在雨天场景下，地面溅水产生的噪点导致部分栅格被误判为占据。解决方案：引入高度阈值过滤（仅保留高于地面0.15m的点）和回波强度校验，误判率从4.2%降至0.7%。
3.2 代价地图：从占据栅格到规划代价
3.2.1 代价地图的层级结构
代价地图（Costmap）在栅格地图基础上叠加多层代价信息，为规划器提供更丰富的决策依据。典型结构包含：静态层（来自高精地图的固定障碍物）、障碍物层（实时传感器检测的动态障碍物）、膨胀层（将障碍物按车辆半径进行膨胀处理）、代价衰减层（距离障碍物越近代价越高）。ROS Navigation框架中的costmap_2d是这一结构的经典实现。
| 代价层级 | 数值范围 | 含义 | 规划器行为 |
|---|---|---|---|
| 致命障碍 | 254 | 车辆中心点不可进入 | 硬约束，路径禁止经过 |
| 膨胀区域 | 1~253 | 车辆边缘可能碰撞 | 软约束，代价随距离增加而减小 |
| 自由空间 | 0 | 完全安全 | 优先选择 |
| 未知区域 | 255 | 传感器未覆盖 | 视策略而定（保守/激进） |
3.2.2 膨胀策略与代价函数设计
膨胀半径（Inflation Radius）通常设置为车辆外接圆半径加上安全余量。例如车辆宽度1.8m，外接圆半径约2.0m，膨胀半径设为2.5m。代价衰减函数常用指数模型：cost = 254 * exp(-α * (d - r_inscribed))，其中d为栅格到障碍物的距离，r_inscribed为车辆内切圆半径，α为衰减系数。衰减系数越大，代价下降越快，路径越贴近障碍物；反之路径更保守。
3.3 障碍物表示：从栅格到语义对象
3.3.1 多层级障碍物表示框架
单一栅格表示难以满足复杂规划需求，现代系统采用三层表示架构：
- 栅格层：提供稠密的环境占据信息，用于碰撞检测和搜索。
- 目标层：通过聚类算法（如DBSCAN、欧式聚类）将相邻占据栅格聚合为障碍物目标，输出位置、尺寸、速度、朝向等属性。
- 语义层：结合视觉识别结果，标注障碍物类型（行人、车辆、自行车、锥桶），并预测其运动意图。
3.3.2 动态障碍物建模
对于动态障碍物，需要建立运动模型。常用方法包括：恒定速度模型（CV）、恒定加速度模型（CA）、交互多模型（IMM）。在局部规划中，通常将动态障碍物表示为带速度矢量的包围盒（Bounding Box），并预测未来2~3秒内的可能位置。预测的不确定性用协方差椭圆表示，规划器需确保轨迹与预测位置保持安全距离。
3.4 传感器数据融合在局部规划中的作用
3.4.1 融合的必要性与挑战
单一传感器存在固有缺陷：LiDAR在雨雪雾天性能衰减严重，摄像头受光照影响大且缺乏深度精度，毫米波雷达角分辨率低但测速准确。多传感器融合通过互补优势，提升环境感知的鲁棒性、完整性和一致性。核心挑战包括：异构数据的时间同步（各传感器帧率不同）、空间对齐（外参标定误差）、以及融合算法的计算开销。
3.4.2 融合策略与实现方法
在局部规划场景中，常用两种融合策略：
- 目标级融合（后融合）：各传感器独立检测目标，再通过匈牙利算法或联合概率数据关联（JPDA）进行目标匹配，输出融合后的目标列表。优点是计算量小、模块解耦；缺点是无法利用原始数据中的细节信息。
- 栅格级融合（前融合）：将各传感器的原始数据统一映射到栅格空间，在栅格层面进行概率融合。例如，LiDAR点云生成占据概率，摄像头语义分割生成类别概率，雷达生成速度概率，最终通过加权平均或贝叶斯公式融合。优点是信息损失小，对小型障碍物检测更鲁棒；缺点是计算量大，需要精确的传感器标定。
3.4.3 案例拆解：城市路口多传感器融合避障
场景背景：某Robotaxi在城市路口左转，车辆前方存在一个正在横穿马路的行人，同时右侧有一辆电瓶车快速接近。传感器配置：1×64线LiDAR、5×摄像头、1×前向毫米波雷达。
初始观测：LiDAR在30m处检测到行人点云（约15个点），但行人被路边灌木部分遮挡；摄像头识别出行人类别，置信度0.85；雷达未检测到行人（反射截面积小）。电瓶车方面：LiDAR检测到目标，但点云稀疏；雷达检测到目标，速度5.2m/s，方位角-15°；摄像头识别为"两轮车"。
融合分析过程：
- 目标级融合：LiDAR行人目标与摄像头目标通过位置关联（距离<1.5m）匹配成功，融合后位置精度提升至±0.15m；电瓶车目标通过雷达+LiDAR融合，速度估计更准确（5.0m/s±0.3m/s）。
- 栅格级融合：将LiDAR占据概率（0.8）与摄像头语义概率（0.7）加权融合，行人栅格占据概率提升至0.9，且膨胀层半径按行人运动速度动态调整（1.0m+0.5m/s×预测时间）。
规划决策：局部规划器在代价地图中看到行人栅格代价为254（致命），电瓶车栅格代价为200（高膨胀）。规划算法生成一条减速并绕行的轨迹，在行人前方2.5m处停车等待，同时与电瓶车保持1.8m横向距离。整个决策耗时约80ms，满足实时性要求。
结果与复盘：车辆安全完成左转。复盘发现，若仅依赖LiDAR，行人被遮挡时检测延迟约0.3s，可能导致制动距离不足；融合后提前0.2s检测到行人，制动距离减少约1.5m。经验：在遮挡场景下，摄像头语义信息可有效补充LiDAR的稀疏点云，但需注意摄像头在逆光时的性能下降，建议融合时动态调整传感器权重。
3.5 常见误区与工程复盘
3.5.1 误区一：栅格分辨率越高越好
高分辨率带来更精细的环境表示，但也带来计算量激增和噪声敏感问题。0.05m分辨率下，一个行人（0.5m×0.5m）仅占10×10个栅格，但传感器抖动可能导致栅格状态频繁翻转。建议根据规划算法需求选择分辨率：A*搜索可容忍0.2m分辨率，而模型预测控制（MPC）需要0.1m以下。
3.5.2 误区二：忽略未知区域的处理
许多事故源于将未知区域当作自由空间。在传感器盲区（如车辆正后方、被遮挡区域），栅格状态为"未知"。保守策略是将未知区域视为高代价（如代价150），规划器尽量避免进入；激进策略则允许低速探索。实际工程中，应根据场景动态调整：高速路段保守，园区低速可适当激进。
3.5.3 误区三：静态膨胀半径
固定膨胀半径无法适应速度变化。高速行驶时，车辆需要更长的制动距离，膨胀半径应随速度增加。推荐公式：r_inflation = r_vehicle + v × t_response + v²/(2×a_max)，其中t_response为系统延迟（通常0.2~0.3s），a_max为最大制动减速度（通常3~4m/s²）。
3.5.4 总结复盘
局部环境感知与地图表示是连接传感器与规划算法的桥梁。一个优秀的局部地图系统应具备：准确性（真实反映环境）、实时性（与规划周期匹配）、鲁棒性（应对传感器噪声和故障）、可扩展性（支持多传感器接入）。在工程实践中，建议采用模块化设计，将栅格更新、代价计算、目标跟踪解耦，便于独立调试和优化。同时，建立完善的离线评测工具链，通过录制的bag包回放，对比不同参数配置下的地图质量和规划成功率，是提升系统性能最有效的手段。