MATLAB上在简单栅格地图中的Theta*算法代码

以下是MATLAB上的Theta*算法代码实现：

function [path, closed] = theta_star(start, goal, map)
    %start：起点坐标
    %goal：终点坐标
    %map：地图数据，0表示可行，1表示障碍物
    
    %初始化路径和封闭集合
    path = [];
    closed = zeros(size(map));
    
    %定义节点类
    classdef Node
        properties
            x;
            y;
            g;
            h;
            f;
            parent;
            obstacle;
        end
        methods
            function obj = Node(x, y, g, h, parent, obstacle)
                obj.x = x;
                obj.y = y;
                obj.g = g;
                obj.h = h;
                obj.f = obj.g + obj.h;
                obj.parent = parent;
                obj.obstacle = obstacle;
            end
        end
    end
    
    %启发式函数
    function h = heuristic(node1, node2)
        h = sqrt((node1.x - node2.x)^2 + (node1.y - node2.y)^2);
    end
    
    %计算代价
    function g = cost(node1, node2)
        if abs(node1.x - node2.x) == 1 && abs(node1.y - node2.y) == 1
            g = sqrt(2);
        else
            g = 1;
        end
    end
    
    %创建起点和终点节点
    start_node = Node(start(1), start(2), 0, heuristic(start, goal), [], false);
    goal_node = Node(goal(1), goal(2), 0, 0, [], false);
    
    %将起点加入开放集合
    open = [];
    open = [open, start_node];
    
    %循环直到开放集合为空
    while ~isempty(open)
        %找到f值最小的节点
        [~, index] = min([open.f]);
        curr_node = open(index);
        
        %将当前节点从开放集合中删除，并加入封闭集合
        open(index) = [];
        closed(curr_node.x, curr_node.y) = 1;
        
        %如果当前节点是终点，则构造路径
        if curr_node.x == goal_node.x && curr_node.y == goal_node.y
            path = [goal_node.x, goal_node.y];
            node = curr_node;
            while ~isempty(node.parent)
                path = [node.parent.x, node.parent.y; path];
                node = node.parent;
            end
            return
        end
        
        %遍历当前节点的邻居节点
        for i = curr_node.x-1:curr_node.x+1
            for j = curr_node.y-1:curr_node.y+1
                %如果越界或者是当前节点，则跳过
                if i < 1 || j < 1 || i > size(map, 1) || j > size(map, 2) || (i == curr_node.x && j == curr_node.y)
                    continue;
                end
                
                %如果是障碍物，则标记为障碍物节点
                if map(i, j) == 1
                    n = Node(i, j, Inf, Inf, [], true);
                %否则创建节点
                else
                    n = Node(i, j, Inf, heuristic([i, j], goal), [], false);
                end
                
                %如果该节点已在封闭集合中，则跳过
                if closed(i, j) == 1
                    continue;
                end
                
                %计算节点的g值
                if abs(i - curr_node.x) == 1 && abs(j - curr_node.y) == 1
                    tentative_g = curr_node.g + sqrt(2)*cost(curr_node, n);
                else
                    tentative_g = curr_node.g + cost(curr_node, n);
                end
                
                %如果该节点已在开放集合中且g值更小，则更新节点的g值和父节点
                index = find([open.x] == n.x & [open.y] == n.y);
                if ~isempty(index) && tentative_g < open(index).g
                    open(index).g = tentative_g;
                    open(index).f = open(index).g + open(index).h;
                    open(index).parent = curr_node;
                %否则将该节点加入开放集合
                elseif isempty(index)
                    n.g = tentative_g;
                    n.f = n.g + n.h;
                    n.parent = curr_node;
                    open = [open, n];
                end
                
                %如果该节点在终点和当前节点之间，并且不是障碍物，则检查是否可以直接连接
                if ~n.obstacle && (abs(i - goal_node.x) + abs(j - goal_node.y) == 1)
                    dx = i - curr_node.x;
                    dy = j - curr_node.y;
                    if map(curr_node.x+dx, curr_node.y+dy) == 0 && map(i-dx, j-dy) == 0
                        g = curr_node.g + cost(curr_node, n) + cost(n, goal_node);
                        if g < goal_node.g
                            goal_node.g = g;
                            goal_node.parent = curr_node;
                        end
                    end
                end
            end
        end
    end
end

该代码实现了Theta*算法，其中使用了节点类来存储节点的属性。算法的核心是启发式函数和代价函数的设计，以及对节点的处理和更新。算法的输出为路径和封闭集合。