A*算法。

一、A*算法原理

代码来自CHH3213，注释仅个人理解

import math
import matplotlib.pyplot as plt
show_animation=True

class AStarPlanner:
    def __init__(self,ox,oy,resolution,rr):
        self.resolution=resolution
        self.rr=rr
        self.min_x,self.min_y=0,0
        self.max_x,self.max_y=0,0
        self.obstacle_map=None
        self.x_width,self.y_width=0,0
        self.motion=self.get_motion_model()
        self.calc_obstacle_map(ox,oy)

    class Node:
        # 定义搜索区域节点类, 每个Node都包含坐标x和y, 移动代价cost和父节点索引。
        def __init__(self,x,y,cost,parent_index):
            self.x=x
            self.y=y
            self.cost=cost  #cost代表g(n)值
            self.parent_index=parent_index

        def __str__(self):
            return str(self.x) +","+str(self.y)+","+str(
                self.cost)+","+str(self.parent_index)

    def planning(self,sx,sy,gx,gy):
        #起始点（sx,sy)  目标点（gx,gy) 全部转换为栅格坐标
        start_node=self.Node(self.calc_xy_index(sx,self.min_x),
                             self.calc_xy_index(sy,self.min_y),0.0,-1)

        goal_node=self.Node(self.calc_xy_index(gx,self.min_x),
                            self.calc_xy_index(gy,self.min_y),0.0,-1)

        open_set,closed_set=dict(),dict() #未探索，已探索
        open_set[self.calc_grid_index(start_node)] =start_node

        while 1 :
            if open_set == 0:
                print("open_set is empty")
                break
            #找出代价f(n)最小的节点作为当前父节点
            c_id =min (
                open_set,
                key=lambda o:open_set[o].cost+self.calc_heuristic(goal_node,open_set[o]))
            current=open_set[c_id]  #更新当前节点

            if show_animation:
                plt.plot(self.calc_grid_position(current.x,self.min_x),
                         self.calc_grid_position(current.y,self.min_y),"xc")
                #按下 'escape' 键时，程序会退出
                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 ==goal_node.x and current.y ==goal_node.y:
                print("final goal")
                goal_node.parent_index=current.parent_index
                goal_node.cost=current.cost
                break

            del open_set[c_id]
            # 将当前已搜索的节点放入closed
            closed_set[c_id]=current

            #搜索当前节点周围8个节点的代价,更新open_set，更新g值
            #B C D
            #E A F
            #G H I
            #如从A到I的直接距离为14，从A到F到I的距离为24>14，那么不更新A到I的g值
            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 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
                else:
                    #更新g值
                    if open_set[n_id].cost > node.cost:
                        open_set[n_id] = node

        rx,ry=self.calc_final_path(goal_node,closed_set)

        return rx,ry

    #等到上面更新目标节点的父节点信息后函数工作，从目标节点回溯路径到开始节点
    def calc_final_path(self,goal_node,closed_set):
        rx,ry=[self.calc_grid_position(goal_node.x,self.min_x)],[
            self.calc_grid_position(goal_node.y,self.min_y)]
        parent_index=goal_node.parent_index
        while parent_index !=-1:
            n=closed_set[parent_index]
            rx.append(self.calc_grid_position(n.x,self.min_x))
            ry.append(self.calc_grid_position(n.y,self.min_y))
            parent_index=n.parent_index

        return rx,ry


    @staticmethod
    def calc_heuristic(n1,n2):  #计算启发函数h(n)估计代价
        w=1.0
        # 为了符合A*算法最优搜索路径，这里h要换成欧式距离
        d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
        # d=w*(abs(n1.x-n2.x)+abs(n1.y-n2.y))
        return d



    def calc_grid_position(self,index,min_position): #将栅格坐标转换为地图坐标
        pos=index*self.resolution+min_position
        return pos

    def calc_xy_index(self,position,min_pos):#将地图坐标转换为栅格坐标
        return round((position-min_pos)/self.resolution)

    def verify_node(self,node):
        px=self.calc_grid_position(node.x,self.min_x)
        py=self.calc_grid_position(node.y,self.min_y)
        if px=self.max_x:
            return False
        elif py>=self.max_y:
            return False

        if self.obstacle_map[node.x][node.y]:
            return False

        return True


    def calc_grid_index(self,node): #计算改节点在栅格地图中的索引值（数值），作为唯一标识
        return (node.y-self.min_y)*self.x_width+(node.x-self.min_x)

    def calc_obstacle_map(self, ox, oy):
#将给定的障碍物坐标转换为障碍物地图
        self.min_x = round(min(ox))
        self.min_y = round(min(oy))
        self.max_x = round(max(ox))
        self.max_y = round(max(oy))
        print("min_x:", self.min_x)
        print("min_y:", self.min_y)
        print("max_x:", self.max_x)
        print("max_y:", self.max_y)

        self.x_width = round((self.max_x - self.min_x) / self.resolution)
        self.y_width = round((self.max_y - self.min_y) / self.resolution)
        print("x_width:", self.x_width)
        print("y_width:", self.y_width)

        # obstacle map generation
        self.obstacle_map = [[False for _ in range(self.y_width)]
                             for _ in range(self.x_width)]
        for ix in range(self.x_width):
            x = self.calc_grid_position(ix, self.min_x)
            for iy in range(self.y_width):
                y = self.calc_grid_position(iy, self.min_y)
                for iox, ioy in zip(ox, oy):
                    d = math.hypot(iox - x, ioy - y)
                    if d <= self.rr:
                        self.obstacle_map[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 obstacle 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()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239

二、A*算法在ros里面的运用

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

    def __init__(self, x, y):
        self.x = x;
        self.y = y

    def __eq__(self, other):
        if self.x == other.x and self.y == other.y:
            return True
        return False

class AStar:
    """
    AStar算法的Python3.x实现
    """

    class Node:  # 描述AStar算法中的节点数据
        def __init__(self, point, endPoint, g=0):
            self.point = point  # 自己的坐标
            self.father = None  # 父节点
            self.g = g  # g值，g值在用到的时候会重新算
            self.h = (abs(endPoint.x - point.x) + abs(endPoint.y - point.y)) * 10  # 计算h值

    def __init__(self, map2d, startPoint, endPoint, passTag=0):
        """
        构造AStar算法的启动条件
        :param map2d: ArrayD类型的寻路数组
        :param startPoint: Point或二元组类型的寻路起点
        :param endPoint: Point或二元组类型的寻路终点
        :param passTag: int类型的可行走标记（若地图数据!=passTag即为障碍）
        """
        # 开启表
        self.openList = []
        # 关闭表
        self.closeList = []
        # 寻路地图
        self.map2d = map2d
        # 起点终点
        if isinstance(startPoint, Point) and isinstance(endPoint, Point):
            self.startPoint = startPoint
            self.endPoint = endPoint
        else:
            self.startPoint = Point(*startPoint)
            self.endPoint = Point(*endPoint)

        # 可行走标记
        self.passTag = passTag

    def getMinNode(self):
        """
        获得openlist中F值最小的节点
        :return: Node
        """
        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 endPointInCloseList(self):
        for node in self.openList:
            if node.point == self.endPoint:
                return node
        return None

    def searchNear(self, minF, offsetX, offsetY):
        """
        搜索节点周围的点
        :param minF:F值最小的节点
        :param offsetX:坐标偏移量
        :param offsetY:
        :return:
        """
        # 越界检测
        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.endPoint, g=minF.g + step)
            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):
        """
        开始寻路
        :return: None或Point列表（路径）
        """
        # 判断寻路终点是否是障碍
        if self.map2d[self.endPoint.x][self.endPoint.y] != self.passTag:
            return None

        # 1.将起点放入开启列表
        startNode = AStar.Node(self.startPoint, self.endPoint)
        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)
            # 判断是否终止
            point = self.endPointInCloseList()
            if point:  # 如果终点在关闭表中，就返回结果
                # print("关闭表中")
                cPoint = point
                pathList = []
                while True:
                    if cPoint.father:
                        # pathList.append(cPoint.point)
                        pathList.append([cPoint.point.y, cPoint.point.x])
                        cPoint = cPoint.father
                    else:
                        #倒置列表，得到从起点到终点的路径
                        return list(reversed(pathList))
            if len(self.openList) == 0:
                return None


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160