# 轻量阿克曼 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` |

典型数据流：

```text
/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` | 可通行 |

相关参数：

```yaml
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 信息：

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

## 5. 锥桶障碍物处理

锥桶输入 topic：

```text
/random_obstacles
```

消息类型：

```text
std_msgs/Float32MultiArray
```

数据格式：

```text
[x, y, radius, x, y, radius, ...]
```

示例：

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

### 5.1 增量添加

当前默认开启：

```yaml
accumulate_obstacles: true
```

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

例如先发布：

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

再发布：

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

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

### 5.2 去重规则

默认参数：

```yaml
obstacle_merge_distance: 0.20
```

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

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

### 5.3 清空锥桶

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

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

## 6. 障碍物膨胀

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

膨胀半径计算：

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

默认参数：

```yaml
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` 的锥桶最终膨胀半径约为：

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

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

## 7. Hybrid A* 搜索模型

规划器状态为：

```text
x, y, yaw, gear
```

其中：

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

### 7.1 状态离散

默认参数：

```yaml
resolution: 0.10
yaw_bins: 72
```

含义：

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

### 7.2 运动原语

每次扩展使用 6 类动作：

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

默认运动原语长度：

```yaml
primitive_length: 0.12
```

碰撞检测采样间隔：

```yaml
collision_sample_step: 0.03
```

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

### 7.3 最小转弯半径

默认：

```yaml
min_turning_radius: 0.40
```

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

## 8. 目标判定

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

默认到达条件：

```yaml
goal_xy_tolerance: 0.20
goal_yaw_tolerance: 0.174533
```

含义：

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

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

## 9. 代价函数

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

```yaml
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 更新会通过定时器限频：

```yaml
replan_rate: 2.0
```

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

单次规划时间预算：

```yaml
planning_timeout_ms: 100.0
```

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

失败策略：

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

## 11. 跟踪器与倒车执行

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

判断逻辑：

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

默认速度：

```yaml
linear_speed: 0.30
reverse_speed: 0.12
```

前进段：

```text
/cmd_vel.linear.x > 0
```

倒车段：

```text
/cmd_vel.linear.x < 0
```

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

## 12. 启动方式

构建：

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

启动规划器和跟踪器：

```bash
ros2 launch planner grid_astar_theta_with_tracker.launch.py
```

发布目标：

```bash
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}}}"
```

发布锥桶：

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

清空锥桶：

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

查看规划输出：

```bash
ros2 topic echo /plan
```

查看状态：

```bash
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`

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

```yaml
localization_error
safety_margin
unknown_is_obstacle
```

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

```yaml
robot_radius
safety_margin
localization_error
```

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

```yaml
resolution
planning_timeout_ms
map_min_x/map_max_x/map_min_y/map_max_y
```

## 16. 总结

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

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

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