Iterative path collision refinement
52 次查看(过去 30 天)
显示 更早的评论
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
0 个评论
回答(0 个)
另请参阅
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!