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

  • 1
    点赞
  • 7
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值