基于栅格地图的改进RRT算法matlab代码实现

以下是基于栅格地图的改进RRT算法的MATLAB代码实现。其中，栅格地图是一个二维数组，0表示障碍物，1表示可通行的区域。

function [path,tree] = rrt_star(map,start,goal,max_iter,delta,max_dist)

% map: 二维栅格地图，0表示障碍物，1表示可通行的区域
% start: 起点坐标
% goal: 终点坐标
% max_iter: 最大迭代次数
% delta: 每次迭代的步长
% max_dist: 搜索半径

% 初始化树
tree(1).coord = start;
tree(1).parent = 0;
tree(1).cost = 0;

% 初始化路径
path = [];

for i = 1:max_iter
    % 生成随机点
    if rand < 0.5
        % 50%的概率选择终点
        rand_point = goal;
    else
        % 50%的概率在地图内随机选择一个点
        rand_point = [randi(size(map,1)),randi(size(map,2))];
    end
    
    % 找到最近的点
    dist = pdist2([tree.coord],rand_point);
    [min_dist,min_idx] = min(dist);
    nearest_node = tree(min_idx);
    
    % 计算新节点坐标
    new_coord = steer(nearest_node.coord,rand_point,delta);
    
    % 判断新节点是否在地图内
    if ~inside_map(new_coord,map)
        continue;
    end
    
    % 判断新节点与最近节点之间是否有障碍物
    if obstacle_free(nearest_node.coord,new_coord,map)
        % 在最近节点的邻居中选择最优的父节点
        near_nodes = find_near_nodes(tree,new_coord,max_dist);
        min_cost = nearest_node.cost + distance(nearest_node.coord,new_coord);
        min_node = [];
        for j = 1:length(near_nodes)
            if obstacle_free(near_nodes(j).coord,new_coord,map)
                cost = near_nodes(j).cost + distance(near_nodes(j).coord,new_coord);
                if cost < min_cost
                    min_cost = cost;
                    min_node = near_nodes(j);
                end
            end
        end
        
        % 添加新节点
        new_node.coord = new_coord;
        new_node.parent = min_node;
        new_node.cost = min_cost;
        tree(end+1) = new_node;
        
        % 更新邻居节点的父节点
        for j = 1:length(near_nodes)
            if obstacle_free(new_coord,near_nodes(j).coord,map) && ...
                    new_node.cost + distance(new_coord,near_nodes(j).coord) < near_nodes(j).cost
                near_nodes(j).parent = length(tree);
            end
        end
    end
    
    % 判断是否到达终点
    if distance(new_coord,goal) < delta
        path = find_path(tree,length(tree));
        break;
    end
end

if isempty(path)
    % 未找到路径
    return;
end

% 可视化路径和树
figure;
plot_path(path,map);
hold on;
plot_tree(tree,map);

end

% 路径可视化
function plot_path(path,map)
path_length = size(path,1);
for i = 2:path_length
    plot([path(i-1,2),path(i,2)],[path(i-1,1),path(i,1)],'r','LineWidth',2);
end
axis([1,size(map,2),1,size(map,1)]);
end

% 树可视化
function plot_tree(tree,map)
for i = 2:length(tree)
    plot([tree(i).coord(2),tree(tree(i).parent).coord(2)],...
         [tree(i).coord(1),tree(tree(i).parent).coord(1)],'b','LineWidth',1);
end
end

% 找到起点到终点的路径
function path = find_path(tree,idx)
path = [];
while idx ~= 0
    path = [tree(idx).coord;path];
    idx = tree(idx).parent;
end
end

% 计算两点间距离
function dist = distance(p1,p2)
dist = norm(p1 - p2);
end

% 判断点是否在地图内
function inside = inside_map(coord,map)
inside = coord(1) >= 1 && coord(1) <= size(map,1) && ...
         coord(2) >= 1 && coord(2) <= size(map,2) && ...
         map(coord(1),coord(2)) == 1;
end

% 判断两点之间是否有障碍物
function free = obstacle_free(p1,p2,map)
free = true;
dist = distance(p1,p2);
n = ceil(dist);
for i = 1:n
    t = i / n;
    coord = round((1-t)*p1 + t*p2);
    if ~inside_map(coord,map) || map(coord(1),coord(2)) == 0
        free = false;
        break;
    end
end
end

% 在最近节点的邻居中找到距离新节点最近的父节点
function near_nodes = find_near_nodes(tree,new_coord,max_dist)
near_nodes = [];
for i = 1:length(tree)
    if distance(tree(i).coord,new_coord) <= max_dist
        near_nodes = [near_nodes,tree(i)];
    end
end
end

% 计算新节点的坐标
function new_coord = steer(from,to,delta)
if distance(from,to) <= delta
    new_coord = to;
else
    new_coord = from + delta * (to - from) / distance(from,to);
end
end