RRT算法（sampling based motion planning）

最近研究了一下机器人运动规划，基于搜索的dfs，bfs，dijstra，A*之类的算法课上大多都讲过，所以选了这个基于采样的方法，做了个小实验。
自己进行了一些算法优化，使其更快收敛。效果还算不错吧。
下面是代码和动画：

clear all;
close all;
global n obstacle dst range step
map=zeros(255,255);
n=9;
obstacle=[170 50;50 36 ;78 89;90 140;205 169;123 94;124 215;150 170;40 200];
range=round(4*rand(1,n)+14);
src=round(25*rand(2,1)+1);
dst=round(45*rand(2,1)+190);
for i=1:n
    x=obstacle(i,1);
    y=obstacle(i,2);
    for j=x-range(1,i):x+range(1,i)
        for k=y-range(1,i):y+range(1,i)
            map(j,k)=1;
        end
    end
end
clear x y;
map(src(1,1),src(2,1))=0;
map(dst(1,1),dst(2,1))=0;
step=3;
iteration=2000;
i=0;
x.parent=0;
x.pos=src;
x.length=0;
T={x};
min_dis=inf;
while i<iteration
    if min_dis>=50
        x_rand=round(253*rand(2,1)+1);
    elseif min_dis>=25
        x_rand=round(100*rand(2,1)+dst-50);
    elseif min_dis>=12
        x_rand=round(50*rand(2,1)+dst-25);
    else
        x_rand=round(50*rand(2,1)+dst-12);
    end
    min=inf;
    x_min=0;
    for j=1:length(T)
        x_pos=T{j}.pos;
        dis=norm(x_rand-x_pos);
        if dis<min
            min=dis;
            x_min=j;
        end
    end
    if min<=0
        continue
    end
    x_new.pos=T{x_min}.pos+round((3+2*rand)*(x_rand-T{x_min}.pos)/norm(x_rand-T{x_min}.pos));
    if x_new.pos(1,1)<=255 && x_new.pos(1,1)>=1 && x_new.pos(2,1)<=255 && x_new.pos(2,1)>=1 && map(x_new.pos(1,1),x_new.pos(2,1))==0
        x_new.parent=x_min;
        x_new.length=norm(x_new.pos-T{x_min}.pos);
        T=[T x_new];
        if min_dis>norm(x_new.pos-dst)
            min_dis=norm(x_new.pos-dst);
        end
        if norm(x_new.pos-dst)<=5
            x_final.pos=dst;
            x_final.parent=length(T);
            x_final.length=norm(x_new.pos-dst);
            T=[T x_final];
            break;
        end
    end
    i=i+1;
end
times=i;
for i=1:n
    xk=[obstacle(i,1)-range(1,i),obstacle(i,1)+range(1,i),obstacle(i,1)+range(1,i),obstacle(i,1)-range(1,i)];
    yk=[obstacle(i,2)-range(1,i),obstacle(i,2)-range(1,i),obstacle(i,2)+range(1,i),obstacle(i,2)+range(1,i)];
    fill(xk,yk,'k')
    hold on
end
plot(src(1,1),src(2,1),'o',"Markersize",10,"MarkerFaceColor",'g')
plot(dst(1,1),dst(2,1),'o',"Markersize",10,"MarkerFaceColor","r")
for i=2:length(T)
    x=[T{i}.pos(1,1) T{T{i}.parent}.pos(1,1)];
    y=[T{i}.pos(2,1) T{T{i}.parent}.pos(2,1)];
    plot(x,y,'b-')
    pause(0.03)
end
clear x y;
num=length(T);
x=[];
y=[];
while num~=0
    x=[x T{num}.pos(1,1)];
    y=[y T{num}.pos(2,1)];
    num=T{num}.parent;
end
plot(x,y,'g-',"LineWidth",2);