基于RRT的路径规划

Rapidly-exploring Random Tree (RRT) 是一种经典的路径规划算法，它在解决高维空间中的路径规划问题中表现出高效性和鲁棒性。RRT算法主要适用于非光滑、动态环境以及复杂约束的路径规划场景，如机器人导航、飞行器控制等。

1. 算法概述： RRT算法的核心思想是构建一棵随机树，从初始节点出发，通过不断随机选择新的扩展节点并连接到已有的树结构上，逐步探索可能的路径。这个过程既包括从现有节点向未探索区域生长，也包括沿着当前路径的回溯尝试。

2. 基本步骤： a. 初始化：开始时，创建一个初始节点，并将其作为树的根节点。 b. 随机选择：生成一个目标区域内的随机位置作为扩展节点候选。 c. 树扩展：计算从当前节点到扩展节点的直接路径，如果新节点不在树中或路径更优，则将其添加到树中，并更新连接。 d. 回溯：沿着最近的路径从扩展节点向树的根节点搜索，如果找到一个更短的路径，就更新该路径。 e. 重复：重复上述步骤，直到达到预设的停止条件（如达到目标节点或达到最大迭代次数）。

3. 优点： a. 高效：对于高维空间，RRT比其他方法如A*更快地找到可行路径。 b. 适应性强：能够处理复杂的约束和障碍物环境。 c. 局部最优：虽然不是全局最优，但在实际应用中通常能找到满意解。

4. 扩展版本：

   • RRT (Rapidly-exploring Random Trees)**：结合了宽度优先搜索和RRT的思想，增加了对全局最优解的搜索。
   • Informed RRT：引入外部信息（如启发式函数），提高搜索效率。
   • RRT-Connect：解决RRT可能导致的孤立分支问题，通过增加连接策略保证路径连贯性。

 设计RRT寻优算法，结果如下：

 

 代码展示：

clc;
clear;
%随机树节点
Tx=[];
Ty=[];
%障碍物坐标
Oxmin(1)=20;
Oxmax(1)=30;
Oymin(1)=0;
Oymax(1)=55;

Oxmin(2)=50;
Oxmax(2)=60;
Oymin(2)=40;
Oymax(2)=100;

Oxmin(3)=70;
Oxmax(3)=80;
Oymin(3)=0;
Oymax(3)=90;

Oxmin(4)=10;
Oxmax(4)=20;
Oymin(4)=65;
Oymax(4)=90;

%起始点,目标点坐标
X_st=10;Y_st=10;
X_go=90;Y_go=40;
%初始化
Tx(1)=X_st;
Ty(1)=Y_st; %随机树
Dis=50;     %新节点与目标点距离
step=1;     %步长
Dis_min=1;  %算法终止，树节点距离目标节点最小距离

%画障碍物
figure(1)
Ox=[Oxmin Oxmin Oxmax Oxmax];
Oy=[Oymin Oymax Oymax Oymin];
Num_z=length(Oxmin);
for i=1:Num_z
    Ox=[Oxmin(i) Oxmin(i) Oxmax(i) Oxmax(i) Oxmin(i)];
    Oy=[Oymin(i) Oymax(i) Oymax(i) Oymin(i) Oymin(i)];
    plot(Ox,Oy,'k','LineWidth',2)
    hold on
end
axis([0 100 0 100])
N=300;
m=moviein(N); 
m1=moviein(N);
i=1;

while(Dis>Dis_min)
   P_rand=100*rand*[1 0]+100*rand*[0 1]; %生成随机点
   P_rand=roundn(P_rand,-1);
   Dis_p=(P_rand(1)-Tx).^2+(P_rand(2)-Ty).^2;
   [Dis_pmin,index]=min(Dis_p);
   P_near=[Tx(index) Ty(index)];%找到随机树中距离随机点最近的节点
   P_new=P_near+(P_rand-P_near)/sqrt(Dis_pmin)*step;
   T=Coll(P_near,P_new,Oxmin,Oxmax,Oymin,Oymax);
   if T==0
       Tx=[Tx P_new(1)];
       Ty=[Ty P_new(2)];
       X=[P_near(1) P_new(1)];
       Y=[P_near(2) P_new(2)];
       plot(X,Y)
       hold on
       Dis=sqrt((P_new(1)-X_go)^2+(P_new(2)-Y_go)^2);
       if i<301
       m(:,i)=getframe;            %将图形保存到m矩阵
    m1(:,i)=getframe(gcf);
    M=getframe(gcf);
    nn=frame2im(M);
    [nn,cm]=rgb2ind(nn,256);
    if i==1
    imwrite(nn,cm,'RRT曲线第一段1.gif','gif','LoopCount',inf,'DelayTime',0);% 说明loopcount只是在i==1的时候才有用
    i=i+1;
         else
    %[nn,cm]=rgb2ind(nn,256);
    imwrite(nn,cm,'RRT曲线第一段1.gif','gif','WriteMode','append','DelayTime',0);%当i>=2的时候loopcount不起作用
    i=i+1;
    end
       end
   end
end
Tx=[Tx X_go];
Ty=[Ty Y_go];%将目标节点加入到随机树中
Num=length(Tx)
%从目标点逆向遍历，画出路径。
T2=1;  %给定跳出循环条件
Path=[X_go Y_go];
PNum=Num-1; %父节点标号
SNum=Num;   %子节点标号

N=100;
mm=moviein(N); 
mm1=moviein(N);
j=1;
while (T2)
    for i=PNum:-1:1
        Dis_2=sqrt((Tx(i)-Tx(SNum))^2+(Ty(i)-Ty(SNum))^2);
        if (Dis_2<1.001)
            Path_x=[Tx(i) Tx(SNum)];
            Path_y=[Ty(i) Ty(SNum)];
            plot(Path_x, Path_y,'r','LineWidth',2)
            hold on
            Path=[Path;Tx(i) Ty(i)];
            PNum=i-1;
            SNum=i;
            
                   if j<101
       mm(:,j)=getframe;            %将图形保存到m矩阵
    mm1(:,j)=getframe(gcf);