第四章：路径搜索算法基础

各位同学好，我是你们这门课的主讲。今天咱们来聊聊路径搜索算法——说白了，就是无人机怎么从A点飞到B点，还能避开障碍物。

我记得刚入行那会儿，带我的老工程师跟我说过一句话：「路径规划的核心，就是搜索。」当时我不太理解，后来做了几个项目才明白，这话一点不假。无论你用多么高级的算法，最终都要落到「怎么找到那条路」这个问题上。

4.1 图搜索的基本概念

先说说图。图是什么？别被数学概念吓到，其实很简单。

图就是一堆节点和连接节点的边。你想想看，无人机要飞过一片区域，我们把这片区域划分成网格，每个网格中心就是一个节点，相邻网格之间就是边。无人机从一个节点飞到相邻节点，这就是一次移动。

我习惯把图分为两种：

• 有向图：边有方向，只能单向走。比如无人机在特定风场中，顺风好走，逆风难行。
• 无向图：边没有方向，可以来回走。大部分地面机器人用这种。

还有一个重要概念——权重。每条边都有一个代价，比如距离、时间、能耗。路径搜索的目标，就是找一条从起点到终点、总权重最小的路径。

下面这张图是我画的，展示了图搜索的基本框架：

4.2 Dijkstra算法：最朴素的寻路方式

Dijkstra算法，说白了就是「从起点开始，一层一层往外扩」。它保证找到的路径是最短的，但代价是——它不知道目标在哪，只能盲目地往外搜。

我当年第一次实现Dijkstra时，是在一个校园送货机器人项目上。地图不大，也就几百个节点，跑起来还挺快。后来换到无人机场景，网格一细化，节点数上万，那叫一个慢啊...

算法原理

Dijkstra的核心思想就一句话：每次选当前代价最小的节点扩展。

具体步骤：

1. 初始化：起点代价为0，其他节点代价为无穷大
2. 从待处理节点中，选代价最小的那个
3. 更新它的邻居节点代价（如果新路径更短）
4. 重复2-3步，直到所有节点都处理完

代码实现

# Python实现Dijkstra算法
import heapq

def dijkstra(graph, start, goal):
    # graph: 邻接表，{node: [(neighbor, cost), ...]}
    # 返回：最短路径和总代价
    
    # 初始化
    distances = {node: float('inf') for node in graph}
    distances[start] = 0
    previous = {node: None for node in graph}
    
    # 优先队列：(当前代价, 节点)
    pq = [(0, start)]
    visited = set()
    
    while pq:
        current_dist, current = heapq.heappop(pq)
        
        if current in visited:
            continue
        visited.add(current)
        
        # 到达目标，可以提前退出
        if current == goal:
            break
            
        # 更新邻居
        for neighbor, weight in graph[current]:
            distance = current_dist + weight
            if distance < distances[neighbor]:
                distances[neighbor] = distance
                previous[neighbor] = current
                heapq.heappush(pq, (distance, neighbor))
    
    # 回溯路径
    path = []
    node = goal
    while node is not None:
        path.append(node)
        node = previous[node]
    path.reverse()
    
    return path, distances[goal]

4.3 A*算法：带「方向感」的搜索

A*算法是Dijkstra的升级版。它多了一个「启发函数」，用来估计当前节点到目标还有多远。有了这个信息，搜索就有了方向，效率大幅提升。

说白了，Dijkstra是「四面开花」，A*是「朝着目标方向开花」。在无人机航迹规划中，A*几乎是标配。

核心公式

A*的代价函数：f(n) = g(n) + h(n)

• g(n)：从起点到当前节点的实际代价
• h(n)：从当前节点到目标的估计代价（启发函数）
• f(n)：总估计代价

启发函数h(n)的选择很关键。我常用的几种：

启发函数	适用场景	特点
曼哈顿距离	四方向移动（上下左右）	计算快，但估计偏大
欧几里得距离	任意方向移动	最准确，但计算稍慢
对角线距离	八方向移动	兼顾速度和精度

代码实现

# Python实现A*算法
import heapq
import math

def heuristic(a, b):
    # 欧几里得距离作为启发函数
    return math.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2)

def a_star(grid, start, goal):
    # grid: 二维网格，0可通行，1障碍物
    # 返回：路径和总代价
    
    rows, cols = len(grid), len(grid[0])
    # 四个方向：上下左右
    directions = [(-1,0), (1,0), (0,-1), (0,1)]
    
    # g值：起点到当前节点的实际代价
    g_score = {start: 0}
    # f值：g + h
    f_score = {start: heuristic(start, goal)}
    
    # 优先队列：(f值, g值, 节点)
    open_set = [(f_score[start], g_score[start], start)]
    came_from = {start: None}
    
    while open_set:
        _, current_g, current = heapq.heappop(open_set)
        
        if current == goal:
            # 回溯路径
            path = []
            node = current
            while node is not None:
                path.append(node)
                node = came_from[node]
            path.reverse()
            return path, g_score[goal]
        
        for dr, dc in directions:
            neighbor = (current[0] + dr, current[1] + dc)
            
            # 检查边界和障碍物
            if (neighbor[0] < 0 or neighbor[0] >= rows or
                neighbor[1] < 0 or neighbor[1] >= cols or
                grid[neighbor[0]][neighbor[1]] == 1):
                continue
            
            # 计算新的g值
            tentative_g = current_g + 1  # 每步代价为1
            
            if (neighbor not in g_score or 
                tentative_g < g_score[neighbor]):
                # 找到更好的路径
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f = tentative_g + heuristic(neighbor, goal)
                f_score[neighbor] = f
                heapq.heappush(open_set, (f, tentative_g, neighbor))
    
    return None, float('inf')  # 无路径

4.4 Dijkstra vs A*：怎么选？

很多同学问我：「老师，实际项目中到底用哪个？」

我的建议是这样的：

• 地图小（几百节点以内）：Dijkstra完全够用，实现简单，不容易出bug
• 地图大（几千节点以上）：必须上A*，否则搜索时间会让你崩溃
• 需要最优解：两者都能保证最优，但A*更快
• 动态环境（障碍物移动）：建议用D* Lite或RRT*，A*需要重新规划

4.5 工程实践中的注意事项

嗯，这里我要多说几句。理论和工程之间，有时候差得还挺远。

第一，网格分辨率怎么定？

网格太粗，路径不精确；网格太细，搜索太慢。我一般取无人机尺寸的2-3倍作为网格大小。比如无人机直径0.5米，网格就设1-1.5米。

第二，障碍物膨胀处理

千万别把无人机当质点处理。实际飞行中，无人机有尺寸，需要把障碍物向外膨胀半个机身宽度。这个坑我踩过——有一次仿真跑得好好的，真机飞出去差点撞墙。

第三，实时性要求

无人机在空中，每秒都要更新位置。如果路径规划耗时超过100ms，基本就废了。我建议把A*的搜索深度限制在200-300个节点以内，超出就改用分层规划。

好了，这一章的内容就到这里。图搜索是路径规划的基础，Dijkstra和A*是两大核心算法。下一章我们会聊更高级的算法，但前提是——你得先把这两个吃透。

有什么问题，欢迎在课程群里讨论。我平时也会在群里分享一些工程案例，都是实际项目中踩过的坑，希望对大家有帮助。

---

公众号:蓝海资料掘金营，微信deep3321