D*路径规划算法 matlab代码

以下是D*路径规划算法的MATLAB代码：

function [path, cost] = Dstar(start, goal, h, get_successors, get_cost)
% D* algorithm for path planning
% start: start node coordinates (x,y)
% goal: goal node coordinates (x,y)
% h: heuristic function handle, h(node) -> cost
% get_successors: function handle, get_successors(node) -> successors
% get_cost: function handle, get_cost(node1, node2) -> cost
% path: optimal path from start to goal
% cost: cost of the optimal path

% initialize
OPEN = {};
CLOSED = {};
g = containers.Map('KeyType', 'int32', 'ValueType', 'double');
rhs = containers.Map('KeyType', 'int32', 'ValueType', 'double');
key = @(node) [min(g(node),rhs(node))+h(node), min(g(node),rhs(node))];
start_key = key(start);
goal_key = key(goal);
g(goal) = 0;
rhs(goal) = 0;
OPEN{1} = goal;
while ~isempty(OPEN) && (min(key(OPEN{1})) < start_key || rhs(start) ~= g(start))
    % get node with lowest key
    cur = OPEN{1};
    OPEN = OPEN(2:end);
    if g(cur) > rhs(cur)
        g(cur) = rhs(cur);
        CLOSED{end+1} = cur;
        for s = get_successors(cur)
            if s ~= start
                rhs(s) = min(rhs(s), get_cost(s, cur) + g(cur));
            end
            if ~ismember(s, CLOSED)
                if ~ismember(s, OPEN)
                    OPEN{end+1} = s;
                end
            else
                CLOSED(find(CLOSED==s, 1)) = [];
            end
        end
    else
        g(cur) = inf;
        for s = [get_successors(cur), cur]
            if s ~= start
                rhs(s) = min(rhs(s), get_cost(s, cur) + g(cur));
            end
            if ~ismember(s, CLOSED)
                if ~ismember(s, OPEN)
                    OPEN{end+1} = s;
                end
            else
                CLOSED(find(CLOSED==s, 1)) = [];
            end
        end
    end
end
path = [];
if isKey(g, start)
    node = start;
    path(end+1, :) = node;
    while ~isequal(node, goal)
        successors = get_successors(node);
        costs = zeros(size(successors));
        for i = 1:length(successors)
            costs(i) = get_cost(node, successors(i));
        end
        [~, index] = min(costs + arrayfun(@(s) rhs(s), successors));
        node = successors(index);
        path(end+1, :) = node;
    end
    cost = g(start);
else
    cost = inf;
end
end

其中，`start`和`goal`是起点和终点的坐标，`h`是启发式函数，`get_successors`是获取节点后继节点的函数，`get_cost`是获取节点和其后继节点之间的代价的函数。函数的返回值是一个最优路径和其代价。