RRT代码matlab

Sure! Here's a simple implementation of the RRT (Rapidly-Exploring Random Trees) algorithm in Matlab:

classdef RRT
    properties
        map % occupancy map
        start % start position
        goal % goal position
        tree % RRT tree
        stepSize % step size for tree expansion
    end
    
    methods
        function obj = RRT(map, start, goal, stepSize)
            obj.map = map;
            obj.start = start;
            obj.goal = goal;
            obj.tree = [start];
            obj.stepSize = stepSize;
        end
        
        function path = plan(obj, maxIterations)
            for i = 1:maxIterations
                qRand = obj.generateRandomConfig();
                qNearIdx = obj.findNearestNeighbor(qRand);
                qNear = obj.tree(qNearIdx,:);
                qNew = obj.extend(qNear, qRand);
                
                if obj.isCollisionFree(qNear, qNew)
                    obj.tree = [obj.tree; qNew];
                    if norm(qNew - obj.goal) < obj.stepSize
                        path = [obj.tree; obj.goal];
                        return;
                    end
                end
            end
            
            path = [];
        end
        
        function qRand = generateRandomConfig(obj)
            xrange = size(obj.map, 2);
            yrange = size(obj.map, 1);
            qRand = [randi(xrange), randi(yrange)];
        end
        
        function qNearIdx = findNearestNeighbor(obj, qRand)
            distances = vecnorm(obj.tree - qRand, 2, 2);
            [~, qNearIdx] = min(distances);
        end
        
        function qNew = extend(obj, qNear, qRand)
            direction = (qRand - qNear) / norm(qRand - qNear);
            qNew = qNear + obj.stepSize * direction;
        end
        
        function collisionFree = isCollisionFree(obj, qNear, qNew)
            indices = round(linspace(qNear(2), qNew(2), 10));
            for idx = round(linspace(qNear(1), qNew(1), 10))
                if obj.map(indices(idx), idx) == 1
                    collisionFree = false;
                    return;
                end
            end
            collisionFree = true;
        end
    end
end

You can use the `RRT` class to plan a path from the start position to the goal position. Here's an example of how to use it:

% Create a map (occupancy grid)
map = zeros(100, 100);
map(30:40, 30:60) = 1;

% Set the start and goal positions
start = [10, 10];
goal = [90, 90];

% Create an instance of the RRT planner
rrt = RRT(map, start, goal, 5);

% Plan the path
path = rrt.plan(1000);

% Plot the map and path
imagesc(map);
colormap(flipud(gray));
hold on;
plot(rrt.tree(:,1), rrt.tree(:,2), 'b.');
plot(path(:,1), path(:,2), 'r', 'LineWidth', 2);
plot(start(1), start(2), 'go', 'MarkerSize', 10);
plot(goal(1), goal(2), 'ro', 'MarkerSize', 10);
hold off;

This code creates a simple occupancy grid map with obstacles, sets the start and goal positions, creates an instance of the RRT planner, plans a path using 1000 iterations, and plots the map and path. You can adjust the map, start, goal, and step size values to suit your specific requirements.

Note that this implementation assumes a 2D grid map and uses Euclidean distance for nearest neighbor calculations.