Iterative path collision refinement

Question

YAAQOB AL-JAMALI 2024-8-30，4:10

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2148929-iterative-path-collision-refinement

I am having a path that is resulted from RRT-connect, then it is pruned using Ramer–Douglas–Peucker. Once we get the path with reduced waypoints we applied cubic B-spline that is a built-in MATLAB function [pathSmooth, qd, qdd, pp] = bsplinepolytraj(cpts,tpts,tvec);

The smoothed path we check wherether it is a collisiono-free or not using the function [cc, iwant, computationTime3] = collisionChecking(map, pathSmooth);

The collsision was detected, so we need to to refinment and that where the problem existed. The refinment function is [pathReducedRefined] = reformCollideSegment(iwant, pathReduced);, and the process while continue while iwant is not empty. The program could not produced a path that is collision-free and goes for infinity.

I have attached the following:

Initial path before prunning name as

2. Prunned path named as 'pathReduced.mat'

3. Smoothed path (that is colliding) named as 'smoothedPath.mat'

4. the map that we are working with name as 'environmentMatrix.mat'

The cod that we used asf follow:

%% Path reduction
% reducepoly function Reduce density of points in ROI using Ramer–Douglas–Peucker
% algorithm.
% P_reduced = reducepoly(P,tolerance) reduces the density of points in array
% P, where tolerance specifies how much a point can deviate from a straight
% line.
tolerance = 0.03;
pathReduced = reducepoly(path,tolerance);
% Check the number of row for pathReduced (must be at least 4 rows)
[row, column] = size(pathReduced);
if row < 4
    pathReduced = path;
end
%% Path smoothing sing B-spline smoothing function
% Inputs:
% cpts = pathReduced';
cpts = path';
% Calling Smoothing Funtion
[pathSmooth, pathLengthS, qd, qdd, pp] = BSpline(cpts);
%% Collision detection for the smoothed path and refinment
% Collision detection is performed by discrete samples
[cc, iwant, computationTime3] = collisionChecking(map, pathSmooth);
keyboard;
while ~isempty(iwant)
    [pathReducedRefined] = reformCollideSegment(iwant, pathReduced);
    pathReduced = pathReducedRefined;
    cpts = pathReduced';
    % Calling Smoothing Funtion
    [pathSmooth, ComputationTime2, pathLengthS] = BSpline(cpts);
    [cc, iwant, computationTime3] = collisionChecking(map, pathSmooth);
end
function [cc, iwant, Time, collideIndices] = collisionChecking(map, pathSmooth)
% % FUNCTION COLLISIONCHECKING test for collision occurence.
% %
% % Inputs:
% %   map         - logical map of size 500x500
% %   pathSmooth  - smoothed path in cartesian form produced using
% %                   [pathSmooth, pathLengthS] = BSpline(cpts).
% %
% % Outputs:
% %   cc          - flage of collision occurence: logic 1 represents
% %   collision, while logic 0 represetns collision-free condition
% %
tic;
v = pathSmooth;
env = map;
cc = true;
% Initialize arrays for storing intersections and their indices
iwant = zeros([],2) ;           % To store the coordinates of intersections
collideIndices = zeros([]);   % To store the indices in v that correspond to iwant
count = 0;
% Loop through the path v
for ii = 1:length(v)-1
    % Check if the point in v is in an obstacle-free area (intersection)
    if env(round(v(ii,1)), round(v(ii,2)))  % Use (y,x) indexing for the environment
        cc = false;
        %         disp('There is no intersection');
    else
        count = count + 1;
        
        iwant(count,:) = [round(v(ii,2)), round(v(ii,1))];  % Store the point in iwant
        collideIndices(count) = ii;                         % Store the index from v
        cc = true;
        %         disp('There is intersection');
    end
end
Time = toc;
end