本文介绍的是基于 A*算法的三维无人机避障路径规划与轨迹跟踪控制 MATLAB 仿真程序。程序构建了一个包含多个三维长方体障碍物的飞行环境，通过占据栅格地图描述空间障碍信息，并在此基础上完成无人机从起点到终点的安全路径搜索、路径平滑以及轨迹跟踪控制仿真。

程序介绍

本文程序主要实现了一个 基于A*算法的三维无人机避障路径规划与轨迹跟踪控制仿真系统。程序首先构建三维飞行环境，并在地图中设置多个长方体障碍物，通过占据栅格的方式描述可飞行区域与障碍区域。在此基础上，利用三维 A* 算法搜索从起点到终点的安全路径，并对原始离散路径进行平滑处理，使其更适合作为无人机后续飞行控制的参考轨迹。

在路径规划完成后，程序进一步建立简化的无人机轨迹跟踪控制模型。无人机根据平滑后的参考路径进行飞行，通过位置误差和速度误差构造 PID 控制律，计算期望加速度、推力以及姿态角，实现对规划路径的连续跟踪。相比单纯的路径搜索程序，该代码不仅给出了可行路径，还进一步模拟了无人机沿路径飞行时的位置、速度、姿态和控制响应过程。

程序运行后，会输出三维路径规划结果、XY 平面投影、高度变化曲线、三轴位置跟踪曲线、跟踪误差曲线、速度与加速度响应、推力变化、姿态角变化以及避障安全距离曲线。同时，命令行还会输出路径长度、规划耗时、A*扩展节点数、平均跟踪误差、最大跟踪误差、P95误差、终端误差、最大速度、最大加速度、最大推力和最小避障距离等性能指标。

整体来看，该程序适合用于展示 三维A*路径规划、无人机避障飞行、路径平滑处理、轨迹跟踪控制和飞行安全性评估 等内容。程序结构完整，结果图较丰富，既可以作为无人机路径规划算法的仿真案例，也可以作为后续改进 RRT、DWA、人工势场法、MPC 控制、多无人机协同避障等算法的基础框架。

运行结果

程序运行后，主要输出以下结果：

1. 三维A*路径规划与无人机轨迹跟踪图
   

三维环境中的障碍物、A*规划路径、平滑参考路径以及无人机实际飞行轨迹。可以直观观察无人机是否能够避开障碍物，并沿着规划路径完成飞行任务。

2. XY平面投影与高度剖面图
   

XY投影图用于观察无人机在水平面内的避障路径，高度剖面图用于分析无人机在飞行过程中的高度变化情况。

3. 三轴位置跟踪曲线
   

分别绘制参考轨迹和实际轨迹在 X、Y、Z 三个方向上的位置变化，用于评估无人机对参考路径的跟踪能力。

4. 跟踪误差曲线
   

输出 X 轴误差、Y 轴误差、Z 轴误差以及综合三维跟踪误差，同时对比水平误差和高度误差，便于分析轨迹跟踪精度。

5. 速度、加速度和推力响应曲线
   

程序绘制无人机三轴速度、速度模值、实际加速度、控制加速度和推力输出，用于分析控制过程是否平稳，以及是否满足飞行器动力学约束。

6. 姿态角变化曲线
   

分别输出横滚角、俯仰角和偏航角变化，用于观察无人机在路径跟踪过程中的姿态响应。

7. 避障安全距离曲线
   

该图用于分析参考轨迹和实际飞行轨迹到最近障碍物的距离变化。如果最近障碍物距离始终大于 0，说明轨迹没有与障碍物发生碰撞。

命令行输出：

这些指标可以较全面地评价路径规划效率、路径质量、轨迹跟踪精度和避障安全性。

MATLAB源代码

程序结构：

部分代码如下：

%% A* 无人机避障与控制算法 - 优化版
% 基于A*算法的三维路径规划 + 路径平滑 + 无人机轨迹跟踪控制
% 作者：matlabfilter
% 接定位与导航、滤波相关的matlab代码定制
% 2026-05-12/Ver1

clear; clc; close all;
rng(0);

%% 环境设置
fprintf('=== 初始化环境 ...\n');
MAP_SIZE = [100, 100, 50];  % 地图尺寸 [X, Y, Z] (米)
GRID_RES = 2.0;             % 网格分辨率 (米/格)
NX = floor(MAP_SIZE(1) / GRID_RES);  % X方向网格数
NY = floor(MAP_SIZE(2) / GRID_RES);  % Y方向网格数
NZ = floor(MAP_SIZE(3) / GRID_RES);  % Z方向网格数
% 定义长方体障碍物 [x, y, z, 宽度, 深度, 高度]
obstacles = [
    20, 20, 0,  10, 10, 15;
    40, 60, 0,  12, 12, 20;
    60, 30, 0,  8,  8,  12;
    30, 70, 0,  15, 10, 18;
    70, 70, 0,  10, 15, 10;
    50, 45, 0,  6,  6,  25;
    80, 20, 0,  8,  8,  8;
    10, 80, 0,  12, 8,  14;
    ];
fprintf('   构建占据网格 (%dx%dx%d) ...\n', NX, NY, NZ);
occ_grid = false(NX, NY, NZ);  % 占据网格，true表示被占据
% 生成网格中心坐标
x_grid = ((1:NX) - 0.5) * GRID_RES;
y_grid = ((1:NY) - 0.5) * GRID_RES;
z_grid = ((1:NZ) - 0.5) * GRID_RES;
[Xg, Yg, Zg] = ndgrid(x_grid, y_grid, z_grid);
% 将障碍物投影到网格
for o = 1:size(obstacles, 1)
    ox = obstacles(o,1); oy = obstacles(o,2); oz = obstacles(o,3);
    ow = obstacles(o,4); oh = obstacles(o,5); od = obstacles(o,6);
    occ_grid = occ_grid | (Xg >= ox & Xg <= ox+ow & Yg >= oy & Yg <= oy+oh & Zg >= oz & Zg <= oz+od);
end
SAFETY_RADIUS = 2;  % 安全膨胀半径 (米)
occ_grid_dilated = inflate_occupancy_grid(occ_grid, SAFETY_RADIUS);

%% 起点和终点
start_pos = [5,  5,  3];   % 起点世界坐标 (米)
goal_pos  = [95, 95, 20];  % 终点世界坐标 (米)
% 转换为网格索引
start_idx = ceil(start_pos / GRID_RES);
goal_idx  = ceil(goal_pos  / GRID_RES);
% 边界裁剪
start_idx = max(1, min([NX, NY, NZ], start_idx));
goal_idx  = max(1, min([NX, NY, NZ], goal_idx));
fprintf('   起点: (%.1f, %.1f, %.1f) -> 网格 (%d, %d, %d)\n', ...
    start_pos(1), start_pos(2), start_pos(3), start_idx(1), start_idx(2), start_idx(3));
fprintf('   终点: (%.1f, %.1f, %.1f) -> 网格 (%d, %d, %d)\n', ...
    goal_pos(1), goal_pos(2), goal_pos(3), goal_idx(1), goal_idx(2), goal_idx(3));
% 检查起终点是否在障碍物内
if occ_grid_dilated(start_idx(1), start_idx(2), start_idx(3))
    error('起点在障碍物内！请重新设置起点。');
end
if occ_grid_dilated(goal_idx(1), goal_idx(2), goal_idx(3))
    error('终点在障碍物内！请重新设置终点。');
end

%% A* 路径规划

完整代码下载链接：
https://download.csdn.net/download/callmeup/92865783

如有代码定制、讲解等需求，可通过下方卡片联系我：