4. 环境感知与障碍物表示
做机器人控制这些年，我越来越觉得一个道理：路径规划做得再好，感知不准也是白搭。你想想看，连路都看不清，怎么走？
这一节，咱们就聊聊环境感知和障碍物表示。说白了，就是让机器人知道「周围有什么、哪里能走、哪里不能走」。我个人习惯把这个问题拆成三个层次：原始数据层（点云）、结构化表示层（栅格地图）、决策支持层（代价地图）。
核心逻辑：传感器数据 → 障碍物点云 → 占据栅格地图 → 代价地图 → 路径规划
4.1 障碍物点云：最原始的感知数据
点云是什么？说白了就是一堆三维坐标点。激光雷达扫一圈，每个反射点都记录下 (x, y, z) 坐标，这就是点云。
我记得刚入行那会儿，第一次看到激光雷达的点云数据，密密麻麻的，感觉挺震撼。但后来发现，点云虽然信息丰富，但直接用它做路径规划，计算量太大了。你想想看，一帧数据可能几万个点，每个点都要判断是不是障碍物，CPU 扛不住。
所以，点云通常只作为中间数据。我们会做几件事：
- 滤波：去掉离群点、地面点。我习惯用 PCL 库的
StatisticalOutlierRemoval，效果不错。 - 降采样：用体素滤波器（VoxelGrid）把点云稀疏化。比如 5cm 的体素，一个格子只保留一个点。
- 聚类：把属于同一个物体的点聚成一堆，方便后续处理。
我的经验：点云处理不要贪多。很多时候，降采样到 10cm 分辨率就够用了。算力留给后面的路径规划，性价比更高。
4.2 占据栅格地图：把世界变成格子
点云处理完了，怎么让机器人理解？我的做法是：把环境划分成一个个小格子，每个格子标记为「占据」「空闲」或「未知」。这就是占据栅格地图（Occupancy Grid Map）。
每个格子用一个概率值表示被占据的可能性。比如：
| 栅格状态 | 概率值 | 含义 |
|---|---|---|
| 空闲 | 0 ~ 0.3 | 这里能走 |
| 占据 | 0.7 ~ 1.0 | 这里有障碍物 |
| 未知 | 0.3 ~ 0.7 | 还没探测到 |
更新栅格地图时，我常用的是贝叶斯更新。每次激光雷达扫到某个格子，就更新它的概率。嗯，这里要注意：不要直接用 0 或 1 赋值，要用概率。为什么？因为传感器有噪声，一次测量可能不准。用概率可以慢慢收敛到真实值。
我曾经踩过的坑：有一次在仓库里做测试，地面有些反光的金属板，激光雷达打上去老是误报。后来我加了「一致性检查」——如果同一个格子连续 3 帧都被检测为占据，才真正标记为障碍物。误报率直接降了 80%。
4.3 代价地图：给路径规划铺路
占据栅格地图告诉机器人「哪里是障碍物」，但路径规划还需要知道「哪里代价高、哪里代价低」。这就是代价地图（Costmap）的作用。
代价地图本质上是在占据栅格地图的基础上，对每个格子赋予一个代价值。代价值越高，路径规划算法越倾向于避开它。
常见的代价赋值策略：
- 障碍物膨胀：把障碍物周围的格子也标上较高的代价。机器人是有半径的，不能贴着墙走。我一般膨胀半径设为机器人半径 + 安全余量（比如 0.2m）。
- 代价衰减：离障碍物越近，代价越高；越远，代价越低。可以用指数衰减或线性衰减。
- 分层代价：有些区域虽然能走，但我不希望机器人走（比如靠近悬崖、湿滑路面）。可以单独加一层代价。
在 ROS 的 costmap_2d 包里，代价地图分为 全局代价地图 和 局部代价地图。全局的用于规划长距离路径，局部的用于实时避障。我个人习惯把全局地图分辨率设低一点（比如 0.1m），局部地图分辨率高一点（比如 0.05m）。
关键点：代价地图不是静态的。机器人移动过程中，新探测到的障碍物要实时更新到代价地图里。这就是「动态避障」的基础。
4.4 三者的关系与实战建议
总结一下这三者的关系：
- 点云：原始数据，信息最丰富，但计算量大。
- 占据栅格地图：结构化表示，适合存储和更新。
- 代价地图：面向决策的表示，直接喂给路径规划算法。
实际项目中，我一般这样搭流程：
// 伪代码示例
sensor_data = get_lidar_scan() // 获取激光雷达数据
pointcloud = to_pointcloud(sensor_data) // 转成点云
filtered_cloud = remove_ground(pointcloud) // 滤除地面
grid_map = update_occupancy_grid(filtered_cloud) // 更新占据栅格
costmap = compute_costmap(grid_map, robot_radius=0.3) // 生成代价地图
path = plan_path(costmap, start, goal) // 路径规划我的建议：刚开始做的时候，别急着调参数。先把整个链路跑通，看看点云能不能正确投影到栅格地图上。很多时候，问题出在坐标变换上——激光雷达坐标系和机器人坐标系没对齐，地图就全歪了。
好了，这一节就聊到这儿。环境感知是路径控制的「眼睛」，眼睛不好使，后面再好的算法也白搭。下一节咱们会深入讲路径规划算法，到时候你会看到，代价地图的质量直接决定了路径的好坏。
可可精品资料库