三维地图上的a*路径规划算法matlab程序

以下是一个简单的三维地图上的A*路径规划算法的Matlab程序示例：

function [path, cost] = Astar_3D(start, goal, obstacles, resolution)
% Astar_3D - A* algorithm for 3D path planning
% Usage:
%     [path, cost] = Astar_3D(start, goal, obstacles, resolution)
% Inputs:
%     start     - starting point [x, y, z]
%     goal      - goal point [x, y, z]
%     obstacles - obstacles matrix, 3D binary matrix (0 for free, 1 for obstacle)
%     resolution - resolution of the obstacles matrix
% Outputs:
%     path      - path from start to goal, a list of [x, y, z] coordinates
%     cost      - total cost of the path
%
% Author: Zexi Shao
% Website: https://github.com/zexi/astar-3d-matlab

% set up search parameters
MAX_ITER = 100000;
EPSILON = 0.1;

% scale start and goal coordinates to the resolution
start = round(start ./ resolution);
goal = round(goal ./ resolution);

% initialize the open and closed list
open_list = [start 0 heuristic(start, goal)];
closed_list = [];

% start the search
for i = 1:MAX_ITER
    % check if the open list is empty
    if isempty(open_list)
        path = [];
        cost = inf;
        return;
    end
    
    % get the node with the lowest cost from the open list
    [~, idx] = min(open_list(:,end));
    current = open_list(idx,:);
    
    % check if we have reached the goal
    if current(1:3) == goal
        path = reconstruct_path(current);
        cost = current(4);
        return;
    end
    
    % move the current node from the open to closed list
    open_list(idx,:) = [];
    closed_list = [closed_list; current];
    
    % expand the current node
    for j = -1:1
        for k = -1:1
            for l = -1:1
                % skip the current node
                if j == 0 && k == 0 && l == 0
                    continue;
                end
                
                % calculate the new node
                next = current + [j, k, l, 1];
                next_cost = current(4) + cost(current, next);
                
                % check if the new node is valid
                if next(1) < 1 || next(1) > size(obstacles,1) || ...
                   next(2) < 1 || next(2) > size(obstacles,2) || ...
                   next(3) < 1 || next(3) > size(obstacles,3) || ...
                   obstacles(next(1), next(2), next(3)) == 1 || ...
                   ismember(next, closed_list, 'rows')
                    continue;
                end
                
                % check if the new node is already in the open list
                idx = find(ismember(open_list(:,1:3), next(1:3), 'rows'));
                if isempty(idx)
                    % add the new node to the open list
                    open_list = [open_list; next next_cost heuristic(next, goal)];
                else
                    % update the cost of the existing node
                    if open_list(idx,4) > next_cost
                        open_list(idx,4) = next_cost;
                        open_list(idx,5) = heuristic(next, goal);
                    end
                end
            end
        end
    end
end

% if we reach here, the search has failed
path = [];
cost = inf;

function c = cost(node1, node2)
% cost - cost function between two nodes
% Inputs:
%     node1 - first node [x, y, z, cost]
%     node2 - second node [x, y, z, cost]
% Outputs:
%     c - cost between the two nodes

if node1(1:3) == node2(1:3)
    c = 0;
else
    c = norm(node1(1:3)-node2(1:3));
end

function h = heuristic(node, goal)
% heuristic - heuristic function
% Inputs:
%     node - current node [x, y, z, cost]
%     goal - goal node [x, y, z]
% Outputs:
%     h - heuristic value

h = norm(node(1:3)-goal);

function path = reconstruct_path(node)
% reconstruct_path - reconstruct the path from start to node
% Inputs:
%     node - the end node [x, y, z, cost]
% Outputs:
%     path - the path from start to node

path = [];
while ~isempty(node)
    path = [node(1:3)*resolution; path];
    node = node(6:end);
end

该程序的输入参数包括起点、终点、障碍物矩阵和分辨率。其中，障碍物矩阵是一个三维的二进制矩阵，表示障碍物的位置，分辨率则是将坐标值转换为矩阵中的索引。程序输出包括路径和总代价。

请注意，这只是一个简单的示例程序，可能需要根据实际应用进行修改和优化。