A* 寻路算法¶

栅格地图¶

栅格地图是把环境划分成一系列栅格，其中每一栅格给定一个可能值，表示该栅格被占据的概率。

如下图，我们用灰色表示灰色表示障碍物，用白色白色表示可以行走区域。

其中左下角我们放置一个粉色表示起始的位置，右上角红色表示我们想要去的目标位置

A* 算法¶

代价计算¶

这里我们允许机器人在栅格地图中沿着八邻域行走，所以我们使用对角距离来计算代价。

代价计算公式如下： $<span><span class="MathJax_Preview">f(x) = g(x)+h(x)</span><script type="math/tex">f(x) = g(x)+h(x)$

g(n) 表示当前节点n距离起点的距离

h(n) 表示当前节点n距离终点的距离

f(n) 表示当前节点距离起点和终点累积代价

实现步骤¶

初始化两个集合：
一个存放要处理点的集合open_set
另一个存放已经处理过点的集合close_set
将起点放入open_set，并设置代价为最高代价0
开始遍历
如果open_set不为空，则从中选取代价最高的点n
若点n为终点：
1. 从终点开始逐渐追踪parent节点，一直到起点
2. 返回结果路径，算法结束
若点n不为终点
1. 将点n从open_set中删除，放到close_set中
2. 遍历节点n的8邻域节点：
  1. 如果邻域节点m在close_set中，则直接调过
  2. 如果邻域节点m不在open_set中：
    1. 设置节点m的parent为节点n
    2. 计算节点m的cost代价
    3. 将节点m加入open_set中

示例代码¶

import sys
import time
import numpy as np
from matplotlib.patches import Rectangle

from point import Point

class AStar:
    def __init__(self, map):
        self.map=map
        # 待遍历的点
        self.open_set = []
        # 已经遍历的节点
        self.close_set = []

    # 计算距离起点的对角距离
    def BaseCost(self, p):
        x_dis = abs(p.x - self.startPoint.x)
        y_dis = abs(p.y - self.startPoint.y)
        # Distance to start point
        return x_dis + y_dis + (np.sqrt(2) - 2) * min(x_dis, y_dis)

    # 计算距离终点的对角距离
    def HeuristicCost(self, p):
        x_dis = abs(self.endPoint.x - p.x)
        y_dis = abs(self.endPoint.y - p.y)
        # Distance to end point
        return x_dis + y_dis + (np.sqrt(2) - 2) * min(x_dis, y_dis)

    # 总的代价
    def TotalCost(self, p):
        return self.BaseCost(p) + self.HeuristicCost(p)

    def IsValidPoint(self, x, y):
        if x < 0 or y < 0:
            return False
        if x >= self.map.size or y >= self.map.size:
            return False
        return not self.map.IsObstacle(x, y)

    def IsInPointList(self, p, point_list):
        for point in point_list:
            if point.x == p.x and point.y == p.y:
                return True
        return False

    def IsInOpenList(self, p):
        return self.IsInPointList(p, self.open_set)

    def IsInCloseList(self, p):
        return self.IsInPointList(p, self.close_set)

    def IsStartPoint(self, p):
        return p.x == self.startPoint.x and p.y == self.startPoint.y

    def IsEndPoint(self, p):
        return p.x == self.endPoint.x and p.y == self.endPoint.y

    def SaveImage(self, plt):
        millis = int(round(time.time() * 1000))
        filename = './' + str(millis) + '.png'
        plt.savefig(filename)
        #plt.pause(0.00001)
        pass

    def ProcessPoint(self, x, y, parent):
        # 若点是无效的点，则直接跳过
        if not self.IsValidPoint(x, y):
            return # Do nothing for invalid point

        # 判断点是否已经处理过了
        p = Point(x, y)
        if self.IsInCloseList(p):
            return # Do nothing for visited point

        # 若点不在待处理的点里面
        if not self.IsInOpenList(p):
            p.parent = parent
            p.cost = self.TotalCost(p)
            self.open_set.append(p)

            print('计算点： [', p.x, ',', p.y, ']', ', cost: ', p.cost)

    def SelectPointInOpenList(self):
        """
            找出代价最小的点
        """
        index = 0
        selected_index = -1
        min_cost = sys.maxsize
        for p in self.open_set:
            # cost = self.TotalCost(p)
            cost = p.cost
            if cost < min_cost:
                min_cost = cost
                selected_index = index
            index += 1
        return selected_index


    def BuildPath(self, p, ax, plt, start_time):
        """
        构建路径
        :param p:
        :param ax:
        :param plt:
        :param start_time:
        :return:
        """
        path = []
        # 根据parent反向将所有的点添加到路径中
        while True:
            # 始终将点插到队列的前面
            path.insert(0, p)
            if self.IsStartPoint(p):
                break
            else:
                p = p.parent

        # 找到的路径绘制出来
        for p in path:
            rec = Rectangle((p.x, p.y), 1, 1, color='g')
            ax.add_patch(rec)
            plt.draw()
            self.SaveImage(plt)

        end_time = time.time()
        print('算法执行完成，耗时：', int(end_time-start_time), ' 秒')

    def run(self, ax, plt,startPoint,endPoint):
        self.startPoint = startPoint
        self.endPoint = endPoint


        start_time = time.time()

        self.startPoint.cost = 0
        self.open_set.append(self.startPoint)

        while True:
            index = self.SelectPointInOpenList()
            if index < 0:
                print('没有找到路径，算法执行失败!!!')
                return
            # 取出点的信息
            p = self.open_set[index]
            # 在图上绘制出要处理的点的信息
            rec = Rectangle((p.x, p.y), 1, 1, color='pink')
            ax.add_patch(rec)
            self.SaveImage(plt)

            # 判断是否已经是终点，若为终点，则反向构建路径
            if self.IsEndPoint(p):
                return self.BuildPath(p, ax, plt, start_time)

            # 将已经处理的点从open_set中删除
            del self.open_set[index]
            # 将已处理的点存到close_set中
            self.close_set.append(p)

            # 处理8个方向相邻的点
            x = p.x
            y = p.y
            self.ProcessPoint(x-1, y+1, p)
            self.ProcessPoint(x-1, y, p)
            self.ProcessPoint(x-1, y-1, p)
            self.ProcessPoint(x, y-1, p)
            self.ProcessPoint(x+1, y-1, p)
            self.ProcessPoint(x+1, y, p)
            self.ProcessPoint(x+1, y+1, p)
            self.ProcessPoint(x, y+1, p)