RRT算法Matlab实现 主函数部分

初始化随机树

初始化随机树，定义树结构体tree以保存新节点及其父节点，便于后续从目标点回推规划的路径。

%% 初始化随机树
tree.child = [];               % 定义树结构体，保存新节点及其父节点
tree.parent = [];
tree.child = x_start;          % 起点作为第一个节点
flag = 1;                      % 标志位
new_node_x = x_start(1,1);     % 将起点作为第一个生成点
new_node_y = x_start(1,2);
new_node = [new_node_x, new_node_y];

主函数部分

主函数中首先生成随机点，并判断是否在地图范围内，若超出范围则将标志位置为0。

rd_x = 30 * rand() - 15;    % 生成随机点
rd_y = 30 * rand() - 15;    
if (rd_x >= x_right_limit || rd_x <= x_left_limit ||... % 判断随机点是否在地图边界范围内
    rd_y >= y_right_limit || rd_y <= y_left_limit)
    flag = 0;
end

调用函数cal_distance计算tree中距离随机点最近的节点的索引，并计算该节点与随机点连线和x正向的夹角。

[angle, min_idx] = cal_distance(rd_x, rd_y, tree);    % 返回tree中最短距离节点索引及对应的和x正向夹角

cal_distance函数定义如下：

function [angle, min_idx] = cal_distance(rd_x, rd_y, tree)
    distance = [];
    i = 1;
    while i<=size(tree.child,1)
        dx = rd_x - tree.child(i,1);
        dy = rd_y - tree.child(i,2);
        d = sqrt(dx^2 + dy^2);
        distance(i) = d;
        i = i+1;
    end
    [~, min_idx] = min(distance);
    angle = atan2(rd_y - tree.child(min_idx,2),rd_x - tree.child(min_idx,1));
end

随后生成新节点。

new_node_x = tree.child(min_idx,1)+grow_distance*cos(angle);% 生成新的节点
new_node_y = tree.child(min_idx,2)+grow_distance*sin(angle);
new_node = [new_node_x, new_node_y];

接下来需要对该节点进行判断：

① 新节点是否在障碍物范围内；

② 新节点和父节点的连线线段是否和障碍物有重合部分。

若任意一点不满足，则将标志位置为0。实际上可以将两个判断结合，即判断新节点和父节点的连线线段上的点是否在障碍物范围内。

for k=1:1:size(ob,1) 
    for i=min(tree.child(min_idx,1),new_node_x):0.01:max(tree.child(min_idx,1),new_node_x)    % 判断生长之后路径与障碍物有无交叉部分
        j = (tree.child(min_idx,2) - new_node_y)/(tree.child(min_idx,1) - new_node_x) *(i - new_node_x) + new_node_y;
        if(i >=ob(k,1)-resolution && i <= ob(k,1)+ob(k,3) && j >= ob(k,2)-resolution && j <= ob(k,2)+ob(k,4))
            flag = 0;
            break
        end
    end
end

在这我采用的方法是写出新节点和父节点连线的直线方程，然后将x变化范围限制在min(tree.child(min_idx,1),new_node_x)max(tree.child(min_idx,1),new_node_x)内，0.01即坐标变换的步长，步长越小判断的越精确，但同时会增加计算量；

步长越大计算速度快但是很可能出现误判，如下图所式。

判断标志位若为1，则可以将该新节点加入到tree中，注意保存新节点和它的父节点，同时显示在figure中，之后重置标志位。

if (flag == true)           % 若标志位为1，则可以将该新节点加入tree中
    tree.child(end+1,:) = new_node;
    tree.parent(end+1,:) = [tree.child(min_idx,1), tree.child(min_idx,2)];
    plot(rd_x, rd_y, '.r');hold on
    plot(new_node_x, new_node_y,'.g');hold on
    plot([tree.child(min_idx,1),new_node_x], [tree.child(min_idx,2),new_node_y],'-b');
end
    
flag = 1;                   % 标志位归位

最后就是把障碍物、起点终点等显示在figure中，并判断新节点到目标点距离。若小于阈值则停止搜索，并将目标点加入到node中，否则重复该过程直至找到目标点。

%% 显示
for i=1:1:size(ob,1)        % 绘制障碍物
    fill([ob(i,1)-resolution, ob(i,1)+ob(i,3),ob(i,1)+ob(i,3),ob(i,1)-resolution],...
         [ob(i,2)-resolution,ob(i,2)-resolution,ob(i,2)+ob(i,4),ob(i,2)+ob(i,4)],'k');
end
hold on

plot(x_start(1,1)-0.5*resolution, x_start(1,2)-0.5*resolution,'b^','MarkerFaceColor','b','MarkerSize',4*resolution); % 起点
plot(goal(1,1)-0.5*resolution, goal(1,2)-0.5*resolution,'m^','MarkerFaceColor','m','MarkerSize',4*resolution); % 终点
set(gca,'XLim',[x_left_limit x_right_limit]); % X轴的数据显示范围
set(gca,'XTick',[x_left_limit:resolution:x_right_limit]); % 设置要显示坐标刻度
set(gca,'YLim',[y_left_limit y_right_limit]); % Y轴的数据显示范围
set(gca,'YTick',[y_left_limit:resolution:y_right_limit]); % 设置要显示坐标刻度
grid on
title('D-RRT');
xlabel('横坐标 x'); 
ylabel('纵坐标 y');
pause(0.05);
if (sqrt((new_node_x - goal(1,1))^2 + (new_node_y- goal(1,2))^2) <= goal_radius) % 若新节点到目标点距离小于阈值，则停止搜索，并将目标点加入到node中
    tree.child(end+1,:) = goal;         % 把终点加入到树中
    tree.parent(end+1,:) = new_node;
    disp('find goal!');
    break
end