帮我写一下matlab的RRT算法

RRT（Rapidly-exploring Random Trees，快速扩展随机树）是一种基于树形结构的路径规划算法，用于解决机器人或其他移动体在复杂环境中的路径规划问题。下面是一个简单的 MATLAB 实现。

首先，定义一个空树，将初始位置作为根节点，并将其添加到树中：

tree = struct('parent', [], 'position', []);
tree(1).parent = 0;
tree(1).position = [x_start, y_start];

接着，定义一个目标位置和最大迭代次数：

x_goal = 10;
y_goal = 10;
max_iter = 1000;

然后，对于每一次迭代，生成一个随机位置，并找到树中最近的节点：

for i = 2:max_iter
    q_rand = [rand()*(x_max-x_min)+x_min, rand()*(y_max-y_min)+y_min];
    [~, q_near] = min(pdist2([tree.position], q_rand), [], 2);

计算从最近节点到随机位置的方向和距离，并将其加入到树中：

    q_near = q_near(1);
    q_new = steer(q_rand, tree(q_near).position, delta);
    if check_collision(tree(q_near).position, q_new, obstacles) == 0
        tree(end+1).parent = q_near;
        tree(end).position = q_new;
    end

检查新节点是否接近目标位置，并在找到一条通向目标的路径时返回：

    if pdist2(q_new, [x_goal, y_goal]) < goal_radius
        q_goal = length(tree);
        if check_collision(q_new, [x_goal, y_goal], obstacles) == 0
            tree(end+1).parent = q_goal;
            tree(end).position = [x_goal, y_goal];
            path = find_path(tree);
            return
        end
    end
end

最后，定义一个函数来沿着树的路径从目标节点向根节点回溯，生成一条从起始位置到目标位置的路径：

function path = find_path(tree)
q_end = length(tree);
path = q_end;
while path(1) ~= 1
    path = [tree(path(1)).parent, path];
end
end

完整代码如下：

function path = RRT(x_start, y_start, x_goal, y_goal, obstacles, x_min, x_max, y_min, y_max, delta, goal_radius, max_iter)

tree = struct('parent', [], 'position', []);
tree(1).parent = 0;
tree(1).position = [x_start, y_start];

for i = 2:max_iter
    q_rand = [rand()*(x_max-x_min)+x_min, rand()*(y_max-y_min)+y_min];
    [~, q_near] = min(pdist2([tree.position], q_rand), [], 2);
    q_near = q_near(1);
    q_new = steer(q_rand, tree(q_near).position, delta);
    if check_collision(tree(q_near).position, q_new, obstacles) == 0
        tree(end+1).parent = q_near;
        tree(end).position = q_new;
    end
    if pdist2(q_new, [x_goal, y_goal]) < goal_radius
        q_goal = length(tree);
        if check_collision(q_new, [x_goal, y_goal], obstacles) == 0
            tree(end+1).parent = q_goal;
            tree(end).position = [x_goal, y_goal];
            path = find_path(tree);
            return
        end
    end
end

function q_new = steer(q_rand, q_near, delta)
if pdist2(q_rand, q_near) < delta
    q_new = q_rand;
else
    q_new = q_near + delta * (q_rand - q_near) / pdist2(q_rand, q_near);
end
end

function collision = check_collision(q1, q2, obstacles)
collision = 0;
for i = 1:size(obstacles, 1)
    if norm(q2 - q1) > 0.01
        t = ((obstacles(i, 1)-q1(1))*(q2(1)-q1(1)) + (obstacles(i, 2)-q1(2))*(q2(2)-q1(2))) / norm(q2 - q1)^2;
        proj = q1 + t * (q2 - q1);
        if t >= 0 && t <= 1 && norm(proj - [obstacles(i, 1), obstacles(i, 2)]) < obstacles(i, 3)
            collision = 1;
            return
        end
    end
end
end

function path = find_path(tree)
q_end = length(tree);
path = q_end;
while path(1) ~= 1
    path = [tree(path(1)).parent, path];
end
end

end

注意，这只是一种基本的 RRT 实现，可能需要根据具体问题进行调整和优化。