rrt算法 matlab代码

以下是一个简单的RRt算法的Matlab实现，供您参考：

% RRT algorithm implementation in Matlab

% Define parameters
q_start = [0, 0]; % Starting configuration
q_goal = [10, 10]; % Goal configuration
delta_q = 0.5; % Step size
max_iter = 1000; % Maximum number of iterations
obstacles = [4, 4, 1; 6, 6, 1]; % Obstacle positions

% Initialize tree
tree = [q_start, 0];

% Iterate until goal is reached or maximum number of iterations is reached
for i = 1:max_iter
    % Generate random configuration
    q_rand = [rand() * 10, rand() * 10];
    
    % Find nearest node in tree
    distances = vecnorm(tree(:,1:2) - q_rand, 2, 2);
    [min_distance, nearest_node_index] = min(distances);
    nearest_node = tree(nearest_node_index, 1:2);
    
    % Calculate new node position
    if min_distance < delta_q
        q_new = q_rand;
    else
        q_new = nearest_node + (q_rand - nearest_node) * delta_q / min_distance;
    end
    
    % Check if new node is collision-free
    if ~check_collision(q_new, obstacles)
        % Add new node to tree
        new_node_index = size(tree, 1) + 1;
        tree(new_node_index, :) = [q_new, nearest_node_index];
        
        % Check if goal is reached
        if vecnorm(q_new - q_goal, 2) < delta_q
            break;
        end
    end
end

% Reconstruct path from goal to start
path = [q_goal, size(tree, 1)];
while path(end) ~= 1
    path(end+1) = tree(path(end), 3);
end
path = flipud(tree(path, 1:2));

% Plot result
figure();
hold on;
plot(q_start(1), q_start(2), 'ro');
plot(q_goal(1), q_goal(2), 'rx');
for i = 1:size(obstacles, 1)
    rectangle('Position', [obstacles(i,1)-obstacles(i,3)/2, obstacles(i,2)-obstacles(i,3)/2, obstacles(i,3), obstacles(i,3)], 'Curvature', [1, 1], 'FaceColor', 'k');
end
plot(tree(:,1), tree(:,2), 'b.');
plot(path(:,1), path(:,2), 'r');
axis equal;

function collision = check_collision(q, obstacles)
    % Check if configuration is collision-free
    for i = 1:size(obstacles, 1)
        if vecnorm(q - obstacles(i, 1:2), 2) <= obstacles(i, 3)
            collision = true;
            return;
        end
    end
    collision = false;
end

这个实现采用了比较简单的随机采样和最近邻搜索方法，可以处理简单的平面路径规划问题。您可以根据需要进行修改和扩展。