路径规划——A*算法

算法原理

为了解决Dijkstra算法效率低的问题，A Star 算法作为一种启发式算法被提出。该算法在广度优先的基础上加入了一个估价函数。如BFS和常规方法如Dijsktra算法结合在一起的算法，有点不同的是，类似BFS的启发式经常给出一个近似解而不是保证最佳解，A Star是路径搜索中最受欢迎的选择，它相当灵活，并且能用于多种多样的情理之中，A Star 潜在的搜索一个很大的区域，A Star算法能用于搜索最短路径，A Star 能用启发式函数引发它自己，在简单的情况中，它和BFS一样快。
BFS算法：Best First Search
Dijkstra算法：Dijkstra

1968年，启发式方法提出了A* ，其中包括将已知区域划分为单个单元，并计算每个单元达到目标的总成本。给定从起点到当前分析像元的总距离以及从分析像元到目标的距离，总路径成本使用以下公式计算：
$\quad(1)\\ F(n)是总路径开销，也是节点n的综合优先级。每次选择下一个要访问的节点时，\\总是选取综合优先级最高的节点，也就是总开销最小的节点；\\ G(n)是从起点到达指定单元的成本和；\\ H(n)是从指定单元到达目标的预估成本，也是A^*算法的启发函数。$

start：路径规划的起始点
goal：路径规划的终点
g_score：从当前点(current_node)到出发点(start)的移动代价
h_score：不考虑障碍物，从当前点(current_node)到目标点的估计移动代价
f_score：f_score=g_score+h_score
openlist：寻找过程中的待检索节点列表
closelist：不需要再次检索的节点列表
came_Form：存储父子节点关系的列表，用于回溯最优路径，非必须，也可以通过节点指针实现

A*算法的核心就是上式，在运算过程中，每次从优先队列中选取F(n)值最小的节点作为下一个待访问的节点。

优点：利用启发式函数，搜索范围小，提高了搜索效率；如果最优路径存在，那么一定能找到最优路径。
缺点：A* 算法不适用于动态环境；不适用于高维空间，计算量大；目标点不可时会造成大量性能消耗。

算法流程：
1.从起点start开始，将start放入open优先队列中，open中存储的是将要遍历的节点，列表closed为空，closed是用来存放已经遍历的节点；
2.遍历start，将start从open中移除并添加到closed中，寻找start的邻居节点，至于最多可以找到多少邻居节点取决于算法规划，这里假设只允许节点向上下左右4个方向寻找那便是最多可以找到4个可通行的邻居节点，并计算F(n),将可通行的邻居节点放入open中；
3.从open中选择F(n)值最小的节点node，将node节点从open中移除并添加到closed中，寻找node的可通行的邻居节点并计算F(n)；
4.如果邻居节点neighbor已经在closed中了，则跳过，不进行其他处理，
5.如果邻居节点neighbor不在open中，则将其添加到open中，并且将node设置为neighbor的父节点；
6.如果邻居节点neighbor已经在open中了，则计算新的路径从start到neighbor的代价即G(n)值：
如果新的G(n)值比之前存储的值更小则更新，并且将neighbor的父节点改为节点node，并计算F(n)，之后存储到open中会根据F(n)值进行排序，
如果新的G(n)值比之前存储的值更大，则说明新的路径代价更高便不会选取这个节点也就不需要做出更新等处理；
7.再次从第3步开始执行，如此循环，直到在第3步中找出的节点node是目标节点goal时结束循环，也就成功找到了路径；或者在执行第3步时open为空，也要结束搜索过程了，此时说明无法找到合适路径。

算法实现

"""
    Filename: a_star.py
    Description: Plan path using A* Algorithm
    Author: Benxiaogu:https://github.com/Benxiaogu
    Date: 2024-08-05
"""
import heapq
import math
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation


class AStar:
    def __init__(self,grid,start,goal,board_size):
        self.grid = grid
        self.start = self.Node(start,0,0)
        self.goal = self.Node(goal,0,0)
        self.board_size = board_size

    class Node:
        def __init__(self, position, g, h, parent=None):
            self.position = position    # position of node
            self.g = g                  # distance from node to start
            self.h = h                  # heuristic value from node to goal
            self.parent = parent        # parent node

        def __eq__(self, other) -> bool:
            return self.position == other.position
        
        def __lt__(self, other):
            # Prioritise nodes based on heuristic value
            return self.g+self.h < other.g+other.h or (self.g+self.h==other.g+other.h and self.h<other.h)
        
    class Neighbor:
        def __init__(self,directions,cost):
            self.directions = directions
            self.cost = cost
    
    def get_neighbors(self, node):
        neighbors = []
        distances = []
        nexts = [self.Neighbor((-1, 0),1), self.Neighbor((0, 1),1), self.Neighbor((0, -1),1), self.Neighbor((1,0),1),
                self.Neighbor((-1,1),math.sqrt(2)), self.Neighbor((1,1),math.sqrt(2)),self.Neighbor((1, -1),math.sqrt(2)), self.Neighbor((-1,-1),math.sqrt(2))]
        for next in nexts:
            neighbor = (node[0] + next.directions[0], node[1] + next.directions[1])
            if self.board_size <= neighbor[0] < len(self.grid)-self.board_size and self.board_size <= neighbor[1] < len(self.grid[0])-self.board_size:
                if self.grid[neighbor[0]][neighbor[1]] == 0:
                    if next.directions[0] != 0 and next.directions[1] != 0:  # 对角线方向
                        if self.grid[node[0] + next.directions[0]][node[1]] == 0 and self.grid[node[0]][node[1] + next.directions[1]] == 0:
                            neighbors.append(neighbor)
                            distances.append(next.cost)
                    else:
                        neighbors.append(neighbor)
                        distances.append(next.cost)

        return neighbors,distances

    def plan(self):
        open = []
        self.searched = [] # Used to record nodes that are searched
        g_cost = {self.start.position:0}

        self.start.h = self.heuristic(self.start.position)
        heapq.heappush(open, self.start)

        while open:
            # Select the node closest to the start node
            current_node = heapq.heappop(open)
            if current_node.position in self.searched:
                continue
            self.searched.append(current_node.position)
            
            # Find the goal
            if current_node == self.goal:
                self.goal = current_node
                self.path = []
                while current_node:
                    self.path.append(current_node.position)
                    current_node = current_node.parent
                self.path = self.path[::-1]
                        
                return self.goal.g
            
            # Find the neighbors of the current node and determine in turn if they have already been closed
            neighbors, distances = self.get_neighbors(current_node.position)
            for neighbor,distance in zip(neighbors,distances):
                if neighbor in self.searched:
                    continue
                h = self.heuristic(neighbor)
                g = current_node.g+distance

                if neighbor not in g_cost or g < g_cost[neighbor]:
                    g_cost[neighbor] = g
                    neighbor_node = self.Node(neighbor,g,h,current_node)
                    heapq.heappush(open,neighbor_node)
                
        return -1

    # def heuristic(self, node):
    #     # Manhattan distance from current node to goal node
    #     return abs(node[0] - self.goal.position[0]) + abs(node[1] - self.goal.position[1])
    
    # def heuristic(self, node):
    #     # Chebyshev Distance
    #     D = 1
    #     D2 = math.sqrt(2)
    #     dx = abs(node[0] - self.goal.position[0])
    #     dy = abs(node[1] - self.goal.position[1])
    #     return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)
    
    def heuristic(self, node):
        # Euclidean Distance
        D = 1
        dx = abs(node[0] - self.goal.position[0])
        dy = abs(node[1] - self.goal.position[1])
        return D * math.sqrt(dx * dx + dy * dy)

结果：
在这里插入图片描述

代码如下，除了Python还提供了C++版本的，欢迎大家Follow andStar：
https://github.com/Benxiaogu/PathPlanning/tree/main/A_Star

启发式函数

曼哈顿距离：
标准的启发式函数是曼哈顿距离（Manhattan distance）。考虑你的代价函数并找到从一个位置移动到邻近位置的最小代价D。因此，地图中的启发式函数应该是曼哈顿距离的D倍，常用于在地图上只能前后左右移动的情况：
$h (n) = D * (ab s (n . x - g o a l . x) + ab s (n . y - g o a l . y))$

function heuristic(node) =
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return D * (dx + dy)

对角距离：
又称为切比雪夫距离。
对角距离是指在路径方案中允许斜着朝邻近的节点移动时的起点到终点的距离。
对角距离的计算如下：

function heuristic(node) =
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)

这里的D2指的是两个斜着相邻节点之间的移动代价。如果所有节点都是正方形，则其值等于sqrt(2)*D

欧几里得距离：
如果你的单位可以沿着任意角度移动（而不是网格方向），那么你也许应该使用直线距离：
$h(n) = D * sqrt((n.x-goal.x)^2 + (n.y-goal.y)^2)$

function heuristic(node) =
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return D * sqrt(dx * dx + dy * dy)

对于网格形式的图，上述启发函数的应用场景：

如果图形中只允许朝上下左右四个方向移动，此时使用曼哈顿距离；
如果图形中允许朝上下左右以及对角线总共八个方向移动，此时使用对角距离；
如果图形中允许朝任何方向移动，此时使用欧几里得距离。

启发函数会影响A Star算法的行为。
1.如果启发函数H(n)始终为0，A Star算法将仅由G(n)决定节点的优先级，此时A Star算法便退化成了Dijkstra算法。
2.如果H(n)始终小于等于节点n到终点的实际代价，则A Star算法一定能够找到最短路径。但是当H(n)的值越小，算法将遍历越多的节点，便就导致了算法越慢。(后面将证明)
3.如果H(n)完全等于节点n到终点的实际代价，则A Star算法将能够找到最佳路径，并且速度也很快。不过并非在所有场景下都能够做到这一点。因为在没有达到终点之前，我们很难确切算出距离终点的代价。
4.如果H(n)大于节点n到终点的实际代价，则A Star算法无法保证找到最短路径，不过此时会很快完成运算过程。
5.如果H(n)相较于G(n)大很多，达到一种极端情况，此时H(n)便直接影响算法的结果，也就变成了贪婪最佳优先搜索，不一定找到最短路径。

以上几种均是近似启发式函数，用于预估代价值。

除了近似启发式，也有精确启发式，但是一般情况下获取精确启发式函数值比较占用时间。精确启发式的计算：