How to put a spline through points representing a path
19 views (last 30 days)
Show older comments
I have this path here. The waypoints are marked in red. The line going through each point (linearly) is in blue. The path was made programmatically - if you'd like the code to make the path it is:
%% Create Path
%have it come from the south going north
r = 15;
start_pos = [-r * 7 0 0];
%initial approach to curve
waypoints = [start_pos;
-r*5 1.9 -0;
-r*3 1.7 0;
-r*2.5 1.8 0;
-r*2 2.4 0;
-r*1.5 3.4 0;
-r*1 3.7 0;
-r*0.5 3.4 0];
%Set constant radius curve
angle_req = 90;
a = linspace(0, angle_req*pi/180, 10);
round_x = r * sin(a) + waypoints(end, 1);
round_y = r * cos(a) - r + waypoints(end, 2);
for i = 1:length(a)
waypoints = [waypoints; round_x(i) round_y(i) 0];
end
%Set exit
last_point = waypoints(end, :);
waypoints = [waypoints;
last_point(1) + r*1*cos(a(end)) last_point(2) - r*1*sin(a(end)) 0;
last_point(1) + r*2.5*cos(a(end)) last_point(2) - r*2.5*sin(a(end)) 0;
last_point(1) + max(r*5, 100)*cos(a(end)) last_point(2) - max(r*5, 100)*sin(a(end)), 0];
%last_point(1) + r*1*cos(a(end))- 0.2*r*sin(a(end)) last_point(2) - r*1*sin(a(end)) 0;
%last_point(1) + r*2.5*cos(a(end)) - 0.5*r*sin(a(end)) last_point(2) - r*2.5*sin(a(end)) 0;
%last_point(1) + max(r*5, 100)*cos(a(end)) - 0.5*r*sin(a(end)) last_point(2) - max(r*5, 100)*sin(a(end)), 0];
%plot
plot(waypoints(:, 2), waypoints(:, 1), 'LineWidth', 5)
hold on;
plot(waypoints(:, 2), waypoints(:, 1), 'ro', 'LineWidth', 3)
hold off;
st = sprintf("A %d degree turn (anticlockwise) of %d metre radius", angle_req, r);
grid on;
title(st)
ylabel('X Distance (m)'); xlabel('Y Distance(m)')
I am worried about the fact that there could be discontinuous curvature, when I put the waypoints into the driving scenario and draw up the vehicle's trajectory.
I would like to put a smooth spline through these points (which will guarantee a curve with continuous curvature) and then resample my waypoints from there. How do I do this, considering most spline fitting tools rely on a one-to-one correspondence between x-values and y-values? I've tried the curve fitting toolbox but it sends me an error relating to this correspondence.
2 Comments
Bruno Luong
on 24 Mar 2021
Cubic spline provides a C^2 interpolation result, meaning the second derivatve is continuous (in fact continuity is enrured for all derivative of order <= 2). So the curvature is continuous, it might change quickly but it is contnuous.
Accepted Answer
Bruno Luong
on 24 Mar 2021
Edited: Bruno Luong
on 24 Mar 2021
Put this next-below to your code
waypoints=unique(waypoints,'rows','stable'); % <- this is required because your example as duplicated points
% Spline interpolation
t=cumsum([0;sqrt(sum(diff(waypoints,1,1).^2,2))]);
ti=linspace(t(1),t(end),512);
xyzi=interp1(t,waypoints,ti,'spline');
%plot
figure
plot(waypoints(:,2),waypoints(:,1),'or',xyzi(:,2),xyzi(:,1),'b');
axis equal
You don't have to think what kind of angle to turn and it can work for multiple winding path.
2 Comments
More Answers (1)
darova
on 24 Mar 2021
What about rotation?
R = @(a)[cosd(a) -sind(a);sind(a) cosd(a)]; % rotation maitrx
v1 = R(45)*[x(:) y(:)]'; % rotate data to make exclusive X coordinate
x1 = linspace(v1(1,1),v1(1,end),100); % denser mesh
y1 = spline(v1(1,:),v1(2,:),x1); % interpolation with spline
v2 = R(-45)*[x1(:) y1(:)]'; % rotate to original position
3 Comments
See Also
Categories
Find more on Vehicle Scenarios in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!