基于A*算法的无人机三维路径规划matlab代码

以下是基于A*算法的无人机三维路径规划的MATLAB代码，代码实现了无人机从起点到终点的路径规划，并考虑了障碍物的避让。

clc;
clear all;
close all;

Map=ones(60,60,60); % 地图大小为60*60*60
Map(5:55,5:55,10:30)=0; % 地图中心区域设置为障碍物

start=[5 5 2]; % 起点坐标
goal=[55 55 55]; % 终点坐标

[Row,Col,Hei]=size(Map); % 地图的行、列、高
startNode=Node(start,[],0,0); % 起点
goalNode=Node(goal,[],0,0); % 终点
openList=startNode; % 开放列表
closeList=[]; % 封闭列表
current=startNode; % 当前节点

while ~isempty(openList)
    [~, minIndex] = min([openList.f]); % f值最小的节点
    current = openList(minIndex); % 当前节点
    if isequal(current.p,goalNode.p) % 到达终点
        path=[];
        while ~isempty(current.p)
            path=[current.p;path];
            current=current.parent;
        end
        path=[start;path;goal];
        break
    end
    index=1;
    for k=-1:1
        for j=-1:1
            for i=-1:1
                if (k~=0 || j~=0 || i~=0) && (current.p(1)+i>=1 && current.p(1)+i<=Row && current.p(2)+j>=1 && current.p(2)+j<=Col && current.p(3)+k>=1 && current.p(3)+k<=Hei)
                    neighbor=Node([current.p(1)+i,current.p(2)+j,current.p(3)+k],current,current.g+1,0);
                    if Map(neighbor.p(1),neighbor.p(2),neighbor.p(3))==0 && ~ismember(neighbor,closeList) % 不是障碍物且不在封闭列表中
                        if ~ismember(neighbor,openList) % 不在开放列表中
                            neighbor.h=heuristic(neighbor,goalNode); % 启发函数
                            neighbor.f=neighbor.g+neighbor.h; % f值
                            openList=[openList neighbor]; % 加入开放列表
                        else % 在开放列表中
                            [oldNode, index]=ismember(neighbor,openList);
                            if neighbor.g<oldNode.g % 更新f值
                                neighbor.f=neighbor.g+neighbor.h;
                                openList(index)=neighbor;
                            end
                        end
                    end
                end
            end
        end
    end
    closeList=[closeList current]; % 当前节点加入封闭列表
    openList(index)=[]; % 当前节点从开放列表中删除
end

% 绘制路径
figure(1);
[x,y,z]=ind2sub(size(Map),find(Map==0));
plot3(x,y,z,'square','MarkerSize',8,'MarkerFaceColor','k');
hold on;
for i=1:size(path,1)-1
    plot3([path(i,1) path(i+1,1)],[path(i,2) path(i+1,2)],[path(i,3) path(i+1,3)],'r','LineWidth',2);
end
axis([1 Row 1 Col 1 Hei]);
xlabel('X');
ylabel('Y');
zlabel('Z');
grid on;

其中，Node类用于表示节点，包括节点的位置、父节点、g值、h值和f值。启发函数采用曼哈顿距离，即当前节点到目标节点的水平、竖直、高度差的绝对值之和。