探索二维栅格路径规划算法：从全局到局部的动态避障之旅

二维栅格路径规划算法全局路径规划算法包括A*jps rrtrrt*等每种都可以单独使用路径剪枝策略减少路径冗余。 局部路径规划有dwa和apf算法。 可以根据图片自动生成栅格地图全局结合局部进行动态避障示意图如下。 局部路径贴合全局路径在机器人导航、游戏地图寻路等众多领域二维栅格路径规划算法起着举足轻重的作用。今天咱们就一起来深入了解下相关算法以及它们如何巧妙结合实现动态避障。全局路径规划算法A*算法A*算法可以说是路径规划里的明星算法了。它结合了Dijkstra算法的广度优先搜索和贪心算法的最佳优先搜索特点。它通过一个评估函数$f(n) g(n) h(n)$来选择下一个扩展节点其中$g(n)$是从起点到节点$n$的实际代价$h(n)$是从节点$n$到目标点的估计代价。import heapq def heuristic(a, b): return abs(a[0] - b[0]) abs(a[1] - b[1]) def a_star(graph, start, goal): open_set [] heapq.heappush(open_set, (0, start)) came_from {} g_score {node: float(inf) for node in graph.keys()} g_score[start] 0 f_score {node: float(inf) for node in graph.keys()} f_score[start] heuristic(start, goal) while open_set: _, current heapq.heappop(open_set) if current goal: path [] while current in came_from: path.append(current) current came_from[current] path.append(start) path.reverse() return path for neighbor in graph[current]: tentative_g_score g_score[current] 1 if tentative_g_score g_score[neighbor]: came_from[neighbor] current g_score[neighbor] tentative_g_score f_score[neighbor] tentative_g_score heuristic(neighbor, goal) if neighbor not in [i[1] for i in open_set]: heapq.heappush(open_set, (f_score[neighbor], neighbor)) return NoneJPS跳跃点搜索算法JPS算法是对A*算法的优化它通过识别可以“跳跃”的点减少搜索空间。在栅格地图中它利用了栅格的结构特点比如对角线方向的跳跃。JPS通过预计算一些可以直接跳跃到达的节点避免了对中间节点的无效搜索从而大大提高了搜索效率。RRT快速探索随机树与RRT*算法RRT算法通过在搜索空间中随机采样点逐步构建一棵树来寻找路径。它非常适合处理复杂的、高维的搜索空间。而RRT*在RRT基础上增加了重布线rewiring操作能够优化已找到的路径逐渐趋向于最优解。import random import math class Node: def __init__(self, x, y): self.x x self.y y self.parent None def distance(node1, node2): return math.sqrt((node1.x - node2.x) ** 2 (node1.y - node2.y) ** 2) def rrt(start, goal, obstacle_list, max_iter): tree [start] for _ in range(max_iter): random_node Node(random.random(), random.random()) nearest_node min(tree, keylambda n: distance(n, random_node)) new_node Node(nearest_node.x, nearest_node.y) dist distance(nearest_node, random_node) if dist 0.1: new_node.x nearest_node.x 0.1 * (random_node.x - nearest_node.x) / dist new_node.y nearest_node.y 0.1 * (random_node.y - nearest_node.y) / dist if all(distance(new_node, obs) 0.1 for obs in obstacle_list): new_node.parent nearest_node tree.append(new_node) if distance(new_node, goal) 0.1: goal.parent new_node tree.append(goal) path [] current goal while current is not None: path.append((current.x, current.y)) current current.parent return path[::-1] return None路径剪枝策略对于上述全局路径规划算法每种都能搭配路径剪枝策略。比如A*算法得到的路径可能包含一些不必要的迂回节点通过路径剪枝可以直接连接相距较远但无障碍物阻挡的节点减少路径冗余让路径更加简洁高效。局部路径规划算法DWA动态窗口法DWA主要用于移动机器人在局部环境中的实时避障。它根据机器人当前的速度和加速度限制生成一系列可能的速度组合然后对每个组合进行模拟评估其是否会导致碰撞以及是否朝着目标前进选择最优的速度组合来控制机器人运动。APF人工势场法APF把机器人所处环境看作是由目标点产生的引力场和障碍物产生的斥力场叠加而成。机器人在这个合成势场中就像一个在力场中运动的粒子受到引力和斥力的共同作用从而朝着目标点移动并避开障碍物。基于图片生成栅格地图借助图像处理技术我们可以根据图片自动生成栅格地图。比如将图片灰度化黑色像素代表障碍物白色像素代表可通行区域然后将其划分成一个个栅格这样就构建好了栅格地图为路径规划算法提供数据基础。全局与局部结合的动态避障在实际应用中我们常常将全局路径规划和局部路径规划结合起来。先通过全局路径规划算法找到从起点到终点的大致路径然后在机器人沿着这条路径行进时利用局部路径规划算法实时避开突然出现的障碍物。并且要确保局部路径贴合全局路径避免机器人偏离原本的大致方向太远。比如机器人在沿着全局路径A*算法规划的路径行走时突然检测到前方有新出现的障碍物此时DWA算法就开始发挥作用引导机器人绕开障碍物同时尽量保持在全局路径的延长线上最终成功到达目标点。通过这种全局与局部的完美协作机器人就能在复杂多变的环境中实现高效、安全的动态避障与路径规划。二维栅格路径规划算法全局路径规划算法包括A*jps rrtrrt*等每种都可以单独使用路径剪枝策略减少路径冗余。 局部路径规划有dwa和apf算法。 可以根据图片自动生成栅格地图全局结合局部进行动态避障示意图如下。 局部路径贴合全局路径