4、分层规划思想：全局规划+局部规划，如何让机器人边思考边走路

各位做机器人的朋友，不知道你有没有遇到过这种情况——机器人刚启动，卡在那里半天不动，像是在“思考人生”。

嗯，我早期做AGV项目时就踩过这个坑。那时候我让机器人一次性规划从A点到B点的完整路径，结果地图稍微大一点，计算时间直接飙到十几秒。更尴尬的是，路径还没算完，周围障碍物已经变了。

说白了，这就是“规划延迟”问题。你想想看，机器人如果每次都要把未来十公里的路全想好才肯迈腿，那它永远也跑不起来。

那怎么办？

答案就是——分层规划。让机器人“边思考边走路”。

4.1 为什么需要分层？

先讲个我自己的经历。有一次做仓储机器人，地图大概2000平米，栅格分辨率5cm。如果用A*一次性规划全局路径，算一次要8秒左右。但机器人移动速度是1m/s，8秒它都跑出去8米了。这8米内如果出现新障碍物，刚才算的路径就废了。

所以，全局规划负责“大方向”，局部规划负责“脚下路”。两者分工明确，互不干扰。

这样做的好处很明显——全局规划可以慢慢算，比如1秒算一次就够了。局部规划必须快，10Hz甚至50Hz。机器人一边沿着全局路径走，一边用局部规划躲开突然出现的箱子、行人。

4.2 全局规划：定方向，不求精细

我个人习惯，全局规划用A*或者Dijkstra。但有个关键点——不要用高分辨率地图做全局规划。

为什么？

你想想看，5cm分辨率的地图，2000平米就是80万个栅格。A*在这上面跑，复杂度是O(n log n)，n越大越慢。我建议把地图降采样到20cm或50cm分辨率，这样栅格数量减少到原来的1/16甚至1/100。全局路径的精度虽然降低了，但方向是对的就行。

另外，全局规划的输出不是一串密集的坐标点，而是一系列关键路点。比如从A到B，中间经过C、D、E三个拐点。局部规划只需要知道“下一个目标点是C”，然后朝着C走就行了。

4.3 局部规划：实时避障，高频响应

局部规划这块，我踩过的坑最多。早期我用DWA（动态窗口法），效果还行，但有个问题——容易陷入局部最优。比如遇到一个U形障碍物，DWA会在里面转圈出不来。

后来我改用TEB（时间弹性带）算法，配合全局路径的指引，效果好了很多。TEB的核心思路是：在全局路径附近，优化出一条平滑、安全、符合运动学约束的局部路径。

局部规划的更新频率，我建议至少10Hz。如果传感器是激光雷达，可以跑到20-30Hz。代码结构大概是这样的：

// 伪代码：局部规划主循环
while(robot_running) {
    // 1. 获取当前位姿和传感器数据
    Pose current_pose = getRobotPose();
    LaserScan scan = getLaserScan();
    
    // 2. 从全局路径中提取局部目标点
    //    比如取全局路径上距离机器人2米处的点
    Pose local_goal = extractLocalGoal(global_path, current_pose, 2.0);
    
    // 3. 运行局部规划算法（TEB/DWA）
    Trajectory best_traj = localPlanner(current_pose, local_goal, scan);
    
    // 4. 发送速度指令
    sendVelocity(best_traj.v, best_traj.w);
    
    // 5. 等待下一个周期（比如50ms）
    sleep(50ms);
}

这里有个细节要注意——局部目标点不能太远也不能太近。太远，局部规划容易偏离全局路径；太近，机器人走两步就要重新选目标，效率低。我一般取2-3米，具体看机器人速度和环境复杂度。

4.4 全局与局部的衔接：平滑切换

分层规划最怕什么？怕全局和局部“打架”。

我曾经遇到一个情况：全局路径规划了一条靠左走的路线，但局部规划因为右侧有障碍物，硬是把机器人往左推。结果全局路径在右边，局部路径在左边，两个方向不一致，机器人开始“蛇形走位”。

解决办法是——给局部规划加一个“全局路径吸引力”。在TEB的代价函数里，除了避障代价、平滑代价，再加一个“偏离全局路径”的惩罚项。偏离越远，代价越大。这样局部规划既会避障，又不会跑偏。

4.5 实战中的避坑指南

讲几个我踩过的坑，希望对你有帮助：

• 全局路径更新太频繁：我曾经让全局规划每100ms更新一次，结果CPU直接跑满。后来改成1秒更新一次，或者只在机器人到达关键路点时更新，问题解决。
• 局部规划目标点跳跃：如果局部目标点每次选的都是全局路径上距离机器人最近的点，那机器人一移动，目标点也跟着动，导致机器人“追着自己的影子跑”。我的做法是：目标点一旦选定，就固定下来，直到机器人到达该点附近（比如0.5米内），再选下一个。
• 地图变化时全局路径失效：比如有人搬走了箱子，全局路径穿过了原本有障碍物的地方。这时候需要触发全局重规划。我一般用“路径碰撞检测”来判断——如果全局路径上有新增障碍物，就重新规划。

4.6 性能对比：分层 vs 不分层

拿我之前做的一个项目数据来说：

指标	不分层（纯A*）	分层（全局A* + 局部TEB）
单次规划耗时	8.2秒	全局0.3秒 + 局部0.02秒
响应延迟（遇到障碍物）	8.2秒（需重规划）	0.02秒（局部避障）
CPU占用率	85%	35%
路径平滑度	一般（栅格路径有锯齿）	好（TEB优化后平滑）

你看，分层之后，响应延迟从8秒降到了0.02秒。这意味着机器人可以“边思考边走路”，而不是“先思考再走路”。

4.7 小结

分层规划说白了就是——大事慢想，小事快做。全局规划负责战略方向，局部规划负责战术执行。两者配合好了，机器人才能既走得远，又走得稳。

我个人觉得，这是路径规划里性价比最高的优化手段之一。不需要改算法本身，只需要改架构思路，就能带来几倍甚至几十倍的性能提升。

下一章我会讲如何用“代价地图”来进一步优化分层规划的效果。到时候会结合ROS的costmap_2d库，手把手教你搭建一个可用的分层规划系统。