路径规划（Path Planning）是指在给定地图、起始点和目标点的情况下，确定应该采取的最佳路径。常见的路径规划算法包括A* 算法、Dijkstra 算法、RRT（Rapidly-exploring Random Tree）等。
目录
一.A*
A*算法找到的1条由图中节点以及边组成的路径。一般而言，A*算法基于BFS(Breath First Search,广度优先遍历)，同时也可以认为是对BFS搜索算法的优化。
1.算法原理
具体执行过程如下：
(1) 搜索区域
以2维平面坐标下的简单搜索为例。
绿色方框代表起点和终点，红色方框代表不可经过的点。
(2) 搜索起点
A*算法从起点开始向四周检索相邻方格并进行扩展，直到扩展扫描到终点，算法结束。
流程如下：
① 从起点S开始，把S作为一个等待检查的方格，将其放入open_list中。
open_list：待走的位置。相当于候选点集合，每到1点，将周围点放入候选集合中，往后有可能往候选点去走。
闭集合：已经走过的位置，不会再被考虑。
② 寻找起点S周围可以到达的方格(最多8个)，并将它们加入open_list，同时设置他们的父方格为S;
③ 从open_list中删除起点S，同时将S加入close_list;
④ 计算每个周围方格的F值(F=G+H);
G:从起点S按照生成的路径移动到指定方格的移动代价。有2种距离计算方式：欧拉距离、曼哈顿距离。
H：从指定方格移动到终点的估算成本。使用Manhattan方法，忽略路径中的障碍物，计算当前方格横向或纵向移动到达目标所经过的方格数。
⑤ 从open_list中选取F值最小的方格a，并将其从open_list中删除，放入close_list中;
⑥ 继续检查a的邻接方格：
a). 忽略不可经过的方格以及被标记在close_list中的方格，将剩余的方格加入open_list，并计算这些方格的F值，同时设置父方格为a；
b). 如果某邻接的方格c已经在open_list中，则需计算新的路径从S到c的值G，判断是否需要更新：
如果新的G值更小一些，则修改父方格为方格a，重新计算F值。注意：H值不需要改变，因为方格到达终点的预计消耗是固定的，不需要重复计算。
如果新的G值更大一些，那么说明新路径不是一条更优的路径，则不需要更新值。
⑦ 继续从open_list中找出F值最小的，跳跃至第⑤步继续循环执行。
⑧ 当open_list出现终点E时，代表路径已经被找到，搜索成功。
如果open_list为空，那么说明没有找到合适的路径，搜索失败。
2.实例
python
'''A*'''
import time
import numpy as np
from matplotlib.patches import Rectangle
import matplotlib.pyplot as plt
import heapq
# 地图上的每1个点都是1个Point对象，用于记录该点的类别、代价等信息
class Point:
def __init__(self, x=0, y=0):
self.x = x
self.y = y
# 0代表可通行，1代表障碍物
self.val = 0
self.cost_g = 0
self.cost_h = 0
self.cost_f = 0
self.parent = None
# 0：不在开集合 1：在开集合 -1：在闭集合
self.is_open = 0
# 用于heapq小顶堆的比较
def __lt__(self, other):
return self.cost_f < other.cost_f
class Map:
def __init__(self, map_size):
self.map_size = map_size
self.width = map_size[0]
self.height = map_size[1]
self.map = [[Point(x, y) for y in range(self.map_size[1])] for x in range(self.map_size[0])]
# 手动设置障碍物，可多次调用设置地图
def set_obstacle(self, topleft, width, height):
for x in range(topleft[0], topleft[0] + width):
for y in range(topleft[1], topleft[1] + height):
self.map[x][y].val = 1
class AStar:
def __init__(self, map, start_point, end_point, connect_num=4):
self.map: Map = map
self.start_point = start_point
self.end_point = end_point
# 开集合，先放入起点，从起点开始遍历
self.open_set = [self.start_point]
self.start_point.is_open = 1
# 连通数，目前支持4连通或8连通
self.connect_num = connect_num
self.diffuse_dir = [(1, 0), (-1, 0), (0, 1), (0, -1)]
def g_cost(self, p):
'''
计算 g 代价:当前点与父节点的距离 + 父节点的 g 代价（欧氏距离）
'''
x_dis = abs(p.parent.x - p.x)
y_dis = abs(p.parent.y - p.y)
return np.sqrt(x_dis ** 2 + y_dis ** 2) + p.parent.cost_g
def h_cost(self, p):
'''
计算 h 代价:当前点与终点之间的距离（欧氏距离）
'''
x_dis = abs(self.end_point.x - p.x)
y_dis = abs(self.end_point.y - p.y)
return np.sqrt(x_dis ** 2 + y_dis ** 2)
def is_valid_point(self, x, y):
# 无效点：超出地图边界或为障碍物
if x < 0 or x >= self.map.width:
return False
if y < 0 or y >= self.map.height:
return False
return self.map.map[x][y].val == 0
def search(self):
self.start_time = time.time()
p = self.start_point
while not (p == self.end_point):
# 弹出代价最小的开集合点，若开集合为空，说明没有路径
try:
p = heapq.heappop(self.open_set)
except:
raise 'No path found, algorithm failed!!!'
p.is_open = -1
# 遍历周围点
for i in range(self.connect_num):
dir_x, dir_y = self.diffuse_dir[i]
self.diffusion_point(p.x + dir_x, p.y + dir_y, p)
return self.build_path(p) # p = self.end_point
def diffusion_point(self, x, y, parent):
# 无效点或者在闭集合中，跳过
if not self.is_valid_point(x, y) or self.map.map[x][y].is_open == -1:
return
p = self.map.map[x][y]
pre_parent = p.parent
p.parent = parent
# 先计算出当前点的总代价
cost_g = self.g_cost(p)
cost_h = self.h_cost(p)
cost_f = cost_g + cost_h
# 如果在开集合中，判断当前点和开集合中哪个点代价小，换成小的，相同x,y的点h值相同，g值可能不同
if p.is_open == 1:
if cost_f < p.cost_f:
# 如果从当前parent遍历过来的代价更小，替换成当前的代价和父节点
p.cost_g, p.cost_h, p.cost_f = cost_g, cost_h, cost_f
else:
# 如果从之前父节点遍历过来的代价更小，保持之前的代价和父节点
p.parent = pre_parent
else:
# 如果不在开集合中，说明之间没遍历过，直接加到开集合里就好
p.cost_g, p.cost_h, p.cost_f = cost_g, cost_h, cost_f
heapq.heappush(self.open_set, p)
p.is_open = 1
def build_path(self, p):
print('search time: ', time.time() - self.start_time, ' seconds')
# 回溯完整路径
path = []
while p != self.start_point:
path.append(p)
p = p.parent
print('search time: ', time.time() - self.start_time, ' seconds')
# 打印开集合、闭集合的数量
print('open set count: ', len(self.open_set))
close_count = 0
for x in range(self.map.width):
for y in range(self.map.height):
close_count += 1 if self.map.map[x][y].is_open == -1 else 0
print('close set count: ', close_count)
print('total count: ', close_count + len(self.open_set))
# path = path[::-1] # path为终点到起点的顺序，可使用该语句翻转
return path
if __name__ == '__main__':
map = Map((6, 6))
# 用于显示plt图
ax = plt.gca()
ax.xaxis.set_ticks_position('top')
ax.set_xlim([0, map.width])
ax.set_ylim([0, map.height])
plt.tight_layout()
plt.grid(True, linestyle="--", color="black", linewidth="1", axis='both')
ax.xaxis.set_ticks_position('top')
ax.invert_yaxis()
# 设置障碍物
map.set_obstacle([2, 2], 3, 1)
map.set_obstacle([2, 3], 1, 2)
# 设置起始点和终点，并创建astar对象
start_point = map.map[1][1]
end_point = map.map[4][4]
# 将障碍物及起点、终点显示到plt上
ax.add_patch(Rectangle([2, 2], width=3, height=1, color='red'))
ax.add_patch(Rectangle([2, 3], width=1, height=2, color='red'))
ax.add_patch(Rectangle([1, 1], width=1, height=1, color='green'))
ax.add_patch(Rectangle([4, 4], width=1, height=1, color='green'))
astar = AStar(map, start_point, end_point)
path = astar.search()
# 可视化起点到终点完整路径
for p in path:
if p!=end_point:
ax.add_patch(Rectangle([p.x, p.y], width=1, height=1, color='blue'))
plt.savefig('res.jpg')
plt.show()二.Dijkstra
Dijkstra是1种适用于非负权值网络的单源最短路径算法，可以给出从指定节点到图中其他节点的最短路径，以及任意2点的最短路径。
1.算法原理
下面用1个示例讲述算法的原理过程，如图所示，1个有向权值图，利用Dijkstra算法找到从节点1到节点6的最短路径。
(1) 首先进行初始化，初始化1个空的open list，close list 以及parent。将起点及其距离信息放入到open list 中，将起点及其父亲节点放入parent中，起点的父节点为None。
open list ：存放已经访问的从该节点到起点有路径的结点(有路径但不一定是最优路径)
close list ：存放那些已经找到最优路径的结点。
parent ：存放结点的父子关系，方便后面路径回溯。
(2) 按步骤执行：
(3) 按步骤执行：
(4) 按步骤执行：
(5) 按步骤执行：
(6) 按步骤执行：
(7) 按步骤执行：
(8) 路径回溯
根据parent中的继承关系，从终点向起点回溯，得到从起点到终点的最短路径为：1=>4=>7=>6，最短路径长为：6。
2.实例
python
import heapq
def dijkstra(graph,start):
distances = {vertex: float('infinity') for vertex in graph}
distances[start] = 0
# 初始化父亲节点
parent = {vertex: None for vertex in graph}
priority_queue = [(0,start)]
while priority_queue:
# 弹出堆中距离最小的节点
current_distance, current_vertex = heapq.heappop(priority_queue)
# 如果当前距离已经大于已知的最短距离，则跳过
if current_distance > distances[current_vertex]:
continue
# 更新相邻节点的距离
for neighbor, weight in graph[current_vertex].items():
distance = current_distance + weight
# 如果找到更短的路径，则更新距离，并将节点加入优先队列
if distance < distances[neighbor]:
distances[neighbor] = distance
parent[neighbor] = current_vertex
heapq.heappush(priority_queue, (distance, neighbor))
# print("加入后的队列:",priority_queue)
return distances, parent
# 输出路径回溯
def get_path(parent,end):
path = []
current = end
while current is not None:
path.append(current)
current = parent[current]
path.reverse()
return path
graph = {
'1': {'2': 2, '4': 1},
'2': {'4': 3, '5': 11 },
'3': {'1': 4, '6': 5},
'4': {'5': 2, '6': 8, '7': 4},
'5': {'7': 6},
'6':{},
'7': {'6': 1},
}
start_node='1'
end_node = '6'
distances, parent = dijkstra(graph, start_node)
path = get_path(parent, end_node)
print(f"从节点 {start_node} 到节点 {end_node} 的路径:", path)三.RRT
1.算法原理
快速随机树算法（Rapid Random Tree,RRT）以初始的1个根节点，通过随机采样的方法在空间搜索，然后添加叶节点来不断扩展随机树。当目标点进入随机树里面后，随机树扩展立即停止，此时能找到1条从起始点到目标点的路径。
算法的计算过程如下：
(1) 初始化随机树。设置起点，目标点
和搜索空间
；将起点作为随机树搜索的起点，此时树中只包含1个节点即根节点；
(2)在中随机采样。在
中随机采样1个点
，若
不在障碍物范围内，则计算随机树中所有节点到
的欧式距离，并找到距离最近的节点
；若
在障碍物范围内则重新采样；
(3)生成新节点。在和
连线方向，由固定步长距离生成1个新的节点
，并判断
是否在障碍物范围内，若不在障碍物范围内则将
添加到随机树中，并将
的父节点设置为
；否则的话返回(2)重新对环境进行随机采样；
(4)停止搜索。重复步骤(2)、(3)，当和
之间的距离小于设定的阈值时，则代表随机树已经到达了目标点，将作为最后1个路径节点加入到随机树中，算法结束并得到所规划的路径 。
2.实例
python
import math
import random
import matplotlib.pyplot as plt
import numpy as np
from celluloid import Camera
class RRT:
class Node:
def __init__(self, x, y):
self.x = x
self.y = y
self.path_x = []
self.path_y = []
self.parent = None
class AreaBounds:
def __init__(self, area):
self.xmin = float(area[0])
self.xmax = float(area[1])
self.ymin = float(area[2])
self.ymax = float(area[3])
def __init__(self,
start,
goal,
obstacle_list,
rand_area,
expand_dis=1.0,
goal_sample_rate=5,
max_iter=500,
play_area=None,
robot_radius=0.0,
):
"""
Setting Parameter
start:起点 [x,y]
goal:目标点 [x,y]
obstacleList:障碍物位置列表 [[x,y,size],...]
rand_area: 采样区域 x,y ∈ [min,max]
play_area: 约束随机树的范围 [xmin,xmax,ymin,ymax]
robot_radius: 机器人半径
expand_dis: 扩展的步长
goal_sample_rate: 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点
"""
self.start = self.Node(start[0], start[1]) # 根节点(0,0)
self.end = self.Node(goal[0], goal[1]) # 终点(6,10)
self.min_rand = rand_area[0] # -2 树枝生长区域xmin
self.max_rand = rand_area[1] # 15 xmax
if play_area is not None:
self.play_area = self.AreaBounds(play_area) # 树枝生长区域，左下(-2,0)==>右上(12,14)
else:
self.play_area = None # 数值无限生长
self.expand_dis = expand_dis # 树枝一次的生长长度
self.goal_sample_rate = goal_sample_rate # 多少概率直接选终点
self.max_iter = max_iter # 最大迭代次数
self.obstacle_list = obstacle_list # 障碍物的坐标和半径
self.node_list = [] # 保存节点
self.robot_radius = robot_radius # 随机点的搜索半径
# 路径规划
def planning(self, animation=True, camara=None):
# 先在节点列表中保存起点
self.node_list = [self.start]
for i in range(self.max_iter):
# 随机选取一个节点
rnd_node = self.sample_free()
# 从已知节点中选择和目标节点最近的节点
nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node)
nearest_node = self.node_list[nearest_ind]
new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
# 判断新点是否在规定的树的生长区域内，新点和最近点之间是否存在障碍物
if self.is_inside_play_area(new_node, self.play_area) and \
self.obstacle_free(new_node, self.obstacle_list, self.robot_radius):
# 都满足才保存该点作为树节点
self.node_list.append(new_node)
# 如果此时得到的节点到目标点的距离小于扩展步长，则直接将目标点作为xrand。
if self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y) <= self.expand_dis:
# 以新点为起点，向终点画树枝
final_node = self.steer(self.node_list[-1], self.end, self.expand_dis)
# 如果最新点和终点之间没有障碍物True
if self.obstacle_free(final_node, self.obstacle_list, self.robot_radius):
return self.generate_final_course(len(self.node_list) - 1)
if animation and i % 5 == 0:
self.draw_graph(rnd_node, camara)
return None # cannot find path
# 距离最近的已知节点坐标，随机坐标，从已知节点向随机节点的延展的长度
def steer(self, from_node, to_node, extend_length=float("inf")):
# d已知点和随机点之间的距离，theta两个点之间的夹角
d, theta = self.calc_distance_and_angle(from_node, to_node)
# 如果树枝的生长长度超出了随机点，就用随机点位置作为新节点
if extend_length >= d:
new_x = to_node.x
new_y = to_node.y
# 如果树生长长度没达到随机点长度，就截取长度为extend_length的节点作为新节点
else:
new_x = from_node.x + math.cos(theta) * extend_length
new_y = from_node.y + math.sin(theta) * extend_length
new_node = self.Node(new_x, new_y)
new_node.path_x = [from_node.x]
new_node.path_y = [from_node.y]
new_node.path_x.append(new_x)
new_node.path_y.append(new_y)
new_node.parent = from_node
return new_node
def generate_final_course(self, goal_ind):
path = [[self.end.x, self.end.y]]
node = self.node_list[goal_ind]
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
def calc_dist_to_goal(self, x, y):
"""计算(x,y)离目标点的距离"""
dx = x - self.end.x
dy = y - self.end.y
return math.hypot(dx, dy)
def sample_free(self):
if random.randint(0, 100) > self.goal_sample_rate:
rnd = self.Node(
random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand))
else: # goal point sampling
rnd = self.Node(self.end.x, self.end.y)
return rnd
# 绘制搜索过程
def draw_graph(self, rnd=None, camera=None):
if camera == None:
plt.clf()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
# 画随机点
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
if self.robot_radius > 0.0:
self.plot_circle(rnd.x, rnd.y, self.robot_radius, '-r')
# 画生成的树
for node in self.node_list:
if node.parent:
plt.plot(node.path_x, node.path_y, "-g")
# 画障碍物
for (ox, oy, size) in self.obstacle_list:
self.plot_circle(ox, oy, size)
# 画出可行区域
if self.play_area is not None:
plt.plot([self.play_area.xmin, self.play_area.xmax,
self.play_area.xmax, self.play_area.xmin,
self.play_area.xmin],
[self.play_area.ymin, self.play_area.ymin,
self.play_area.ymax, self.play_area.ymax,
self.play_area.ymin],
"-k")
# 画出起点和目标点
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.end.x, self.end.y, "xr")
plt.axis("equal")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
if camera != None:
camera.snap()
@staticmethod
def plot_circle(x, y, size, color="-b"):
deg = list(range(0, 360, 5))
deg.append(0)
xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
plt.plot(xl, yl, color)
@staticmethod
def get_nearest_node_index(node_list, rnd_node):
# 计算所有已知节点和随机节点之间的距离
dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y) ** 2
for node in node_list]
minind = dlist.index(min(dlist))
return minind
@staticmethod
def is_inside_play_area(node, play_area):
if play_area is None:
return True
if node.x < play_area.xmin or node.x > play_area.xmax or \
node.y < play_area.ymin or node.y > play_area.ymax:
return False
else:
return True
@staticmethod
def obstacle_free(node, obstacleList, robot_radius):
# 目标点，障碍物中点和半径，移动时的占地半径
if node is None:
return False
for (ox, oy, size) in obstacleList:
dx_list = [ox - x for x in node.path_x]
dy_list = [oy - y for y in node.path_y]
d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]
if min(d_list) <= (size + robot_radius) ** 2:
return False
return True
@staticmethod
def calc_distance_and_angle(from_node, to_node):
"""计算两个节点间的距离和方位角"""
dx = to_node.x - from_node.x
dy = to_node.y - from_node.y
d = math.hypot(dx, dy)
theta = math.atan2(dy, dx)
return d, theta
def main(gx=6.0, gy=10.0):
print("start " + __file__)
fig = plt.figure(1)
camera = Camera(fig) # 保存动图时使用
show_animation = True
obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),
(9, 5, 2), (8, 10, 1)] # [x, y, radius]
# Set Initial parameters
rrt = RRT(
start=[0, 0],
goal=[12, 12],
rand_area=[-2, 15],
obstacle_list=obstacleList,
play_area=[-2, 15, 0, 15],
# 搜索半径
robot_radius=0.2
)
path = rrt.planning(animation=show_animation, camara=camera)
if path is None:
print("Cannot find path")
else:
print("found path!!")
# 绘制最终路径
if show_animation:
print('start drawing the final path')
rrt.draw_graph(camera=camera)
plt.grid(True)
plt.pause(0.01)
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
if camera != None:
camera.snap()
animation = camera.animate()
animation.save('trajectory_res.gif')
plt.show()
if __name__ == '__main__':
main()