RRT算法用matlab仿真的代码

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

% 设置起点和终点
start = [0,0];
goal = [10,10];

% 设置障碍物
obs = [3,3,2,2; 5,5,2,2];

% 设置树的根节点
tree = start;

% 设置步长
step_size = 0.5;

% 设置最大迭代次数
max_iter = 1000;

% 迭代
for i = 1:max_iter
    % 生成一个随机点
    random_point = [rand()*10, rand()*10];
    
    % 寻找树中距离随机点最近的节点
    dist = pdist2(tree, random_point);
    [min_dist, min_index] = min(dist);
    nearest_point = tree(min_index, :);
    
    % 计算朝随机点的方向
    direction = (random_point - nearest_point) / min_dist;
    
    % 计算新节点
    new_point = nearest_point + step_size * direction;
    
    % 判断新节点是否在障碍物内
    if ~is_collision(new_point, obs)
        % 将新节点添加到树中
        tree = [tree; new_point];
        
        % 判断是否到达终点
        if pdist2(new_point, goal) < step_size
            % 到达终点，结束迭代
            break;
        end
    end
end

% 绘制结果
plot(tree(:,1), tree(:,2), 'o');
hold on;
plot(obs(1,:), obs(2,:), 'r');
plot(start(1), start(2), 'gx', 'MarkerSize', 10);
plot(goal(1), goal(2), 'rx', 'MarkerSize', 10);
axis equal;

其中，`is_collision`函数是用来判断一个点是否在障碍物内的函数，可以根据实际需求进行实现。