• 算法 | A*算法和权重A* 算法


    A* and Weighted A* Search

    思路

    启发式搜索算法

    要理解A*搜寻算法,还得从启发式搜索算法开始谈起。

    所谓启发式搜索,就在于当前搜索结点往下选择下一步结点时,可以通过一个启发函数(Heuristic Function)来进行选择,选择代价最少的结点作为下一步搜索结点而跳转其上(遇到有一个以上代价最少的结点,不妨选距离当前搜索点最近一次展开的搜索点进行下一步搜索)。

    DFS和BFS在展开子结点时均属于盲目型搜索,也就是说,它不会选择哪个结点在下一次搜索中更优而去跳转到该结点进行下一步的搜索。在运气不好的情形中,均需要试探完整个解集空间, 显然,只能适用于问题规模不大的搜索问题中。

    而与DFS,BFS不同的是,一个经过仔细设计的启发函数,往往在很快的时间内就可得到一个搜索问题的最优解,对于NP问题,亦可在多项式时间内得到一个较优解。是的,关键就是如何设计这个启发函数。

    A*搜寻算法

    A*搜寻算法,俗称A星算法,作为启发式搜索算法中的一种,这是一种在图形平面上,有多个节点的路径,求出最低通过成本的算法。常用于游戏中的NPC的移动计算,或线上游戏的BOT的移动计算上。该算法像Dijkstra算法一样,可以找到一条最短路径;也像BFS一样,进行启发式的搜索。

    A*算法最为核心的部分,就在于它的一个估值函数的设计上:

    (f(n)=g(n)+h(n))

    其中(f(n))是每个可能试探点的估值,它有两部分组成:

    • 一部分,为g(n),它表示从起始搜索点到当前点的代价(通常用某结点在搜索树中的深度来表示)。
    • 另一部分,即h(n),它表示启发式搜索中最为重要的一部分,即当前结点到目标结点的估值,

    h(n)设计的好坏,直接影响着具有此种启发式函数的启发式算法的是否能称为A*算法。

    一种具有(f(n)=g(n)+h(n))策略的启发式算法能成为A*算法的充分条件是:
    1、搜索树上存在着从起始点到终了点的最优路径。
    2、问题域是有限的。
    3、所有结点的子结点的搜索代价值(>0)
    4、(h(n)=<h*(n))(h*(n))为实际问题的代价值)。

    当此四个条件都满足时,一个具有(f(n)=g(n)+h(n))策略的启发式算法能成为A*算法,并一定能找到最优解。

    对于一个搜索问题,显然,条件1,2,3都是很容易满足的,而条件4: (h(n)<=h*(n))是需要精心设计的,由于(h*(n))显然是无法知道的,所以,一个满足条件4的启发策略(h(n))就来的难能可贵了。

    不过,对于图的最优路径搜索和八数码问题,有些相关策略h(n)不仅很好理解,而且已经在理论上证明是满足条件4的,从而为这个算法的推广起到了决定性的作用。

    (h(n))距离(h*(n))的呈度不能过大,否则(h(n))就没有过强的区分能力,算法效率并不会很高。对一个好的(h(n))的评价是:(h(n))(h*(n))的下界之下,并且尽量接近(h*(n))

    示例程序

    """
    
    A* grid planning
    
    author: Atsushi Sakai(@Atsushi_twi)
            Nikos Kanargias (nkana@tee.gr)
    
    See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
    
    """
    
    import math
    
    import matplotlib.pyplot as plt
    
    show_animation = True
    
    
    class AStarPlanner:
    
        def __init__(self, ox, oy, reso, rr):
            """
            Initialize grid map for a star planning
    
            ox: x position list of Obstacles [m]
            oy: y position list of Obstacles [m]
            reso: grid resolution [m]
            rr: robot radius[m]
            """
    
            self.reso = reso
            self.rr = rr
            self.calc_obstacle_map(ox, oy)
            self.motion = self.get_motion_model()
    
        class Node:
            def __init__(self, x, y, cost, pind):
                self.x = x  # index of grid
                self.y = y  # index of grid
                self.cost = cost
                self.pind = pind
    
            def __str__(self):
                return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
    
        def planning(self, sx, sy, gx, gy):
            """
            A star path search
    
            input:
                sx: start x position [m]
                sy: start y position [m]
                gx: goal x position [m]
                gy: goal y position [m]
    
            output:
                rx: x position list of the final path
                ry: y position list of the final path
            """
    
            nstart = self.Node(self.calc_xyindex(sx, self.minx),
                               self.calc_xyindex(sy, self.miny), 0.0, -1)
            ngoal = self.Node(self.calc_xyindex(gx, self.minx),
                              self.calc_xyindex(gy, self.miny), 0.0, -1)
    
            open_set, closed_set = dict(), dict()
            open_set[self.calc_grid_index(nstart)] = nstart
    
            while 1:
                if len(open_set) == 0:
                    print("Open set is empty..")
                    break
    
                c_id = min(
                    open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, open_set[o]))
                current = open_set[c_id]
    
                # show graph
                if show_animation:  # pragma: no cover
                    plt.plot(self.calc_grid_position(current.x, self.minx),
                             self.calc_grid_position(current.y, self.miny), "xc")
                    # for stopping simulation with the esc key.
                    plt.gcf().canvas.mpl_connect('key_release_event',
                            lambda event: [exit(0) if event.key == 'escape' else None])
                    if len(closed_set.keys()) % 10 == 0:
                        plt.pause(0.001)
    
                if current.x == ngoal.x and current.y == ngoal.y:
                    print("Find goal")
                    ngoal.pind = current.pind
                    ngoal.cost = current.cost
                    break
    
                # Remove the item from the open set
                del open_set[c_id]
    
                # Add it to the closed set
                closed_set[c_id] = current
    
                # expand_grid search grid based on motion model
                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], c_id)
                    n_id = self.calc_grid_index(node)
    
    
                    # If the node is not safe, do nothing
                    if not self.verify_node(node):
                        continue
    
                    if n_id in closed_set:
                        continue
    
                    if n_id not in open_set:
                        open_set[n_id] = node  # discovered a new node
                    else:
                        if open_set[n_id].cost > node.cost:
                            # This path is the best until now. record it
                            open_set[n_id] = node
    
            rx, ry = self.calc_final_path(ngoal, closed_set)
    
            return rx, ry
    
        def calc_final_path(self, ngoal, closedset):
            # generate final course
            rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
                self.calc_grid_position(ngoal.y, self.miny)]
            pind = ngoal.pind
            while pind != -1:
                n = closedset[pind]
                rx.append(self.calc_grid_position(n.x, self.minx))
                ry.append(self.calc_grid_position(n.y, self.miny))
                pind = n.pind
    
            return rx, ry
    
        @staticmethod
        def calc_heuristic(n1, n2):
            w = 1.0  # weight of heuristic
            d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
            return d
    
        def calc_grid_position(self, index, minp):
            """
            calc grid position
    
            :param index:
            :param minp:
            :return:
            """
            pos = index * self.reso + minp
            return pos
    
        def calc_xyindex(self, position, min_pos):
            return round((position - min_pos) / self.reso)
    
        def calc_grid_index(self, node):
            return (node.y - self.miny) * self.xwidth + (node.x - self.minx)
    
        def verify_node(self, node):
            px = self.calc_grid_position(node.x, self.minx)
            py = self.calc_grid_position(node.y, self.miny)
    
            if px < self.minx:
                return False
            elif py < self.miny:
                return False
            elif px >= self.maxx:
                return False
            elif py >= self.maxy:
                return False
    
            # collision check
            if self.obmap[node.x][node.y]:
                return False
    
            return True
    
        def calc_obstacle_map(self, ox, oy):
    
            self.minx = round(min(ox))
            self.miny = round(min(oy))
            self.maxx = round(max(ox))
            self.maxy = round(max(oy))
            print("minx:", self.minx)
            print("miny:", self.miny)
            print("maxx:", self.maxx)
            print("maxy:", self.maxy)
    
            self.xwidth = round((self.maxx - self.minx) / self.reso)
            self.ywidth = round((self.maxy - self.miny) / self.reso)
            print("x", self.xwidth)
            print("y", self.ywidth)
    
            # obstacle map generation
            self.obmap = [[False for i in range(self.ywidth)]
                          for i in range(self.xwidth)]
            for ix in range(self.xwidth):
                x = self.calc_grid_position(ix, self.minx)
                for iy in range(self.ywidth):
                    y = self.calc_grid_position(iy, self.miny)
                    for iox, ioy in zip(ox, oy):
                        d = math.hypot(iox - x, ioy - y)
                        if d <= self.rr:
                            self.obmap[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 = 10.0  # [m]
        gx = 50.0  # [m]
        gy = 50.0  # [m]
        grid_size = 2.0  # [m]
        robot_radius = 1.0  # [m]
    
        # set obstable positions
        ox, oy = [], []
        for i in range(-10, 60):
            ox.append(i)
            oy.append(-10.0)
        for i in range(-10, 60):
            ox.append(60.0)
            oy.append(i)
        for i in range(-10, 61):
            ox.append(i)
            oy.append(60.0)
        for i in range(-10, 61):
            ox.append(-10.0)
            oy.append(i)
        for i in range(-10, 40):
            ox.append(20.0)
            oy.append(i)
        for i in range(0, 40):
            ox.append(40.0)
            oy.append(60.0 - i)
    
        if show_animation:  # pragma: no cover
            plt.plot(ox, oy, ".k")
            plt.plot(sx, sy, "og")
            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()
    

    核心代码

        def planning(self, sx, sy, gx, gy):
            """
            A star path search
    
            input:
                sx: start x position [m]
                sy: start y position [m]
                gx: goal x position [m]
                gy: goal y position [m]
    
            output:
                rx: x position list of the final path
                ry: y position list of the final path
            """
    
            nstart = self.Node(self.calc_xyindex(sx, self.minx),
                               self.calc_xyindex(sy, self.miny), 0.0, -1)
            ngoal = self.Node(self.calc_xyindex(gx, self.minx),
                              self.calc_xyindex(gy, self.miny), 0.0, -1)
    
            open_set, closed_set = dict(), dict()
            open_set[self.calc_grid_index(nstart)] = nstart
    
            while 1:
                if len(open_set) == 0:
                    print("Open set is empty..")
                    break
    
                c_id = min(
                    open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, open_set[o]))
                current = open_set[c_id]
    
                # show graph
                if show_animation:  # pragma: no cover
                    plt.plot(self.calc_grid_position(current.x, self.minx),
                             self.calc_grid_position(current.y, self.miny), "xc")
                    # for stopping simulation with the esc key.
                    plt.gcf().canvas.mpl_connect('key_release_event',
                            lambda event: [exit(0) if event.key == 'escape' else None])
                    if len(closed_set.keys()) % 10 == 0:
                        plt.pause(0.001)
    
                if current.x == ngoal.x and current.y == ngoal.y:
                    print("Find goal")
                    ngoal.pind = current.pind
                    ngoal.cost = current.cost
                    break
    
                # Remove the item from the open set
                del open_set[c_id]
    
                # Add it to the closed set
                closed_set[c_id] = current
    
                # expand_grid search grid based on motion model
                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], c_id)
                    n_id = self.calc_grid_index(node)
    
    
                    # If the node is not safe, do nothing
                    if not self.verify_node(node):
                        continue
    
                    if n_id in closed_set:
                        continue
    
                    if n_id not in open_set:
                        open_set[n_id] = node  # discovered a new node
                    else:
                        if open_set[n_id].cost > node.cost:
                            # This path is the best until now. record it
                            open_set[n_id] = node
    
            rx, ry = self.calc_final_path(ngoal, closed_set)
    
            return rx, ry
    

    其中,
    c_id = min(open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, open_set[o])) current = open_set[c_id]
    把min函数的比较函数换成open_set[o].cost + self.calc_heuristic(ngoal, open_set[o]),也即(f(n)=g(n)+h(n)),通过这两行,找到在open_set中cost与heuristic值最小的节点。比如,在常见的机器人搜索路径问题中,cost指的是从起点到该节点走过的距离,heuristic是指从该节点到达终点的距离。那么很好理解,每一步都能选取到一个最佳路径点。

    参考

  • 相关阅读:
    三菱Q系列PLC MC协议通讯
    相机常用属性配置简介[转]---Labview IMAQ 修改相机曝光等参数的方法
    数码显微镜的实际放大倍数的正确计算方法【转载】
    VS2012 C# 配置log4net
    CHM格式帮助文档无法打开的问题
    win10 下安装win7虚拟机
    杂记:使用RawCap和Wireshark对 127.0.0.1或localhost 进行抓包
    杂记:01
    linux应用编程一:文件IO和目录操作
    QTableWidget常用函数及注意事项
  • 原文地址:https://www.cnblogs.com/casperwin/p/12495816.html
Copyright © 2020-2023  润新知