1.2.2 - 采样搜索算法（以RRT和RRT*为例） - Python运动规划库教程（Python Motion Planning）

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

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

python 复制代码

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

打印规划结果：

复制代码

[(5, 5), (4.771426907962659, 9.994772701694892), (4.146819953608962, 14.95560591322025), (7.595267976529573, 18.57613159787088), (8.140654910970852, 20.351857470764628), (11.710213969483886, 19.259381435008308), (16.5716134378916, 20.42848160546643), (21.474677851991277, 19.448706452082046), (25.521418626996706, 16.5120605592464), (26.18397115095867, 11.556152554350462), (28.275306073720905, 7.01453189198388), (33.2635891537154, 7.3566311668159745), (34.94476916317723, 12.065519971596348), (36.536019764598414, 16.805553887453566), (36.26977232128116, 21.798460085143148), (40.964074570672786, 23.519949670170227), (45, 25)]
{'success': True, 'start': (5, 5), 'goal': (45, 25), 'length': 74.88933689304858, 'cost': 74.88933689304858, 'expand': {(5, 5): Node((5, 5), None, 0, 0), ...}}

可视化。

复制代码

vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.plot_path(path, style="--", color="C4")
vis.plot_expand_tree(path_info["expand"])   # sample-search-featured expand 
vis.show()
vis.close()

完整的可运行代码:

python 复制代码

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, 51], [0, 31]])

map_.fill_boundary_with_obstacles()
map_.type_map[10:21, 15] = TYPES.OBSTACLE
map_.type_map[20, :15] = TYPES.OBSTACLE
map_.type_map[30, 15:] = TYPES.OBSTACLE
map_.type_map[40, :16] = TYPES.OBSTACLE

map_.inflate_obstacles(radius=3)

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

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

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

vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.plot_path(path, style="--", color="C4")
vis.plot_expand_tree(path_info["expand"])
vis.show()
vis.close()

采样搜索算法默认在连续空间中进行规划。如果你想在离散栅格地图上进行规划，请设置参数discrete为True，例如：

python 复制代码

planner = RRT(map_=map_, start=start, goal=goal, discrete=True)

打印结果：

复制代码

[(5, 5), (5, 10), (4, 15), (7, 19), (8, 20), (12, 19), (17, 20), (22, 18), (26, 15), (28, 10), (33, 10), (36, 14), (35, 19), (40, 19), (40, 22), (44, 25), (45, 25)]
{'success': True, 'start': (5, 5), 'goal': (45, 25), 'length': 70.60470734303811, 'cost': 70.60470734303811, 'expand': {(5, 5): Node((5, 5), None, 0, 0), ...}}

对于像RRT*这样的渐近最优采样搜索规划器，你可以将参数stop_until_sample_num设置为True，并配置sample_num以等待更好的结果。例如：

python 复制代码

RRTStar(map_=map_, start=start, goal=goal, sample_num=1000, stop_until_sample_num=True)

如需了解更多采样搜索规划器及其参数，请参阅官方教程文档中的API Reference部分。