双向RRT路径搜索算法matlab代码

一、实现效果

见：双向RRT路径搜索算法流程、注意事项-CSDN博客

二、代码

完整代码见：https://download.csdn.net/download/m0_69611489/89882862

clc
clear
close all
%% 设置环境
S=[0.5 0.5];
E=[99.5 99.5];
Limit=[0 100 0 100];
ob=[50 50;70 80];
ob_r=[15;7];
step=1;
iu_all=1000;%总搜索步数为1000步
%% 算法搜索
tree1=[S 0];
tree2=[E 0];
pg=0.3;%以30%概率选择终点作为prand
en=0;
for iu=1:iu_all
    %确定随机点
    pnow=rand(1);
    if pnow<pg
        prand=E;
    else
        prand=[Limit(1)+rand(1)*(Limit(2)-Limit(1)) Limit(3)+rand(1)*(Limit(4)-Limit(3))];
    end
    %确定新生成节点pnew
    d=ones(1,size(tree1,1));
    for i=1:size(tree1,1)
        d(i)=norm(tree1(i,1:2)-prand);
    end
    [~,index]=min(d);
    index=index(end);
    pch=tree1(index,1:2);
    X_pch=prand-pch;
    X_pch=X_pch/norm(X_pch)*step;
    pnew=pch+X_pch;
    %判断新节点
    io=0;
    for i=1:size(ob,1)
        dist = pointToSegment(ob(i,:),pch,pnew);
        if dist<ob_r(i)
            io=1;
            break
        end
    end
    if io==0
        tree1=[tree1;pnew index];
    end
    for i=1:size(tree2,1)
        if norm(pnew-tree2(i,1:2))<=step
            en=1;
            break
        end
    end
    if en==1
        tree2=tree2(1:i,:);
        break
    end

    %确定随机点
    pnow=rand(1);
    if pnow<pg
        prand=S;
    else
        prand=[Limit(1)+rand(1)*(Limit(2)-Limit(1)) Limit(3)+rand(1)*(Limit(4)-Limit(3))];
    end
     %确定新生成节点pnew
    d=ones(1,size(tree2,1));
    for i=1:size(tree2,1)
        d(i)=norm(tree2(i,1:2)-prand);
    end
    [~,index]=min(d);
    index=index(end);
    pch=tree2(index,1:2);
    X_pch=prand-pch;
    X_pch=X_pch/norm(X_pch)*step;
    pnew=pch+X_pch;
    %判断新节点
    io=0;
    for i=1:size(ob,1)
        dist = pointToSegment(ob(i,:),pch,pnew);
        if dist<ob_r(i)
            io=1;
            break
        end
    end
    if io==0
        tree2=[tree2;pnew index];
    end
    for i=1:size(tree1,1)
        if norm(pnew-tree1(i,1:2))<=step
            en=1;
            break
        end
    end
    if en==1
        tree1=tree1(1:i,:);
        break
    end
end
%% 路径显示
if iu==iu_all
    disp('未搜索到路径')
else
    route1=tree1(end,1:2);
    now_ch=tree1(end,3);
    while now_ch~=1
        now_p=tree1(now_ch,1:2);
        now_ch=tree1(now_ch,3);
        route1=[route1;now_p];
    end
    route1=[route1;S];
    route1=flipud(route1);

    route2=tree2(end,1:2);
    now_ch=tree2(end,3);
    while now_ch~=1
        now_p=tree2(now_ch,1:2);
        now_ch=tree2(now_ch,3);
        route2=[route2;now_p];
    end
    route2=[route2;E];
    
    figure(1)
    route=[route1;route2];
    plot(S(1),S(2),'.m','MarkerSize',20)
    hold on
    plot(E(1),E(2),'.r','MarkerSize',20)
    hold on
    theta = linspace(0, 2*pi, 100);
    for i=1:size(ob,1)
        x = ob(i,1) + ob_r(i) * cos(theta);
        y = ob(i,2) + ob_r(i) * sin(theta);
        fill(x, y, 'k');
        hold on
    end
    axis(Limit)
    plot(route(:,1),route(:,2),'-b','LineWidth',2)
    text(S(1),S(2),'起点')
    hold on
    text(E(1),E(2),'终点')
    hold on
end

%% 路径显示 分开表示
if iu==iu_all
    disp('未搜索到路径')
else
    route1=[route1;route2(1,:)];
    figure(2)
    plot(S(1),S(2),'.m','MarkerSize',20)
    hold on
    plot(E(1),E(2),'.r','MarkerSize',20)
    hold on
    theta = linspace(0, 2*pi, 100);
    for i=1:size(ob,1)
        x = ob(i,1) + ob_r(i) * cos(theta);
        y = ob(i,2) + ob_r(i) * sin(theta);
        fill(x, y, 'k');
        hold on
    end
    axis(Limit)
    plot(route1(:,1),route1(:,2),'-b','LineWidth',2)
    hold on
    plot(route2(:,1),route2(:,2),'-g','LineWidth',2)
    hold on
    text(S(1),S(2),'起点')
    hold on
    text(E(1),E(2),'终点')
    hold on
end