基于MATLAB的三维A*路径规划程序

以下是一个基于MATLAB的三维A*路径规划程序，包含详细注释和可视化功能，可直接运行：

```matlab
% 三维A*路径规划

%% 初始化参数
clear; clc; close all;

% 定义三维空间尺寸（5x5x5）
mapSize = [5, 5, 5];

% 创建三维障碍物地图（1表示障碍物）
obstacleMap = zeros(mapSize);
obstacleMap(:,:,3) = [0 0 0 0 0;   % 第三层设置障碍物
                      0 1 1 1 0;
                      0 1 1 1 0;
                      0 1 1 1 0;
                      0 0 0 0 0];

% 定义起点和终点坐标（[x,y,z]格式）
startPos = [1, 1, 1];
goalPos = [5, 5, 5];

% 可视化参数设置
showAnimation = true;  % 显示搜索过程动画
pathColor = [1 0 0];   % 路径颜色（红色）
obsColor = [0.5 0.5 0.5]; % 障碍物颜色（灰色）

%% A*算法实现
% 初始化节点结构体数组
nodes = struct('pos', {}, 'g', {}, 'h', {}, 'f', {}, 'parent', {});

% 创建初始节点
startNode = struct('pos', startPos, 'g', 0, 'h', 0, 'f', 0, 'parent', []);
nodes(1) = startNode;

% 创建开放列表和关闭列表
openList = containers.Map(num2str(startPos), startNode);
closedList = containers.Map;

% 定义移动方向（6邻域）
directions = [1 0 0;  % 右
             -1 0 0;  % 左
             0 1 0;   % 前