A*算法（三）算法实现

编程语言 2019-09-07 08:52:08 阅读次数: 0
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接： https://blog.csdn.net/qq_32618327/article/details/100112075

A*算法（三）算法实现

1. Array2D类

通用类Array2D用于描述地图的宽和高，并且存储地图的数据

class Array2D:
    """
        1.构造方法需要两个参数，即二维数组的宽和高
        2.成员变量w和h是二维数组的宽和高
        3.使用：‘对象[x][y]’可以直接取到相应的值
        4.数组的默认值都是0
    """

    def __init__(self, w, h):
        self.w = w  # 地图的宽
        self.h = h  # 地图的高
        self.data = []  # 地图的存储数据
        self.data = [[0 for y in range(h)] for x in range(w)]  # 数据初始化赋0

    def __getitem__(self, item):
        return self.data[item]  # 设置获取所存储的数据

2. Point类

通用类Point用于描述地图结点的坐标
并且重载等号运算符，可以判断两个Point坐标是否相等
最后设置了打印时的格式

class Point:
    """
    表示一个结点
    """

    def __init__(self, x, y):
        self.x = x  # 结点的x坐标
        self.y = y  # 结点的y坐标

    def __eq__(self, other):  # 判断引用是否相等
        if self.x == other.x and self.y == other.y:
            return True
        return False

    def __str__(self):  # 定义打印时的格式
        return "x:" + str(self.x) + ",y:" + str(self.y)

3. AStar类

AStar算法，启发式函数默认曼哈顿距离

class AStar:
    # 描述AStar算法中的结点数据
    class Node:
        def __init__(self, point, goalPoint, g=0, hef='MD'):
            self.point = point  # 自己的坐标
            self.father = None  # 父结点
            self.g = g  # g值，当前已产生的代价
            self.D = 10  # D倍
            # h值，未来可能产生的代价
            if hef == 'DD':  # 对角线距离
                D2 = np.sqrt(2) * self.D
                h_diagonal = min(abs(point.x - goalPoint.x), abs(point.y - goalPoint.y))
                h_straight = (abs(point.x - goalPoint.x) + abs(point.y - goalPoint.y))
                self.h = D2 * h_diagonal + self.D * (h_straight - 2 * h_diagonal)
            elif hef == 'ED':  # 欧几里得距离
                self.h = np.sqrt(pow(point.x - goalPoint.x, 2) + pow(point.y - goalPoint.y, 2))
            else:  # 曼哈顿距离
                self.h = (abs(point.x - goalPoint.x) + abs(point.y - goalPoint.y)) * self.D

    def __init__(self, map2d, startPoint, goalPoint, passTag=0, hef='MD'):
        # 启发式函数
        if hef != 'MD' and hef != 'DD' and hef != 'ED':
            hef = 'MD'
            print("启发式函数输入有误，应为MD DD ED\n默认设置曼哈顿距离")
        self.hef = hef
        # 开启表，保存已产生而未访问的结点
        self.openList = []
        # 关闭表，保存已访问过的结点
        self.closeList = []
        # 寻路地图
        self.map2d = map2d
        # 起点终点
        if isinstance(startPoint, Point) and isinstance(goalPoint, Point):
            self.startPoint = startPoint
            self.goalPoint = goalPoint
        else:
            self.startPoint = Point(*startPoint)
            self.goalPoint = Point(*goalPoint)

        # 可行走标记
        self.passTag = passTag

    def getMinNode(self):
        currentNode = self.openList[0]
        for node in self.openList:
            if node.g + node.h < currentNode.g + currentNode.h:
                currentNode = node
        return currentNode

    def pointInCloseList(self, point):
        for node in self.closeList:
            if node.point == point:
                return True
        return False

    def pointInOpenList(self, point):
        for node in self.openList:
            if node.point == point:
                return node
        return None

    def goalPointeInCloseList(self):
        for node in self.closeList:
            if node.point == self.goalPoint:
                return node
        return None

    def searchNear(self, minF, offsetX, offsetY):
        # 越界检测
        if minF.point.x + offsetX < 0 or minF.point.x + offsetX > self.map2d.w - 1 or \
                minF.point.y + offsetY < 0 or minF.point.y + offsetY > self.map2d.h - 1:
            return
        # 如果是障碍，就忽略
        if self.map2d[minF.point.x + offsetX][minF.point.y + offsetY] != self.passTag:
            return
        # 如果在关闭表中，就忽略
        currentPoint = Point(minF.point.x + offsetX, minF.point.y + offsetY)
        if self.pointInCloseList(currentPoint):
            return
        # 设置单位代价
        if offsetX == 0 or offsetY == 0:
            step = 10
        else:
            step = 14
        # 如果不再openList中，就把它加入openlist
        currentNode = self.pointInOpenList(currentPoint)
        if not currentNode:
            currentNode = AStar.Node(currentPoint, self.goalPoint, g=minF.g + step, hef=self.hef)
            currentNode.father = minF
            self.openList.append(currentNode)
            return
        # 在openList中，判断minF到当前点的g值是否更小
        if minF.g + step < currentNode.g:  # 如果更小，就重新计算g值，并且改变father
            currentNode.g = minF.g + step
            currentNode.father = minF

    def start(self):
        # 判断起始点是否是障碍
        if self.map2d[self.startPoint.x][self.startPoint.y] != self.passTag:
            return None

        # 判断目标点是否是障碍
        if self.map2d[self.goalPoint.x][self.goalPoint.y] != self.passTag:
            return None

        # 1.将起点放入开启列表
        startNode = AStar.Node(self.startPoint, self.goalPoint, hef=self.hef)
        self.openList.append(startNode)
        # 2.主循环逻辑
        while True:
            # 找到F值最小的点
            minF = self.getMinNode()
            # 把这个点加入closeList中，并且在openList中删除它
            self.closeList.append(minF)
            self.openList.remove(minF)
            # 判断这个目标点的上下左右结点，默认不允许对角运动
            self.searchNear(minF, 0, -1)
            self.searchNear(minF, 0, 1)
            self.searchNear(minF, -1, 0)
            self.searchNear(minF, 1, 0)
            # 若启发式函数非曼哈顿距离，允许对角运动
            if self.hef != 'MD':
                self.searchNear(minF, 1, 1)
                self.searchNear(minF, 1, -1)
                self.searchNear(minF, -1, 1)
                self.searchNear(minF, -1, -1)
            # 判断是否终止
            point = self.goalPointeInCloseList()
            if point:  # 如果终点在关闭表中，就返回结果
                cPoint = point
                pathList = []
                while True:
                    if cPoint.father:
                        pathList.append(cPoint.point)
                        cPoint = cPoint.father
                    else:
                        return list(reversed(pathList))
            if len(self.openList) == 0:
                return None

下面来详细了解代码：

3.1 Node类

首先创建一个类Node，包含自己的坐标point ，父结点father ，g值，h值
不同的启发式函数，对应不同的h值运算
默认MD：曼哈顿距离，DD：对角线距离，ED：欧几里得距离

class Node:
    def __init__(self, point, goalPoint, g=0, hef='MD'):
        self.point = point  # 自己的坐标
        self.father = None  # 父结点
        self.g = g  # g值，当前已产生的代价
        self.D = 10  # D倍
        # h值，未来可能产生的代价
        if hef == 'DD':  # 对角线距离
            D2 = np.sqrt(2) * self.D
            h_diagonal = min(abs(point.x - goalPoint.x), abs(point.y - goalPoint.y))
            h_straight = (abs(point.x - goalPoint.x) + abs(point.y - goalPoint.y))
            self.h = D2 * h_diagonal + self.D * (h_straight - 2 * h_diagonal)
        elif hef == 'ED':  # 欧几里得距离
            self.h = np.sqrt(pow(point.x - goalPoint.x, 2) + pow(point.y - goalPoint.y, 2))
        else:  # 曼哈顿距离
            self.h = (abs(point.x - goalPoint.x) + abs(point.y - goalPoint.y)) * self.D

3.2 初始化处理

然后初始化处理
先确保正确的启动式函数hef
然后创建开启表openList ，保存已产生而未访问的结点
然后创建关闭表closeList ，保存已访问过的结点
初始化寻路地图map2d、起点startPoint、终点goalPoint、可行走标记passTag

def __init__(self, map2d, startPoint, goalPoint, passTag=0, hef='MD'):
    # 启发式函数
    if hef != 'MD' and hef != 'DD' and hef != 'ED':
        hef = 'MD'
        print("启发式函数输入有误，应为MD DD ED\n默认设置曼哈顿距离")
    self.hef = hef
    # 开启表，保存已产生而未访问的结点
    self.openList = []
    # 关闭表，保存已访问过的结点
    self.closeList = []
    # 寻路地图
    self.map2d = map2d
    # 起点终点
    if isinstance(startPoint, Point) and isinstance(goalPoint, Point):
        self.startPoint = startPoint
        self.goalPoint = goalPoint
    else:
        self.startPoint = Point(*startPoint)
        self.goalPoint = Point(*goalPoint)

    # 可行走标记
    self.passTag = passTag

定义函数来获得openlist中F值最小的结点

def getMinNode(self):
    currentNode = self.openList[0]
    for node in self.openList:
        if node.g + node.h < currentNode.g + currentNode.h:
            currentNode = node
    return currentNode

3.3 判断函数

定义一些用于判断的函数，从字面上也比较容易理解
pointInCloseList ：判断结点是否在CloseList，也就是判断节点是否已经访问过
pointInOpenList ：判断结点是否在OpenList，也就是判断节点是否产生，且未访问过
goalPointeInCloseList：判断目标点是否在CloseList，也就是判断目标点是否已经访问过，如果存在则返回目标结点

def pointInCloseList(self, point):
    for node in self.closeList:
        if node.point == point:
            return True
    return False


def pointInOpenList(self, point):
    for node in self.openList:
        if node.point == point:
            return node
    return None


def goalPointeInCloseList(self):
    for node in self.closeList:
        if node.point == self.goalPoint:
            return node
    return None

3.4 搜索结点周围的点

创建函数用于搜索结点周围的点

def searchNear(self, minF, offsetX, offsetY):
    # 越界检测
    if minF.point.x + offsetX < 0 or minF.point.x + offsetX > self.map2d.w - 1 or \
            minF.point.y + offsetY < 0 or minF.point.y + offsetY > self.map2d.h - 1:
        return
    # 如果是障碍，就忽略
    if self.map2d[minF.point.x + offsetX][minF.point.y + offsetY] != self.passTag:
        return
    # 如果在关闭表中，就忽略
    currentPoint = Point(minF.point.x + offsetX, minF.point.y + offsetY)
    if self.pointInCloseList(currentPoint):
        return
    # 设置单位代价
    if offsetX == 0 or offsetY == 0:
        step = 10
    else:
        step = 14
    # 如果不再openList中，就把它加入openlist
    currentNode = self.pointInOpenList(currentPoint)
    if not currentNode:
        currentNode = AStar.Node(currentPoint, self.goalPoint, g=minF.g + step, hef=self.hef)
        currentNode.father = minF
        self.openList.append(currentNode)
        return
    # 在openList中，判断minF到当前点的g值是否更小
    if minF.g + step < currentNode.g:  # 如果更小，就重新计算g值，并且改变father
        currentNode.g = minF.g + step
        currentNode.father = minF

值得注意的是：
因为搜索有先后循序之分，所以可能存在同一批结点拓散搜索周围结点时
而周围结点的父结点可能不一定是距离最近的结点
如果该结点恰好为寻路路径，则可能会造成多余的路径存在

所以，如果周围的点已经存在openlist，需要判断minF到当前点的g值是否更小
如果更小，就重新计算g值，并且改变父结点father
这样就 消除了之前结点存在非最小g值的父结点隐患，从而避免 生成多余的路径
举个简单的例子：
在这里插入图片描述

7 逆时针顺序搜索到8、5、4，假如5的f值最小，逆时针顺序搜索到9、6、3、2、1
如果 5 方向道路存在障碍切换到 4 作为路径，逆时针顺序搜索到 2、1等
1、2就已经存在openlist中
重新搜索时 1 的父结点就需要改变成 4，因为 1 离 4 结点距离更短
而 2 仍保留 5 作为父结点，因为 2 离 5 结点距离更短

3.5 寻路

创建函数用于寻路
开始是判断起始点startPoint和目标点goalPoint是否合理
然后将起点startPoint放入开启列表openList
接着开始主循环逻辑：
1、在openlist中找到F值最小的点
2、把这个点加入closeList中，并且在openList中删除它
3、搜索的周围结点，默认4个，允许对角运动时，为8个
4、如果目标点在关闭表中，就中止循环返回结果，否则返回循环起点

def start(self):
    # 判断起始点是否是障碍
    if self.map2d[self.startPoint.x][self.startPoint.y] != self.passTag:
        return None

    # 判断目标点是否是障碍
    if self.map2d[self.goalPoint.x][self.goalPoint.y] != self.passTag:
        return None

    # 1.将起点放入开启列表
    startNode = AStar.Node(self.startPoint, self.goalPoint, hef=self.hef)
    self.openList.append(startNode)
    # 2.主循环逻辑
    while True:
        # 找到F值最小的点
        minF = self.getMinNode()
        # 把这个点加入closeList中，并且在openList中删除它
        self.closeList.append(minF)
        self.openList.remove(minF)
        # 判断这个结点的上下左右结点，默认不允许对角运动
        self.searchNear(minF, 0, -1)
        self.searchNear(minF, 0, 1)
        self.searchNear(minF, -1, 0)
        self.searchNear(minF, 1, 0)
        # 若启发式函数非曼哈顿距离，允许对角运动
        if self.hef != 'MD':
            self.searchNear(minF, 1, 1)
            self.searchNear(minF, 1, -1)
            self.searchNear(minF, -1, 1)
            self.searchNear(minF, -1, -1)
        # 判断是否终止
        point = self.goalPointeInCloseList()
        if point:  # 如果目标点在关闭表中，就返回结果
            cPoint = point
            pathList = []
            while True:
                if cPoint.father:
                    pathList.append(cPoint.point)
                    cPoint = cPoint.father
                else:
                    return list(reversed(pathList))
        if len(self.openList) == 0:
            return None

下面是简约的流程图：
在这里插入图片描述

4. 地图显示

用于显示在地图上A*算法计算的轨迹

def Display_map(map, start=None, goal=None, title=None):
    plt.rcParams['font.sans-serif'] = ['SimHei']  # 设置正常显示中文
    plt.xlim(- 1, map.w)
    plt.ylim(- 1, map.h)
    plt.xticks(np.arange(0, map.w, 1))
    plt.yticks(np.arange(0, map.h, 1))
    plt.grid(lw=2)
    obstaclesX, obstaclesY = [], []
    pathx, pathy = [], []
    for x in range(map.w):
        for y in range(map.h):
            if map[x][y] == 1:
                obstaclesX.append(x)
                obstaclesY.append(y)
            elif map[x][y] == 'o':
                pathx.append(x)
                pathy.append(y)
    if obstaclesX != []:
        plt.plot(obstaclesX, obstaclesY, 'xr', markersize=10, label='障碍')
    if pathx != []:
        plt.plot(pathx, pathy, 'og', markersize=10, label='路径')
    if start != None:
        plt.plot(start[0], start[1], 'or', markersize=10, label='起始')
    if goal != None:
        plt.plot(goal[0], goal[1], 'ob', markersize=10, label='目标')
    if title != None:
        plt.title(title)  # 设置标题
    plt.legend()  # 设置图例
    plt.show()

5. 计算测试

if __name__ == '__main__':
    # 创建一个10*10的地图
    mapw, maph = 10, 10
    map2d = Array2D(mapw, maph)

    # 设置障碍
    obstacle = [[4, 9], [4, 8], [4, 7], [4, 6], [4, 5]]
    for i in obstacle:
        map2d[i[0]][i[1]] = 1
    # 显示地图设置障碍后的样子
    Display_map(map2d, title="设置障碍")

    # 设置起点，终点
    startx, starty = 0, 0
    goalx, goaly = 9, 8
    # 显示地图设置起点和终点后的样子
    Display_map(map2d, [startx, starty], [goalx, goaly], title="规划准备")
    
    # 创建AStar对象
    aStar = AStar(map2d, Point(startx, starty), Point(goalx, goaly), hef='MD')
    # 开始寻路
    pathList = aStar.start()

    # 遍历路径点,在map2d上以'o'表示
    for point in pathList:
        map2d[point.x][point.y] = 'o'
    # 再次显示地图
    Display_map(map2d, [startx, starty], [goalx, goaly], title="轨迹规划")

下面来详细了解代码：

5.1 创建地图

创建一个10*10的地图

mapw, maph = 10, 10
map2d = Array2D(mapw, maph)

5.2 设置障碍

在 [4, 9], [4, 8], [4, 7], [4, 6], [4, 5] 结点设置障碍
然后显示此时设置障碍后的地图

obstacle = [[4, 9], [4, 8], [4, 7], [4, 6], [4, 5]]
for i in obstacle:
   map2d[i[0]][i[1]] = 1
Display_map(map2d, title="设置障碍")

在这里插入图片描述

5.3 设置起点和终点

设置起点（0, 0）和终点（9, 8）
然后显示此时设置起点和终点后的地图

startx, starty = 0, 0
goalx, goaly = 9, 8
Display_map(map2d, [startx, starty], [goalx, goaly], title="规划准备")

在这里插入图片描述

5.4 曼哈顿距离

启动式函数采用曼哈顿距离，开始寻路

# 创建AStar对象
aStar = AStar(map2d, Point(startx, starty), Point(goalx, goaly), hef='MD')
# 开始寻路
pathList = aStar.start()

# 遍历路径点,在map2d上以'o'表示
for point in pathList:
    map2d[point.x][point.y] = 'o'
# 再次显示地图
Display_map(map2d, [startx, starty], [goalx, goaly], title="曼哈顿距离轨迹规划")

在这里插入图片描述

5.5 对角线距离

启动式函数采用对角线距离，开始寻路

# 创建AStar对象
aStar = AStar(map2d, Point(startx, starty), Point(goalx, goaly), hef='DD')
# 开始寻路
pathList = aStar.start()

# 遍历路径点,在map2d上以'o'表示
for point in pathList:
    map2d[point.x][point.y] = 'o'
# 再次显示地图
Display_map(map2d, [startx, starty], [goalx, goaly], title="对角线距离轨迹规划")

在这里插入图片描述

5.6 欧几里得距离

启动式函数采用欧几里得距离，开始寻路

# 创建AStar对象
aStar = AStar(map2d, Point(startx, starty), Point(goalx, goaly), hef='ED')
# 开始寻路
pathList = aStar.start()

# 遍历路径点,在map2d上以'o'表示
for point in pathList:
    map2d[point.x][point.y] = 'o'
# 再次显示地图
Display_map(map2d, [startx, starty], [goalx, goaly], title="欧几里得距离轨迹规划")

在这里插入图片描述

[1] python的代码地址:
https://github.com/JoveH-H/A-simple-explanation/blob/master/A%20star.py
[2] jupyter notebook的代码地址:
https://github.com/JoveH-H/A-simple-explanation/blob/master/ipynb/A%20star.ipynb

相关推荐：
A* 算法（二）启发式算法
A* 算法（一）算法导言

谢谢！

猜你喜欢

转载自blog.csdn.net/qq_32618327/article/details/100112075
今日推荐
周排行
Leetcode简单题61~80
解决zookeeper磁盘IO高的问题
多线程相关方法详解
Maven-setting.xml文件详解
Maven 项目的 classpath 理解
渊亭科技大数据笔试题
配置JVM内存分配
计算机网络个人学习笔记 （三）网络层 ：第三部分 连载
js中两个等号(==)和三个等号(===)的区别
用C程序自动打开电脑上的程序
每日归档
更多
2024-09-18(0)
2024-09-17(0)
2024-09-16(0)
2024-09-15(0)
2024-09-14(0)
2024-09-13(0)
2024-09-12(0)
2024-09-11(0)
2024-09-10(0)
2024-09-09(0)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022