编程语言
首页 > 编程语言> > A*算法(三)算法实现

A*算法(三)算法实现

作者:互联网

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 ,父结点fatherg值,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

定义函数来获得openlistF值最小的结点

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* 算法(一)算法导言


谢谢!

标签:结点,point,实现,self,minF,goalPoint,算法,map2d
来源: https://blog.csdn.net/qq_32618327/article/details/100112075