PRM路径规划代码问题：移除越障蓝线与修复绿线越障

问题描述 #

我编写了用于A*算法的PRM地图生成代码，目前存在两个问题：

• 代码保留了代表原始PRM的蓝色线条，这些线条会穿越障碍物，无法找到移除方法；
• 绿色线条本不应穿越障碍物，但实际却出现了越障情况。

完整代码如下：

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

问题原因及修复方案 #

1. 蓝色线条无法移除的问题 #

蓝色线条来自check_obstacle函数里的my_line = line([node_x,col],[node_y,row]);语句——每次调用该函数都会创建一条默认蓝色的线条对象，且没有主动删除，导致这些线条一直留在图中。

修复方法：
不需要创建实际的line对象来获取坐标，直接计算线段的坐标点即可，修改后的check_obstacle函数：

function on_line = check_obstacle(map,node_x,node_y,row,col)
    on_line = false;
    % 直接计算线段坐标，无需创建line对象
    dx = abs(node_x - col);
    dy = abs(node_y - row);
    line_spacing = max(dx, dy) + 1;
    x_coordinates_line = round(linspace(node_x, col, line_spacing));
    y_coordinates_line = round(linspace(node_y, row, line_spacing));
    
    for i = 1:line_spacing
        % Matlab矩阵索引是(y,x)，对应行、列
        if map(y_coordinates_line(i), x_coordinates_line(i)) == 1
            disp("ON OBSTACLE: " + x_coordinates_line(i) + " " + y_coordinates_line(i));
            on_line = true;
            break;
        end
    end
end

2. 绿色线条越障的问题 #

有两个核心错误：

• 坐标索引颠倒：check_obstacle函数中原本使用map(x_coordinates_line(i),y_coordinates_line(i))，但Matlab的矩阵索引规则是(行,列)，对应(y坐标,x坐标)，颠倒后会错误读取地图位置，导致障碍物检测失效。
• 变量复用错误：绘制绿色线条时，line_on_obstacle是外层循环中最后一次检测的结果，并非当前要连接的节点的检测结果。实际上能进入distances数组的节点已经通过了障碍物检测，无需重复判断。

修复后的主逻辑片段：

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));
        % 能进入distances的节点已通过障碍物检测，直接绘制绿色线条
        disp("Line is not on obstacle")
        hold on
        plot([node_x,col],[node_y,row],"green","LineWidth",1.5);
    end
    disp("==========================")
end

内容的提问来源于stack exchange，提问作者Resul Bulbul