rrt路径规划算法matlab

以下是一个简单的RRT路径规划算法的MATLAB实现，仅供参考。

% RRT路径规划算法
% 作者: Yijun Yuan (yyuan@mit.edu)
% 
% 输入:
%    start_pos: 起点坐标
%    goal_pos: 终点坐标
%    obstacle_list: 障碍物列表
%    max_iter: 最大迭代次数
%    step_size: 步长
% 输出:
%    path: 路径坐标列表

function path = RRT(start_pos, goal_pos, obstacle_list, max_iter, step_size)

% 初始化树
tree = [start_pos, 0];

for i = 1:max_iter
    % 随机生成一个点
    rand_pos = [rand*10, rand*10];
    
    % 找到距离随机点最近的树上节点
    dist = sqrt((tree(:,1) - rand_pos(1)).^2 + (tree(:,2) - rand_pos(2)).^2);
    [~, nearest_idx] = min(dist);
    nearest_node = tree(nearest_idx,:);
    
    % 计算朝向随机点的向量
    dir_vec = rand_pos - nearest_node(1:2);
    dir_vec = dir_vec / norm(dir_vec);
    
    % 计算新节点位置
    new_pos = nearest_node(1:2) + dir_vec * step_size;
    
    % 如果新节点不在障碍物内，则加入树
    if ~IsInObstacle(new_pos, obstacle_list)
        new_node = [new_pos, nearest_idx];
        tree = [tree; new_node];
        
        % 如果新节点距离目标点小于一定阈值，则认为找到了一条路径
        if norm(new_pos - goal_pos) < 0.5
            path = BacktrackPath(tree, nearest_idx);
            return
        end
    end
end

% 没有找到路径
path = [];

end

function path = BacktrackPath(tree, goal_idx)
% 回溯路径
path = tree(goal_idx, 1:2);
while goal_idx ~= 1
    goal_idx = tree(goal_idx, 3);
    path = [tree(goal_idx, 1:2); path];
end
end

function is_in = IsInObstacle(pos, obstacle_list)
% 判断点是否在障碍物内
is_in = false;
for i = 1:length(obstacle_list)
    obs = obstacle_list{i};
    if pos(1) >= obs(1) && pos(1) <= obs(3) && pos(2) >= obs(2) && pos(2) <= obs(4)
        is_in = true;
        break;
    end
end
end

需要注意的是，此实现仅考虑二维平面，且障碍物为矩形。如果需要考虑更复杂的情况，需要对代码进行相应的修改。