RRT算法matlab实现（未改进）

基本思想

1.在已知的静态地图中随机生成点，可以预设这个点是目标点的概率使其随机性减小

2.寻找随机生成点的最近点，连接并在最近点延时固定步长，判断是否与障碍物碰撞

3.重复上述，直到离终点一个范围，到达终点

代码

clear()
ob=[10,10;50,80;70,20;40,40;80,50];
ob_r=[3;5;10;6;5];

%% map setting
rows=100;
cols=100;
start=[5,5];
goal=[100,100];
figure(1)
plot(start(1),start(2),'*r');
hold on
plot(goal(1),goal(2),'*g');
%obstacle draw
aplha=0:pi/40:2*pi;
for ob_r_count=1:length(ob_r)
    x=ob(ob_r_count,1)+ob_r(ob_r_count)*cos(aplha);
    y=ob(ob_r_count,2)+ob_r(ob_r_count)*sin(aplha);
    plot(x,y,'-');
    axis equal
end
%% parameter setting
current_point=start;
current_parent=[];
current_cost=[];
tree_point=start;
tree_parent=start;
tree_cost=0;
step=5;

%% loop
while 1
    if current_point==goal
        %node recall
        line([current_point(1),parent_point(1)],[current_point(2),parent_point(2)]);
        break
    end
    %generate random dot trace,the goal point chance is 10%
    rand_count=rand(1);
    if rand_count<=0.1
        new_point=goal;
    else
        new_point=rand(1,2)*diag([rows,cols],0);
    end
    %find parent point
    parent_point=parent(new_point,tree_point);
    point=parent_point+step*(new_point-parent_point)/norm(parent_point-new_point);
    %judge the distance from obstacles
    if distance(ob,ob_r,point,parent_point,step)
        plot(point(1),point(2),'.b',parent_point(1),parent_point(2),'.b');
        line([point(1),parent_point(1)],[point(2),parent_point(2)]);
        %pause(0.2);%animation
        current_point=point;
        tree_point=[tree_point;point];
        tree_parent=[tree_parent;parent_point];
        tree_cost=[tree_cost,];
    end
    if norm(current_point-goal)<=step
        tree_parent=[tree_parent;current_point];
        current_point=goal;
        tree_point=[tree_point;current_point];
    end
end

%% function parent
function parent_point=parent(new_point,tree_point)
point=dsearchn(tree_point,new_point);
parent_point=tree_point(point,:);
end
%% function distance
function feasibility=distance(ob,ob_r,point,parent_point,step)
feasibility=1;
for count2=1:length(ob_r)
    if norm(point-ob(count2,:))<=ob_r(count2)
        feasibility=0;
        break
    end
    line=ob(count2,:)-parent_point;
    line1=point-parent_point;
    if norm(line)<=step+ob_r(count2) && abs(sum(line.*line1)/(norm(line)*norm(line1)))>=(norm(line)^2-ob_r(count2)^2)^0.5/norm(line)
        feasibility=0;
    end
end
end