rrt代码matlab

以下是一个简单的RRT算法的MATLAB代码实现：

clear all;
close all;
clc;

% 设置起始点和目标点
start = [0,0];
goal = [5,5];

% 设置障碍物
obstacles = [2,2,1;
             3,3,1;
             4,4,1];

% 设置RRT参数
step_size = 0.5; % 步长
max_iter = 1000; % 最大迭代次数

% 初始化RRT
tree = [start, 0];

% 迭代生成RRT
for i=1:max_iter
    % 随机采样一个点
    rnd = [rand*5, rand*5];
    
    % 寻找树中距离该点最近的节点
    [~, nearest_idx] = pdist2(tree(:,1:2), rnd, 'euclidean', 'Smallest', 1);
    nearest_node = tree(nearest_idx,:);
    
    % 计算新的节点
    dir = (rnd - nearest_node(1:2))/norm(rnd - nearest_node(1:2));
    new_node = [nearest_node(1:2)+dir*step_size, nearest_idx];
    
    % 检查新节点是否与障碍物相交
    is_collision = false;
    for j=1:size(obstacles,1)
        if norm(new_node(1:2)-obstacles(j,1:2)) < obstacles(j,3)
            is_collision = true;
            break;
        end
    end
    
    % 如果新节点不与障碍物相交，则添加到树中
    if ~is_collision
        tree = [tree; new_node];
        
        % 如果新节点距离目标点小于一个阈值，则停止生成RRT
        if norm(new_node(1:2)-goal) < step_size
            break;
        end
    end
end

% 寻找最短路径
path = [goal, size(tree,1)];
while path(end) ~= 1
    path = [tree(path(end),1:2)', tree(path(end),3); path];
end

% 显示结果
figure;
hold on;
plot(start(1), start(2), 'go', 'MarkerSize', 10, 'LineWidth', 2);
plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
for i=1:size(obstacles,1)
    viscircles(obstacles(i,1:2), obstacles(i,3), 'Color', 'k');
end
for i=2:size(tree,1)
    plot([tree(i,1), tree(tree(i,3),1)], [tree(i,2), tree(tree(i,3),2)], 'b-', 'LineWidth', 1);
end
plot(path(:,1), path(:,2), 'r-', 'LineWidth', 2);
axis equal;
xlim([-1, 6]);
ylim([-1, 6]);