给出rtt路径规划算法 matlab代码

以下是一个简单的RTT路径规划算法的Matlab代码：

function [path] = rtt(start, goal, obstacles, max_iter, step_size)
    % 初始化根节点
    tree = [start, 0, 0];
    % 迭代max_iter次
    for i = 1:max_iter
        % 随机采样一个点
        sample = [rand()*10, rand()*10];
        % 找到树中距离采样点最近的节点
        [closest_node, closest_index] = closest(tree(:,1:2), sample);
        % 从最近的节点向采样点移动一个步长
        new_node = move(closest_node, sample, step_size);
        % 如果新节点没有碰到障碍物，则将其添加到树中
        if ~collides(new_node, obstacles)
            new_cost = tree(closest_index, 3) + norm(new_node - closest_node);
            tree = [tree; new_node, closest_index, new_cost];
            % 如果新节点足够接近终点，则直接返回路径
            if norm(new_node - goal) < step_size
                path = getPath(tree, size(tree, 1));
                return
            end
        end
    end
    % 如果达到最大迭代次数，则返回最接近终点的节点所在路径
    [~, goal_index] = closest(tree(:,1:2), goal);
    path = getPath(tree, goal_index);
end

function [closest_node, closest_index] = closest(nodes, sample)
    % 找到距离采样点最近的节点
    distances = vecnorm(nodes - sample, 2, 2);
    [~, closest_index] = min(distances);
    closest_node = nodes(closest_index,:);
end

function [new_node] = move(start, end_, step_size)
    % 从起始点向终点移动一个步长
    direction = (end_ - start) / norm(end_ - start);
    new_node = start + direction * step_size;
end

function [path] = getPath(tree, goal_index)
    % 从终点节点回溯到起点，构建路径
    path = [tree(goal_index,1:2)];
    parent_index = tree(goal_index,3);
    while parent_index ~= 0
        path = [tree(parent_index,1:2); path];
        parent_index = tree(parent_index,3);
    end
end

function [collision] = collides(node, obstacles)
    % 判断节点是否碰到障碍物
    collision = 0;
    for i = 1:size(obstacles,1)
        if norm(node - obstacles(i,:)) < 0.5
            collision = 1;
            return
        end
    end
end

这个算法实现了一个基本的RTT路径规划算法，可以在二维空间中规划路径，避开障碍物，最终到达目标点。