PythonRobotics 如何用时空 A* 在动态障碍物环境中规划时间最优路径-育师

PythonRobotics 如何用时空 A* 在动态障碍物环境中规划时间最优路径

【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics

在带动态障碍物的栅格环境中做路径规划时，普通 A* 的代价是格子数，无法保证路径在时间上最优。PythonRobotics 的TimeBasedPathPlanning模块提供了一套"时空 A*"（Space-time A*）示例代码：代价改为到达节点所用的时间步数，从而在避开移动障碍物的前提下规划出时间最优路径。本文说明如何安装依赖、运行官方示例、用单元测试验证结果，以及如何调整网格和障碍物参数。

准备环境

按仓库文档的要求，示例代码建议使用Python 3.12.x（其他版本可能可用，但官方只在该版本上做过测试）。先克隆仓库并进入根目录，然后在根目录安装依赖：

pip install -r requirements/requirements.txt

requirements/requirements.txt 中固定了numpy == 2.3.5、matplotlib == 3.11.0等版本，时空 A* 的示例和测试依赖其中的 numpy、matplotlib 与 pytest。

算法工作方式

运行前先了解几个关键点，便于看懂输出：

与标准 A* 不同，时空 A* 的代价g(n)是到达该节点所用的时间步数，启发函数是到目标的曼哈顿距离（abs(dx) + abs(dy)）。在"每个时间步可移动 1 格、也可以原地停留"的假设下，时间启发与距离启发等价，最终路径在到达目标所需时间上最优。
环境由 GridWithDynamicObstacles.py 中的Grid类建模：内部维护一个 x、y、time 三维的 reservation matrix（预订矩阵），障碍物在创建时把完整运动轨迹写入该矩阵。
后继节点有 5 种：原地停留和上下左右移动。一个新节点只有在接下来 2 个时间步（1 步进入、1 步离开）都合法才会被生成，这保证机器人在任何时刻都能离开当前格子。
SpaceTimeAStar.py 中额外引入了 expanded set：已扩展过的节点不再重复扩展，文档给出的示例数据显示，同一场景下节点扩展次数从 204490 次降到 2348 次，规划耗时从 1.72 秒降到约 0.016 秒（文档示例结果）。
BaseClasses.py 里对random和numpy.random统一设置了RANDOM_SEED = 50，随机障碍物的布局在多次运行间可复现。

运行官方示例

示例入口是 SpaceTimeAStar.py，其main()的配置为：起点(1, 5)、终点(19, 19)、21×21 网格、40 个障碍物，障碍物排布取ARRANGEMENT1（在网格中央沿 y 排成一行、沿 x 左右往返移动）。按仓库文档"进入目录后执行脚本"的方式运行：

cd PathPlanning/TimeBasedPathPlanning python SpaceTimeAStar.py

脚本中的两个模块级变量控制运行行为：

show_animation = True（默认）：规划完成后用PlotNodePath绘制机器人与障碍物随时间移动的动画；
verbose = False（默认）：传给SpaceTimeAStar.plan()，置为True时会逐个打印被扩展的节点。

运行结束会输出形如Planning took: x.xxxxx seconds的规划耗时（具体数值取决于你的机器）。

验证规划结果

仓库提供了对应的单元测试 tests/test_space_time_astar.py。该测试使用ARRANGEMENT1排布、起点(1, 11)、终点(19, 19)的 21×21 网格，关闭动画后执行规划，并断言三点：

路径包含 31 个节点；
路径最后一个节点的位置等于终点；
path.expanded_node_count < 1000。

在仓库根目录运行：

pytest tests/test_space_time_astar.py

三条断言全部通过即说明该场景下时空 A* 的规划结果符合预期。NodePath对象（定义见 Node.py）还提供goal_reached_time()（到达终点的时刻）和positions_at_time（每个时间步的位置映射），可直接用于检查结果。

自定义网格与障碍物

在仓库根目录下编写脚本即可复用Grid和SpaceTimeAStar（以下代码取自示例main()与测试文件的实际用法）：

from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( Grid, ObstacleArrangement, Position, ) from PathPlanning.TimeBasedPathPlanning.SpaceTimeAStar import SpaceTimeAStar import numpy as np start = Position(1, 5) goal = Position(19, 19) grid = Grid( np.array([21, 21]), num_obstacles=40, obstacle_avoid_points=[start, goal], obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, ) path = SpaceTimeAStar.plan(grid, start, goal, verbose=True) print(path.goal_reached_time(), path.expanded_node_count)

Grid构造函数的关键参数（默认值见源码）：

grid_size：np.array([x 方向格数, y 方向格数])；
num_obstacles（默认 40）：障碍物数量。若大于网格总格数，构造时抛出Number of obstacles is greater than grid size!异常；
obstacle_avoid_points：障碍物永远不会占据这些点，官方示例用它避开起点和终点，避免出现无解场景；
obstacle_arrangement（默认RANDOM）：RANDOM为随机位置、随机移动；ARRANGEMENT1为中央一列障碍物左右往返；NARROW_CORRIDOR为静态障碍（跳过中间一行）；
time_limit（默认 100）：仿真的总时间步数。规划时，时间满足time + 1 >= time_limit的节点会被跳过，因此该值过小时可能找不到路径。

如果开集的节点全部耗尽仍到达不了终点，plan()会抛出No path found异常——这是文档给出的明确失败信号，此时可调大time_limit、减少num_obstacles或更换障碍物排布。

文档给出的替代方案：Safe Interval Path Planning

time_based_grid_search 文档对比了同场景下的 SIPP（Safe Interval Path Planning，实现见 SafeInterval.py）：它预先计算每个格子的空闲时间区间来减少后继节点生成，文档示例数据显示 Arrangement 1 从(1, 18)出发时，SIPP 用 322 次扩展、0.00730 秒，而时空 A* 用 2717154 次扩展、20.51330 秒。如果你的场景扩展次数很高，可以按同一套Grid接口换用它作为可选分支。

相关文档：docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst 是该模块的总说明，tests/test_safe_interval_path_planner.py 与 tests/test_space_time_astar.py 分别是两个规划器的验证入口。

【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics

创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考