如何删除障碍物上的线 PRM 示例

Question

我有以下代码来生成将用于 A* 应用程序的 PRM 映射。代码存在两个问题

1. 它保留代表原始 PRM 的蓝线，其中线可以越过障碍物。我不想保留蓝线，但找不到删除它们的方法。
2. 绿线正在越过障碍，尽管它们不应该越过障碍

   代码如下

   clc;
   clear all;
   close all;
   
   
   seed = 123512;
   rng(seed);
   
   xaxis = 100;
   yaxis = 100;
   
   obstacles = false(xaxis,yaxis);
   [X,Y] = meshgrid(1:xaxis,1:yaxis);
   
   obstacles(50:75,50:75) = true;
   obstacles(25:35,30:40) = true;
   obstacles(25:35,60:80) = true;
   
   figure;
   imshow(~obstacles,"InitialMagnification",1000);
   axis([0 xaxis 0 yaxis]);
   axis xy;
   axis on;
   
   %PRM PARAMETERS
   max_nodes_connect = 4;
   max_connect_len = 40;
   segments = 1;
   max_nodes_grid = 30;
   skipped = 0;
   
   %PRM ALGO
   nodes = 0; %Counter
   map = zeros(size(obstacles)); %generate map
   map(obstacles) = 1; %put the obstacles
   Graph_connections = Inf(max_nodes_grid,max_nodes_connect + 1); %rows = # nodes cols = ID and neighbors 
   
   while (nodes < max_nodes_grid)
       node_x = randi(xaxis);
       node_y = randi(yaxis);
   
       if(map(node_y,node_x)==1 || map(node_y,node_x)==2)
           continue;
       end
   
       nodes = nodes + 1; %a valid node generated
       map(node_y,node_x) = 2; %2 means there exists a node at that location
   
       hold on 
       scatter(node_x,node_y,"red","filled")
       
       %NODES TO CONNECT
       nodes_to_connect = [];
       distances = [];
   
       for i= 1:numel(Graph_connections(:,1))
           if(Graph_connections(i,1)==Inf)
               break
           end
           [row,col] = ind2sub(size(map),Graph_connections(i,1));
   
           %Check if within range
           if(norm([node_y,node_x]-[row,col])>max_connect_len)
               continue;
           end
   
           line_on_obstacle = check_obstacle(map,node_x,node_y,row,col);
           %Check if obstacle thru line HAS TO BE WRITTEN
           if(line_on_obstacle)
               disp("Check Obstacle: " + line_on_obstacle);
               skipped = skipped + 1;
               continue;
           end
   
           nodes_to_connect = [nodes_to_connect, Graph_connections(i,1)];
           distances = [distances; [Graph_connections(i,1),norm([node_y,node_x]-[row,col])]];
       end
           Graph_connections(nodes,1) = sub2ind(size(map),node_y,node_x);
           if(size(distances)>0)
               sorted_distances = sortrows(distances,2);
               for i = 1:min(max_nodes_connect,size(sorted_distances,1))
                   Graph_connections(nodes,i+1) = sorted_distances(i,1);
                   [row,col] = ind2sub(size(map),sorted_distances(i,1));
                   if(line_on_obstacle==false)
                       disp("Line is not on obstacle")
                       hold on
                       plot([node_x,col],[node_y,row],"green","LineWidth",1.5);
                       continue;                    
                   else
                       disp("Line is on obstacle: " + [node_x,col] + " " + [node_y,row]);
                       break;
                   end
               end
               disp("==========================")
           end
   end
   
   function on_line = check_obstacle(map,node_x,node_y,row,col)
       on_line = 0;
       my_line = line([node_x,col],[node_y,row]);
       line_spacing = max(abs(my_line.XData(1) - my_line.XData(2))+1,abs(my_line.XData(1) - my_line.XData(2))+1);
       x_coordinates_line = round(linspace(my_line.XData(1),my_line.XData(2),line_spacing));
       y_coordinates_line = round(linspace(my_line.YData(1),my_line.YData(2),line_spacing));
       for i = 1:line_spacing
           if(map(x_coordinates_line(i),y_coordinates_line(i))==1)
               disp("ON OBSTACLE: " + x_coordinates_line(i) + " " + y_coordinates_line(i));
               on_line = true;
               break;
           end
       end
   end
   

   check_obstacle 函数用于检查线上的点是否在障碍物的边界内。我在这里错过了什么？

Answer 1

close all;clear all;clc
format short

nx=100;ny=100;     % grid size
[X,Y]=meshgrid(1:nx,1:ny);

Nobst=3      % amount obstacles
Nnet=30      % amount net points
Dmax=100     % net segment max length

% OBSTACLES
% define obstacles. 
% Following are as defined in question
P1=[50 50; 75 50; 75 75; 50 75; 50 50];
P2=[25 30; 25 40; 35 40; 35 30; 25 30];
P3=[25 60; 25 80; 35 80;35 60;25 60];

% obstacle points all in one array
Pobst=[P1(:);P2(:);P3(:)];
Pobst=reshape(Pobst,[size(P1,1),size(P1,2),Nobst]);

% plot obstacles
hp=[]
figure(1)
ax=gca
hp=patch(squeeze(Pobst(:,1,:)),squeeze(Pobst(:,2,:)),[.5 .5 .5])
axis([1 nx 1 ny]);grid on
hp.EdgeAlpha=0;
ax.DataAspectRatio=[1 1 1]
hold(ax,'on')

% obstacle segments list : [x1 y1 x2 y2 d(X1,Y1)]

Lobst1=[]
for k=2:1:size(P1,1)
    Lobst1=[Lobst1; [P1(k-1,:) P1(k,:) ((P1(k-1,1)-P1(k,1))^2+(P1(k-1,2)-P1(k,2))^2)^.5]];
end
Lobst2=[]
for k=2:1:size(P2,1)
    Lobst2=[Lobst2; [P2(k-1,:) P2(k,:) ((P2(k-1,1)-P2(k,1))^2+(P2(k-1,2)-P2(k,2))^2)^.5]];
end
Lobst3=[]
for k=2:1:size(P3,1)
    Lobst3=[Lobst3; [P3(k-1,:) P3(k,:) ((P3(k-1,1)-P3(k,1))^2+(P3(k-1,2)-P3(k,2))^2)^.5]];
end
Lobst=[Lobst1;Lobst2;Lobst3]

%% NETWORK
% all grid points outside obstacles
[in1,on1]=inpolygon(X(:),Y(:),P1(:,1),P1(:,2));
[in2,on2]=inpolygon(X(:),Y(:),P2(:,1),P2(:,2));
[in3,on3]=inpolygon(X(:),Y(:),P3(:,1),P3(:,2));

Xout=X(~in1 & ~in2 & ~in3);Yout=Y(~in1 & ~in2 & ~in3);
% plot(ax,Xout,Yout,'og','LineStyle','none')  % check

% choose Nnet points outside obstacles
nP2=randi([1 numel(Xout)],Nnet,1);
Pnet=[Xout(nP2) Yout(nP2)];

plot(ax,Pnet(:,1),Pnet(:,2),'or','LineStyle','none')

% net segments list [x1 y1 x2 y2 d(X2,Y2) 0/1] 6th column [0 1] 1: draw 0: do not draw
nLnet=nchoosek([1:size(Pnet,1)],2);
Lnet=[Pnet(nLnet(:,1),1) Pnet(nLnet(:,1),2) ...   % segment 1st point
         Pnet(nLnet(:,2),1) Pnet(nLnet(:,2),2) ...    % segment 2nd point
         ((Pnet(nLnet(:,1),1)-Pnet(nLnet(:,1),2)).^2+(Pnet(nLnet(:,2),1)+Pnet(nLnet(:,2),2)).^2).^.5 ... % segment length
         ones(size(nLnet,1),1)];   

% check all net links are plotted
for k=1:1:size(Lnet,1)
    plot(ax,[Lnet(k,1) Lnet(k,3)],[Lnet(k,2) Lnet(k,4)],'b');
    hold(ax,'on')
end

% remove segments longer than Dmax
Lnet=sortrows(Lnet,5,'descend');
[~,n1]=max(Lnet(:,5)<=Dmax);
Lnet(1:n1-1,:)=[];
for k=1:1:size(Lnet,1)
    plot(ax,[Lnet(k,1) Lnet(k,3)],[Lnet(k,2) Lnet(k,4)],'r');
    hold(ax,'on')
end

%%

重绘和不是删除长于Dmax 的网段

close all
hp=[]
figure(1)
ax=gca
hp=patch(squeeze(Pobst(:,1,:)),squeeze(Pobst(:,2,:)),[.5 .5 .5])
axis([1 nx 1 ny]);grid on
hp.EdgeAlpha=0;
ax.DataAspectRatio=[1 1 1]
hold(ax,'on')
plot(ax,Pnet(:,1),Pnet(:,2),'or','LineStyle','none')
Lnet=[Pnet(nLnet(:,1),1) Pnet(nLnet(:,1),2) ...   % segment 1st point
         Pnet(nLnet(:,2),1) Pnet(nLnet(:,2),2) ...    % segment 2nd point
         ((Pnet(nLnet(:,1),1)-Pnet(nLnet(:,1),2)).^2+(Pnet(nLnet(:,2),1)+Pnet(nLnet(:,2),2)).^2).^.5 ... % segment length
         ones(size(nLnet,1),1)];

% check what pair segments intersect
for k2=1:1:size(Lnet,1)
    allclear=ones(1,size(Lobst,1));
%     allclear=zeros(1,size(Lobst,1));
    for k1=1:1:size(Lobst,1)
        
        % segments are contained in lines : check lines intersect
        x1o=Lobst(k1,1);y1o=Lobst(k1,2);x2o=Lobst(k1,3);y2o=Lobst(k1,4);
        x1n=Lnet(k2,1);y1n=Lnet(k2,2);x2n=Lnet(k2,3);y2n=Lnet(k2,4);
        
        x1=x1o;x2=x2o;y1=y1o;y2=y2o;
        x3=x1n;x4=x2n;y3=y1n;y4=y2n;
        
        % t1 : x parameter
        t1=((x1-x3)*(y3-y4)-(y1-y3)*(x3-x4))/((x1-x2)*(y3-y4)-(y1-y2)*(x3-x4));  
        xp=x1+t1*(x2-x1);  % xp : crossing x coordinage
        
        % u1 : y parameter
        u1=-((x1-x2)*(y1-y3)-(y1-y2)*(x1-x3))/((x1-x2)*(y3-y4)-(y1-y2)*(x3-x4));
        yp=y1+t1*(y2-y1);  % yp : crossing y coordinate

        % checks
        plot(ax,x1o,y1o,'c*');plot(ax,x2o,y2o,'c*');plot(ax,[x1o x2o],[y1o y2o],'c')
        plot(ax,x1n,y1n,'m*');plot(ax,x2n,y2n,'m*'); % plot(ax2,[x1n x2n],[y1n y2n],'m')
         
        % this intersection calculation does not cover vertical lines
        m1o=(y2o-y1o)/(x2o-x1o);       % slopes
        m2n=(y2n-y1n)/(x2n-x1n) ; 
  
        if (t1>=0 && t1<=1 && u1>=0 && u1<=1) && ...
           (xp>=0 || xp<=nx || yp>=0 || yp<=ny)
                allclear(k1)=0;  % then do not plot this segment
        end

    end
    
    if sum(allclear)==size(Lobst,1)   %k2-th net segment hits no obstacles
        plot(ax,x1n,y1n,'m*');plot(ax,x2n,y2n,'m*');plot(ax,[x1n x2n],[y1n y2n],'m')
        elseif sum(allclear)~=size(Lobst,1)
            Lnet(k2,end)=0;
    end
     
end

注释

1.-请注意，我添加了format short，因为format long在几个交点的底部有小数增量，不管你信不信，都会导致一些错误的结果。

2.-我喜欢 Torsen 关于查找可用线段交叉点的解释 here。

3.-有更快的方法来实现主循环，但我让它遍历所有网络段与障碍元素，以防您可能需要这种方式，例如计算命中。

4.-Lobst 的生成方式也有改进的余地。