4、规划模块调试：全局路径规划调试、局部路径规划调试、行为规划调试

规划模块，说白了就是自动驾驶的「大脑」。它决定了车往哪开、怎么开、遇到情况怎么办。我调试这个模块的时间，占了整个项目周期的大头。你想想看，路径规划一旦出问题，车要么绕远路，要么急刹车，严重的时候直接停在路中间不动了。

今天咱们就聊聊规划模块的三个核心部分：全局路径、局部路径、行为规划。每个部分我都踩过不少坑，咱们一个一个说。

4.1 全局路径规划调试

全局路径规划，就是给车画一条从起点到终点的「大路线」。Autoware 里用的是 lane_planner 或者 lanelet2_global_planner。我个人的习惯是，先看地图数据对不对，再看路径生成逻辑。

4.1.1 地图数据检查

全局路径依赖高精地图。地图数据有问题，路径肯定歪。我遇到过最离谱的一次，是地图里有一条车道线连到了对面车道，结果规划出来的路径直接逆行。

调试步骤：

• 检查 Lanelet2 地图文件：用 lanelet2_visualization 工具加载地图，看看车道、路口、停止线是否完整。
• 验证拓扑连接：确保每条车道的前驱后继关系正确。可以用 lanelet2_routing 的 RoutePlanner 测试。
• 坐标对齐：地图坐标系和车辆定位坐标系必须一致。我习惯用 tf2_echo 检查 map 到 base_link 的变换。

4.1.2 路径生成参数调优

全局路径生成有几个关键参数，调不好就会绕路或者穿墙。

参数名	作用	推荐值	我的经验
lane_change_cost	变道代价	1000-5000	设太低车会频繁变道，设太高又不会变道
minimum_lane_length	最小车道长度	5.0 m	太短会导致路径断断续续
routing_graph_filename	路由图文件	自动生成	记得每次改地图后重新生成

我曾经在一条 2 公里长的直道上，车突然要变道到对面车道。查了半天，原来是 lane_change_cost 设成了 100，变道代价太低，路径规划器觉得变道更「划算」。

4.1.3 调试命令速查

# 可视化全局路径
rosrun lanelet2_visualization lanelet2_visualization_node

# 测试路由规划
rosrun lanelet2_routing route_planner_test

# 查看路径话题
rostopic echo /planning/mission_planning/route

4.2 局部路径规划调试

局部路径规划，负责处理「眼前的事」。全局路径是宏观的，局部路径是微观的。Autoware 里常用 op_planner 或者 astar_planner。我调试局部路径时，最头疼的是「抖动」和「卡死」。

4.2.1 路径抖动问题

车在直道上走，方向盘却左右微调，这就是抖动。原因通常是路径点太密或者平滑度不够。

我建议这样排查：

• 检查路径点间隔：用 rostopic echo /planning/scenario_planning/lane_following/trajectory 看相邻点的距离。如果小于 0.1 米，点太密了。
• 调整平滑参数：path_smoother 里的 weight_smooth 和 weight_lateral。我一般从 0.1 开始调，每次加 0.05。
• 检查车辆模型：局部路径规划器需要知道车辆的最小转弯半径。设错了，路径会强行让车转急弯。

4.2.2 障碍物避让失败

局部路径规划的核心是避障。我遇到过车明明看到障碍物了，却直接撞上去。嗯，这里要注意，问题往往出在「代价地图」上。

调试步骤：

1. 查看代价地图：在 Rviz 里添加 /costmap/costmap 话题，看看障碍物膨胀半径是否合理。
2. 检查障碍物检测：用 rviz 显示 /detection/objects，确认障碍物位置和大小正确。
3. 调整规划器参数：obstacle_distance_threshold 设得太小，车会贴障碍物太近。我习惯设 1.5 米。

说白了，局部路径规划器就是个「优化问题」。它在找一条既安全又平滑的路径。如果找不到，就会卡住不动。

4.2.3 局部路径规划参数速查

参数	说明	调试建议
trajectory_resolution	轨迹点间隔	0.3-0.5 m，太密会抖动
max_steering_angle	最大转向角	根据车辆实际设置，一般 0.6 rad
velocity_max	最大速度	先设 5 m/s 测试，再逐步提高

4.3 行为规划调试

行为规划，就是「决策」。什么时候变道、什么时候停车、什么时候让行。Autoware 里行为规划在 scenario_planner 模块里。我调试行为规划时，最怕遇到「犹豫不决」——车在路口来回切换状态。

4.3.1 状态机调试

行为规划本质是一个状态机。常见状态有：LANE_FOLLOWING、LANE_CHANGE、INTERSECTION、STOP。状态切换出问题，车就会乱来。

我建议用 rqt_graph 看状态机运行情况。也可以用命令行：

# 查看当前行为状态
rostopic echo /planning/scenario_planning/scenario

# 查看状态切换日志
rosrun rqt_console rqt_console

我曾经遇到一个 bug：车在路口明明该停车，却直接冲过去了。查日志发现，状态机从 INTERSECTION 跳到了 LANE_FOLLOWING，原因是路口检测的触发条件设得太宽松。

4.3.2 变道决策调试

变道是行为规划里最复杂的部分。车什么时候变道？变道到哪条车道？我调试变道时，主要看三个东西：

• 变道触发条件：lane_change_available 是否为 true。如果一直 false，检查前方车辆距离。
• 变道安全距离：minimum_lane_change_distance。设太大会错过变道时机，设太小会危险。
• 目标车道选择：行为规划器会评估每条车道的「代价」。代价包括车道速度、前方车辆数量等。

4.3.3 行为规划常见问题

现象	可能原因	解决方法
车在路口反复切换状态	路口检测阈值太敏感	增大 intersection_detection_range
车不变道	变道代价太高	降低 lane_change_cost
车频繁变道	变道触发条件太宽松	增加 lane_change_interval

4.4 规划模块调试流程图

下面这张图，是我自己总结的规划模块调试流程。每次遇到问题，我就按这个顺序排查，基本都能找到根因。

调试规划模块，说白了就是「先全局、再局部、后行为」。按这个顺序来，能省不少时间。我刚开始做的时候，总喜欢直接调行为规划，结果全局路径都是错的，调了半天白费功夫。

嗯，规划模块的调试就聊到这儿。记住，调试不是一蹴而就的事。多跑几遍数据，多看看日志，问题总会找到的。