基于路径长度的样条插补算法（自动驾驶和路径跟踪控制适用）

news2024/11/17 0:06:35

以前在做车辆跟踪控制的时候发现在针对有多个X和多个Y对应的路径插补时候，总是报错，因为MATLAB里面的interp1插补函数它要求x要唯一对应一个y，当路径以单独的x或者y来求插补时候的时候就报错。由于在使用Matlab的interp1函数进行插值时，采样点必须是唯一的，否则可能会出现问题。如果输入的采样点包含重复的值，interp1函数可能无法确定应该在哪个点进行插值。这会导致函数返回错误或不确定的结果。为了解决这个问题，你可以先对输入的采样点进行处理，确保它们是唯一的。你可以使用Matlab的unique函数来去除重复值，或者通过其他方法确保采样点的唯一性。

针对这个问题，我根据路径的长度来插补，而不是利用单一的x或者单一的y。在遇到类似的情况时候，也可以用范数来插补。具体代码如下

clc,clear
close all
cx = [];
cy= [];
x0 = @(t_step) 15*sin(2 * t_step + 1);
y0 = @(t_step) -20*cos(t_step + 0.5);
for theta=0.9*pi:pi/200:1.8*pi
    cx(end + 1) = x0(theta);
    cy(end + 1) = y0(theta);
end
refer_path_primary= [cx', cy'];
x = refer_path_primary(:, 1)';
y = refer_path_primary(:, 2)';
points = [x; y]';
ds = 0.01 ;%等距插值处理的间隔
distance = [0, cumsum(hypot(diff(x, 1), diff(y, 1)))]';
distance_specific = 0:ds:distance(end);
hypot(diff(x, 1), diff(y, 1));
diff(x, 1);
diff(y, 1);
s = 0:ds:distance(end);
refer_path= interp1(distance, points, distance_specific, 'spline');
len_path = length(refer_path);
refer_path_x = refer_path(:,1);  % x
refer_path_y = refer_path(:,2); % y
for i=1:length(refer_path)
    if i==1
        dx = refer_path(i + 1, 1) - refer_path(i, 1);
        dy = refer_path(i + 1, 2) - refer_path(i, 2);
        ddx = refer_path(3, 1) + refer_path(1, 1) - 2 * refer_path(2, 1);
        ddy = refer_path(3, 2) + refer_path(1, 2) - 2 * refer_path(2, 2);
    elseif  i==length(refer_path)
        dx = refer_path(i, 1) - refer_path(i - 1, 1);
        dy = refer_path(i, 2) - refer_path(i - 1, 2);
        ddx = refer_path(i, 1) + refer_path(i - 2, 1) - 2 * refer_path(i - 1, 1);
        ddy = refer_path(i, 2) + refer_path(i - 2, 2) - 2 * refer_path(i - 1, 2);
    else
        dx = refer_path(i + 1, 1) - refer_path(i, 1);
        dy = refer_path(i + 1, 2) - refer_path(i, 2);
        ddx = refer_path(i + 1, 1) + refer_path(i - 1, 1) - 2 * refer_path(i, 1);
        ddy = refer_path(i + 1, 2) + refer_path(i - 1, 2) - 2 * refer_path(i, 2);
    end
    refer_path(i,3)=atan2(dy, dx);%
    refer_path(i,4)=(ddy * dx - ddx * dy) / ((dx ^ 2 + dy ^ 2) ^ (3 / 2));
    refer_path(i,5) = dx;
    refer_path(i,6) = dy;
end
for i=1:length(refer_path)
    if i==1
        dphai = refer_path(i + 1, 3) - refer_path(i, 3);
    elseif  i==length(refer_path)
        dphai = refer_path(i, 3) - refer_path(i - 1, 3);
    else
        dphai = refer_path(i + 1, 3) - refer_path(i, 3);
    end
    refer_path(i, 7) = dphai;%yaw
end
x_d=refer_path_x;
y_d=refer_path_y;
phai_d = refer_path(:, 3);
dx_d = refer_path(:, 5);
dy_d = refer_path(:, 6);
dphai_d = refer_path(:, 7);
figure(1)
plot(refer_path(:, 1), refer_path(:,2), Color='m',LineStyle='--',LineWidth=2)
axis equal
grid on