A*算法在路径规划中的实现与应用。内容涵盖算法理论基础、评估函数设计、数据结构（开放/关闭列表、最小堆）以及 C 语言代码实现。同时探讨了曼哈顿距离与欧几里得距离的对比应用、网格地图建模方法、三维空间扩展及动态避障策略。文章还涉及航点生成与平滑轨迹插值技术，旨在为无人机及机器人系统的自主导航提供完整的技术参考与优化方案。
在无人机自主飞行系统中，路径规划是决定其智能程度的核心模块。A*算法因其高效性与最优性被广泛应用于二维或三维空间的航路点搜索。该算法结合了 Dijkstra 算法的完备性与启发式搜索的效率，通过评估函数 f(n) = g(n) + h(n) 来选择最优节点扩展，其中 g(n) 表示从起点到当前节点的实际代价，h(n) 为当前节点到目标点的启发式估计（常用欧几里得或曼哈顿距离）。
为实现 A*算法，需定义节点结构体以存储坐标、代价及父节点信息：
x, y; g, h, f; parent_x, parent_y; } Node;
该结构支持在网格地图中追踪路径生成过程。
| 启发式方法 | 计算公式 | 适用场景 |
|---|---|---|
| 曼哈顿距离 | abs(dx) + abs(dy) | 仅允许四方向移动 |
| 欧几里得距离 | sqrt(dxdx + dydy) | 支持任意方向飞行 |
启发式搜索通过引入问题领域的先验知识，显著提升搜索效率。其关键在于评估函数的设计，引导算法优先探索更可能接近目标的路径。
A*算法的核心是评估函数：f(n) = g(n) + h(n)，其中：
def a_star(start, goal, graph):
open_set = PriorityQueue()
open_set.put((0, start))
g_score = {node: float('inf') for node in graph}
g_score[start] = 0
f_score = {node: float('inf') for node in graph}
f_score[start] = heuristic(start, goal)
上述代码初始化优先队列与评分表，heuristic() 函数提供启发式估计，通常采用欧几里得或曼哈顿距离。
当启发函数 h(n) 满足可接纳性（即不超过真实代价），A*能保证找到最优路径，兼具完备性与最优性。
在 A*路径搜索算法中，开放列表与关闭列表的设计直接影响算法效率。为实现高效的节点管理，通常采用动态数组或链表结构。
为快速获取 F 值最小的节点，开放列表可基于最小堆实现：
typedef struct { int x, y; float f, g, h; } Node;
Node open_list[1000];
int size = 0;
该结构通过数组存储节点，配合下沉操作维护堆序性，插入和弹出时间复杂度分别为 O(log n)。
关闭列表用于记录已访问坐标，使用二维布尔数组将查找复杂度降至 O(1)：
| 坐标 (x,y) | 状态 |
|---|---|
| (3,4) | 已访问 |
| (5,6) | 未访问 |
在航迹评估中，位置偏差的度量方式直接影响分析精度。曼哈顿距离与欧几里得距离作为两种常用的空间距离度量方法，分别适用于不同场景。
// 计算二维点间欧几里得距离
#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);
}
上述代码实现了两种距离的计算逻辑。欧几里得距离对角度敏感，适合高精度雷达轨迹比对；曼哈顿距离抗噪声能力强，在航路分段评估中表现稳健。
| 指标 | 欧几里得距离 | 曼哈顿距离 |
|---|---|---|
| 计算复杂度 | 较高（含开方） | 低 |
| 空间适应性 | 连续自由空间 | 网格约束路径 |
在移动机器人导航中，网格地图是一种将连续环境离散化为规则网格的常用建模方式。每个网格单元代表环境中的一小块区域，通过赋值表示其通行状态。
通常采用数值编码区分自由、障碍和未知区域：
int grid_map[100][100]; // 初始化 100x100 地图
for(int i=0; i<100; i++) {
for(int j=0; j<100; j++) {
grid_map[i][j] = 0; // 全部设为自由空间
}
}
grid_map[50][50] = 1; // 在 (50,50) 设置障碍物
上述代码构建了一个二维整型数组模拟网格地图，数值 1 表示该位置存在障碍物，便于快速查询与路径规划算法集成。
更高级的方法使用概率占据模型，每个栅格存储障碍物存在的置信度，提升动态环境下的鲁棒性。
graph LR
A[开始] --> B{当前节点是否为目标?}
B -- 是 --> C[回溯路径]
B -- 否 --> D[生成邻居节点]
D --> E[计算 f = g + h]
E --> F[加入开放列表]
F --> G[选取最小 f 节点]
G --> B
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); // 曼哈顿距离
}
该结构体定义了 A*算法中节点的核心属性，heuristic 函数用于估算从当前点到目标点的距离，直接影响搜索效率与路径最优性。
在三维路径规划场景中，传统 A*算法需扩展至立体网格空间。节点状态由二维坐标 (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
该函数计算两点间直线距离，确保启发式满足可接纳性，避免高估导致非最优路径。
为提升搜索效率，引入二叉堆维护 open list：
在动态环境中，机器人需实时响应移动障碍物的干扰。为此，系统采用基于传感器反馈的动态窗口法（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.5m | 高优先级 | 紧急减速 + 轨迹重算 |
| 路径偏离 > 0.3m | 中优先级 | 局部修正路径 |
在自动驾驶路径规划中，航点生成是连接全局路径与局部控制的关键环节。原始路径通常由离散路点构成，需通过插值算法生成连续、可行驶的平滑轨迹。
常用方法包括等距采样与曲率自适应采样。后者在弯道区域加密航点，提升轨迹精度：
采用三次样条插值（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' 指定边界二阶导为零，避免端点抖动。
A*算法在路径规划中具有广泛的应用价值。通过合理的数据结构设计（如最小堆、哈希表）和启发式函数选择，可以显著提升搜索效率。在实际嵌入式或无人机系统中，需注意内存管理与实时性保障，结合 C 语言底层特性进行优化，确保算法在资源受限环境下的稳定运行。
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML转Markdown 互为补充。 在线工具，Markdown转HTML在线工具，online