informed RRT*的MATLAB源码

以下是informed RRT*算法的MATLAB源码：

function [path, tree] = informed_rrt_star(start, goal, map, varargin)
%INFORMED_RRT_STAR Informed RRT* path planning algorithm
%
%   [PATH, TREE] = INFORMED_RRT_STAR(START, GOAL, MAP) plans a path from
%   START to GOAL avoiding obstacles defined in the binary occupancyMap MAP.
%   The output PATH is an M-by-2 matrix of [X,Y] coordinates and the output
%   TREE is a robotics.RRT object representing the search tree. The default
%   parameters for the RRT algorithm are used.
%
%   [PATH, TREE] = INFORMED_RRT_STAR(START, GOAL, MAP, NAME, VALUE) specifies
%   additional name-value pairs described below:
%
%   'MaxIterations'  Limits the maximum number of iterations.
%                    Default: 1e4
%
%   'GoalBias'       Probability of selecting the goal as the target node
%                    for expansion. Values should be in the range [0,1].
%                    Default: 0.05
%
%   'MaxConnectionDistance'  Maximum distance between a node and its
%                            nearest neighbor in the tree.
%                            Default: 0.5
%
%   'InflationRadius'        Inflation radius of obstacles in the map.
%                            Default: 0.4
%
%   'SegmentLength'          Maximum length of tree segments.
%                            Default: 0.1
%
%   'CostThreshold'          Cost threshold for pruning the tree.
%                            Default: Inf
%
%   'HeuristicFcn'   Function handle to a function that calculates the
%                    heuristic cost between a given point and the goal.
%                    Default: @(p1,p2)norm(p1-p2)
%
%   'Plot'           Set to true to visualize the algorithm. Default: false
%
%   Example:
%       load exampleMaps.mat
%       map = binaryOccupancyMap(simpleMap);
%       start = [2.0 1.5];
%       goal = [12.0 10.0];
%       [path, tree] = informed_rrt_star(start, goal, map, 'Plot', true);
%
%   See also robotics.RRT, robotics.RRTStar, robotics.RRTConnect,
%   robotics.PRM, robotics.PurePursuit, robotics.BinaryOccupancyGrid

%   Copyright 2021 The MathWorks, Inc.

% Parse inputs
parser = inputParser;
parser.addRequired('start', @(x)validateattributes(x,{'numeric'},...
    {'real','vector','numel',2}));
parser.addRequired('goal', @(x)validateattributes(x,{'numeric'},...
    {'real','vector','numel',2}));
parser.addRequired('map', @(x)isa(x,'binaryOccupancyMap'));
parser.addParameter('MaxIterations', 1e4, @(x)isnumeric(x) && isscalar(x));
parser.addParameter('GoalBias', 0.05, @(x)validateattributes(x,{'numeric'},...
    {'real','scalar','>=',0,'<=',1}));
parser.addParameter('MaxConnectionDistance', 0.5, @(x)isnumeric(x) && isscalar(x));
parser.addParameter('InflationRadius', 0.4, @(x)isnumeric(x) && isscalar(x));
parser.addParameter('SegmentLength', 0.1, @(x)isnumeric(x) && isscalar(x));
parser.addParameter('CostThreshold', Inf, @(x)isnumeric(x) && isscalar(x));
parser.addParameter('HeuristicFcn', @(p1,p2)norm(p1-p2), @(x)isa(x,'function_handle'));
parser.addParameter('Plot', false, @(x)islogical(x) && isscalar(x));
parser.parse(start, goal, map, varargin{:});

% Create RRT object
tree = robotics.RRT('MaxConnectionDistance', parser.Results.MaxConnectionDistance,...
    'InflationRadius', parser.Results.InflationRadius,...
    'SegmentLength', parser.Results.SegmentLength,...
    'CostThreshold', parser.Results.CostThreshold,...
    'HeuristicFcn', parser.Results.HeuristicFcn);

% Set up visualization
if parser.Results.Plot
    figure;
    show(map);
    hold on;
    plot(start(1), start(2), 'go', 'MarkerSize', 10, 'LineWidth', 2);
    plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
end

% Initialize tree
tree.addVertex(start);
costs = [0];

% Calculate heuristic cost to goal
heuristicCost = parser.Results.HeuristicFcn(start, goal);

% Main loop
for i = 1:parser.Results.MaxIterations
    
    % With a small probability, select the goal as the target node for expansion
    if rand < parser.Results.GoalBias
        targetNode = goal;
        targetCost = Inf;
    else
        % Select a random point in the search space
        targetNode = tree.randomConfiguration();
        
        % Calculate its cost to the root of the tree
        [targetNodeIdx, targetCost] = tree.nearest(targetNode);
        targetCost = targetCost + norm(targetNode - tree.States(targetNodeIdx,:));
        
        % Only expand nodes that are within a certain distance of the goal
        if targetCost > heuristicCost
            continue;
        end
    end
    
    % Connect target node to tree
    [newNode, cost] = tree.extend(targetNodeIdx, targetNode);
    
    % If no new node was added, skip to the next iteration
    if isempty(newNode)
        continue;
    end
    
    % Update cost
    costs(end+1) = cost;
    
    % If the new node is close to the goal, check if the goal is reached
    if norm(newNode - goal) < parser.Results.MaxConnectionDistance
        [goalIdx, goalCost] = tree.nearest(goal);
        if goalCost + norm(newNode - goal) < heuristicCost
            % Goal reached, exit main loop
            tree.addVertex(goal);
            tree.addEdge(numel(tree.States), goalIdx);
            path = tree.findpath(start, numel(tree.States));
            if parser.Results.Plot
                plot(path(:,1), path(:,2), 'r', 'LineWidth', 2);
            end
            return;
        end
    end
    
    % Estimate cost to goal through new node
    newCost = cost + parser.Results.HeuristicFcn(newNode, goal);
    
    % Check if a better path to the new node is found
    nodesInRange = tree.nodesInRange(newNode, parser.Results.InflationRadius);
    for j = 1:numel(nodesInRange)
        costToNode = costs(nodesInRange(j)) + norm(newNode - tree.States(nodesInRange(j),:));
        if costToNode < newCost
            % New node is not better, skip to next iteration
            continue;
        end
        % Rewire the tree to use the new node
        [newNodeIdx, newCost] = tree.addVertex(newNode);
        tree.addEdge(nodesInRange(j), newNodeIdx);
        tree.rewire(newNodeIdx, parser.Results.InflationRadius);
        costs(newNodeIdx) = newCost;
    end
    
    % Update heuristic cost to goal
    heuristicCost = min(heuristicCost, newCost + parser.Results.HeuristicFcn(newNode, goal));
    
    % Visualize the search
    if parser.Results.Plot
        plot(tree.States(:,1), tree.States(:,2), 'k.', 'MarkerSize', 6);
        for j = 1:numel(tree.Edges)
            if ~isempty(tree.Edges{j})
                plot(tree.States([j tree.Edges{j}],1), tree.States([j tree.Edges{j}],2), 'b');
            end
        end
        pause(0.01);
    end
    
end

% Maximum number of iterations reached, return best path found
path = tree.findpath(start, tree.nearest(goal));
if parser.Results.Plot
    plot(path(:,1), path(:,2), 'r', 'LineWidth', 2);
end

end

该代码实现了informed RRT*算法的主要部分。要使用该代码，只需将其保存为名为"informed_rrt_star.m"的MATLAB函数文件，并将其放在MATLAB搜索路径下即可。调用函数时，需要传递起始点、目标点和地图对象作为必需参数，以及其他可选参数。函数返回一个路径和一个表示搜索树的对象。要显示搜索过程，请将"Plot"参数设置为true。