yiliao2026/src/planner/ACKERMANN_HYBRID_ASTAR_PLAN.md

轻量阿克曼 Hybrid A* 路径规划方案说明

1. 方案目标

本方案用于阿克曼底盘小车的轻量路径规划，重点满足以下要求：

• 考虑小车最小转弯半径。
• 支持前进、倒车和转向。
• 支持运动过程中动态添加锥桶障碍物。
• 支持锥桶障碍物累计保留和去重。
• 使用融合后的定位和代价地图，不直接处理雷达、相机、IMU、轮式里程计原始数据。
• 输出仍保持 ROS 标准 /plan nav_msgs/Path，避免引入复杂自定义消息。

当前实现是在 planner 包已有代码基础上升级完成的，核心规划器仍使用原可执行文件名 grid_astar_theta_node，但内部已经从二维 A*/Theta* 改为轻量阿克曼 Hybrid A*/State Lattice。

2. 总体架构

系统由两个主要节点组成：

节点	文件	作用
grid_astar_theta_planner	src/planner/src/grid_astar_theta_node.cpp	轻量 Hybrid A* 路径规划，融合 costmap 和锥桶障碍物，输出 /plan
topology_pure_pursuit	src/planner/scripts/topology_pure_pursuit_node.py	跟踪 /plan，根据路径 yaw 判断前进或倒车，输出 /cmd_vel

典型数据流：

/odom
/goal_pose
/global_costmap/costmap
/random_obstacles
        |
        v
grid_astar_theta_planner
        |
        v
/plan
        |
        v
topology_pure_pursuit
        |
        v
/cmd_vel
        |
        v
ackermann_cmd_bridge.py
        |
        v
/ackermann_cmd

3. 输入与输出

3.1 规划器输入

Topic	类型	说明
/odom	nav_msgs/Odometry	小车当前融合定位，至少需要 x, y, yaw
/goal_pose	geometry_msgs/PoseStamped	目标位姿，必须包含 x, y, yaw
/global_costmap/costmap	nav_msgs/OccupancyGrid	全局代价地图
/random_obstacles	std_msgs/Float32MultiArray	锥桶障碍物，格式 [x, y, radius, ...]
/clear_random_obstacles	std_msgs/Bool	清空已累计锥桶

所有坐标默认工作在 map 坐标系下。如果 /odom 或 /goal_pose 的 frame_id 不是 map，则必须存在 TF 变换。

3.2 规划器输出

Topic	类型	说明
/plan	nav_msgs/Path	规划结果
/ackermann_lattice_status	std_msgs/String	规划状态和失败原因

/plan 的每个 Pose 有特殊语义：

• pose.position.x/y 表示轨迹点位置。
• pose.orientation.yaw 表示小车车身朝向。
• 如果路径点前进方向与车身 yaw 相反，则该段表示倒车。

4. 代价地图处理

规划器订阅 /global_costmap/costmap，类型为 nav_msgs/OccupancyGrid。

默认规则：

costmap 值	解释
>= 50	障碍物
-1	未知区域，默认视为障碍
< 50 且非 -1	可通行

相关参数：

costmap_topic: /global_costmap/costmap
costmap_occupied_threshold: 50
unknown_is_obstacle: true
outside_costmap_is_obstacle: true

当前规划器内部搜索分辨率默认是 0.10m。如果收到 costmap，且 fit_map_to_costmap: true，规划器会用 costmap 的物理边界更新规划范围。

需要注意：

• costmap 的实际像素宽高以运行时 OccupancyGrid.info.width/height 为准。
• 静态地图 my_map.pgm 是 199 x 266 像素，分辨率 0.05m/像素。
• 静态地图 zhihui.pgm 是 109 x 115 像素，分辨率 0.05m/像素。

运行时查看 costmap 信息：

ros2 topic echo --once /global_costmap/costmap --field info

5. 锥桶障碍物处理

锥桶输入 topic：

/random_obstacles

消息类型：

std_msgs/Float32MultiArray

数据格式：

[x, y, radius, x, y, radius, ...]

示例：

ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \
"{data: [1.2, 0.6, 0.15]}"

5.1 增量添加

当前默认开启：

accumulate_obstacles: true

这表示每次发布新锥桶后，规划器不会清空旧锥桶，而是将新锥桶合并到已有锥桶集合中。

例如先发布：

ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \
"{data: [1.2, 0.6, 0.15]}"

再发布：

ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \
"{data: [2.0, 1.1, 0.15]}"

规划器内部会保留两个锥桶。

5.2 去重规则

默认参数：

obstacle_merge_distance: 0.20

如果新锥桶中心与已有锥桶中心距离小于 0.20m，认为是同一个锥桶：

• 更新锥桶中心位置。
• 半径取新旧较大值。

5.3 清空锥桶

如果误检或需要重新开始累计，可以发布：

ros2 topic pub -1 /clear_random_obstacles std_msgs/msg/Bool "{data: true}"

6. 障碍物膨胀

锥桶障碍物不会只按原始半径参与碰撞检测，而是会进行安全膨胀。

膨胀半径计算：

inflated_radius =
  max(obstacle_radius, unknown_obstacle_radius)
  + robot_radius
  + localization_error
  + latency_margin
  + safety_margin

默认参数：

default_obstacle_radius: 0.15
unknown_obstacle_radius: 0.15
robot_radius: 0.14
localization_error: 0.10
latency_margin: 0.05
safety_margin: 0.05

按默认值，一个半径 0.15m 的锥桶最终膨胀半径约为：

0.15 + 0.14 + 0.10 + 0.05 + 0.05 = 0.49m

这个值偏保守，适合定位误差较大或控制延迟明显的情况。如果实际通道较窄，可以降低 localization_error 或 safety_margin。

7. Hybrid A* 搜索模型

规划器状态为：

x, y, yaw, gear

其中：

• x, y 是位置。
• yaw 是车身朝向。
• gear 是挡位方向，包含前进和倒车。

7.1 状态离散

默认参数：

resolution: 0.10
yaw_bins: 72

含义：

• 平面位置按 0.10m 分辨率离散。
• yaw 被离散成 72 份，即每份约 5 度。

7.2 运动原语

每次扩展使用 6 类动作：

gear	steering	含义
前进	直行	向前直行
前进	左转	向前左转
前进	右转	向前右转
倒车	直行	向后直行
倒车	左转	向后左转
倒车	右转	向后右转

默认运动原语长度：

primitive_length: 0.12

碰撞检测采样间隔：

collision_sample_step: 0.03

也就是说每条原语会被分成多个点做碰撞检测，而不是只检测终点。

7.3 最小转弯半径

默认：

min_turning_radius: 0.40

转弯原语按该半径积分生成，规划阶段会保证轨迹曲率不小于这个半径限制。

8. 目标判定

目标输入为 /goal_pose，必须包含 x, y, yaw。

默认到达条件：

goal_xy_tolerance: 0.20
goal_yaw_tolerance: 0.174533

含义：

• 位置误差小于 0.20m。
• 朝向误差小于约 10 度。

如果目标只给 x, y 而 yaw 没有意义，那么最终车头方向可能不符合预期。因此对于阿克曼小车，建议目标始终提供合理 yaw。

9. 代价函数

搜索过程中使用以下代价项：

reverse_penalty: 1.3
turn_penalty: 0.05
gear_change_penalty: 0.20
steering_change_penalty: 0.02

含义：

参数	作用
reverse_penalty	倒车惩罚，越大越不愿意倒车
turn_penalty	转弯惩罚，减少不必要转向
gear_change_penalty	前进/倒车切换惩罚，减少频繁换挡
steering_change_penalty	转向变化惩罚，让路径更稳定

当前默认允许倒车，但不会无理由倒车。只有倒车明显降低路径代价或满足空间约束时，才更容易出现倒车段。

10. 重规划机制

重规划触发条件：

• 收到新的 /goal_pose。
• 收到新的 /global_costmap/costmap。
• 收到新的 /random_obstacles。
• 收到清空锥桶命令。

新目标会立即规划。

障碍物和 costmap 更新会通过定时器限频：

replan_rate: 2.0

表示最多 2Hz 重规划，避免障碍物高频更新导致 CPU 被规划器占满。

单次规划时间预算：

planning_timeout_ms: 100.0

如果超过该时间还没有找到路径，则本次规划失败。

失败策略：

• 如果旧路径仍然没有碰撞，则继续使用旧路径。
• 如果旧路径已被障碍物挡住，则发布空 /plan，跟踪器停车。

11. 跟踪器与倒车执行

跟踪器读取 /plan，并根据路径点 yaw 判断当前段是前进还是倒车。

判断逻辑：

如果 路径段方向 与 车身 yaw 方向相反，则认为是倒车段

默认速度：

linear_speed: 0.30
reverse_speed: 0.12

前进段：

/cmd_vel.linear.x > 0

倒车段：

/cmd_vel.linear.x < 0

实车启动时由 planner/scripts/ackermann_cmd_bridge.py 桥接到 /ackermann_cmd。

12. 启动方式

构建：

colcon build --packages-select planner
source install/setup.bash

启动规划器和跟踪器：

ros2 launch planner grid_astar_theta_with_tracker.launch.py

发布目标：

ros2 topic pub -1 /goal_pose geometry_msgs/msg/PoseStamped \
"{header: {frame_id: map}, pose: {position: {x: 3.0, y: 1.5, z: 0.0}, orientation: {w: 1.0}}}"

发布锥桶：

ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \
"{data: [1.2, 0.6, 0.15]}"

清空锥桶：

ros2 topic pub -1 /clear_random_obstacles std_msgs/msg/Bool "{data: true}"

查看规划输出：

ros2 topic echo /plan

查看状态：

ros2 topic echo /ackermann_lattice_status

13. 当前方案优点

• 轻量，主要逻辑是 C++ 实现。
• 不依赖高频雷达融合或复杂局部规划器。
• 规划阶段考虑阿克曼最小转弯半径。
• 支持倒车和换挡。
• 支持运动过程中动态添加锥桶。
• 支持锥桶累计、去重和清空。
• 仍使用标准 /plan nav_msgs/Path 输出，接口简单。

14. 当前限制

• /plan 本身没有显式 gear 字段，倒车语义依赖 Pose yaw 约定。
• 目前没有单独实现局部地图或局部避障器。
• costmap 的动态障碍处理依赖上游是否正确发布 /global_costmap/costmap。
• Hybrid A* 是轻量版本，不包含复杂解析扩展或高级平滑器。
• 如果地图非常大，100ms 超时可能导致复杂场景找不到路径，需要调整分辨率、搜索范围或超时时间。

15. 推荐调参顺序

建议按以下顺序调参：

1. min_turning_radius
2. robot_radius
3. localization_error
4. safety_margin
5. obstacle_merge_distance
6. primitive_length
7. planning_timeout_ms
8. reverse_penalty
9. lookahead_distance
10. reverse_speed

如果小车过于保守、容易找不到路，优先降低：

localization_error
safety_margin
unknown_is_obstacle

如果小车离障碍物太近，优先增加：

robot_radius
safety_margin
localization_error

如果规划太慢，优先调整：

resolution
planning_timeout_ms
map_min_x/map_max_x/map_min_y/map_max_y

16. 总结

这套方案本质上是一个面向低算力阿克曼小车的轻量 Hybrid A* 规划框架。它不处理底层传感器融合，只消费已经融合好的定位、代价地图和锥桶全局坐标。

相比普通二维 A*，它在规划阶段加入了车身朝向、最小转弯半径、前进/倒车和换挡代价，因此输出路径更符合阿克曼底盘运动约束。

相比完整 Nav2 Hybrid A* 或 MPPI，它更轻量、接口更简单、可控性更强，适合竞赛场景中地图规模较小、障碍物数量有限、需要快速重规划的任务。