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

固定随机种子，以确保结果可被复现。

import random
random.seed(0)

import numpy as np
np.random.seed(0)

导入库。

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *

定义栅格地图并添加和膨胀障碍物。

栅格地图是路径规划领域最常用的一种地图形式。世界坐标系是描述机器人与环境在真实世界中的全局坐标系，而栅格地图坐标系是用于栅格地图表示的局部或离散坐标系，通常与地图分辨率和索引相关。

bounds参数定义离散栅格地图在世界坐标系的范围。随后会结合分辨率参数自动计算出栅格地图的大小。Grid类的分辨率默认是1，因此计算出来的离散栅格地图尺寸是31*31*31格。

创建栅格地图后，我们为其添加一些障碍物用于测试路径规划器。并且膨胀障碍物，以防止路径规划器规划的路径过于靠近障碍物。这里膨胀半径设置为3格，参数可调整。

map_ = Grid(bounds=[[0, 31], [0, 31], [0, 31]], resolution=1.0)
for i in range(75):     # 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)

可视化检查3D网格地图。

vis = Visualizer3D()
vis.plot_grid_map(map_)
vis.show()
vis.close()

完整的可运行代码：

import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *

map_ = Grid(bounds=[[0, 31], [0, 31], [0, 31]], resolution=1.0)
for i in range(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)

vis = Visualizer3D()
vis.plot_grid_map(map_)
vis.show()
vis.close()

3D可视化基于PyVista。若想保存图形，需将参数off_screen设置为True，并注释掉vis.show()。以下是一个保存图形的示例：

vis = Visualizer3D(show_axes=False, off_screen=True)
vis.plot_grid_map(map_)
# vis.show()
vis.savefig("../assets/grid_map_3d.png")
vis.close()