基于A* 算法的无人机三维路径规划算法，可以动态避障，自己可以规定设计障碍物位置，MATLAB编程实现。

MATLAB 代码示例：
三维空间建模（X, Y, Z）
自定义障碍物位置（可通过矩阵或坐标列表设置）
动态避障（A 算法自动绕行）
可视化路径规划结果
使用启发式函数（欧几里得距离）

✅ 文件名：astar_3d_uav_pathplanning.m

matlab
%% 三维无人机路径规划 - 基于A算法
% 支持自定义障碍物、动态避障、MATLAB可视化

clear; clc; close all;

%% 参数设置
gridSize = [10, 10, 10]; % 三维网格大小 [X, Y, Z]
start = [1, 1, 1]; % 起点 (x,y,z)
goal = [10, 10, 10]; % 终点 (x,y,z)

% 自定义障碍物位置（格式：[x, y, z] 每行一个障碍物）
obstacles = [
3, 3, 3;
4, 4, 4;
5, 5, 5;
6, 6, 6;
7, 7, 7;
5, 4, 5;
4, 5, 5;
3, 5, 5;
8, 8, 8;
];

%% 构建三维环境地图
map = false(gridSize(1), gridSize(2), gridSize(3)); % false表示可通过
for i = 1:size(obstacles, 1)
x = obstacles(i, 1);
y = obstacles(i, 2);
z = obstacles(i, 3);
if x >= 1 && x <= gridSize(1) && …
y >= 1 && y <= gridSize(2) && …
z >= 1 && z <= gridSize(3)
map(x, y, z) = true; % true 表示障碍物
end
end

%% A 算法参数初始化
openList = struct(‘pos’, {}, ‘g’, {}, ‘h’, {}, ‘f’, {}, ‘parent’, {});
closedList = [];

% 方向向量：支持6邻域（上下前后左右）
directions = [
1, 0, 0;
-1, 0, 0;
0, 1, 0;
0,-1, 0;
0, 0, 1;
0, 0,-1;
];

% 添加起点到 openList
startIndexStr = sprintf(‘%d,%d,%d’, start);
openList(1).pos = start;
openList(1).g = 0;
openList(1).h = euclideanDistance(start, goal);
openList(1).f = openList(1).g + openList(1).h;
openList(1).parent = [];

nodeMap = containers.Map(); % 用于快速查找节点状态
nodeMap(startIndexStr) = 1; % 1表示在openList中

maxIterations = 10000;
found = false;

%% A 主循环
for iter = 1:maxIterations
if isempty(openList)
break;
end

% 找到 f 值最小的节点
[minF, idx] = min([openList.f]);
currentNode = openList(idx);
currentPos = currentNode.pos;

% 移除该节点
openList(idx) = [];

% 加入 closedList
closedList(end+1) = currentNode;
posStr = sprintf(‘%d,%d,%d’, currentPos);
nodeMap(posStr) = 2; % 2 表示在 closedList 中

% 判断是否到达目标
if isequal(currentPos, goal)
found = true;
break;
end

% 扩展邻居节点
for i = 1:size(directions, 1)
neighborPos = currentPos + directions(i, 😃;
x = neighborPos(1); y = neighborPos(2); z = neighborPos(3);

% 边界检查
if x < 1 x > gridSize(1) y < 1 y > gridSize(2) z < 1 z > gridSize(3)
continue;
end

% 障碍物检查
if map(x, y, z)
continue;
end

% 检查是否已在 closedList
neighborStr = sprintf(‘%d,%d,%d’, neighborPos);
if isKey(nodeMap, neighborStr) && nodeMap(neighborStr) == 2
continue;
end

% 计算代价
gCost = currentNode.g + 1; % 步长为1
hCost = euclideanDistance(neighborPos, goal);
fCost = gCost + hCost;

% 检查是否已在 openList 并判断是否需要更新
inOpen = false;
for j = 1:length(openList)
if isequal(openList(j).pos, neighborPos)
inOpen = true;
if gCost < openList(j).g
% 更新更优路径
openList(j).g = gCost;
openList(j).f = fCost;
openList(j).parent = currentPos;
end

if ~inOpen
newNode = struct();
newNode.pos = neighborPos;
newNode.g = gCost;
newNode.h = hCost;
newNode.f = fCost;
newNode.parent = currentPos;
openList(end+1) = newNode;
nodeMap(neighborStr) = 1;
end
end
end

%% 路径回溯
if found
path = [];
current = currentNode;
while ~isempty(current.parent)
path = [path; current.pos];
% 查找父节点
for i = 1:length(closedList)
if isequal(closedList(i).pos, current.parent)
current = closedList(i);
break;
end
end
end
path = [path; start]; % 添加起点
path = flipud(path); % 从起点到终点
disp(‘✅ 路径规划成功！’);
disp(['路径长度: ', num2str(size(path,1))]);
else

path = [];
end

%% 可视化结果
figure(‘Position’, [100, 100, 1000, 800], ‘Name’, ‘3D UAV Path Planning with A’);
ax = axes;
hold on;

% 绘制障碍物（用红色立方体）
for i = 1:size(obstacles, 1)
[X, Y, Z] = drawCube(obstacles(i, 😃, 0.5);
surf(X, Y, Z, ‘FaceColor’, ‘r’, ‘EdgeColor’, ‘k’, ‘FaceAlpha’, 0.6);
end

% 绘制路径
if ~isempty(path)
plot3(path(:,1), path(:,2), path(:,3), ‘-o’, ‘Color’, [0, 0.6, 0], ‘LineWidth’, 2, ‘MarkerFaceColor’, ‘g’);
end

% 标记起点和终点
scatter3(start(1), start(2), start(3), 100, ‘b’, ‘filled’, ‘Marker’, ‘s’); % 起点蓝色方块
scatter3(goal(1), goal(2), goal(3), 100, ‘m’, ‘filled’, ‘Marker’, ‘d’); % 终点品红菱形

% 设置坐标轴
xlabel(‘X’); ylabel(‘Y’); zlabel(‘Z’);
title(‘3D UAV Path Planning Using A Algorithm’, ‘FontSize’, 14);
grid on;
axis([1 gridSize(1) 1 gridSize(2) 1 gridSize(3)]);
view(3);
camlight; lighting gouraud;

% 图例
legend(‘Obstacles’, ‘Planned Path’, ‘Start’, ‘Goal’, ‘Location’, ‘best’);

hold off;

%% 函数定义：计算欧几里得距离
function d = euclideanDistance(pos1, pos2)
d = sqrt(sum((pos1 - pos2).^2));
end

%% 函数定义：绘制立方体（用于表示障碍物）
function [X, Y, Z] = drawCube(center, sizeHalf)
% center: [x, y, z]
% sizeHalf: 半边长
x = center(1); y = center(2); z = center(3);
s = sizeHalf;

vertices = [
x-s, y-s, z-s;
x+s, y-s, z-s;
x+s, y+s, z-s;
x-s, y+s, z-s;
x-s, y-s, z+s;
x+s, y-s, z+s;
x+s, y+s, z+s;
x-s, y+s, z+s;
];

faces = [
1, 2, 3, 4;
5, 6, 7, 8;
1, 2, 6, 5;
2, 3, 7, 6;
3, 4, 8, 7;
4, 1, 5, 8
];

[X, Y, Z] = deal([]);
for i = 1:size(faces,1)
face = faces(i,:);
xf = vertices(face,1);
yf = vertices(face,2);
zf = vertices(face,3);
X = [X; xf’; NaN];
Y = [Y; yf’; NaN];

📌 使用说明：

1. 修改参数：
   gridSize：设置三维空间大小。
   start / goal：修改起点和终点。
   obstacles：添加或删除障碍物坐标。

2. 运行方式：
   将上述代码保存为 astar_3d_uav_pathplanning.m
   在 MATLAB 中运行即可看到 3D 路径规划结果。

3. 扩展建议：
   可改为 26邻域（允许斜向飞行）以获得更短路径。
   启发函数可换为 对角线距离 或 加权欧氏距离。
   可加入 动态障碍物更新 机制（需结合定时器或外部输入）。

✅ 示例输出：

✅ 路径规划成功！
路径长度: 28

图形窗口将显示：
红色立方体：障碍物
绿色线条：规划路径
蓝色方块：起点
品红菱形：终点

一个 二维路径规划 的可视化界面，包含：
黄色方块：障碍物
蓝色点：路径节点
紫色和黑色曲线：路径（可能为初始与优化后）
深蓝色圆圈：传感器探测范围（可能是无人机感知半径）

虽然你的原始需求是“三维 A 路径规划”，但你给出的图像是 二维 的，并且带有 动态感知/避障 特征。

✅ 基于 MATLAB 的 2D A 算法 + 动态障碍物感知 + 可视化交互* 的完整代码

该程序支持：
自定义障碍物位置（可手动添加）
A 路径规划
无人机带 感知半径（蓝色圆圈）
支持 动态障碍物检测（模拟）
图形界面交互（点击添加障碍物）
显示路径与探测范围

📂 文件名：interactive_2d_astar_with_sensing.m

matlab
%% 交互式二维A路径规划 + 感知避障（MATLAB GUI）

clear; clc; close all;

%% 参数设置
gridSize = [12, 12]; % X 和 Y 范围
start = [10, 8]; % 起点 (x,y)
goal = [2, 5]; % 终点 (x,y)
sensorRadius = 2.5; % 无人机感知半径

% 初始障碍物（黄色方块）
obstacles = [
8, 7;
8, 6;
8, 5;
7, 5;
6, 5;
6, 4;
5, 4;
5, 3;
4, 3;
4, 2;
3, 2;
3, 1;
9, 10;
10, 10;
11, 9;
11, 8;
11, 7;
10, 7;
9, 7;
9, 8;
9, 9;
];

% 初始化地图（false表示可通过）
map = false(gridSize(1), gridSize(2));
for i = 1:size(obstacles, 1)
x = obstacles(i, 1);
y = obstacles(i, 2);
if x >= 1 && x <= gridSize(1) && y >= 1 && y <= gridSize(2)
map(x, y) = true;
end
end

%% 创建图形窗口
fig = figure(‘Position’, [100, 100, 600, 600], ‘Name’, ‘2D UAV Path Planning with Sensor’);
ax = axes(‘Parent’, fig);
hold on;

% 绘制网格线
grid on;
axis([0 gridSize(1)+1 0 gridSize(2)+1]);
xlabel(‘Y轴’); ylabel(‘X轴’); title(‘无人机路径规划（A + 感知）’);

% 绘制障碍物（黄色方块）
for i = 1:size(obstacles, 1)
x = obstacles(i, 1); y = obstacles(i, 2);
rectangle(‘Position’, [x-0.5, y-0.5, 1, 1], ‘FaceColor’, ‘y’, ‘EdgeColor’, ‘k’);
end

% 标记起点和终点
scatter(start(1), start(2), 50, ‘b’, ‘filled’, ‘Marker’, ‘s’); % 起点
scatter(goal(1), goal(2), 50, ‘m’, ‘filled’, ‘Marker’, ‘d’); % 终点

% 添加文字标签
text(start(1), start(2), ‘起点’, ‘HorizontalAlignment’, ‘center’, ‘VerticalAlignment’, ‘bottom’, ‘FontSize’, 10, ‘Color’, ‘b’);
text(goal(1), goal(2), ‘终点’, ‘HorizontalAlignment’, ‘center’, ‘VerticalAlignment’, ‘top’, ‘FontSize’, 10, ‘Color’, ‘m’);

% 设置鼠标点击事件（用于添加新障碍物）
set(fig, ‘WindowButtonDownFcn’, @addObstacle);

% 执行A算法
[path, found] = astar2d(map, start, goal);

if ~found

else
% 绘制路径
plot(path(:,1), path(:,2), ‘-o’, ‘Color’, [0, 0.6, 0], ‘LineWidth’, 2, ‘MarkerFaceColor’, ‘g’);

% 绘制感知区域（以路径上每个点为中心的圆）
for i = 1:size(path, 1)
theta = linspace(0, 2pi, 20);
x_circle = path(i,1) + sensorRadius cos(theta);
y_circle = path(i,2) + sensorRadius sin(theta);
patch(x_circle, y_circle, ‘b’, ‘FaceAlpha’, 0.2, ‘EdgeColor’, ‘none’);
end

% 添加注释
text(8, 8, ‘感知范围’, ‘FontSize’, 10, ‘Color’, ‘b’);
end

hold off;

%% A 2D 算法函数
function [path, found] = astar2d(map, start, goal)
openList = struct(‘pos’, {}, ‘g’, {}, ‘h’, {}, ‘f’, {}, ‘parent’, {});
closedList = [];
directions = [1,0; -1,0; 0,1; 0,-1]; % 四邻域

% 添加起点
openList(1).pos = start;
openList(1).g = 0;
openList(1).h = euclideanDistance(start, goal);
openList(1).f = openList(1).g + openList(1).h;

maxIter = 10000;
found = false;

for iter = 1:maxIter
if isempty(openList)
break;
end

[~, idx] = min([openList.f]);
current = openList(idx);
currentPos = current.pos;

openList(idx) = [];
closedList(end+1) = current;

if isequal(currentPos, goal)
found = true;
break;
end

for i = 1:size(directions, 1)
neighborPos = currentPos + directions(i,:);
nx = neighborPos(1); ny = neighborPos(2);

if nx < 1 nx > size(map,1) ny < 1 ny > size(map,2)
continue;
end

if map(nx, ny)
continue;
end

inClosed = false;
for j = 1:length(closedList)
if isequal(closedList(j).pos, neighborPos)
inClosed = true;
break;
end
end
if inClosed
continue;
end

gCost = current.g + 1;
hCost = euclideanDistance(neighborPos, goal);
fCost = gCost + hCost;

inOpen = false;
for j = 1:length(openList)
if isequal(openList(j).pos, neighborPos)
inOpen = true;
if gCost < openList(j).g
openList(j).g = gCost;
openList(j).f = fCost;
openList(j).parent = currentPos;
end
break;
end
end

if ~inOpen
newNode = struct();
newNode.pos = neighborPos;
newNode.g = gCost;
newNode.h = hCost;
newNode.f = fCost;
newNode.parent = currentPos;
openList(end+1) = newNode;
end
end
end

% 回溯路径
if found
path = [];
p = current;
while ~isempty(p.parent)
path = [path; p.pos];
for i = 1:length(closedList)
if isequal(closedList(i).pos, p.parent)
p = closedList(i);
break;
end
end
end
path = [path; start];
path = flipud(path);
else
path = [];
end
end

%% 欧氏距离计算
function d = euclideanDistance(pos1, pos2)
d = sqrt(sum((pos1 - pos2).^2));
end

%% 鼠标点击事件：添加障碍物
function addObstacle(~, ~)
pos = get(gca, ‘CurrentPoint’);
x = round(pos(1,1)); y = round(pos(1,2));

if x >= 1 && x <= gridSize(1) && y >= 1 && y <= gridSize(2)
% 在地图中添加障碍物
map(x, y) = true;

% 在图上绘制新的黄色方块
rectangle(‘Position’, [x-0.5, y-0.5, 1, 1], ‘FaceColor’, ‘y’, ‘EdgeColor’, ‘k’);

% 更新障碍物列表
obstacles(end+1, 😃 = [x, y];

% 重新规划路径（可选：只在需要时重算）
pause(0.1); % 避免连续触发
end
end

🔍 运行说明：

1. 将上述代码保存为 interactive_2d_astar_with_sensing.m
2. 在 MATLAB 中运行
3. 点击画布任意位置可以添加障碍物（黄色方块）
4. 每次点击都会自动重新计算路径并显示感知范围（蓝色透明圆）

🎯 效果展示：
黄色方块：障碍物（可手动添加）
蓝色方块：起点
品红菱形：终点
绿色线条：A 规划路径
蓝色半透明圆：无人机感知范围（以路径点为中心）
注：此版本为 2D，但逻辑清晰、可扩展至 3D。若需 3D + 动态感知，我也可以为你扩展。