matlab中的三维A*算法

三维A*算法是一种用于在三维空间中搜索最短路径的算法。它基于A*算法，但需要考虑第三个维度（高度）的影响。以下是一个简单的Matlab实现：

首先，定义节点类：

classdef Node
    properties
        x; % x坐标
        y; % y坐标
        z; % z坐标
        g; % 起点到该节点的实际代价
        h; % 该节点到终点的预估代价
        f; % f = g + h
        parent; % 父节点
        is_obstacle; % 是否为障碍物
    end
    
    methods
        function node = Node(x, y, z, is_obstacle)
            node.x = x;
            node.y = y;
            node.z = z;
            node.g = Inf;
            node.h = Inf;
            node.f = Inf;
            node.parent = [];
            node.is_obstacle = is_obstacle;
        end
        
        function node = set_g(node, g)
            node.g = g;
            node.f = node.g + node.h;
        end
        
        function node = set_h(node, h)
            node.h = h;
            node.f = node.g + node.h;
        end
        
        function cost = get_cost(node, other_node)
            % 计算两个节点之间的代价
            dx = abs(node.x - other_node.x);
            dy = abs(node.y - other_node.y);
            dz = abs(node.z - other_node.z);
            if dx > dy
                temp = dx;
                dx = dy;
                dy = temp;
            end
            if dy > dz
                temp = dy;
                dy = dz;
                dz = temp;
            end
            cost = sqrt(3)*dx + sqrt(2)*(dy-dx) + (dz-dy);
        end
        
        function bool = is_equal(node, other_node)
            bool = (node.x == other_node.x) && (node.y == other_node.y) && (node.z == other_node.z);
        end
    end
end

然后，定义一个三维A*搜索类：

classdef AStar3D
    properties
        start_node; % 起点
        end_node; % 终点
        open_list; % 开放列表（未遍历的节点）
        closed_list; % 封闭列表（已遍历的节点）
        nodes; % 所有节点的列表
        grid_size; % 网格大小（x, y, z）
        resolution; % 分辨率
        obstacles; % 障碍物
    end
    
    methods
        function astar = AStar3D(grid_size, resolution)
            astar.grid_size = grid_size;
            astar.resolution = resolution;
            astar.obstacles = false(grid_size);
            astar.nodes = cell(grid_size);
            for x = 1:grid_size(1)
                for y = 1:grid_size(2)
                    for z = 1:grid_size(3)
                        astar.nodes{x,y,z} = Node(x, y, z, astar.obstacles(x,y,z));
                    end
                end
            end
        end
        
        function astar = set_obstacle(astar, x, y, z, is_obstacle)
            astar.obstacles(x,y,z) = is_obstacle;
            astar.nodes{x,y,z}.is_obstacle = is_obstacle;
        end
        
        function astar = set_start(astar, x, y, z)
            astar.start_node = astar.nodes{x,y,z};
        end
        
        function astar = set_end(astar, x, y, z)
            astar.end_node = astar.nodes{x,y,z};
        end
        
        function path = find_path(astar)
            % 初始化起点
            astar.start_node.g = 0;
            astar.start_node.h = astar.start_node.get_cost(astar.end_node);
            astar.start_node.f = astar.start_node.h;
            astar.open_list = [astar.start_node];
            astar.closed_list = [];
            
            while ~isempty(astar.open_list)
                % 从开放列表中取出代价最小的节点
                current_node = astar.open_list(1);
                for i = 1:length(astar.open_list)
                    if astar.open_list(i).f < current_node.f
                        current_node = astar.open_list(i);
                    end
                end
                
                % 如果当前节点为终点，则返回路径
                if current_node.is_equal(astar.end_node)
                    path = astar.get_path(current_node);
                    return;
                end
                
                % 从开放列表中移除当前节点，并加入封闭列表中
                astar.open_list = setdiff(astar.open_list, current_node);
                astar.closed_list = [astar.closed_list, current_node];
                
                % 遍历当前节点的所有相邻节点
                neighbors = astar.get_neighbors(current_node);
                for i = 1:length(neighbors)
                    neighbor = neighbors(i);
                    
                    % 如果相邻节点已在封闭列表中，则跳过
                    if ismember(neighbor, astar.closed_list)
                        continue;
                    end
                    
                    % 计算从起点到该相邻节点的代价
                    tentative_g = current_node.g + current_node.get_cost(neighbor);
                    
                    % 如果相邻节点不在开放列表中，或者从起点到该相邻节点的代价更小
                    if ~ismember(neighbor, astar.open_list) || tentative_g < neighbor.g
                        % 更新相邻节点的代价
                        neighbor.parent = current_node;
                        neighbor = neighbor.set_g(tentative_g);
                        neighbor = neighbor.set_h(neighbor.get_cost(astar.end_node));
                        
                        % 如果相邻节点不在开放列表中，则加入开放列表中
                        if ~ismember(neighbor, astar.open_list)
                            astar.open_list = [astar.open_list, neighbor];
                        end
                    end
                end
            end
            
            % 如果开放列表为空且未找到路径，则返回空
            path = [];
        end
        
        function path = get_path(astar, node)
            % 返回从起点到指定节点的路径
            path = [node];
            while ~isempty(node.parent)
                node = node.parent;
                path = [node, path];
            end
        end
        
        function neighbors = get_neighbors(astar, node)
            % 返回当前节点的所有相邻节点
            neighbors = [];
            for x = -1:1
                for y = -1:1
                    for z = -1:1
                        if x == 0 && y == 0 && z == 0
                            continue;
                        end
                        neighbor_x = node.x + x;
                        neighbor_y = node.y + y;
                        neighbor_z = node.z + z;
                        if neighbor_x < 1 || neighbor_x > astar.grid_size(1) || ...
                           neighbor_y < 1 || neighbor_y > astar.grid_size(2) || ...
                           neighbor_z < 1 || neighbor_z > astar.grid_size(3)
                            continue;
                        end
                        neighbor = astar.nodes{neighbor_x, neighbor_y, neighbor_z};
                        if neighbor.is_obstacle
                            continue;
                        end
                        neighbors = [neighbors, neighbor];
                    end
                end
            end
        end
        
        function plot_path(astar, path)
            % 绘制路径
            [x,y,z] = meshgrid(1:astar.grid_size(1), 1:astar.grid_size(2), 1:astar.grid_size(3));
            figure;
            scatter3(x(astar.obstacles), y(astar.obstacles), z(astar.obstacles), 'k', 'filled');
            hold on;
            scatter3(astar.start_node.x, astar.start_node.y, astar.start_node.z, 'r', 'filled');
            scatter3(astar.end_node.x, astar.end_node.y, astar.end_node.z, 'g', 'filled');
            for i = 1:length(path)-1
                plot3([path(i).x, path(i+1).x], [path(i).y, path(i+1).y], [path(i).z, path(i+1).z], 'b', 'LineWidth', 2);
            end
            axis equal;
            xlabel('x');
            ylabel('y');
            zlabel('z');
        end
    end
end

最后，使用以下代码创建一个AStar3D对象，设置起点、终点、障碍物，并搜索路径：

grid_size = [10, 10, 10];
resolution = 1;
astar = AStar3D(grid_size, resolution);
astar.set_start(1, 1, 1);
astar.set_end(10, 10, 10);
astar.set_obstacle(3, 3, 3, true);
astar.set_obstacle(4, 3, 3, true);
astar.set_obstacle(5, 3, 3, true);
path = astar.find_path();
astar.plot_path(path);