一、算法原理

Astar算法流程圖如下:

?Astar算法是一種圖形搜索算法，常用于路徑規劃。它是個以廣度優先搜索為基礎，集Dijkstra算法與最佳優先(best fit)算法特點于一身的一種算法。

Astar算法從起點開始向周圍擴張，通過估計函數不斷計算周圍相鄰點的cost，并選擇cost最小的節點作為下一次進行擴張的節點，按此規則一直循環直到搜索到終點。

其中每個擴展節點對應的估值函數記為：

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

g(n) 表示當前節點的實際代價函數；

h(n) 表示當前節點至目標節點的估值函數，常用的估值方法有歐式距離、曼哈頓距離等；

二、代碼實現及注釋

# init astar plannet resolution = grid_size rr = robot_radius min_x, min_y = 0, 0 max_x, max_y = 0, 0 obstacle_map = None x_width, y_width = 0, 0 motion = get_motion_model()### calc_obstacle_map min_x = round(min(ox)) min_y = round(min(oy)) max_x = round(max(ox)) max_y = round(max(oy)) # cal map width x_width = round((max_x - min_x) / resolution) y_width = round((max_y - min_y) / resolution) # generate map with obstacle obstacle_map = [[False for _ in range(y_width)]for _ in range(x_width)] for ix in range(x_width):x = calc_grid_position(ix, min_x)for iy in range(y_width):y = calc_grid_position(iy, min_y)for iox, ioy in zip(ox, oy):d = math.hypot(iox - x, ioy - y)if d <= rr:obstacle_map[ix][iy] = Truebreak ### start planning # define node start_node = Node(calc_xy_index(sx, min_x),calc_xy_index(sy, min_y), 0.0, -1) goal_node = Node(calc_xy_index(gx, min_x),calc_xy_index(gy, min_y), 0.0, -1) # define open_set, closed_set = dict(), dict() open_set[calc_grid_index(start_node)] = start_nodesearch_index = 0 while 1 :if len(open_set) == 0:print("Open set is empty..")breakc_id = min(open_set,key=lambda o: open_set[o].cost + calc_heuristic(goal_node,open_set[o]))current = open_set[c_id]if current.x == goal_node.x and current.y == goal_node.y:print("Find goal")goal_node.parent_index = current.parent_indexgoal_node.cost = current.costbreak# Remove the item from the open setdel open_set[c_id]# Add it to the closed setclosed_set[c_id] = current# expand_grid search grid based on motion modelfor i, _ in enumerate(motion):node = Node(current.x + motion[i][0],current.y + motion[i][1],current.cost + motion[i][2], c_id)n_id = calc_grid_index(node)# If the node is not safe, do nothingif not verify_node(node):continueif n_id in closed_set:continueif n_id not in open_set:open_set[n_id] = node # discovered a new nodeelse:if open_set[n_id].cost > node.cost:# This path is the best until now. record itopen_set[n_id] = nodesearch_index = search_index + 1rx, ry = calc_final_path(goal_node, closed_set)

三、結果分析

在代碼中可以設置啟發函數的權重w，可以看到不同的w對搜索的速度與結果有直接影響

1、w = 10 的搜索結果，可以看到最終的搜索步數為43

2、 w = 0 的搜索結果，可以看到最終的搜索步數為146

3、 w = 2 的搜索結果，可以看到最終的搜索步數為88

?

?

總結

以上是生活随笔為你收集整理的自动驾驶算法详解(6)：Astar算法原理以及路径规划应用在python与ros平台实现的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。