当前位置：首页 > news >正文

2.2.1 - 3D图搜索算法（以A*为例） - Python运动规划库教程（Python Motion Planning）

news 2026/5/12 19:37:43

前序教程请参照专栏，如您是从0开始阅读，可以直接跳到Python运动规划库教程（Python Motion Planning）-0-简介与安装。

定义起点和终点。本库直接用元组表示坐标点。在栅格地图坐标系里，元组中的数值都是整数。如果是世界坐标系则是浮点数。

start=(25,5,5)goal=(5,25,25)

将起点和终点添加到栅格地图中。这个是为了方便起点和终点的可视化，同时可以清除对应点的障碍物防止规划失败。

map_.type_map[start]=TYPES.START map_.type_map[goal]=TYPES.GOAL

创建路径规划器并规划路径。这里以A*算法为例。规划函数返回地图坐标系路径和详细规划信息（包括成功与否、路径长度、路径代价、扩展结点等）。

planner=AStar(map_=map_,start=start,goal=goal)path,path_info=planner.plan()print(path)print(path_info)

打印规划结果：

[(25, 5, 5), (24, 6, 6), (23, 7, 7), (22, 7, 8), (21, 7, 9), (20, 7, 10), (19, 7, 10), (18, 7, 11), (17, 7, 12), (16, 8, 13), (15, 9, 14), (14, 9, 15), (13, 10, 16), (12, 11, 17), (11, 12, 18), (10, 12, 18), (9, 13, 19), (9, 14, 19), (8, 15, 20), (7, 16, 21), (6, 17, 22), (5, 18, 23), (5, 19, 23), (5, 20, 23), (5, 21, 23), (5, 22, 23), (5, 23, 24), (5, 24, 25), (5, 25, 25)] {'success': True, 'start': (25, 5, 5), 'goal': (5, 25, 25), 'length': 40.09831818981128, 'cost': 40.09831818981128, 'expand': {(25, 5, 5): Node((25, 5, 5), None, 0, 34.64101615137755), ...}}

可视化。

map_.fill_expands(path_info["expand"])vis=Visualizer3D()vis.plot_grid_map(map_)vis.plot_path(path)vis.show()vis.close()

完整的可运行代码:

importrandom random.seed(0)importnumpyasnp np.random.seed(0)frompython_motion_planning.commonimport*frompython_motion_planning.path_plannerimport*frompython_motion_planning.controllerimport*map_=Grid(bounds=[[0,31],[0,31],[0,31]],resolution=1.0)foriinrange(75):rd_p=tuple(np.random.randint(0,30,size=3))map_.type_map[rd_p[0],rd_p[1],:rd_p[2]]=TYPES.OBSTACLE map_.inflate_obstacles(radius=3)start=(25,5,5)goal=(5,25,25)map_.type_map[start]=TYPES.START map_.type_map[goal]=TYPES.GOAL planner=AStar(map_=map_,start=start,goal=goal)path,path_info=planner.plan()print(path)print(path_info)map_.fill_expands(path_info["expand"])vis=Visualizer3D()vis.plot_grid_map(map_)vis.plot_path(path)vis.show()vis.close()