轨迹规划：基于查找的(search-based)路径规划算法

背景

基于查找的路径规划方法，即，将可能的路径空间建模为一个roadmap，在roadmap中查找最优路径，生成最优路径。roadmap一般都是以图的形式存储，在图中查找路径

基于搜索的算法一般流程为：建立一个待处理节点容器，每次从待处理节点中选出一个节点进行处理并将他的邻居节点(已处理的节点除外)加入待处理节点，并将该节点本身标记已处理。如此循环直到处理完全部节点或者处理到终点节点。

不同方法的主要区别就是从待处理节点容器中选取节点的规则不同，常见方法有：深度优先查找，广度优先查找，迪杰斯特拉算法，A*算法。

配置空间Configure Sapce

；路径规划时，机器人本体有体积，障碍物也有体积，判断可执行空间时较为麻烦，可以直接只看机器人本体的中心点，对障碍物按照小车的体积进行膨胀

迪杰斯特拉算法

迪杰斯特拉算法（Dijkstra’s Algorithm）是一种用于在带权图中找到单源最短路径的经典算法，由荷兰计算机科学家 Edsger W. Dijkstra 于1956年提出。它适用于非负权重的有向图或无向图，广泛应用于路径规划（如地图导航、网络路由等场景）。

• 复杂度：对于n个节点的图，算法复杂度为$O(n^2)$
• 核心思想：通过逐步扩展当前已知的最短路径集合，最终找到从起点到所有其他节点的最短路径。算法采用贪心策略，每次选择当前距离起点最近的未处理节点，并更新其相邻节点的距离。

算法步骤

1. 初始化

   • 为每个节点分配一个临时距离值：起点设为 0，其他节点设为 ∞。
   • 创建一个集合（或优先队列）保存所有未处理的节点。

2. 循环处理节点

   • 选择当前距离最小的未处理节点（称为当前节点 u）。
   • 对 u 的每个相邻节点 v 进行松弛操作（Relaxation）：如果 distance[u] + weight(u, v) < distance[v]，则更新 distance[v] = distance[u] + weight(u, v)
   • 将 u 标记为已处理。

3. 终止条件

   • 当所有节点都被处理，或目标节点已被标记为已处理时结束。

示例演示

假设需要找到从节点 Node0 到其他节点的最短路径，图的边权重如下（非负）：

执行过程：

1. 初始化：distance[0]=0，其他节点为 ∞。
2. 第一轮：
   • 选择距离最小的未处理节点 Node0。
   • 更新相邻未处理节点：distance[1]=3.5，distance[2]=2.1。
3. 第二轮：
   • 选择当前最小的未处理节点 Node2（distance[2]=2.1）。
   • 更新相邻未处理节点：distance[4]=5.0+2.1=7.1。
4. 第三轮：
   • 选择当前最小未处理节点 Node1（distance[1]=3.5）。
   • 更新相邻未处理节点：distance[3]=3.5+1.2=4.7。
5. 第四轮：
   • 选择当前最小的未处理节点 Node3（distance[3]=4.7）
   • 更新相邻未处理节点：distance[4]=min(distance[3]+3.3,distance[4])=min(4.7+3.3, 7.1)。
6. 第五轮
   • 处理节点 Node4，无未处理节点，结束。

实现

python代码：

import networkx as nx # 用于创建图
import matplotlib.pyplot as plt
import heapq # 用于维护优先队列

def dijsktra(graph, start, end):

    # 初始化距离字典，所有节点的距离为无穷大，起点的距离为0
    distances = {node: float('inf') for node in graph.nodes()}
    distances[start] = 0
    # 优先队列，存储待访问的节点及其距离
    priority_queue = [(0, start)]
    # 记录最短路径的前驱节点
    previous_nodes = {node: None for node in graph.nodes()}

    while priority_queue:
        current_distance, current_node = heapq.heappop(priority_queue)

        # 如果当前节点是终点，则停止搜索
        if current_node == end:
            break

        # 遍历当前节点的邻居
        for neighbor, weight in graph[current_node].items():
            distance = current_distance + weight['weight']

            # 如果新距离比已知距离短，则更新距离和前驱节点
            if distance < distances[neighbor]:
                distances[neighbor] = distance
                previous_nodes[neighbor] = current_node
                heapq.heappush(priority_queue, (distance, neighbor))

    # 重建最短路径
    path = []
    current_node = end
    while previous_nodes[current_node] is not None:
        path.insert(0, current_node)
        current_node = previous_nodes[current_node]
    if path:
        path.insert(0, current_node)

    return path, distances[end]

if __name__ == '__main__':
    # 创建无向有权图
    G = nx.Graph()
    G.add_nodes_from([0, 1, 2, 3, 4])
    edges = [
        (0, 1, 3.5),
        (0, 2, 2.1),
        (1, 2, 4.8),
        (1, 3, 1.2),
        (2, 4, 5.0),
        (3, 4, 3.3)
    ]
    G.add_weighted_edges_from(edges)

    # 定义节点和边的样式
    node_colors = ['skyblue', 'lightgreen', 'lightcoral', 'gold', 'violet']
    node_sizes = [3000, 3000, 3000, 3000, 3000]
    edge_colors = ['gray', 'gray', 'gray', 'gray', 'gray', 'gray']
    edge_widths = [edges[i][2] for i in range(len(edges))]
    # 计算节点布局
    pos = nx.spring_layout(G, seed=42)
    # 绘制节点
    nx.draw_networkx_nodes(G, pos, 
                        nodelist=G.nodes(), 
                        node_color=node_colors, 
                        node_size=node_sizes, 
                        alpha=0.8)
    # 绘制边
    nx.draw_networkx_edges(G, pos, 
                        edgelist=G.edges(), 
                        edge_color=edge_colors, 
                        width=edge_widths, 
                        style='dashed')
    # 添加节点标签
    nx.draw_networkx_labels(G, pos, 
                        labels={node: f'Node {node}' for node in G.nodes()}, 
                        font_size=14, 
                        font_weight='bold')
    # 添加边权重标签
    edge_labels = nx.get_edge_attributes(G, 'weight')
    nx.draw_networkx_edge_labels(G, pos, 
                            edge_labels=edge_labels, 
                            font_color='red', 
                            font_size=12)
    # 隐藏坐标轴
    plt.axis('off')
    
    # 计算最短路径
    start, end = 0, 3
    path, distance = dijsktra(G, start, end)
    print(f'The shortest path from {start} to {end} is: {path}')
    print(f'The distance is: {distance}')
    
    # 显示图形
    plt.show()

• 优先队列（小顶堆）：用于快速获取当前距离最小的节点，时间复杂度从 O(V²) 优化到 O((V+E) log V)（V为节点数，E为边数）。
• 路径记录：通过维护前驱节点（predecessor）可回溯具体路径。

优缺点

• 优点：
  • 保证找到最短路径（非负权重时）。
  • 高效，适合稠密图或需要计算到所有节点的场景。
• 缺点：
  • 无法处理负权边（可能陷入无限循环或错误结果）。
  • 对大规模图效率较低（需结合A*等启发式算法优化）。

A*算法

如果边的权重全部相等时，迪杰斯特拉算法就等同于广度优先算法了。A*算法（A-Star Algorithm）是一种广泛使用的路径规划算法，结合了 Dijkstra算法 的最短路径保证和 启发式搜索 的高效性。

核心思想

A*算法通过权衡实际代价和预估代价，智能地选择搜索方向，从而快速找到从起点到终点的最优路径。其核心公式为：$f(n)=g(n)+h(n)$

• $g(n)$：从起点到当前节点 (n) 的实际代价（已知）。
• $h(n)$：从当前节点 (n) 到终点的预估代价（启发式函数，需满足特定条件）。
• $f(n)$：节点的综合优先级，决定下一步扩展哪个节点。

算法步骤

1. 初始化

   • 创建两个集合：Open List（待探索节点）和 Closed List（已探索节点）。
   • 将起点加入 Open List，并记录其 (g) 值（起点为0）、(h) 值和 (f) 值。

2. 循环处理节点

   • 从 Open List 中选择 (f(n)) 最小 的节点 (n)。
   • 如果 (n) 是终点，结束搜索并回溯路径。
   • 遍历 (n) 的所有相邻节点 (m)：
     • 计算 (m) 的临时 (g) 值：g_temp = g(n) + 从n到m的移动代价。
     • 如果 $m$ 不在 Open List 或 Closed List 中：
       • 记录 $m$ 的 $g=g_{temp}$，计算 $h(m)$ 和 $f(m)$。
       • 将 (m) 加入 Open List，并设置父节点为 $n$。
     • 如果 (m) 已在 Open List 中，且新的 $g_{t}emp<g(m)$：
       • 更新 (m) 的 (g) 值和父节点为 (n`。
   • 将 (n) 移出 Open List，加入 Closed List。

3. 终止条件

   • 找到终点时，通过父节点回溯路径。
   • 如果 Open List 为空且未找到终点，说明路径不存在。

关键点

1. 启发式函数 $h(n)$ 的设计

• 必须满足可接受性（Admissible）：即 $h(n)$ 永远不超过 实际从 n 到终点的最小代价（否则可能找不到最优路径）。
• 常见启发函数：
  • 曼哈顿距离：
    $h(n)=|x_n-x_{goal}|+|y_n-y_{goal}|$
    • 适用于网格图，允许4方向移动时可以用,当机器人可以任意方向移动时就不是admissible的了，因为从(0,0)到(1,1)机器人实际需要移动距离为$\sqrt{2}$，估计出来的距离是$|1-0|+|1-0|=2$
  • 欧几里得距离（适用于连续空间或允许对角移动）：
    $h(n)=\sqrt{(x_n-x_{goal}{)}^2+(y_n-y_{goal}{)}^2}$
  • 对角线距离（8方向移动）：结合曼哈顿和欧氏距离的优化。

2. 算法效率

• 启发函数越接近真实代价，效率越高。若 $h(n)=0$，A*退化为Dijkstra算法。
• Open List 的优先级队列通常用二叉堆实现，保证快速获取最小 $f(n)$ 节点。

示例演示

假设在一个网格地图中寻找从起点 S(0,0) 到终点 G(4,4) 的最短路径，移动允许上下左右（4方向），使用欧式距离作为启发函数。
地图示例（无障碍物）：

import heapq
import random
import networkx as nx
import matplotlib.pyplot as plt

def heuristic(a, b):
    return pow(a[0] - b[0],2) + pow(a[1] - b[1],2)

def a_star(G, start, end):
    hscore = {node: heuristic(node, end) for node in G.nodes()}
    gscore = {node: float('inf') for node in G.nodes()}
    gscore[start] = 0
    fscore = {node: gscore[node] + hscore[node] for node in G.nodes()}
    open_heap = [(fscore[start], start)]
    close_set = set()
    came_from = {}

    next_vec = [(0,1),(0,-1),(1,0),(-1,0)]
    while open_heap:
        current = heapq.heappop(open_heap)[1]
        print(current)
        if current == end:
            route = []
            while current in came_from:
                route.append(current)
                current = came_from[current]
            route.append(start)
            route.reverse()
            return route
        
        close_set.add(current)
        for i, j in next_vec:
            neighbor = current[0] + i, current[1] + j
            if G.has_node(neighbor):
                if G.has_edge(current, neighbor):
                    new_g_score = gscore[current] + G[current][neighbor]['weight']
                    if new_g_score < gscore[neighbor]:
                        came_from[neighbor] = current
                        gscore[neighbor] = new_g_score
                        fscore[neighbor] = gscore[neighbor] + hscore[neighbor]
                        heapq.heappush(open_heap, (fscore[neighbor], neighbor))
        
    return None
        

if __name__ == '__main__':
    dim = 5
    G = nx.Graph()
    nodes = [(x,y) for x in range(dim) for y in range(dim)]
    G.add_nodes_from(nodes)
    for i in range(dim):
        for j in range(dim):
            if i < dim-1:
                G.add_edge((i,j),(i+1,j),weight=1)
            if j < dim-1:
                G.add_edge((i,j),(i,j+1),weight=1)
    
    
    start = (0,0)
    end = (4,4)
    route = a_star(G, start, end)
    # 画出路径
    if route:
        for i in range(dim):
            for j in range(dim):
                nx.draw_networkx_nodes(G, pos={(i,j):(i,j)}, nodelist=[(i,j)], node_color='skyblue', node_size=3000)
        for i in range(len(route)-1):
            nx.draw_networkx_nodes(G, pos={route[i]:route[i],route[i+1]:route[i+1]}, nodelist=[route[i],route[i+1]],
                                   node_color='lightgreen', node_size=3000)
            nx.draw_networkx_edges(G, pos={route[i]:route[i],route[i+1]:route[i+1]}, edgelist=[(route[i],route[i+1])], edge_color='r', width=2)
        plt.show()
    print(route)
    

效果

优缺点

优点：

1. 高效：相比Dijkstra，大幅减少搜索范围（优先向目标方向扩展）。
2. 保证最优路径（前提是启发函数可接受且一致）。
   • 可以通过为启发函数添加系数来实现weighted A*:$f(n)=g(n)+\omega h(n),\omega >1$，使得A*算法更倾向于贪心策略，以更快的速度获得一个次优的路线
3. 灵活：通过调整启发函数适应不同场景。

缺点：

1. 依赖启发函数质量：若 (h(n)) 设计不当，可能退化为Dijkstra或效率低下。
2. 内存消耗大：需存储所有扩展的节点。
3. 无法处理动态环境或负权边。

优化

• 地图的表示：可以使用gridmap来表示，这样可以更快索引，且占内存也小
• 双向A*：从起点和终点同时搜索，加快速度。
• 动态A*（D* Lite）：适用于动态变化的环境（如障碍物移动）。
• 分层A*：将地图分层，先粗后细规划。
• Tie-breaker主要思想就是构建具有相同f的路径之间的倾向性。常见做法有：微调h避免相同f的情况；在h中添加一个对偏离直线段的惩罚项

掌握A算法后，可以进一步学习其变种或结合其他算法（如RRT用于高维空间），以应对更复杂的路径规划问题。

JPS算法

JPS算法（Jump Point Search，跳点搜索）是一种基于 A*算法 优化的高效路径规划算法，专门用于均匀网格地图（如栅格地图）。它通过跳过冗余节点和剪枝对称路径，显著减少搜索空间，尤其适合处理大规模网格环境下的最短路径问题。

核心思想

JPS的核心是 “跳点”（Jump Point） 和 “强制邻居”（Forced Neighbors） 的概念。算法通过识别地图中的关键节点（跳点），直接跳跃式扩展这些节点，避免处理大量无效的中间节点，从而大幅提升搜索效率。

1. 跳点（Jump Point）

• 定义：在网格中，如果某个节点是到达其后续节点的最优路径必经点，则该节点称为跳点。
• 作用：仅扩展跳点，忽略其他普通节点。

2. 强制邻居（Forced Neighbors）

• 定义：在移动过程中，若必须检查某个邻居节点以避免忽略更优路径，则该邻居称为强制邻居。
• 作用：帮助识别跳点并防止路径遗漏。

算法步骤

1. 初始化

   • 与A*类似，维护 Open List 和 Closed List，起点加入 Open List。

2. 跳跃式扩展节点

   • 从 Open List 中取出 (f(n)) 最小的节点 (n)。
   • 沿水平、垂直或对角线方向跳跃式搜索，直到遇到跳点或障碍物：
     • 若跳跃过程中发现强制邻居，则停止跳跃，当前节点即为跳点。
     • 将跳点加入 Open List，记录其父节点和代价。

3. 处理跳点

   • 对跳点的所有可能方向递归执行跳跃搜索，直至找到终点。

4. 回溯路径

   • 找到终点后，通过父节点回溯路径。

跳跃规则

• 水平/垂直跳跃：沿直线方向跳跃，直到遇到障碍物或强制邻居。
• 对角线跳跃：先沿对角线移动，再分别向水平和垂直方向递归跳跃。

强制邻居的判定

• 场景：当沿某一方向移动时，若存在障碍物使得某些邻居成为唯一可达路径的必经点，则这些邻居为强制邻居。
• 示例：向右移动时，若右侧有障碍物且右上方可通过，则右上方为强制邻居。
• 注意事项：为了避免出现对称路径，JSP采用了邻居裁剪，即：将节点分为劣性节点和自然节点，并认为劣性节点只能从

  • 劣性节点：从父节点到此节点小于(等于，直线运动时为小于等于，斜线运动时为小于)从该节点到此节点的距离时，此节点称为劣性节点

  • 

  • 对于图1中的1234678都是劣性节点，因为从父节点4到这些节点的距离小于等于从x到这些节点的距离，同理图2中的14678为劣性节点，由于是斜线运动，只取小于的节点，故235不算。

实现

import numpy as np
import heapq
import random
import matplotlib.pyplot as plt

# 还使用字典的形式存储太复杂了,将Node整合为一个类
class Node:
    def __init__(self, position, parent=None):
        self.position = position
        self.parent = parent
        self.g = 0
        self.h = 0
        self.f = 0

    def __lt__(self, other):
        return self.f < other.f

class JPS:
    def __init__(self, grid):
        self.grid = grid
        self.height, self.width = grid.shape
        self.movements = [(0, 1), (1, 0), (0, -1), (-1, 0),
                          (1, 1), (1, -1), (-1, 1), (-1, -1)]

    def heuristic(self, a, b):
        # 欧式距离
        return (a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2

    def is_valid(self, pos):
        x, y = pos
        return 0 <= x < self.height and 0 <= y < self.width

    def is_walkable(self, pos):
        return self.grid[pos] == 0

    def find_forced_neighbors(self, pos, direction):
        x, y = pos
        neighbors = []
        dx, dy = direction
        
        # 直线移动
        if dx == 0 or dy == 0:
            # 检查两侧是否有障碍物
            perpendiculars = [(dy, dx), (-dy, -dx)] if dx == 0 else [(-dy, dx), (dy, -dx)]
            for p in perpendiculars:
                nx, ny = x + p[0], y + p[1]
                if self.is_valid((nx, ny)) and not self.is_walkable((nx, ny)):
                    # 检测强制邻居
                    fn_pos = (x + dx + p[0], y + dy + p[1])
                    if self.is_valid(fn_pos) and self.is_walkable(fn_pos):
                        neighbors.append(fn_pos)
        return neighbors

    def jump(self, current, direction, goal):
        x, y = current
        dx, dy = direction
        next_node = (x + dx, y + dy)
        
        if not self.is_valid(next_node) or not self.is_walkable(next_node):
            return None
        
        if next_node == goal:
            return next_node
        
        # 检查强制邻居，如果存在强制邻居则返回当前节点
        if self.find_forced_neighbors(next_node, direction):
            return next_node
        
        # 对角线移动需要同时检查两个直线方向
        if dx != 0 and dy != 0:
            if self.jump(next_node, (dx, 0), goal) or self.jump(next_node, (0, dy), goal): #任意一个方向有路径就返回
                return next_node
        
        return self.jump(next_node, direction, goal) #如果没有强制邻居，继续前进

    def search(self, start, end):
        open_list = []
        start_node = Node(start)
        end_node = Node(end)
        closed_list = set()
        
        heapq.heappush(open_list, start_node)
        
        while open_list:
            current_node = heapq.heappop(open_list)
            closed_list.add(current_node.position)
            
            if current_node.position == end:
                path = []
                while current_node:
                    path.append(current_node.position)
                    current_node = current_node.parent
                return path[::-1]
            
            for direction in self.movements:
                jump_point = self.jump(current_node.position, direction, end)
                if jump_point and jump_point not in closed_list:
                    new_node = Node(jump_point, current_node)
                    new_node.g = current_node.g + max(abs(direction[0]), abs(direction[1]))
                    new_node.h = self.heuristic(jump_point, end)
                    new_node.f = new_node.g + new_node.h
                    
                    # 检查是否已经在open_list中,如果位置相同，f值更小则无需添加，否则插入新节点
                    in_open = False
                    for node in open_list:
                        if node.position == jump_point and node.f <= new_node.f:
                            in_open = True
                            break
                    if not in_open:
                        heapq.heappush(open_list, new_node)
        
        return None  # 没有找到路径

def visualize(grid, path=None, start=None, end=None):
    plt.figure(figsize=(8, 8))
    plt.imshow(grid, cmap='binary', origin='lower')
    
    if path:
        path_x = [p[1] for p in path]
        path_y = [p[0] for p in path]
        plt.plot(path_x, path_y, 'r-', linewidth=2)
    
    if start:
        plt.plot(start[1], start[0], 'gs', markersize=12)
    if end:
        plt.plot(end[1], end[0], 'bs', markersize=12)
    
    plt.xticks(range(grid.shape[1]))
    plt.yticks(range(grid.shape[0]))
    plt.grid(which='both', color='gray', linestyle='-', linewidth=0.5)
    plt.show()

# 测试样例
if __name__ == "__main__":
    # 创建地图 (0表示可行走，1表示障碍物)
    # grid = np.array([
    #     [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
    #     [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
    #     [0, 0, 1, 1, 1, 1, 1, 0, 0, 0],
    #     [0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
    #     [0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
    #     [0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
    #     [0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
    #     [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
    #     [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
    #     [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
    # ])
    dim = 10
    grid = np.zeros((dim, dim))
    for i in range(1,dim):
        for j in range(dim-1):
            if random.random() < 0.2:
                grid[i][j] = 1
    
    jps = JPS(grid)
    start = (0, 0)
    end = (8, 8)
    
    path = jps.search(start, end)
    print("找到的路径:", path)
    
    visualize(grid, path, start, end)

效果:

优缺点

优点：

1. 效率极高：相比A*，搜索速度可提升10倍以上（尤其在复杂网格中）。
2. 内存友好：减少Open List中的节点数量。
3. 保留最优性：确保找到最短路径。

缺点：

1. 仅适用于均匀网格：对非网格图（如拓扑图）不适用。
2. 实现复杂：跳跃逻辑和强制邻居的判断需要精细处理。
3. 动态环境不友好：难以处理实时变化的障碍物。

算法变种

• JPS+：预处理地图中的跳跃点信息，进一步加速实时搜索。
• Any-Angle JPS：支持任意角度移动（如Theta*的混合方法）。

总结：JPS算法通过跳跃式搜索和剪枝冗余节点，在网格地图中实现了接近理论极限的高效路径规划。尽管实现复杂且受限于网格结构，但其性能优势使其成为游戏开发和机器人领域的重要工具。

算法对比

以下是 Dijkstra、A（A-Star）* 和 JPS（Jump Point Search） 三种路径规划算法的对比

• A vs JPS*：在网格地图中，JPS效率远超A*，但牺牲了通用性。
• Dijkstra vs A*：A*可视为Dijkstra的启发式优化版本。
• 动态环境：若需处理动态障碍物，可结合D* Lite或RRT*等算法。