RRT算法MATLAB实现

qs=[00]‘;
qf=[4050]‘;     %起始点和目标点
d=3;               %步长
N1 = 1;           %T1中的点的个数
T1（:N1） = qs;

r=3;
nr = 10;
n_rand = 0;
flag = 0;
z_real1=zeros（22*nr）;
theta = 0:pi/200:2*pi;     %画障碍物圆以及留出一定阈值后的障碍区域
for i6=1:nr                       %生成10个障碍物
    x0（1i6）=randi（4011）; 
    y0（1i6）=randi（5011）; 
    circ_yx（:i6） = [x0（1i6）y0（1i6）]‘;
    xr（i6:） = x0（1i6）+r*cos（theta）;          
    yr（i6:） = y0（1i6）+r*sin（theta）;
    plot（xr（i6:）yr（i6:）‘r‘）;
    hold on;
end

for i0 = 1:30
    x_rand = randi（4011）;
    y_rand = randi（5011）;
    n_rand = n_rand+1;
    plot（x_randy_rand‘ko‘）;
    hold on;
    z_rand（:n_rand） = [x_randy_rand]‘;   %在空间中生成随机点
    for i7 = 1:nr
        if （norm（circ_yx（:i7）-qf） <=r）
            flag = flag+1;
        else
        end
        if （flag == 1）
            break;
        end
    end
     if （flag == 1）
            break;
     end
    nj = 0;
    nT1 = size（T12）;                 %T1中点的个数
    z_near（:1） = T1（:1）;
    for i1 = 1:nT1
        if （norm（z_near（:1） - z_rand（:n_rand））>norm（T1（:i1）-z_rand（:n_rand）））
            z_near（:1） = T1（:i1）;
        else
        end
    end
    %plot（z_near（11）z_near（21）‘b+‘）;
    %hold on;
    k=（y_rand-z_near（21））/（x_rand-z_near（11））;
    b=y_rand-k*x_rand;
    if （z_near（11）<=x_rand）
        x=z_near（11）:0.01:x_rand;
    else
        x=x_rand:0.01:z_near（11）;
    end
    y=k*x+b;       %起始点与随机点的连线
    plot（xy‘b‘）;
    hold on;
    for i2=1:nr
        [xjyj]=solve（‘yj=k*xj+b‘‘（xj-x0）^2+（yj-y0）^2=r^2‘‘xj‘‘yj‘）;          %求障碍物与连线的交点
         xj=eval（xj）;
         yj=eval（yj）;
         x_real=real（xj）;
         y_real=real（yj）;
         x_img=imag（xj）;         %交点的虚部
         y_img=imag（yj）;
          if （x_img（1i2）==0）    %连线与障碍物有交点
              if （（（x_real（1i2）>x_rand） & （x_real（2i2）x_rand）））
                  nj=nj+1;           %交点个数
                  if （（（x_real（1i2）-z_near（11））^2+（y_real（1i2）-z_near（21））^2）<=（（（x_real（2i2）-z_near（11））^2+（y_real（2i2）-z_near（21））^2）））
                        z_real1（1nj）=x_real（1i2）;
                        z_real1（2nj）=y_real（1i2）;
                  else
                       z_real1（1nj）=x_real（2i2）;
                       z_real1（2nj）=y_real（2i2）;
                  end
              else
                  if （z_near（11）<=x_rand）
                      if （（x_real（1i2）<=x_rand） & （x_real（2i2）<=x_rand） & （x_real（1i2）>=z_near（11）） & （x_real（2i2）>=z_near（11）））
                           nj=nj+2;
                               if （x_real（1i2）>x_real（2i2））
                                     z_real1（1nj-1）=x_real（2i2）;
                                     z_real1（2nj-1）=y_real（2i2）;
                                     z_real1（1nj）=x_real（1i2）;
                                     z_real1（2nj）=y_real（1i2）;