初始化隨機(jī)樹

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

%% 初始化隨機(jī)樹
tree.child = [];               % 定義樹結(jié)構(gòu)體，保存新節(jié)點(diǎn)及其父節(jié)點(diǎn)
tree.parent = [];
tree.child = x_start;          % 起點(diǎn)作為第一個(gè)節(jié)點(diǎn)
flag = 1;                      % 標(biāo)志位
new_node_x = x_start(1,1);     % 將起點(diǎn)作為第一個(gè)生成點(diǎn)
new_node_y = x_start(1,2);
new_node = [new_node_x, new_node_y];

主函數(shù)部分

主函數(shù)中首先生成隨機(jī)點(diǎn)，并判斷是否在地圖范圍內(nèi)，若超出范圍則將標(biāo)志位置為0。

rd_x = 30 * rand() - 15;    % 生成隨機(jī)點(diǎn)
rd_y = 30 * rand() - 15;    
if (rd_x >= x_right_limit || rd_x <= x_left_limit ||... % 判斷隨機(jī)點(diǎn)是否在地圖邊界范圍內(nèi)
    rd_y >= y_right_limit || rd_y <= y_left_limit)
    flag = 0;
end

調(diào)用函數(shù)cal_distance計(jì)算tree中距離隨機(jī)點(diǎn)最近的節(jié)點(diǎn)的索引，并計(jì)算該節(jié)點(diǎn)與隨機(jī)點(diǎn)連線和x正向的夾角。

[angle, min_idx] = cal_distance(rd_x, rd_y, tree);    % 返回tree中最短距離節(jié)點(diǎn)索引及對(duì)應(yīng)的和x正向夾角

cal_distance函數(shù)定義如下：

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é)點(diǎn)。

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

接下來(lái)需要對(duì)該節(jié)點(diǎn)進(jìn)行判斷：

① 新節(jié)點(diǎn)是否在障礙物范圍內(nèi)；

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

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

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)    % 判斷生長(zhǎng)之后路徑與障礙物有無(wú)交叉部分
        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é)點(diǎn)和父節(jié)點(diǎn)連線的直線方程，然后將x變化范圍限制在min(tree.child(min_idx,1),new_node_x)max(tree.child(min_idx,1),new_node_x)內(nèi)，0.01即坐標(biāo)變換的步長(zhǎng)，步長(zhǎng)越小判斷的越精確，但同時(shí)會(huì)增加計(jì)算量；

步長(zhǎng)越大計(jì)算速度快但是很可能出現(xiàn)誤判，如下圖所式。

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

if (flag == true)           % 若標(biāo)志位為1，則可以將該新節(jié)點(diǎn)加入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;                   % 標(biāo)志位歸位

最后就是把障礙物、起點(diǎn)終點(diǎn)等顯示在figure中，并判斷新節(jié)點(diǎn)到目標(biāo)點(diǎn)距離。若小于閾值則停止搜索，并將目標(biāo)點(diǎn)加入到node中，否則重復(fù)該過(guò)程直至找到目標(biāo)點(diǎn)。

%% 顯示
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); % 起點(diǎn)
plot(goal(1,1)-0.5*resolution, goal(1,2)-0.5*resolution,'m^','MarkerFaceColor','m','MarkerSize',4*resolution); % 終點(diǎn)
set(gca,'XLim',[x_left_limit x_right_limit]); % X軸的數(shù)據(jù)顯示范圍
set(gca,'XTick',[x_left_limit:resolution:x_right_limit]); % 設(shè)置要顯示坐標(biāo)刻度
set(gca,'YLim',[y_left_limit y_right_limit]); % Y軸的數(shù)據(jù)顯示范圍
set(gca,'YTick',[y_left_limit:resolution:y_right_limit]); % 設(shè)置要顯示坐標(biāo)刻度
grid on
title('D-RRT');
xlabel('橫坐標(biāo) x'); 
ylabel('縱坐標(biāo) y');
pause(0.05);
if (sqrt((new_node_x - goal(1,1))^2 + (new_node_y- goal(1,2))^2) <= goal_radius) % 若新節(jié)點(diǎn)到目標(biāo)點(diǎn)距離小于閾值，則停止搜索，并將目標(biāo)點(diǎn)加入到node中
    tree.child(end+1,:) = goal;         % 把終點(diǎn)加入到樹中
    tree.parent(end+1,:) = new_node;
    disp('find goal!');
    break
end