二维栅格路径规划算法，全局路径规划算法包括A*，jps ，rrt，rrt*等，每种都可以单独使用路径剪枝策略，减少路径冗余。 局部路径规划有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 None

JPS（跳跃点搜索）算法

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, key=lambda 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 ，rrt，rrt*等，每种都可以单独使用路径剪枝策略，减少路径冗余。 局部路径规划有dwa和apf算法。 可以根据图片自动生成栅格地图，全局结合局部进行动态避障，示意图如下。 局部路径贴合全局路径