How to put a spline through points representing a path

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.

[Bruno Luong]

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.

[Amritz Ansara]

Ah I see. Thank you!

Answer 1

[Bruno Luong]

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.

[Amritz Ansara]

Wonderful, this works for my 180 degree turns too. The spline is cubic, right?

[Bruno Luong]

Yes 'spline' option provides cublic spline.

Answer 2

[darova]

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

[Amritz Ansara]

This works! Thank you very much!

[Amritz Ansara]

Though, I think Bruno Luong's answer is much more robust and flexible. I may have to accept his answer instead - but thank you very much, your solution works too :)

[darova]

you are welcome