8. 障碍感知与代价地图

本文描述 Autonomy 当前实际承担障碍感知职能的 map/costmap_2d 模块：ObstacleLayer、VoxelLayer 与膨胀层。

Perception 模块完整实现后，障碍几何可经 Perception 插件输出并写入同一 costmap 管线。当前导航栈依赖本章所述路径。

8.1 架构位置

传感器观测
    │
    ▼
ObservationBuffer（时间缓冲）
    │
    ▼
ObstacleLayer / VoxelLayer（标记/清除）
    │
    ▼
InflationLayer（安全膨胀）
    │
    ▼
Costmap2DWrapper → Planning / Control

源码：autonomy/map/costmap_2d/layers/。

8.2 ObstacleLayer

8.2.1 职责

功能	说明
Marking	将障碍点投影到 costmap 栅格，设为 `LETHAL_OBSTACLE`
Clearing	沿射线追踪，将自由空间栅格设为 `FREE_SPACE`
高度过滤	忽略 `z < min_obstacle_height` 或 `z > max_obstacle_height` 的点

8.2.2 激光处理流程

接收 LaserScan，遍历每个有效 range \(r_i\)
TF 变换：传感器系 → global_frame
计算终点 \(p_{end} = o + r_i \cdot \hat{d}_i\)
清除：从 \(o\) 到 \(p_{end}\) 沿射线标记自由
标记：\(p_{end}\) 处标记障碍

8.2.3 配置示例

obstacle_layer = {
    enabled = true,
    observation_sources = "scan",
    scan = {
        topic = "/scan",
        sensor_frame = "laser_link",
        data_type = "LaserScan",
        marking = true,
        clearing = true,
        inf_is_valid = false,
        obstacle_range = 2.5,
        raytrace_range = 3.0,
        max_obstacle_height = 2.0,
        min_obstacle_height = 0.0,
    },
}

8.3 VoxelLayer

3D 点云障碍处理，适用于深度相机或 3D 激光：

步骤	说明
体素化	将点云划分为 3D 体素网格
高度投影	将 3D 障碍投影到 2D costmap
清除/标记	同 ObstacleLayer 逻辑

源码：voxel_layer.hpp / voxel_layer.cpp。

8.4 膨胀层（InflationLayer）

8.4.1 目的

在障碍周围生成代价梯度，使规划器保持安全距离。

8.4.2 数学

对障碍栅格 \(p\)，膨胀半径 \(r_{infl}\) 内栅格 \(q\) 的代价：

\[ \mathrm{cost}(q) = \max_{p \in \mathcal{O}} f(\|q - p\|) \]

常用分段函数：

\[\begin{split} f(d) = \begin{cases} \mathrm{LETHAL} & d \leq r_{robot} \\ \mathrm{INSCRIBED} \cdot e^{-\lambda(d - r_{robot})} & r_{robot} < d \leq r_{infl} \\ 0 & d > r_{infl} \end{cases} \end{split}\]

详见 03_math.md §3.6。

8.4.3 配置

inflation_layer = {
    enabled = true,
    inflation_radius = 0.55,       -- 膨胀半径 (m)
    cost_scaling_factor = 10.0,    -- 衰减系数 λ
}

8.5 代价地图代价值

值	宏	含义
0	`FREE_SPACE`	自由
1–252	梯度代价	距障碍越近代价越高
253	`INSCRIBED_INFLATED_OBSTACLE`	内切圆膨胀边界
254	`LETHAL_OBSTACLE`	致命障碍
255	`NO_INFORMATION`	未知

8.6 ObservationBuffer

class ObservationBuffer {
    void bufferObservation(const Observation& obs);
    void getObservations(std::vector<Observation>& observations);
    void resetLastUpdated();
};

参数	说明
`observation_keep_time`	观测保留时间窗口
`expected_update_rate`	预期传感器更新频率

8.7 与 Planning / Control 的接口

// Costmap2DWrapper 提供
bool getCost(x, y);           // 查询单栅格代价
bool isCollisionFree(pose);   // 碰撞检测
void updateMap();             // 触发图层更新

Planning 全局规划器与 Control 局部 costmap 均通过 Costmap2DWrapper 访问障碍信息。

8.8 与 Perception 的融合（规划）

完整 Perception 实现后，障碍融合策略：

策略	说明
叠加	Perception 障碍与激光障碍取并集（max cost）
优先级	视觉检测优先于激光（语义障碍）
独立层	新增 `SemanticLayer` 插件，与 ObstacleLayer 并列

-- 规划
plugins = { "static_layer", "obstacle_layer", "semantic_layer", "inflation_layer" }

8.9 调试建议

工具	用途
RViz costmap 显示	可视化障碍/膨胀区域
`autonomy_planning_test`	离线验证路径与障碍关系
日志 `obstacle_range`	确认激光数据是否到达

现象	处理
costmap 全未知	检查 static_layer 是否加载地图
障碍不更新	确认 `/scan` topic 与 `sensor_frame` TF
虚假障碍	调整 `min_obstacle_height`、过滤地面点