手把手教用matlab做無人駕駛（六）-路徑規劃RRT

這裏介紹路徑規劃算法RRT應用在2D環境，這裏算法步驟：

1.產生隨機點q_rand，matlab程序實現如下：

for i = 1:1:numNodes
    q_rand = [floor(rand(1)*x_max) floor(rand(1)*y_max)];
    plot(q_rand(1), q_rand(2), 'x', 'Color',  [0 0.4470 0.7410])

2.通過q_rand找到 最近的q_near 節點（就是隨機產生q_rand,然後從nodes找到離q_rand最近的點）

ndist = [];
    for j = 1:1:length(nodes)
        n = nodes(j);
        tmp = dist(n.coord, q_rand);
        ndist = [ndist tmp];
    end
    [val, idx] = min(ndist);
    q_near = nodes(idx);

3.指引q_near 到q_rand，如果距離太遠，就通過steer產生新的q_new,然後通過判斷是不是障礙物，產生q_new

q_new.coord = steer(q_rand, q_near.coord, val, EPS);
    if noCollision(q_rand, q_near.coord, obstacle)
        line([q_near.coord(1), q_new.coord(1)], [q_near.coord(2), q_new.coord(2)], 'Color', 'k', 'LineWidth', 2);
        drawnow
        hold on
        q_new.cost = dist(q_new.coord, q_near.coord) + q_near.cost;
        

 

4.通過nodes和給定半徑r，找到q_new附件的點q_nearnest(目的就是爲接下來找到最小代價路徑)

 q_nearest = [];
        r = 60;
        neighbor_count = 1;
        for j = 1:1:length(nodes)
            if noCollision(nodes(j).coord, q_new.coord, obstacle) && dist(nodes(j).coord, q_new.coord) <= r
                q_nearest(neighbor_count).coord = nodes(j).coord;
                q_nearest(neighbor_count).cost = nodes(j).cost;
                neighbor_count = neighbor_count+1;
            end
        end

 

5.q_nearest找到以q_new爲中心，半徑r中,nodes中的點到q_new最小代價路徑

        for k = 1:1:length(q_nearest)
            if noCollision(q_nearest(k).coord, q_new.coord, obstacle) && q_nearest(k).cost + dist(q_nearest(k).coord, q_new.coord) < C_min
                q_min = q_nearest(k);
                C_min = q_nearest(k).cost + dist(q_nearest(k).coord, q_new.coord);
                line([q_min.coord(1), q_new.coord(1)], [q_min.coord(2), q_new.coord(2)], 'Color', 'g');                
                hold on
            end
        end
        
        % Update parent to least cost-from node
        for j = 1:1:length(nodes)
            if nodes(j).coord == q_min.coord
                q_new.parent = j;
            end
        end
        

6.增加到q_new到node

  nodes = [nodes q_new];

 

7.繼續循環產生隨機數，直到完成

 

最後完成如下，紅色就是最終路徑。黑色就是通過q_near，q_rand 找到的q_new。綠色就是通過半徑r，q_nearest中的nodes最優路徑

具體代碼地址：https://download.csdn.net/download/caokaifa/10680028