自动驾驶算法详解(6)：Astar算法原理以及路径规划应用在python与ros平台实现

一、算法原理

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平台实现相关推荐

Tesla AI Day：特斯拉如何实现自动驾驶的详解
之前上课的时候偶然间看到了知乎上关于特斯拉如何实现自动驾驶的详解,因为我本身就是做算法的,对自动驾驶也很感兴趣,得知信息来源于Tesla的AI Day,于是下课后马不停蹄就去找了回放好好学习学习. 视 ...
自动驾驶算法详解(3): LQR算法进行轨迹跟踪，lqr_speed_steering_control( )的python实现
前言: LQR算法在自动驾驶应用中,一般用在NOP.TJA.LCC这些算法的横向控制中,一般与曲率的前馈控制一起使用,来实现轨迹跟踪的目标,通过控制方向盘转角来实现横向控制. 本文将使用python来 ...
算法详解_常用算法详解——打印杨辉三角形
杨辉三角,是二项式系数在三角形中的一种几何排列.在中国南宋数学家杨辉1261年所著的<详解九章算法>一书中出现.在欧洲,这个表叫做帕斯卡三角形.帕斯卡(1623----1662)是在165 ...
蒙哥马利java算法_算法详解 - 蒙哥马利算法的概念与原理
算法的详解扯了一大顿,终于引出了今天文章的主角,前面讲到的两个算法,第一个就是蒙哥马利乘模,第二个就是蒙哥马利约减.下面我们来讲这两个算法的详解. 正如前面提到的蒙哥马利算法的三个特性之一是,不是基 ...
深度学习 --- BP算法详解（BP算法的优化）
上一节我们详细分析了BP网络的权值调整空间的特点,深入分析了权值空间存在的两个问题即平坦区和局部最优值,也详细探讨了出现的原因,本节将根据上一节分析的原因进行改进BP算法,本节先对BP存在的缺点进行全 ...
manacher算法详解（马拉车算法）
马拉车算法 Manacher算法是由题目"求字符串中最长回文子串的长度"而来.比如 abcdcb 的最长回文子串为 bcdcb ,其长度为5. 回文:正着念,反着念都一样暴力解不 ...
量子计算机算法详解,量子计算机量子算法以及物理实现.pdf
CN43-1258/TP 计算机工程与科学 2012年第34卷第8期 V01．34,No．8,2012 ISSN1007-130X ENGINEERING&SCIENCE COMPUTER 文 ...
【数据挖掘】:FP增长算法详解
FP-growth算法,fpgrowth算法详解使用FP-growth算法来高效发现频繁项集前言你用过搜索引擎挥发现这样一个功能:输入一个单词或者单词的一部分,搜索引擎酒会自动补全查询词项,用户 ...
KMP算法详解及各种应用
KMP算法详解: KMP算法之所以叫做KMP算法是因为这个算法是由三个人共同提出来的,就取三个人名字的首字母作为该算法的名字.其实KMP算法与BF算法的区别就在于KMP算法巧妙的消除了指针i的回溯问题 ...
字符串匹配之KMP算法详解
kmp算法又称"看毛片"算法,是一个效率非常高的字符串匹配算法.不过由于其难以理解,所以在很长的一段时间内一直没有搞懂.虽然网上有很多资料,但是鲜见好的博客能简单明了地将其讲清楚. ...

自动驾驶算法详解(6)：Astar算法原理以及路径规划应用在python与ros平台实现

自动驾驶算法详解(6)：Astar算法原理以及路径规划应用在python与ros平台实现相关推荐

最新文章

热门文章