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

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

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

Matlab 复制代码
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
相关推荐
leobertlan18 分钟前
痛苦系列 | DSP-04 截断时空的“窗”:FIR 滤波器设计与线性相位
算法
秋天的怀念_David1 小时前
算法一千零一夜 · 第三十五夜 · 恢复风暴
java·程序人生·算法
泯泷3 小时前
别再“裸聊”大模型了!你家的 AI 为什么总胡说八道?根子在 RAG
人工智能·算法·架构
2601_967980114 小时前
Python中heapq模块的各种用法实例
数据结构·python·算法·应用实例·heapq模块
漂流瓶jz6 小时前
UVA-1615 高速公路 题解答案代码 算法竞赛入门经典第二版
数据结构·算法·贪心算法·贪心·aoapc·算法竞赛入门经典·uva
Evand J6 小时前
【MATLAB例程】基于APF的多无人机动态路径规划与避障仿真。避碰及安全性能分析。附完整代码的下载链接
开发语言·matlab·无人机
凌冰_16 小时前
数据结构 - 时间空间复杂度(一)
数据结构·算法·软件工程
Pniubi17 小时前
力扣2 . 两数相加
算法·leetcode·职场和发展
戴·菲瑜17 小时前
为什么PPO难以做出顶级棋类引擎
算法·博弈论·rl·ppo