初始化隨機樹

初始化隨機樹，定義樹結構體tree以保存新節(jié)點及其父節(jié)點，便于后續(xù)從目標點回推規(guī)劃的路徑。

%% 初始化隨機樹
tree.child = [];               % 定義樹結構體，保存新節(jié)點及其父節(jié)點
tree.parent = [];
tree.child = x_start;          % 起點作為第一個節(jié)點
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中距離隨機點最近的節(jié)點的索引，并計算該節(jié)點與隨機點連線和x正向的夾角。

[angle, min_idx] = cal_distance(rd_x, rd_y, tree);    % 返回tree中最短距離節(jié)點索引及對應的和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

隨后生成新節(jié)點。

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

接下來需要對該節(jié)點進行判斷：

① 新節(jié)點是否在障礙物范圍內；

② 新節(jié)點和父節(jié)點的連線線段是否和障礙物有重合部分。

若任意一點不滿足，則將標志位置為0。實際上可以將兩個判斷結合，即判斷新節(jié)點和父節(jié)點的連線線段上的點是否在障礙物范圍內。

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

在這我采用的方法是寫出新節(jié)點和父節(jié)點連線的直線方程，然后將x變化范圍限制在min(tree.child(min_idx,1),new_node_x)max(tree.child(min_idx,1),new_node_x)內，0.01即坐標變換的步長，步長越小判斷的越精確，但同時會增加計算量；

步長越大計算速度快但是很可能出現誤判，如下圖所式。

判斷標志位若為1，則可以將該新節(jié)點加入到tree中，注意保存新節(jié)點和它的父節(jié)點，同時顯示在figure中，之后重置標志位。

if (flag == true)           % 若標志位為1，則可以將該新節(jié)點加入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中，并判斷新節(jié)點到目標點距離。若小于閾值則停止搜索，并將目標點加入到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) % 若新節(jié)點到目標點距離小于閾值，則停止搜索，并將目標點加入到node中
    tree.child(end+1,:) = goal;         % 把終點加入到樹中
    tree.parent(end+1,:) = new_node;
    disp('find goal!');
    break
end