C 语言 A*路径规划实现：从基础到无人机应用

• 曼哈顿距离：abs(dx) + abs(dy)，适合只允许四方向移动的网格。
• 欧几里得距离：sqrt(dx*dx + dy*dy)，支持任意方向飞行，代价计算更精确。

实现很简单：

#include <math.h>
double euclidean(double x1, double y1, double x2, double y2) {
    double dx = x1 - x2;
    double dy = y1 - y2;
    return sqrt(dx*dx + dy*dy);
}
double manhattan(double x1, double y1, double x2, double y2) {
    return fabs(x1-x2) + fabs(y1-y2);
}

在无人机实际飞行中，如果航线允许斜角，欧几里得距离更贴近真实能耗；曼哈顿距离则计算更快，适合低算力芯片。

算法主循环

流程可以用 mermaid 图概括：

graph LR
A[开始] --> B{当前节点是否为目标?}
B -- 是 --> C[回溯路径]
B -- 否 --> D[生成邻居节点]
D --> E[计算 f = g + h]
E --> F[加入开放列表]
F --> G[选取最小 f 节点]
G --> B

转为 C 代码的关键在于用最小堆模拟优先队列，以及高效的邻居检测。一个精简的主循环实现思路：

typedef struct { int x, y, g, h, f; struct Node* parent; } Node;
int heuristic(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2); // 曼哈顿
}
// 主循环伪代码
while (open_list_not_empty) {
    current = pop_min_f();
    if (is_goal(current)) {
        reconstruct_path(current);
        break;
    }
    close_list_add(current);
    for each neighbor of current {
        if (neighbor in close_list) continue;
        tentative_g = current.g + cost(current, neighbor);
        if (tentative_g < neighbor.g) {
            neighbor.parent = current;
            neighbor.g = tentative_g;
            neighbor.f = neighbor.g + heuristic(neighbor, goal);
            open_list_add(neighbor);
        }
    }
}

实际编码时需要注意边界检查和障碍物判断，8 邻域移动会让邻居生成复杂一些，但逻辑不变。

扩展到三维和无人机避障

无人机路径规划通常是在三维空间进行的。把节点从 (x,y) 换成 (x,y,z)，启发式改成三维欧几里得：

def heuristic(a, b):
    return ((a.x - b.x) ** 2 + (a.y - b.y) ** 2 + (a.z - b.z) ** 2) ** 0.5

开放列表仍然可以用二叉堆，只不过每个节点的坐标多了一维，对性能影响不大，除非地图极大。

但实际飞行中环境是动态的，突然冒出的障碍需要实时响应。这时单靠全局 A* 不够，需要结合局部规划。动态窗口法 (DWA) 是一个常见选择，它根据当前速度和加速度约束，在速度空间中搜索安全轨迹：

// DWA 局部规划器速度采样逻辑
void DynamicWindowApproach::computeDynamicWindow() {
    double min_v = max(0.0, current_vel - acc_limit * dt);
    double max_v = min(max_speed, current_vel + acc_limit * dt);
    double min_w = max(-max_omega, current_omega - omega_acc * dt);
    double max_w = min(max_omega, current_omega + omega_acc * dt);
}

当传感器检测到障碍物距离小于阈值（比如 0.5 米），系统会紧急重规划，生成局部避障路径。局部重规划触发的典型条件：

航点生成与轨迹平滑

全局路径得到的是离散路点序列，直接发给飞控会让无人机频繁加减速，影响稳定。因此需要用插值算法生成平滑轨迹。三次样条插值 (Cubic Spline) 能保证位置、速度、加速度连续：

import numpy as np
from scipy.interpolate import CubicSpline
x = np.array([0, 1, 2, 3])
y = np.array([0, 1, 0, 1])
cs = CubicSpline(x, y, bc_type='natural')
smooth_x = np.linspace(0, 3, 100)
smooth_y = cs(smooth_x)

bc_type='natural' 让边界二阶导为零，避免端点异常抖动。如果是在嵌入式 C 环境里跑，可以考虑用线性插值或 Catmull-Rom 样条，计算量更小。

A* 算法的 C 实现不算复杂，但细节不少。数据结构的选择直接影响性能，特别是在嵌入式设备上。实际落地时，三维扩展、动态避障和轨迹平滑这些工程问题往往比算法本身更耗精力。另外，内存管理和实时性是硬约束，写代码时要时刻留意。

这篇文章给出的代码片段和思路，希望能帮你在自己的项目里快速搭建一个可用的路径规划模块。