路径规划基础：图搜索算法在目标跟踪中的应用

说实话，做机器人目标跟踪这些年，我踩过最大的坑就是——路径规划没选对算法。你想想看，目标点一直在动，你拿个静态规划算法去硬套，结果就是机器人像个无头苍蝇。今天咱们聊聊图搜索算法里的两个老大哥：Dijkstra 和 A*。它们虽然经典，但在目标跟踪场景下，用法可大有讲究。

图搜索算法到底在解决什么问题？

先问个问题：机器人怎么知道从A点到B点该走哪条路？

说白了，就是把环境抽象成一张图。节点代表位置，边代表可通行的路径。然后算法在这张图上找一条最优路径。我习惯把这种思路叫做「地图的数学化」。你想想看，现实世界那么复杂，但一旦变成图，问题就清晰多了。

Dijkstra：稳如老狗，但有点慢

Dijkstra 算法，我入行时第一个学的搜索算法。它的逻辑很简单：从起点开始，一层层往外扩，每次都选当前代价最小的节点。保证找到最短路径。

但问题来了——它太「老实」了。我在做仓储机器人项目时，用 Dijkstra 规划路径，结果机器人绕了半个仓库才到目标点。为什么？因为它不知道目标在哪，只能盲目地往外扩。

伪代码长这样：

function Dijkstra(Graph, start):
    dist[start] = 0
    for each node v in Graph:
        if v != start: dist[v] = INF
        add v to unvisited_set
    
    while unvisited_set is not empty:
        u = node with smallest dist in unvisited_set
        remove u from unvisited_set
        
        for each neighbor v of u:
            alt = dist[u] + cost(u, v)
            if alt < dist[v]:
                dist[v] = alt
                prev[v] = u
    return dist, prev

嗯，这里要注意：Dijkstra 适合静态环境、目标点固定的场景。但目标跟踪里，目标一直在动，你每次都要重新规划，Dijkstra 的计算量就上来了。

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

A* 算法，说白了就是 Dijkstra 的升级版。它加了一个启发式函数，告诉算法「目标大概在哪个方向」。这样搜索范围就小多了。

核心公式：f(n) = g(n) + h(n)

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

启发式函数怎么选？我建议用欧几里得距离或曼哈顿距离。但要注意——h(n) 不能高估实际代价，否则 A* 可能找不到最优解。这叫「可采纳性」，我当年面试时被问过这个问题。

代码实现：

function AStar(Graph, start, goal):
    open_set = {start}
    came_from = empty map
    g_score[start] = 0
    f_score[start] = h(start, goal)
    
    while open_set is not empty:
        current = node in open_set with lowest f_score
        if current == goal:
            return reconstruct_path(came_from, current)
        
        open_set.remove(current)
        for neighbor of current:
            tentative_g = g_score[current] + cost(current, neighbor)
            if tentative_g < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f_score[neighbor] = g_score[neighbor] + h(neighbor, goal)
                if neighbor not in open_set:
                    open_set.add(neighbor)
    return failure

目标跟踪场景下的算法选择

目标跟踪和普通路径规划最大的区别是什么？目标位置在变。所以你不能只规划一次，得持续更新。

我整理了一个对比表，方便你选型：

场景	推荐算法	原因
静态环境，目标缓慢移动	Dijkstra + 定时重规划	实现简单，稳定性高
动态环境，目标快速移动	A* + 局部避障	搜索效率高，实时性好
大规模地图	A* + 分层规划	先粗后细，减少计算量
目标轨迹可预测	A* + 轨迹预测	提前规划，减少延迟

我个人习惯是：先用 A* 做全局规划，再用 DWA（动态窗口法）做局部避障。这样既保证了路径最优性，又能应对突发障碍物。

一个实际案例：无人机跟踪地面目标

我记得做过一个无人机项目，要跟踪地面移动的车辆。环境是城市街区，有建筑物遮挡。

一开始我用纯 A*，每秒钟重规划一次。结果无人机飞得歪歪扭扭，因为每次重规划路径都不一样。后来我做了两件事：

1. 路径缓存：如果目标移动距离小于阈值，沿用上次路径
2. 启发式函数优化：把目标速度方向也考虑进去，让搜索偏向目标前进方向

效果立竿见影。无人机飞行平滑多了，而且跟踪误差从 2 米降到了 0.5 米。

本章知识体系

下面这张图，是我梳理的图搜索算法在目标跟踪中的核心逻辑。你看一眼就能明白整体脉络：

这张图把整个流程串起来了。从输入地图和目标位置开始，根据环境动态性选择算法，然后输出平滑的跟踪路径。记住：核心不是算法本身，而是怎么把它用对地方。

最后说几句

图搜索算法是路径规划的基石。Dijkstra 和 A* 虽然老，但非常实用。我建议你动手实现一遍，哪怕是在一个 10x10 的网格地图上跑一跑。只有亲手写过代码，你才能真正理解「启发式函数怎么影响搜索效率」这种问题。

嗯，今天就聊到这儿。记住：算法是工具，场景是老师。多在实际项目中打磨，你自然就知道什么时候该用 Dijkstra，什么时候该上 A*。

可可精品资料库