How to define a path of vehicle

I would like to find an approach to define a path of vehicle in path-length coordinate.

As input I have an array of Cartesian coordinates (X, Y). I need to convert it to some function (object) which allow me to get a curvature of path, X, Y as functions of path length. Also there is a issue that I have a closed path like the following:

What can I use for such task?

Answer 1

[Anurag Ojha]

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.