rrt算法 matlab代码

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

clear all;
close all;
clc;

% 定义目标点和起始点
start_point = [0, 0];
goal_point = [10, 10];

% 定义地图尺寸
map_width = 12;
map_height = 12;

% 定义障碍物
obstacles = [3, 3, 2, 2;
             8, 8, 2, 2;
             3, 8, 2, 2;
             8, 3, 2, 2];

% 定义 RRT 树的参数
max_iter = 1000;
step_size = 0.5;

% 初始化 RRT 树
rrt_tree = [start_point, -1];
plot(start_point(1), start_point(2), 'bo');
hold on;

% 迭代 RRT 算法
for i = 1:max_iter
    % 随机生成一个点
    rnd_point = [rand()*map_width, rand()*map_height];
    
    % 找到 RRT 树中距离该点最近的节点
    distances = sqrt(sum((rrt_tree(:,1:2) - repmat(rnd_point, size(rrt_tree,1), 1)).^2, 2));
    [~, nearest_node] = min(distances);
    nearest_point = rrt_tree(nearest_node, 1:2);
    
    % 计算从最近节点到随机点的方向和距离
    direction = (rnd_point - nearest_point) / norm(rnd_point - nearest_point);
    new_point = nearest_point + direction * step_size;
    
    % 判断新节点是否在地图内，且没有碰到障碍物
    if new_point(1) >= 0 && new_point(1) <= map_width && ...
            new_point(2) >= 0 && new_point(2) <= map_height && ...
            ~collision(new_point, obstacles)
        % 将新节点加入 RRT 树
        rrt_tree = [rrt_tree; new_point, nearest_node];
        
        % 绘制新节点
        plot(new_point(1), new_point(2), 'bo');
        
        % 判断是否到达目标点
        if norm(new_point - goal_point) < step_size
            % 到达目标点，退出迭代
            break;
        end
    end
end

% 回溯 RRT 树，生成路径
path = [goal_point];
current_node = size(rrt_tree, 1);
while current_node ~= 1
    current_point = rrt_tree(current_node, 1:2);
    current_node = rrt_tree(current_node, 3);
    path = [current_point; path];
end

% 绘制路径
plot(path(:,1), path(:,2), 'r', 'LineWidth', 2);

function col = collision(point, obstacles)
% 判断点是否与障碍物碰撞
col = 0;
for i = 1:size(obstacles, 1)
    if point(1) >= obstacles(i,1) && point(1) <= obstacles(i,1)+obstacles(i,3) && ...
            point(2) >= obstacles(i,2) && point(2) <= obstacles(i,2)+obstacles(i,4)
        col = 1;
        break;
    end
end
end

这份代码中，首先定义了起始点和目标点，地图尺寸以及障碍物信息。然后通过 RRT 算法生成一棵树，并回溯该树以生成路径。其中，`collision` 函数用于判断点是否与障碍物碰撞。可以根据需要进行修改来适应不同的场景。