Мы обсудим алгоритм планирования пути A-star, как он работает, псевдокод и его реализацию с помощью python и matplotlib.
Он используется во многих областях информатики из-за полноты, оптимальности и оптимальной эффективности. В предыдущей статье мы обсуждали алгоритм Дейкстры. Чтобы получить лучшее и ясное понимание после прочтения предыдущей статьи, мы сохраним ту же терминологию, но изменим код, чтобы повысить его ясность и понимание.
Можно сказать, что A* — это расширение Дейкстры, использующее эвристические функции для повышения эффективности. Оба алгоритма используются для поиска кратчайшего пути между двумя узлами во взвешенном графе.
Сравнение алгоритма Дейкстры и алгоритма A star
Алгоритм Дейкстры предназначен для поиска кратчайшего пути от указанного исходного узла ко всем другим достижимым узлам в графе, создавая в процессе дерево кратчайших путей. Алгоритм A*, напротив, использует эвристическую функцию для направления поиска к определенному целевому узлу, что позволяет более эффективно находить кратчайший путь между указанным источником и целевым узлом. Однако это достигается за счет того, что A* не генерирует все дерево кратчайших путей, что означает, что он не может найти кратчайший путь ко всем возможным целям. Поэтому выбор алгоритма зависит от конкретных требований решаемой задачи.
Что такое эвристика и зачем нам ее использовать?
Эвристики — это стратегии или методы решения проблем, которые используют практический опыт и здравый смысл для поиска решения. Они используются в информатике для улучшения процессов решения проблем, делая их более эффективными и действенными.
В контексте алгоритма A* эвристики используются для направления процесса поиска к целевому состоянию. Он использует комбинацию фактической стоимости для достижения узла от начального узла (известного как G) и расчетной стоимости от текущего узла до цели (известного как H), чтобы определить, какой узел посетить следующим. Общая стоимость узла равна сумме G и H или F = G + H.
Они помогают алгоритму принимать обоснованные решения о том, какие узлы исследовать дальше в пространстве поиска.
Допустимая эвристика никогда не завышает фактическую стоимость достижения цели.
Непротиворечивая эвристика удовлетворяет условию, что оценочная стоимость от текущего узла до целевого узла всегда меньше или равна оценочному расстоянию от любой соседней вершины до цели плюс стоимость достижения этого соседа.
Cost(a -> c) <= Cost(a -> b) + Cost(b -> c)
Хорошая эвристика для алгоритма A* допустима и непротиворечива.
Теперь мы кратко обсудим F = G+H
F = G + H
G
Это то же самое, что мы называем стоимостью в алгоритме Дейкстры. Там написано, на сколько единиц мы переместились из начального узла. Это не дает интуитивного представления о том, где находится целевой узел. Его даже не волнует, где находится целевой узел.
H
Он дает оценку того, где находится целевой узел. Обычно используемыми эвристическими функциями являются евклидово расстояние, манхэттенское расстояние, расстояние Чебышева, октильное расстояние и т. д. Мы должны убедиться, что стоимость никогда не завышена.
Пользовательские функции могут быть использованы в зависимости от ситуации. Например, во время путешествия может быть доступно несколько вариантов маршрута, и обычно учитываются такие факторы, как условия движения, безопасность дорожного движения и другие важные факторы, в качестве полезных эвристик, помогающих в принятии решений при выборе маршрута.
F
В конце концов, общая стоимость представляет собой сумму как расчетной стоимости от начального узла до текущего узла, так и расчетной стоимости от целевого узла до текущего узла.
псевдокод
Давайте напишем код в реальном времени на простом примере, чтобы понять, как работает A* и эвристика
Код очень похож на алгоритм Дейкстры. Здесь мы в основном фокусируемся на эвристическом расчете и добавляем его к стоимости при поиске кратчайшего узла.
Путешествуя по дороге, мы видим разные пути, каждый из которых имеет разные эвристики, давайте рассмотрим аналогичную ситуацию и решим ее. заштрихованная часть в два раза больше эвристики, чем остальные.
зеленый узел => Начальный узел (10,45)
синий узел => Целевой узел (90,45)
черные точки =› препятствия
голубая зона пересечения => Это грубый путь, стоимость которого высока. (20,0) => (80,50). У нас есть 2 разных эвристики для грубого пути, стоимость которого больше.
Если нет концепции разных путей, кратчайший путь - это прямая линия, это не проблема, алгоритм Дейкстры выводит прямую линию, поскольку он не учитывает эвристику. Теперь мы увидим, как алгоритм A-star находит кратчайший путь.
import matplotlib.pyplot as plt
import math
show_animation = True
class AStarPlanner:
def __init__(self, ox, oy, resolution, robot_radius):
self.x_min = 0
self.y_min = 0
self.x_max = 0
self.y_max = 0
self.x_width = 0
self.y_width = 0
self.obstacle_map = 0
self.resolution = resolution
self.robot_radius = robot_radius
self.calc_obstacle_map(ox, oy)
self.motion = self.get_motion_model()
class Node:
def __init__(self, x, y, cost, parent_index):
self.x = x # index of grid
self.y = y # index of grid
self.cost = cost
self.parent_index = parent_index # index of previous Node
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.parent_index)
def planning(self,sx,sy,gx,gy):
start_node = self.Node(self.calc_xy_index(sx, self.x_min),self.calc_xy_index(sy, self.y_min), 0.0, -1)
goal_node = self.Node(self.calc_xy_index(gx, self.x_min),self.calc_xy_index(gy, self.y_min), 0.0, -1)
open_set, closed_set = dict(), dict()
open_set[self.calc_index(start_node)] = start_node
print(str(start_node))
while True:
if len(open_set) == 0:
print("Open set is empty..")
break
curr_id = min(open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node,open_set[o]))
current = open_set[curr_id]
if current.x == goal_node.x and current.y == goal_node.y:
print("Find Goal")
goal_node.parent_index = current.parent_index
goal_node.cost = current.cost
break
del open_set[curr_id]
closed_set[curr_id] = current
for i, _ in enumerate(self.motion):
node = self.Node(current.x + self.motion[i][0],
current.y + self.motion[i][1],
current.cost + self.motion[i][2], curr_id)
n_id = self.calc_index(node)
if n_id in closed_set:
continue
if not self.verify_node(node):
continue
if n_id not in open_set:
open_set[n_id] = node
else:
if open_set[n_id].cost > node.cost:
open_set[n_id] = node
rx,ry = self.calc_final_path(goal_node,closed_set)
# returns distance
return rx,ry
def calc_position(self,index,minp):
pos = index*self.resolution + minp
return pos
def calc_final_path(self,goal_node,closed_set):
rx,ry = [self.calc_position(goal_node.x ,self.x_min)], [self.calc_position(goal_node.y,self.y_min)]
parent_index = goal_node.parent_index
print(goal_node.cost)
while parent_index != -1:
n = closed_set[parent_index]
rx.append(self.calc_position(n.x,self.x_min))
ry.append(self.calc_position(n.y,self.y_min))
parent_index = n.parent_index
return rx,ry
def calc_index(self, node):
return (node.y - self.y_min) * self.x_width + (node.x - self.x_min)
def calc_xy_index(self,position,minp):
return round((position - minp)/self.resolution)
@staticmethod
def calc_heuristic(n1,n2):
if n2.x >= 20 and n2.x <= 80 and n2.y <= 50:
d = max(abs(n1.x-n2.x) ,abs(n1.y-n2.y))*2
else:
d = max(abs(n1.x-n2.x) ,abs(n1.y-n2.y))
return d
def verify_node(self,node):
px = self.calc_position(node.x,self.x_min)
py = self.calc_position(node.y,self.y_min)
if px < self.x_min: return False
if py < self.y_min: return False
if px >= self.x_max: return False
if py >= self.y_max: return False
if self.obstacle_map[node.x][node.y]: return False
return True
def calc_obstacle_map(self,ox,oy):
self.x_min = round(min(ox))
self.x_max = round(max(ox))
self.y_min = round(min(oy))
self.y_max = round(max(oy))
self.x_width = round((self.x_max - self.x_min) / self.resolution)
self.y_width = round((self.y_max - self.y_min) / self.resolution)
self.obstacle_map = [[False for _ in range(self.y_width)] for _ in range(self.x_width)]
for ix in range(self.x_width):
x = self.calc_position(ix,self.x_min)
for iy in range(self.y_width):
y = self.calc_position(iy,self.y_min)
for iox,ioy in zip(ox,oy):
d = math.hypot(iox-x,ioy -y)
if d <= self.robot_radius:
self.obstacle_map[ix][iy] = True
break
@staticmethod
def get_motion_model():
#dx,dy,cost
motion = [[1,0,1],[0,1,1],[-1,0,1],[0,-1,1],[-1,-1,math.sqrt(2)],
[-1,1,math.sqrt(2)],[1,-1,math.sqrt(2)],[1,1,math.sqrt(2)]]
return motion
def main():
print( __file__ + "start!!" )
# start and goal position
sx = 10.0 #[m]
sy = 45.0 #[m]
gx = 90.0 #[m]
gy = 45.0 #[m]
grid_size = 1.0
robot_radius = 1.0
# set obstacle positions
ox,oy =[],[]
hx,hy = [],[]
for i in range(20,81,10):
for j in range(0,51,10):
hx.append(i)
hy.append(j)
for i in range(0,101):
ox.append(i)
oy.append(0.0)
for i in range(0,101):
ox.append(i)
oy.append(100.0)
for i in range(0,101):
ox.append(100.0)
oy.append(i)
for i in range(0,101):
ox.append(0.0)
oy.append(i)
for i in range(30,70):
ox.append(i)
oy.append(50)
if show_animation:
plt.plot(ox,oy, ".k")
plt.plot(sx,sy, "og")
plt.plot(hx,hy, "-b")
plt.plot(gx,gy, "xb")
plt.grid(True)
plt.axis("equal")
a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
rx, ry = a_star.planning(sx, sy, gx, gy)
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.pause(0.001)
plt.show()
if __name__ == '__main__':
main()
Класс узла содержит x, y (индексы), стоимость и родительский индекс. Каждая точка сетки считается узлом.
Работающий
Инициализируйте начальный узел и целевой узел. Создайте 2 набора open_set для хранения исследуемых узлов и Closed_set для хранения самых коротких узлов.
curr_id хранит индекс кратчайшего узла в open_set в начале каждой итерации. current содержит кратчайший узел.
Эвристика добавляется при сравнении стоимости для поиска кратчайшего узла. когда мы добавляем затраты при перемещении, мы не учитываем эвристику предыдущего узла.
Наносится текущая позиция алгоритма. события клавиатуры перехватываются и запускаются для выхода из программы, если нажата клавиша escape.
Когда целевой узел достигнут, цикл прерывается. Если нет, цикл продолжается
ближайший узел удаляется из open_set и добавляется в Closed_Set.
Для каждого движения мы находим соответствующий узел и находим его индекс. Если узел уже находится в closed_set, он уже исследован и это был самый короткий узел, поэтому, если мы снова исследуем тот же узел, это означает, что мы попадем в цикл, поэтому мы должны остановить итерацию и перейти к следующей итерации. Проверьте узел, если он не удовлетворяет заданным ограничениям, затем остановите итерацию и перейдите к следующей итерации.
Если нового узла нет в open_set, добавьте его. Если он уже присутствует, сравните стоимость, если новая стоимость меньше, чем текущий путь является кратчайшим, поэтому замените узел (это похоже на замену родительского индекса и стоимости).
Теперь все новые узлы добавляются в open_list, и в новой итерации мы находим ближайший узел в существующем open_set, и поток продолжается……
Наконец, в конце концов, итерации проходят через target_node и Closed_Set, чтобы найти окончательный путь.
Мы обсудим функцию, используемую в
calc_heuristic-›Чтобы понять, как двигаться к желаемой конечной точке, мы можем использовать эвристику, которая может быть сформулирована на основе определенных критериев.
def calc_heuristic(n1,n2):
# n1 => goal node n2 => current node
if n2.x >= 20 and n2.x <= 80 and n2.y <= 50:
d = max(abs(n1.x-n2.x) ,abs(n1.y-n2.y))*2
else:
d = max(abs(n1.x-n2.x) ,abs(n1.y-n2.y))
return d
Общая эвристика:
Евклидово расстояние -> sqrt((x2 — x1)² + (y2 — y1)²)
Манхэттенское расстояние -> |x2 — x1| + |у2 — у1|
Расстояние Чебышева -> max(|x2 — x1|, |y2 — y1|)
Октильное расстояние -> max(|x2 — x1|, |y2 — y1|) + (sqrt(2) — 1) * min(|x2 — x1|, |y2 — y1|)
calc_position – преобразует индекс в метры (положение).
calc_xy_index -> Преобразует метры (положение) в индекс в сетке
calc_index -> Когда мы добавляем узлы в open_set и Closed_Set, для каждого узла должен быть уникальный идентификатор (уникальный индекс при сохранении узлов). Мы можем использовать любое выражение, уникальное для всех узлов или координат.
verify_node -›когда мы перемещаемся, нам нужно проверить, находится ли перемещенный узел в заданных ограничениях или нет. Оно не должно быть ни препятствием, ни границей, ни переходить границы.
calc_obstacle_map -›Он принимает позиции препятствий в качестве входных данных и преобразует их в индексы и блокирует узлы от прохождения через него.
get_motion_model -›Содержит возможное движение и добавленную стоимость из текущего узла.
calc_final_path ->Теперь у нас есть target_node (index) и closed_set. преобразовать target_node из index в position. повторите его через родительский индекс и преобразуйте в позицию и добавьте его.
Из функции calc_final_path мы получаем конечные позиции пути и наносим их на график.
Когда вся сетка имеет одинаковую эвристику, то очевидно прямой маршрут является кратчайшим путем, который мы можем видеть в алгоритме Дейкстры. Когда регион имеет другую эвристику, стоимость проезда через него увеличивается, поэтому она корректируется соответствующим образом.
GitHub: https://github.com/vamsi1961
