Astar3D示例代码matlab

Astar3D是一种三维路径规划算法，它可以在三维空间中找到最短路径。下面是一个简单的Astar3D示例代码的MATLAB实现：

% 定义地图大小和起点、终点坐标
mapSize = [10, 10, 10];
startPos = [1, 1, 1];
goalPos = [10, 10, 10];

% 创建地图
map = zeros(mapSize);

% 设置障碍物
obstacles = [2, 2, 2; 3, 3, 3; 4, 4, 4]; % 障碍物坐标
for i = 1:size(obstacles, 1)
    map(obstacles(i, 1), obstacles(i, 2), obstacles(i, 3)) = 1;
end

% 定义启发式函数
heuristic = @(pos) norm(pos - goalPos);

% 定义移动代价函数
cost = @(pos1, pos2) norm(pos1 - pos2);

% 初始化起点和终点节点
startNode = struct('pos', startPos, 'g', 0, 'h', heuristic(startPos), 'f', 0);
goalNode = struct('pos', goalPos, 'g', Inf, 'h', 0, 'f', Inf);

% 初始化开放列表和关闭列表
openList = startNode;
closedList = [];

% 开始搜索
while ~isempty(openList)
    % 从开放列表中选择f值最小的节点作为当前节点
    [~, idx] = min([openList.f]);
    currentNode = openList(idx);
    
    % 如果当前节点是终点节点，则搜索完成
    if isequal(currentNode.pos, goalNode.pos)
        break;
    end
    
    % 将当前节点从开放列表中移除，并加入关闭列表
    openList(idx) = [];
    closedList = [closedList, currentNode];
    
    % 获取当前节点的邻居节点
    neighbors = getNeighbors(currentNode.pos, mapSize);
    
    for i = 1:size(neighbors, 1)
        neighborPos = neighbors(i, :);
        
        % 如果邻居节点在关闭列表中，则忽略
        if isNodeInList(neighborPos, closedList)
            continue;
        end
        
        % 计算邻居节点的g值
        g = currentNode.g + cost(currentNode.pos, neighborPos);
        
        % 如果邻居节点不在开放列表中，则加入开放列表
        neighborNode = struct('pos', neighborPos, 'g', g, 'h', heuristic(neighborPos), 'f', 0);
        if ~isNodeInList(neighborPos, openList)
            openList = [openList, neighborNode];
        else
            % 如果邻居节点已经在开放列表中，并且新的g值更小，则更新该节点的g值
            idx = getNodeIndex(neighborPos, openList);
            if g < openList(idx).g
                openList(idx).g = g;
            end
        end
    end
end

% 从终点开始回溯路径
path = [];
currentNode = goalNode;
while ~isequal(currentNode.pos, startNode.pos)
    path = [currentNode.pos; path];
    neighbors = getNeighbors(currentNode.pos, mapSize);
    minG = Inf;
    for i = 1:size(neighbors, 1)
        neighborPos = neighbors(i, :);
        if isNodeInList(neighborPos, closedList)
            idx = getNodeIndex(neighborPos, closedList);
            if closedList(idx).g < minG
                minG = closedList(idx).g;
                currentNode = closedList(idx);
            end
        end
    end
end
path = [startNode.pos; path];

% 显示地图和路径
figure;
hold on;
for i = 1:size(obstacles, 1)
    plot3(obstacles(i, 1), obstacles(i, 2), obstacles(i, 3), 'ro', 'MarkerSize', 10);
end
plot3(path(:, 1), path(:, 2), path(:, 3), 'b-', 'LineWidth', 2);
xlabel('X');
ylabel('Y');
zlabel('Z');
grid on;
axis equal;

% 获取邻居节点的函数
function neighbors = getNeighbors(pos, mapSize)
    neighbors = [];
    for dx = -1:1
        for dy = -1:1
            for dz = -1:1
                newPos = pos + [dx, dy, dz];
                if all(newPos >= 1) && all(newPos <= mapSize) && any([dx, dy, dz] ~= [0, 0, 0])
                    neighbors = [neighbors; newPos];
                end
            end
        end
    end
end

% 判断节点是否在列表中的函数
function isInList = isNodeInList(pos, nodeList)
    isInList = false;
    for i = 1:length(nodeList)
        if isequal(nodeList(i).pos, pos)
            isInList = true;
            break;
        end
    end
end

% 获取节点在列表中的索引的函数
function idx = getNodeIndex(pos, nodeList)
    idx = -1;
    for i = 1:length(nodeList)
        if isequal(nodeList(i).pos, pos)
            idx = i;
            break;
        end
    end
end

这段代码实现了一个简单的Astar3D算法，包括地图的创建、障碍物的设置、启发式函数和移动代价函数的定义、节点的初始化、开放列表和关闭列表的维护、路径搜索和路径回溯等功能。