Iterative path collision refinement

18 次查看（过去 30 天）
YAAQOB AL-JAMALI
YAAQOB AL-JAMALI 2024-8-30
评论： Walter Roberson 2025-3-12
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);
Once collsision is detected, we need to to refine the collide path. The refinment function is [pathReducedRefined] = reformCollideSegment(iwant, pathReduced);, and the process will continue while iwant is not empty. The program could not produced a path that is collision-free and goes for infinity.
The issue is: refinment cannot achieved due to some reasons which are not clear to me. Can anyone suggest something to tackl such issue
I have attached the following:
  1. 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
  1 个评论
Walter Roberson
Walter Roberson 2025-3-12
Your for ii loop is setting cc according to the last item checked. That is probably wrong.
If you want cc to be false if any of the checks are false, then set cc = true before the loop and remove the cc = true line but keep the cc = false line -- that way cc will become false if even one of the checks fails.
If you want cc to be true if any of the checks are true, then set cc = false before the loop and remove the "cc = false| line, and leave the cc = true line -- that way cc will become true if even one of the checks works.

请先登录，再进行评论。

请先登录，再回答此问题。

回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Whos 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by