基于里程计反馈与 A* 算法的差速底盘静态全局路径规划

本页对应 src/ground/diff_drive_astar_odom_nav。在 Gazebo 中搭建两轮差速底盘与静态障碍场景：A* 在静态占用栅格上规划全局路径，纯追踪控制器仅用轮式里程计 /odom 反馈跟踪，Gazebo 真值 /ground_truth 只用于事后评估。

与仓库已有的 ros2_pure_pursuit 相比，本模块不重复纯追踪控制本身，而是补上静态全局规划与 Gazebo 差速底盘闭环，并给出真实运行数据：

1. 本模块实际实现范围

2. 运行环境

3. 场景与地图

geometry.py 是唯一几何来源，同一份矩形列表同时生成：

• worlds/static_obstacles.world：12 m × 12 m 房间、中间隔墙（1.2 m 门洞）与 4 个箱子；
• maps/static_map.pgm：240 × 240、分辨率 0.05 m 的占用栅格。

起点 (-4.5, -4.5)，目标 (4.5, 4.5)，二者被隔墙分开，A* 必须从门洞绕行。

4. 运行步骤

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

cd src/ground/diff_drive_astar_odom_nav
python3 main.py                 # 无界面，完整实验 + 自动出图
python3 main.py --gui           # 有显示器时查看 Gazebo 画面

标准 launch 方式：

ros2 launch diff_drive_astar_odom_nav main.launch.py

修改场景后重新生成地图与世界（否则地图和仿真会不一致）：

python3 -m diff_drive_astar_odom_nav.geometry .

5. 原理

5.1 A* 全局规划

• 8 邻接，直行代价 1、对角 √2，octile 启发式，weight=1 时最优；
• 严格穿角检查，避免路径擦过障碍角；
• 机器人半径 0.22 m + 0.05 m 余量，对障碍做可分离膨胀（与膨胀半径无关的 O(HW) 实现）；
• 起点取 /odom 当前位姿；目标落在障碍内时用 BFS 修复到最近空闲格；
• 静态全局规划，只规划一次，不随位姿反复重规划。

5.2 里程计反馈纯追踪

每个控制周期（30 Hz）：

1. 在路径上找距里程计位姿最近的点，索引单调前进；
2. 沿路径前视 0.55 m 得到目标点，$w = v \cdot 2y_{local}/L^2$；
3. 按前视方位角与剩余距离减速，线速度限幅 0.75 m/s，角速度限幅 1.4 rad/s；
4. 距终点 0.12 m 内停车。

5.3 指标

6. 真实运行结果

一次完整运行（results/run_01，25.05 s，502 个 20 Hz 采样）：

6.1 Gazebo 实际运行画面

上图是在 Gazebo Classic 11 中加载 worlds/static_obstacles.world、生成 urdf/diff_robot.urdf 后的真实运行画面（含 3D 视口与底部实时状态栏），不是手工绘制的示意图。

6.2 轨迹与误差

7. 测试与复现

python3 -m pytest -q test        # 17 项：A*、纯追踪、地图/场景一致性
python3 main.py --out results/run_01

一次运行自动产出 trajectory.csv、plan.csv、metrics.json、metrics.md、trajectory.png、error.png、speed.png、trajectory.gif。仓库内 results/run_01 保留 CSV 与指标文件，trajectory.png、error.png、trajectory.gif 只保存在 docs/img/，避免同一二进制重复入库；results/run_01 中数据为真实运行结果，未手工修改。

8. 大模型使用声明

本模块代码与文档在开发过程中使用了 AI 编程助手辅助生成；作者已逐行核对并在 Gazebo 中实际运行验证，提交的实验数据与图表均由真实运行产生。