一、算法简介
1、曲线插值的方法是按照车辆在某些特定条件(安全、快速、高效)下,进行路径的曲线拟合,常见的有多项式曲线、双圆弧段曲线、正弦函数曲线、贝塞尔曲线、B样条曲线等。
二、算法思想
1、曲线插值法的核心就是基于预先构造的曲线类型,根据车辆期望达到的状态(比如要求车辆到达某点的速度和加速度为期望值),将此期望值作为边界条件代入曲线类型进行方程求解,获得曲线的相关系数。
2、曲线所有的相关系数一旦确定,轨迹规划随之完成。
三、多项式曲线插值法
1、多项式曲线分为三次多项式曲线、五次多项式曲线、七次多项式曲线。
2、多项式曲线一般而言都是奇数,这是由边界条件引起的。边界条件一般包括两个点的车辆状态,如换道轨迹起点和终点,因此2倍的车辆状态导致有唯一解的方程系数为偶数。故偶数个系数的多项式也就是奇数多项式。
3、针对三次多项式曲线,最多能确定每一个期望点的两个维度的期望状态,一般来说就是位置和速度。
4、针对五次多项式曲线,最多能确定每一个期望点的三个维度的期望状态,一般来说就是位置、速度、加速度。
5、针对七次多项式曲线,最多能确定每一个期望点的四个维度的期望状态,一般来说就是位置、速度、加速度、力加加速度(冲击度,jerk) 。
6、故根据自身轨迹规划的需求,合理选择对应的多项式曲线。
四、五次多项式插值法
1、以五次多项式曲线为例讲解曲线插值法轨迹跟踪。
2、在起点设时间为t0,位置、速度、加速度均已知,纵向和横向分别得到三个方程。
3、定义换道终点时间为t1,横纵向均有期望的位置、速度、加速度,又分别可以得到三个方程,不再列出。
4、因此把起末两点的横纵方程统一用矩阵表达为:
5、多项式曲线的自变量为时间t,故一旦求解系数矩阵,曲线唯一确定后,则曲线上每一点的导数就代表了车辆经过该点时的速度。这表明多项式曲线换道轨迹规划是路径+速度的耦合结果。
6、五次多项式换道轨迹曲线特指横向位置/纵向位置是关于时间t的五次多项式,而不是指纵向位置y关于横向位置x的五次多项式。
% 曲线插值法
% 作者:Ally
% 日期:2021/1/16
clc
clear
close all
%% 场景定义
% 换道场景路段与车辆相关参数的定义
d = 3.5; % 道路标准宽度
len_line = 30; % 直线段长度
W = 1.75; % 车宽
L = 4.7; % 车长
x1 = 20; %1号车x坐标
% 车辆换道初始状态与终点期望状态
t0 = 0;
t1 = 3;
state_t0 = [0, -d/2; 5, 0; 0, 0]; % x,y; vx,vy; ax,ay
state_t1 = [20, d/2; 5, 0; 0, 0];
x2 = state_t0(1);
%% 画场景示意图
figure(1)
% 画灰色路面图
GreyZone = [-5,-d-0.5; -5,d+0.5; len_line,d+0.5; len_line,-d-0.5];
fill(GreyZone(:,1),GreyZone(:,2),[0.5 0.5 0.5]);
hold on
% 画小车
fill([x1,x1,x1+L,x1+L],[-d/2-W/2,-d/2+W/2,-d/2+W/2,-d/2-W/2],'b') %1号车
fill([x2,x2,x2-L,x2-L],[-d/2-W/2,-d/2+W/2,-d/2+W/2,-d/2-W/2],'y') %2号车
% 画分界线
plot([-5, len_line],[0, 0], 'w--', 'linewidth',2); %分界线
plot([-5,len_line],[d,d],'w','linewidth',2); %左边界线
plot([-5,len_line],[-d,-d],'w','linewidth',2); %左边界线
% 设置坐标轴显示范围
axis equal
set(gca, 'XLim',[-5 len_line]);
set(gca, 'YLim',[-4 4]);
%% 五次多项式轨迹生成
% 计算A和B两个系数矩阵
X = [state_t0(:,1); state_t1(:,1)];
Y = [state_t0(:,2); state_t1(:,2)];
T = [ t0^5 t0^4 t0^3 t0^2 t0 1;
5*t0^4 4*t0^3 3*t0^2 2*t0 1 0;
20*t0^3 12*t0^2 6*t0 1 0 0;
t1^5 t1^4 t1^3 t1^2 t1 1;
5*t1^4 4*t1^3 3*t1^2 2*t1 1 0;
20*t1^3 12*t1^2 6*t1 1 0 0];
A = T \ X;
B = T \ Y;
% 将时间从t0到t1离散化,获得离散时刻的轨迹坐标
t=(t0:0.05:t1)';
path=zeros(length(t),4);%1-4列分别存放x,y,vx,vy
for i = 1:length(t)
% 纵向位置坐标
path(i,1) = [t(i)^5, t(i)^4, t(i)^3, t(i)^2, t(i), 1] * A;
% 横向位置坐标
path(i,2) = [t(i)^5, t(i)^4, t(i)^3, t(i)^2, t(i), 1] * B;
% 纵向速度
path(i,3) = [5*t(i)^4, 4*t(i)^3, 3*t(i)^2, 2*t(i), 1, 0] * A;
% 横向速度
path(i,4) = [5*t(i)^4, 4*t(i)^3, 3*t(i)^2, 2*t(i), 1, 0] * B;
end
% 画换道轨迹
plot(path(:,1),path(:,2),'r--','linewidth',1.5);
%% 分析速度
% 横向速度
figure
plot(t, path(:,4), 'k');
xlabel('时间 / s ');
ylabel('横向速度 / m/s ');
% 纵向速度
figure
plot(t, path(:,3), 'k');
xlabel('时间 / s ');
ylabel('纵向速度 / m/s ');
学习自B站:小黎的Ally