MATLAB 实现 RRT 算法并对路径进行平滑处理的示例代码，同时将最终路径输出为一张图片

以下是一个简单的 MATLAB 实现 RRT 算法并对路径进行平滑处理的示例代码，同时将最终路径输出为一张图片：

clear all;
close all;
clc;

% 定义起点和终点
start_point = [0,0];
goal_point = [10,10];

% 定义搜索区域的边界
x_min = -2;
x_max = 12;
y_min = -2;
y_max = 12;

% 定义搜索区域中的障碍物
obstacle = [4,4,2,2;
            6,6,2,2];

% 定义 RRT 算法的参数
max_iter = 1000;
step_size = 0.5;
goal_radius = 0.5;

% 初始化 RRT 树
rrt_tree.vertices = start_point;
rrt_tree.edges = [];

% 进行 RRT 算法的迭代
for i = 1:max_iter
    % 随机采样一个点
    if rand < 0.5
        sample_point = [rand*(x_max-x_min)+x_min, rand*(y_max-y_min)+y_min];
    else
        sample_point = goal_point;
    end
    
    % 寻找 RRT 树中距离采样点最近的节点
    nearest_node = rrt_tree.vertices(1,:);
    min_distance = norm(sample_point - nearest_node);
    for j = 2:size(rrt_tree.vertices,1)
        distance = norm(sample_point - rrt_tree.vertices(j,:));
        if distance < min_distance
            nearest_node = rrt_tree.vertices(j,:);
            min_distance = distance;
        end
    end
    
    % 计算从最近节点到采样点的方向和距离
    direction = (sample_point - nearest_node) / min_distance;
    new_point = nearest_node + step_size * direction;
    
    % 检查新节点是否与障碍物相交
    collision_flag = false;
    for j = 1:size(obstacle,1)
        if norm(new_point - obstacle(j,1:2)) < obstacle(j,3)
            collision_flag = true;
            break;
        end
    end
    
    % 如果新节点没有与障碍物相交，则将其添加到 RRT 树中
    if ~collision_flag
        rrt_tree.vertices = [rrt_tree.vertices; new_point];
        rrt_tree.edges = [rrt_tree.edges; size(rrt_tree.vertices,1)-1, find(all(bsxfun(@eq,rrt_tree.vertices,new_point),2))];
        % 如果新节点距离终点小于等于 goal_radius，则算法结束
        if norm(new_point - goal_point) <= goal_radius
            rrt_tree.vertices = [rrt_tree.vertices; goal_point];
            rrt_tree.edges = [rrt_tree.edges; size(rrt_tree.vertices,1)-1, size(rrt_tree.vertices,1)];
            break;
        end
    end
end

% 对 RRT 树进行路径搜索
path = [];
current_node = size(rrt_tree.vertices,1);
while current_node ~= 1
    parent_node = rrt_tree.edges(find(rrt_tree.edges(:,2)==current_node),1);
    path = [rrt_tree.vertices(current_node,:); path];
    current_node = parent_node;
end
path = [start_point; path];

% 对路径进行平滑处理
smooth_path = path;
change_flag = true;
while change_flag
    change_flag = false;
    for i = 2:size(smooth_path,1)-1
        for j = i+1:size(smooth_path,1)
            if ~collision_check(smooth_path(i,:),smooth_path(j,:),obstacle)
                new_path = [smooth_path(1:i-1,:); smooth_path(i,:); smooth_path(j,:); smooth_path(j+1:end,:)];
                smooth_path = new_path;
                change_flag = true;
            end
        end
    end
end

% 绘制搜索区域和障碍物
figure;
hold on;
axis([x_min x_max y_min y_max]);
rectangle('Position',[obstacle(1,1) obstacle(1,2) obstacle(1,3)*2 obstacle(1,4)*2],'Curvature',[1 1],'FaceColor','r');
rectangle('Position',[obstacle(2,1) obstacle(2,2) obstacle(2,3)*2 obstacle(2,4)*2],'Curvature',[1 1],'FaceColor','r');

% 绘制 RRT 树
for i = 1:size(rrt_tree.edges,1)
    x1 = rrt_tree.vertices(rrt_tree.edges(i,1),1);
    y1 = rrt_tree.vertices(rrt_tree.edges(i,1),2);
    x2 = rrt_tree.vertices(rrt_tree.edges(i,2),1);
    y2 = rrt_tree.vertices(rrt_tree.edges(i,2),2);
    plot([x1 x2],[y1 y2],'k-');
end

% 绘制路径和平滑后的路径
plot(path(:,1),path(:,2),'b-','linewidth',2);
plot(smooth_path(:,1),smooth_path(:,2),'g-','linewidth',2);

% 输出最终路径为一张图片
saveas(gcf,'path.png');

其中，`collision_check` 函数用于检查两个点之间是否与障碍物相交：

function flag = collision_check(point1,point2,obstacle)
    flag = false;
    for i = 1:size(obstacle,1)
        center = obstacle(i,1:2);
        radius = obstacle(i,3);
        if norm(point2-point1) < norm(point2-center) || norm(point1-point2) < norm(point1-center)
            continue;
        end
        v1 = point2 - point1;
        v2 = center - point1;
        if norm(v2 - dot(v1,v2)/norm(v1)^2*v1) < radius
            flag = true;
            break;
        end
    end
end

运行代码后，将会在当前目录下生成一张名为 `path.png` 的图片，显示搜索区域、障碍物、RRT 树、路径和平滑后的路径。