手把手教用matlab做无人驾驶（六）-路径规划RRT_无人车路径规划matlab

这里介绍路径规划算法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