matlabRRT算法代码

以下是一个简单的MATLAB示例代码，演示了如何实现基本的Rapidly-exploring Random Tree (RRT)算法：

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

% 初始化RRT树，将起始点作为根节点
tree.vertices = start;
tree.edges = [];

% 设置参数
maxIterations = 1000;
stepSize = 0.5;

% RRT算法主循环
for i = 1:maxIterations
    % 生成随机点
    randomPoint = [rand()*10, rand()*10];
    
    % 在RRT树中找到最近邻节点
    nearestIndex = findNearest(tree.vertices, randomPoint);
    nearestPoint = tree.vertices(nearestIndex, :);
    
    % 根据步长计算新的节点
    direction = (randomPoint - nearestPoint)/norm(randomPoint - nearestPoint);
    newPoint = nearestPoint + stepSize * direction;
    
    % 将新节点添加到RRT树中
    tree.vertices = [tree.vertices; newPoint];
    tree.edges = [tree.edges; nearestIndex, size(tree.vertices, 1)];
    
    % 判断是否接近目标点
    if norm(newPoint - goal) < stepSize
        tree.vertices = [tree.vertices; goal];
        tree.edges = [tree.edges; size(tree.vertices, 1)-1, size(tree.vertices, 1)];
        break;
    end
end

% 可视化结果
figure;
plot(tree.vertices(:, 1), tree.vertices(:, 2), 'bo'); % 绘制树节点
hold on;
for i = 1:size(tree.edges, 1)
    edge = tree.edges(i, :);
    line([tree.vertices(edge(1), 1), tree.vertices(edge(2), 1)], [tree.vertices(edge(1), 2), tree.vertices(edge(2), 2)], 'Color', 'k'); % 绘制树边
end
axis equal;
xlabel('X');
ylabel('Y');

% 找到最近邻节点的函数
function index = findNearest(vertices, point)
    distances = vecnorm(vertices - point, 2, 2);
    [~, index] = min(distances);
end

请注意，这只是一个基本的RRT算法的示例代码，可能需要根据具体问题和需求进行修改和扩展。此代码将生成一个RRT树，并绘制树节点和边。你可以根据需要自定义起始点、目标点、参数等。