Informed RRT* with improved converging rate by adopting wrapping procedure

https://link.springer.com/article/10.1007%2Fs11370-017-0242-9

这篇文章是基于informed-rrt*，只是他对于初始得到的路径，进行处理，把informed-RRT*的初始椭圆的长轴a的值减小，将采样区域减小。

clc
clear
map=im2bw(imread('5.png')); 
 path=[
   20 480;
   75 223;
   322 243;
    347 414
   ];

imshow(map);
hold on
plot(path(:,1),path(:,2),'b')
m=size(path,1);
newpath=path;

for i=1:2
    np=get1(newpath(i,:),newpath(i+1,:),newpath(i+2,:),map);
    newpath(i+1,:)=np;
    newpath(:,:);
end
hold on
line(newpath(:,1),newpath(:,2),'linewidth',2,'color','r')
function flag=check(p,q,map)
flag=0;
    for n=0:0.01:1
        c=(1-n)*p+n*q; 
        if(map(int32(c(2)),int32(c(1)))==0)
            flag=1;
            break
        end
    end
end
function newnode2=get1(mm,p,q,map)
    d=norm(p-q);
    theta=atan2(q(2)-p(2),q(1)-p(1));

     for d1=0:1:d
        newpoint=p+d1*[cos(theta),sin(theta)];
        if (check(mm,newpoint,map)==1)
            newnode1=p+(d1-0.1)*[cos(theta),sin(theta)];
  
             break
        else
            newnode1=q;
        end
     end
    dd=norm(newnode1-mm);
    theta1=atan2(newnode1(2)-mm(2),newnode1(1)-mm(1));
    for d2=0:1:dd
        newpoint1=mm+d2*[cos(theta1),sin(theta1)];
        if (check(newpoint1,q,map)==0)
            newnode2=newpoint1;
            break
        else
            newnode2=q;
        end      
    end

end

有错误请指出。

标签：converging,newpath,newnode1,end,map,mm,wrapping,theta,improved
来源： https://blog.csdn.net/ljq31446/article/details/88952029