2.4 A*算法的优化
A*算法是一种常用的启发式搜索算法，用于在图形或图形网络中找到从起始节点到目标节点的最优路径。通过综合考虑节点的实际成本和启发式估计成本，A*算法能够在较短的时间内找到最优解。其优化包括使用堆优化开放列表、避免重复访问已访问的节点、剪枝等措施，以提高搜索效率。
2.4.1 基本的优化措施
请看下面的实例文件a_star_optimised.py，实现了一个基于A*算法的路径规划器，用于在给定的二维环境中找到起点到终点的最优路径。该算法考虑了障碍物的存在，并通过使用启发式函数和堆优化开放列表等技术来提高搜索效率。通过执行 find_path 方法，可以找到起始点到目标点的最短路径，并通过可视化方式展示出来。
在这个实例中用到了如下所示的优化措施：
- 使用堆优化开放列表：在 find_path 方法中，使用Python 标准库中的 heapq 模块来实现优先队列，即 open_heap。这样可以确保每次从开放列表中选择下一个要探索的节点时，都是选择总成本最低的节点，从而使搜索更加高效。
- 避免重复访问已访问的节点：在算法中使用了两个字典 open_diction 和 visited_diction 来分别存储待访问的节点和已访问的节点。这样可以在将节点添加到开放列表之前检查它是否已经在已访问的节点中，从而避免了重复访问相同的节点，减少了不必要的计算。
- 剪枝：在向开放列表添加新节点之前，会检查新节点的总成本是否比已经存在于开放列表中的相同节点的总成本更高。如果是，则不将新节点添加到开放列表中，这样可以减少开放列表中的节点数量，提高搜索效率。
- 启发式函数优化：使用启发式函数来评估从当前节点到目标节点的估计成本，这里使用了欧几里得距离。启发式函数的良好选择可以加速A*算法的搜索过程，使其更快地找到最优路径。
通过这些优化措施，该算法可以更有效地在给定的图形环境中找到起始点到目标点的最优路径。
实例2-4：A*算法的基本优化（codes/2/a_star_optimised.py）
实例文件a_star_optimised.py的具体实现代码如下所示。
import copy
import heapq as hq
import math
import matplotlib.pyplot as plt
import numpy as np
# 从集合中导入 Set 类（已废弃，Python 3 中无需导入）
# 车辆状态 = (x,y)
# 状态元组 (f,g,(x,y), [(x1,y1),(x2,y2)...])
# 总成本 f(n) = 实际成本 g(n) + 启发式成本 h(n)
# 障碍物 = [(x,y), ...]
# min_x, max_x, min_y, max_y 是环境的边界
class a_star:
def __init__(self, min_x, max_x, min_y, max_y, \
obstacle = [], resolution = 1, robot_size = 1) :
##TODO
self.min_x = min_x
self.max_x = max_x
self.min_y = min_y
self.max_y = max_y
self.obstacle = obstacle
self.resolution = resolution
self.robot_size = robot_size
def euc_dist(self, position, target):
val1 = np.sqrt(((position[0] - target[0]) ** 2) + ((position[1] - target[1]) ** 2))
#val2 = abs(position[0]-target[0]) + abs(position[1]-target[1])
return val1
def costfunction(self, position, target):
return 1
# 状态: (总成本 f, 上一个成本 g, 当前位置 (x,y), \
# 上一个运动id, 路径[(x1,y1),...])
# 起始点 = (sx, sy)
# 终点 = (gx, gy)
# sol_path = [(x1,y1),(x2,y2), ...]
def Sort_Tuple(self,tup):
# reverse = None（按升序排序）
# 使用子列表的第二个元素进行排序 lambda 函数被使用
tup.sort(key = lambda x: x[1])
return tup
def find_path(self, start, end):
open_heap = [] # 元素为(cost,node)的列表
open_diction={} # 元素为 node: (cost,parent) 的字典
visited_diction={} # 元素为 node: (cost,parent) 的字典
obstacles = set(self.obstacle)
cost_to_neighbour_from_start = 0
hq.heappush(open_heap,((cost_to_neighbour_from_start + self.euc_dist(start, end),start)))
open_diction[start]=(cost_to_neighbour_from_start + self.euc_dist(start, end),start)
possible_neighbours=[(-1,-1),(-1,0),(-1,1),(0,1),(1,1),(1,0),(1,-1),(0,-1)]
costs=[1.1, 1, 1.1, 0.5, 0.1, 0, 0.1, 0.5] # 目标总是位于起始节点的右侧
# 当开放列表长度大于 0 时
while len(open_heap) > 0:
# 选择具有最小总成本以探索的节点
chosen_node_cost = open_heap[0]
chosen_node=chosen_node_cost[1]
chosen_cost=chosen_node_cost[0]
visited_diction[chosen_node]=open_diction[chosen_node]
if end in visited_diction:
rev_final_path=[end] # 最终路径的反向
node=end
m=1
while m==1:
contents=visited_diction[node]
parent_of_node=contents[1]
rev_final_path.append(parent_of_node)
node=parent_of_node
if node==start:
rev_final_path.append(start)
break
final_path=[]
for p in rev_final_path:
final_path.append(p)
return final_path
# 探索所选元素的邻居
hq.heappop(open_heap)
for i in range(0,8):
cost_to_neighbour_from_start = chosen_cost-self.euc_dist(chosen_node, end)
neigh_coord=possible_neighbours[i]
neighbour = (chosen_node[0]+neigh_coord[0],chosen_node[1]+neigh_coord[1])
if ((neighbour not in obstacles) and \
(neighbour[0] >= self.min_x) and (neighbour[0] <= self.max_x) and \
(neighbour[1] >= self.min_y) and (neighbour[1] <= self.max_y)) :
heurestic = self.euc_dist(neighbour,end)
cost_to_neighbour_from_start = 1+ cost_to_neighbour_from_start
total_cost = heurestic+cost_to_neighbour_from_start+costs[i]
skip=0
# 如果前往该邻居的成本比已存在于开放列表中的路径成本更高，跳过该邻居
found_lower_cost_path_in_open=0
if neighbour in open_diction:
if total_cost>open_diction[neighbour][0]:
skip=1
elif neighbour in visited_diction:
if total_cost>visited_diction[neighbour][0]:
found_lower_cost_path_in_open=1
if skip==0 and found_lower_cost_path_in_open==0:
hq.heappush(open_heap,(total_cost,neighbour))
open_diction[neighbour]=(total_cost,chosen_node)
#print(open_set_sorted)
return []
def main():
print(__file__ + " start!!")
grid_size = 1 # [m]
robot_size = 1.0 # [m]
sx, sy = -10, -10 # 原始值为 -10, -10
gx, gy = 10,10 # 原始值为 10, 10
obstacle = []
for i in range(30):
obstacle.append((i-15, -15))
obstacle.append((i-14, 15))
obstacle.append((-15, i-14))
obstacle.append((15, i-15))
for i in range(3):
obstacle.append((0,i))
obstacle.append((0,-i))
# 障碍物.append((-9, -9)) # 原始代码中没有这行
plt.plot(sx, sy, "xr")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
simple_a_star = a_star(-15, 15, -15, 15, obstacle=obstacle, \
resolution=grid_size, robot_size=robot_size)
path = simple_a_star.find_path((sx,sy), (gx,gy))
print (path)
rx, ry = [], []
for node in path:
rx.append(node[0])
ry.append(node[1])
plt.plot(rx, ry, "-r")
plt.show()
if __name__ == '__main__':
main()上述代码实现了一个基本的A*路径规划算法，并在处理过程中进行了一些优化，例如使用堆来管理开放列表，以提高搜索效率。同时，通过将已访问的节点信息存储在字典中，避免了重复访问相同的节点，从而减少了计算量。对上述代码的具体说明如下所示：
- 首先，定义了类 a_star，其构造函数 __init__ 初始化了算法所需的一些参数，如环境边界、障碍物等。
- 函数euc_dist的功能是计算两个点之间的欧几里得距离。
- 函数costfunction的功能是定义了从一个点到另一个点的成本。在此实现中，该函数始终返回 1，表示从一个点移动到另一个相邻点的固定成本。
- 函数Sort_Tuple的功能是排序元组列表中的元素，按照元组中的第二个元素进行排序。
- 函数find_path是实际执行A*搜索的部分。它维护了一个开放列表 open_heap 和两个字典 open_diction 和 visited_diction，用于存储待访问和已访问的节点信息。
- 在主函数 main 中，分别定义了起始点、目标点和障碍物，并创建了一个 a_star 类的实例。然后调用 find_path 方法找到路径，并将结果可视化。
- 最后，在 __main__ 中调用 main 函数以执行主程序。
执行后会打印输出如下从起始点到目标点的最优路径的节点列表，并绘制最优路径在给定环境中的具体位置可视化图，如图2-8所示。
[(10, 10), (10, 9), (9, 8), (8, 7), (8, 6), (7, 5), (6, 4), (6, 3), (5, 2), (4, 1), (3, 0), (2, -1), (1, -2), (0, -3), (-1, -3), (-2, -4), (-3, -4), (-4, -5), (-5, -5), (-6, -6), (-7, -7), (-8, -8), (-9, -9), (-10, -10), (-10, -10)]
图2-8 最优路径可视化图
2.4.2 Hybrid A*算法优化
Hybrid A*算法是对A*算法的改进，它考虑了车辆动态学，并生成了更平滑的路径，以便车辆可以跟随。这可能意味着在生成路径时考虑车辆的转弯半径、加速度、速度限制等因素，以确保生成的路径更适合于车辆实际行驶。
Hybrid A*算法的核心思想是结合传统A*算法和车辆动力学模型，以生成更加平滑、更适合实际车辆行驶的路径。传统A*算法主要基于离散的节点进行路径搜索，而Hybrid A*算法在此基础上引入了车辆动力学模型，考虑了车辆的实际运动特性，例如转弯半径、速度限制和加速度等。通过在搜索过程中考虑这些因素，Hybrid A*算法能够生成更平滑、更适合车辆行驶的路径，从而提高了路径规划的实用性和可行性。
Hybrid A*算法的实现流程如下所示：
（1）初始化参数：设置起始点和目标点，定义环境边界和障碍物等信息，初始化车辆动力学参数。
（2）生成初始路径：使用传统A*算法或其他启发式搜索算法生成一个初始路径作为起点。
（3）优化路径：对初始路径进行优化，以考虑车辆的动力学特性。这可能涉及到调整路径以减小转弯半径、平滑路径以降低加速度变化等操作。
（4）评估路径：根据路径的平滑程度、长度、通过障碍物的情况等因素对路径进行评估，并计算路径的代价。
（5）搜索新路径：根据评估结果，在路径搜索空间中寻找更优的路径。这可能涉及到在路径附近搜索新的路径片段，或者通过改变路径的一些参数来调整路径。
（6）迭代优化：重复执行优化和评估步骤，直到找到满足条件的最优路径或达到搜索的终止条件。
（7）打印输出最优路径：输出找到的最优路径，以便后续的车辆导航或控制。
例如下面的实例实现了Hybrid A*算法，用于在给定的二维环境中找到从起始点到目标点的最优路径。该算法结合了离散的节点表示和连续的车辆动力学模型，以考虑车辆的实际运动特性。通过搜索离散节点和连续状态的组合，以及考虑转向和速度控制的影响，算法能够生成更加平滑、更适合实际车辆行驶的路径。最终，通过可视化方式展示找到的最优路径。
实例2-5：使用Hybrid A*算法平滑处理路径（codes/2/hybrid_a_star_optimised.py）
实例文件hybrid_a_star_optimised.py的具体实现代码如下所示。
# 可能的转向控制
possible_str = {
'l': -10,
'l+': -50,
'r+': +50,
'r': +10,
's': 0,
'a': -25,
'b': 25
}
# 可能的速度控制
possible_sp = {
'f': 1,
'b': -1
}
# 总成本 f(n) = 实际成本 g(n) + 启发式成本 h(n)
class hybrid_a_star:
def __init__(self, min_x, max_x, min_y, max_y, \
obstacle=[], resolution=1, vehicle_length=2):
##TODO
self.min_x = min_x
self.max_x = max_x
self.min_y = min_y
self.max_y = max_y
self.obstacle = obstacle
self.resolution = resolution
self.vehicle_length = vehicle_length
self.obstacles = set(self.obstacle)
def euc_dist(self, position, target):
output = np.sqrt(((position[0] - target[0]) ** 2) + ((position[1] - target[1]) ** 2)+(math.radians(position[2]) - math.radians(target[2])) ** 2)
return float(output)
def costfunction(self, position, target):
ratioDelta = 1
output = ratioDelta * abs(position[2] - target[2])
return float(output)
"""
对于每个节点 n，我们需要存储：
(离散 x, 离散 y, 车头角度 theta),
(连续 x, 连续 y, 车头角度 theta)
成本 g, f,
路径 [(连续 x, 连续 y, 连续 theta),...]
start: 离散 (x, y, theta)
end: 离散 (x, y, theta)
sol_path = [(x1,y1,theta1),(x2,y2,theta2), ...]
"""
def Sort_Tuple(self,tup):
# reverse = None (升序排序)
# 使用子列表的第二个元素进行排序 lambda 函数被使用
tup.sort(key = lambda x: x[1])
return tup
def find_path(self, start, end):
steering_inputs = [-40,0,40]
cost_steering_inputs= [0.1,0,0.1]
speed_inputs = [-1,1]
cost_speed_inputs = [1,0]
start = (float(start[0]), float(start[1]), float(start[2]))
end = (float(end[0]), float(end[1]), float(end[2]))
# 上述两个在离散坐标中
open_heap = [] # 元素为 (cost,node_d) 的列表
open_diction={} # 元素为 node_d:(cost,node_c,(parent_d,parent_c)) 的字典
visited_diction={} # 元素为 node_d:(cost,node_c,(parent_d,parent_c)) 的字典
obstacles = set(self.obstacle)
cost_to_neighbour_from_start = 0
# 这里选择了堆。 (cost, path) 是一个元组，被推入 open_set_sorted 中。
# 主要优势是随着更多的 (cost, path) 被添加到 open_set，堆会自动排序，
# 并且第一个元素自动成为最低成本的一个。
# 这里的路径是 [(),()....]，每个 () 对应一个节点的 (离散,连续)。
# 对于路径正常的附加是通过 heap 实现的。如果在此使用 heap，
# 元素会被排序，我们不希望发生这种情况。我们希望保留我们移动的顺序
# 从起始节点到目标节点
hq.heappush(open_heap,(cost_to_neighbour_from_start + self.euc_dist(start, end),start))
open_diction[start]=(cost_to_neighbour_from_start + self.euc_dist(start, end), start,(start,start))
while len(open_heap)>0:
# 选择总成本最低的节点进行探索
chosen_d_node = open_heap[0][1]
chosen_node_total_cost=open_heap[0][0]
chosen_c_node=open_diction[chosen_d_node][1]
visited_diction[chosen_d_node]=open_diction[chosen_d_node]
if self.euc_dist(chosen_d_node,end)<1:
rev_final_path=[end] # 最终路径的反向
node=chosen_d_node
m=1
while m==1:
visited_diction
open_node_contents=visited_diction[node] # (cost,node_c,(parent_d,parent_c))
parent_of_node=open_node_contents[2][1]
rev_final_path.append(parent_of_node)
node=open_node_contents[2][0]
if node==start:
rev_final_path.append(start)
break
final_path=[]
for p in rev_final_path:
final_path.append(p)
return final_path
hq.heappop(open_heap)
for i in range(0,3) :
for j in range(0,2):
delta=steering_inputs[i]
velocity=speed_inputs[j]
cost_to_neighbour_from_start = chosen_node_total_cost-self.euc_dist(chosen_d_node, end)
neighbour_x_cts = chosen_c_node[0] + (velocity * math.cos(math.radians(chosen_c_node[2])))
neighbour_y_cts = chosen_c_node[1] + (velocity * math.sin(math.radians(chosen_c_node[2])))
neighbour_theta_cts = math.radians(chosen_c_node[2]) + (velocity * math.tan(math.radians(delta))/(float(self.vehicle_length)))
neighbour_theta_cts=math.degrees(neighbour_theta_cts)
neighbour_x_d = round(neighbour_x_cts)
neighbour_y_d = round(neighbour_y_cts)
neighbour_theta_d = round(neighbour_theta_cts)
neighbour = ((neighbour_x_d,neighbour_y_d,neighbour_theta_d),(neighbour_x_cts,neighbour_y_cts,neighbour_theta_cts))
if (((neighbour_x_d,neighbour_y_d) not in obstacles) and \
(neighbour_x_d >= self.min_x) and (neighbour_x_d <= self.max_x) and \
(neighbour_y_d >= self.min_y) and (neighbour_y_d <= self.max_y)) :
heurestic = self.euc_dist((neighbour_x_d,neighbour_y_d,neighbour_theta_d),end)
cost_to_neighbour_from_start = abs(velocity)+ cost_to_neighbour_from_start +\
cost_steering_inputs[i] + cost_speed_inputs[j]
total_cost = heurestic+cost_to_neighbour_from_start
# 如果去到此后继节点的成本大于在开放列表中到此节点的已存在路径的成本，
# 跳过此后继节点
skip=0
found_lower_cost_path_in_open=0
if neighbour[0] in open_diction:
if total_cost>open_diction[neighbour[0]][0]:
skip=1
elif neighbour[0] in visited_diction:
if total_cost>visited_diction[neighbour[0]][0]:
found_lower_cost_path_in_open=1
if skip==0 and found_lower_cost_path_in_open==0:
hq.heappush(open_heap,(total_cost,neighbour[0]))
open_diction[neighbour[0]]=(total_cost,neighbour[1],(chosen_d_node,chosen_c_node))
print("未找到目标 - 它无法达到。")
return []
def main():
print(__file__ + " 开始!!")
# 起始和目标位置
#(x, y, theta) 单位：米，米，度
sx, sy, stheta= -5, -5, 0
gx, gy, gtheta = 5, 5, 0 #2,4,0 几乎精确
# 创建障碍物
obstacle = []
for i in range(3):
obstacle.append((0,i))
obstacle.append((0,-i))
ox, oy = [], []
for (x,y) in obstacle:
ox.append(x)
oy.append(y)
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
hy_a_star = hybrid_a_star(-6, 6, -6, 6, obstacle=obstacle, resolution=1, vehicle_length=2)
path = hy_a_star.find_path((sx,sy,stheta), (gx,gy,gtheta))
rx, ry = [], []
for node in path:
rx.append(node[0])
ry.append(node[1])
plt.plot(rx, ry, "-r")
plt.show()
if __name__ == '__main__':
main()对上述代码的具体说明如下所示：
- 字典possible_str和possible_sp：分别定义了可能的转向控制和速度控制，其中键表示控制名称，值表示对应的转向角度或速度。
- 类hybrid_a_star：实现了Hybrid A*算法的路径规划功能。该类包含了初始化函数__init__、欧氏距离函数euc_dist、成本函数costfunction、路径搜索函数find_path等方法，用于初始化环境参数、计算节点间距离和成本、搜索最优路径等功能。
- 函数Sort_Tuple：用于对元组列表进行排序，按照元组中的第二个元素（即成本）进行升序排序。
- 函数find_path：实现了Hybrid A*算法的路径搜索功能，该函数使用堆优化的方式搜索最优路径，考虑了转向和速度控制的影响，并返回找到的最优路径。
- 主函数main：程序的入口函数，用于设置起始点、目标点、障碍物等环境信息，并调用Hybrid A*算法的find_path函数进行路径搜索。最后，根据搜索结果可视化展示找到的最优路径和环境信息，如图2-8所示。其中图中的黑点表示障碍物。
图2-8 最优路径和环境信息的可视化
本实例使用了如下所示的优化技术：
- 堆优化：在路径搜索过程中，使用堆来存储和管理待探索的节点，以确保每次选择总成本最低的节点进行探索，从而提高了搜索效率。
- 预先计算：在初始化过程中，预先计算了可能的转向和速度控制的影响，并存储在字典中，避免了在搜索过程中重复计算，提高了运行效率。
- 早期终止：在搜索过程中，如果发现当前节点到目标节点的距离小于一定阈值，即认为找到了最优路径，提前结束搜索，避免不必要的计算。
- 有序列表：使用列表来存储路径中的节点信息，并通过Sort_Tuple函数对列表进行排序，确保路径中的节点按照成本从低到高的顺序排列，提高了路径搜索效率。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
