Hey Alexandr
Kindly go through the MATLAB code below that defines a path of vehicle in path-length coordinate.I have take certain assumptions while answering. Kindly make changes as per your use case.
% Example input: closed wavy path
theta = linspace(0, 2*pi, 100);
X = cos(theta) + 0.2 * sin(5 * theta);
Y = sin(theta) + 0.2 * cos(5 * theta);
% Ensure X and Y are column vectors
X = X(:);
Y = Y(:);
% Step 1: Calculate path lengths
path_lengths = calculate_path_lengths(X, Y);
% Step 2: Interpolate coordinates
[X_interp, Y_interp] = interpolate_coordinates(path_lengths, X, Y);
% Generate a dense set of path lengths for smooth plotting
s_dense = linspace(0, path_lengths(end), 1000);
% Interpolated coordinates
X_dense = X_interp(s_dense);
Y_dense = Y_interp(s_dense);
% Compute curvature along the dense path lengths
curvatures = arrayfun(@(s) compute_curvature(X_interp, Y_interp, s), s_dense);
% Plot the original path and the interpolated path
figure;
subplot(2, 1, 1);
plot(X, Y, 'ro-', 'LineWidth', 2, 'DisplayName', 'Original Path');
hold on;
plot(X_dense, Y_dense, 'b-', 'LineWidth', 1.5, 'DisplayName', 'Interpolated Path');
legend;
xlabel('X');
ylabel('Y');
title('Path Interpolation');
grid on;
axis equal;
% Plot curvature along the path length
subplot(2, 1, 2);
plot(s_dense, curvatures, 'k-', 'LineWidth', 1.5);
xlabel('Path Length (s)');
ylabel('Curvature');
title('Curvature along the Path');
grid on;
% Function to calculate path lengths
function path_lengths = calculate_path_lengths(X, Y)
distances = sqrt(diff(X).^2 + diff(Y).^2);
path_lengths = [0; cumsum(distances)];
end
% Function to interpolate coordinates
function [X_interp, Y_interp] = interpolate_coordinates(path_lengths, X, Y)
X_interp = @(s) interp1(path_lengths, X, s, 'pchip', 'extrap');
Y_interp = @(s) interp1(path_lengths, Y, s, 'pchip', 'extrap');
end
% Function to compute curvature
function curvature = compute_curvature(X_interp, Y_interp, s)
dx = derivative(X_interp, s);
dy = derivative(Y_interp, s);
ddx = derivative(X_interp, s, 2);
ddy = derivative(Y_interp, s, 2);
curvature = (dx .* ddy - dy .* ddx) ./ (dx.^2 + dy.^2).^(3/2);
end
% Function to compute derivatives
function d = derivative(f, s, n)
if nargin < 3
n = 1;
end
h = 1e-6;
d = zeros(size(s));
for i = 1:length(s)
if n == 1
d(i) = (f(s(i) + h) - f(s(i) - h)) / (2 * h);
elseif n == 2
d(i) = (f(s(i) + h) - 2 * f(s(i)) + f(s(i) - h)) / (h^2);
end
end
end
I hope this resolves your query.
