RRT路徑規划算法（matlab實現）

基於快速擴展隨機樹（RRT / rapidly exploring random tree）的路徑規划算法，通過對狀態空間中的采樣點進行碰撞檢測，避免了對空間的建模，能夠有效地解決高維空間和復雜約束的路徑規划問題。該方法的特點是能夠快速有效地搜索高維空間，通過狀態空間的隨機采樣點，把搜索導向空白區域，從而尋找到一條從起始點到目標點的規划路徑，適合解決多自由度機器人在復雜環境下和動態環境中的路徑規划。與PRM類似，該方法是概率完備且不最優的。

RRT是一種多維空間中有效率的規划方法。它以一個初始點作為根節點，通過隨機采樣增加葉子節點的方式，生成一個隨機擴展樹，當隨機樹中的葉子節點包含了目標點或進入了目標區域，便可以在隨機樹中找到一條由從初始點到目標點的路徑。基本RRT算法如下面偽代碼所示：

 

 

 

 

 

function feasible=collisionChecking(startPose,goalPose,map)%沖突檢查：判斷起始點到終點之間是否有障礙物

feasible=true;%可行的，可執行的
dir=atan2(goalPose(1)-startPose(1),goalPose(2)-startPose(2));%目標點和起始點之間的角度
for r=0:0.5:sqrt(sum((startPose-goalPose).^2))%sqrt(sum((startPose-goalPose).^2)):兩點間的距離
    posCheck = startPose + r.*[sin(dir) cos(dir)];%以0.5的間隔得到中間的點
    if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) && ...
            feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) && feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))
        feasible=false;break;
    end
    if ~feasiblePoint([floor(goalPose(1)),ceil(goalPose(2))],map), feasible=false; end

end

function feasible=feasiblePoint(point,map)%判斷點是否在地圖內，且沒有障礙物
feasible=true;
if ~(point(1)>=1 &&  point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(2),point(1))==255)%map(point(2),point(1))==255：沒有障礙物
    feasible=false;
end

function distance=Distance(start_Pt,goal_Pt)
distance=sqrt((start_Pt(1)-goal_Pt(1))^2+(start_Pt(2)-goal_Pt(2))^2);

function [X_near,index]=Near(X_rand,T)
min_distance=sqrt((X_rand(1)-T.v(1).x)^2+(X_rand(2)-T.v(1).y)^2);
for T_iter=1:size(T.v,2)
    temp_distance=sqrt((X_rand(1)-T.v(T_iter).x)^2+(X_rand(2)-T.v(T_iter).y)^2);
    if temp_distance<=min_distance
        min_distance=temp_distance;
        X_near(1)=T.v(T_iter).x
        X_near(2)=T.v(T_iter).y
        index=T_iter;
    end
end

function X_rand=Sample(map,goal)
% if rand<0.5
%     X_rand = rand(1,2) .* size(map);   % random sample
% else 
%     X_rand=goal;
% end

if unifrnd(0,1)<0.5
   X_rand(1)= unifrnd(0,1)* size(map,1);   % 均勻采樣
   X_rand(2)= unifrnd(0,1)* size(map,2);   % random sample
else
   X_rand=goal;
end

function X_new=Steer(X_rand,X_near,StepSize)
theta = atan2(X_rand(1)-X_near(1),X_rand(2)-X_near(2));  % direction to extend sample to produce new node
X_new = X_near(1:2) + StepSize * [sin(theta)  cos(theta)];

% dir_x = X_rand(1)- X_near(1);
% dir_y = X_rand(2)- X_near(2);
% dir = sqrt(dir_x^2 + dir_y^2);
% X_new(1) = dir_x * StepSize/dir+X_near(1);
% X_new(2) = dir_y * StepSize/dir+X_near(2);

 

function X_rand=Sample(map,goal)
% if rand<0.5
%     X_rand = rand(1,2) .* size(map);   % random sample
% else 
%     X_rand=goal;
% end

if unifrnd(0,1)<0.5
   X_rand(1)= unifrnd(0,1)* size(map,1);   % 均勻采樣
   X_rand(2)= unifrnd(0,1)* size(map,2);   % random sample
else
   X_rand=goal;
end

 

 

%***************************************
%Author: Chenglong Qian
%Date: 2019-11-5
%***************************************
%% 流程初始化
clear all; close all;
x_I=1; y_I=1;           % 設置初始點
x_G=700; y_G=700;       % 設置目標點
goal(1)=x_G;
goal(2)=y_G;
Thr=50;                 %設置目標點閾值 當到這個范圍內時則認為已到達目標點
Delta= 30;              % 設置擴展步長，擴展結點允許的最大距離
%% 建樹初始化
T.v(1).x = x_I;         % T是我們要做的樹，v是節點，這里先把起始點加入到T里面來
T.v(1).y = y_I; 
T.v(1).xPrev = x_I;     % 起始節點的父節點仍然是其本身
T.v(1).yPrev = y_I;
T.v(1).dist=0;          %從父節點到該節點的距離，這里可取歐氏距離
T.v(1).indPrev = 0;     %父節點的索引
%% 開始構建樹——作業部分
figure(1);
ImpRgb=imread('newmap.png');  
Imp=rgb2gray(ImpRgb);
imshow(Imp)
xL=size(Imp,1);%地圖x軸長度
yL=size(Imp,2);%地圖y軸長度
hold on
plot(x_I, y_I, 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');
plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');% 繪制起點和目標點
count=1;
for iter = 1:3000
%     x_rand=[];
    %Step 1: 在地圖中隨機采樣一個點x_rand
    %提示：用（x_rand(1),x_rand(2)）表示環境中采樣點的坐標
    x_rand=Sample(Imp,goal);
%     x_near=[];
    %Step 2: 遍歷樹，從樹中找到最近鄰近點x_near 
    %提示：x_near已經在樹T里
    [x_near,index]= Near(x_rand,T);
    plot(x_near(1), x_near(2), 'go', 'MarkerSize',2);
%     x_new=[];
    %Step 3: 擴展得到x_new節點
    %提示：注意使用擴展步長Delta
    x_new=Steer(x_rand,x_near,Delta);
    %檢查節點是否是collision-free
    if ~collisionChecking(x_near,x_new,Imp) %如果有障礙物則跳出
       continue;
    end
    count=count+1;
    
    %Step 4: 將x_new插入樹T 
    %提示：新節點x_new的父節點是x_near
    T.v(count).x = x_new(1);        
    T.v(count).y = x_new(2); 
    T.v(count).xPrev = x_near(1);     % 起始節點的父節點仍然是其本身
    T.v(count).yPrev = x_near(2);
    T.v(count).dist=Distance(x_new,x_near);          %從父節點到該節點的距離，這里可取歐氏距離
    T.v(count).indPrev = index;     %父節點的索引
    %Step 5:檢查是否到達目標點附近 
    %提示：注意使用目標點閾值Thr，若當前節點和終點的歐式距離小於Thr，則跳出當前for循環
    if Distance(x_new,goal) < Thr
        break;
    end
   %Step 6:將x_near和x_new之間的路徑畫出來
   %提示 1：使用plot繪制，因為要多次在同一張圖上繪制線段，所以每次使用plot后需要接上hold on命令
   %提示 2：在判斷終點條件彈出for循環前，記得把x_near和x_new之間的路徑畫出來
%    plot([x_near(1),x_near(2)],[x_new(1),x_new(2)]);
%    hold on
  line([x_near(1),x_new(1)],[x_near(2),x_new(2)]);
   pause(0.1); %暫停0.1s，使得RRT擴展過程容易觀察
end
%% 路徑已經找到，反向查詢
if iter < 2000
    path.pos(1).x = x_G; path.pos(1).y = y_G;
    path.pos(2).x = T.v(end).x; path.pos(2).y = T.v(end).y;
    pathIndex = T.v(end).indPrev; % 終點加入路徑
    j=0;
    while 1
        path.pos(j+3).x = T.v(pathIndex).x;
        path.pos(j+3).y = T.v(pathIndex).y;
        pathIndex = T.v(pathIndex).indPrev;
        if pathIndex == 1
            break
        end
        j=j+1;
    end  % 沿終點回溯到起點
    path.pos(end+1).x = x_I; path.pos(end).y = y_I; % 起點加入路徑
    for j = 2:length(path.pos)
        plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);
    end
else
    disp('Error, no path found!');
end

 

 參考鏈接：https://www.cnblogs.com/flyinggod/p/8727951.html