自动驾驶——基于五次多项式螺旋线方程的换道曲线规划

news2024/12/23 13:32:28

1.BackGround

已知：换道初始纵坐标y0（横向距离），换道初始航向角tan0，换道时间t，换道结束纵坐标yf，换道结束航向角tanf，车速VehSpd，曲线中点曲率q且曲率变化率为0。求解期望的规划曲线。

2.Algorithm

在这里插入图片描述

3.Reference

自动驾驶——ADAS车道线方程推导
基于多项式采样的换道路径规划

============20230609对上述的算法约束条件

附：

对上述的条件考虑使用QP二次规划对生成曲线添加目标函数：
在这里插入图片描述
其中下述的代码中 K_Hg = 1表示W1,K_Curve = 1表示W2 K_CurveRate表示W3;
注意上述的“曲线中点曲率变化率为0”的条件去掉。

Coding

clear
K_Hg = 1;
K_Curve = 1; 
K_Dist  = 0;
K_CurveRate = 0;
[x_Set,y_Set,PhiRes,CurveRes] = OptimalPlan(K_Hg,K_Curve,K_Dist,K_CurveRate);
plot(x_Set,y_Set);
% hold on
% K_Hg = 1;
% K_Curve = 2;
% [x_Set,y_Set,PhiRes,CurveRes] = OptimalPlan(K_Hg,K_Curve);
% plot(x_Set,y_Set);

hold on
K_Hg = 0;
K_Curve = 1;
K_Dist  = 0;
K_CurveRate = 10;
[x_Set,y_Set,PhiRes,CurveRes] = OptimalPlan(K_Hg,K_Curve,K_Dist,K_CurveRate);
plot(x_Set,y_Set);
legend('KHg = 1 KCurve = 1 KDist  = 0 K_CurveRate = 0;','KHg = 0 KCurve = 1 KDist  = 0 K_CurveRate = 1;')

function [x_Set,y_Set,PhiRes,CurveRes] = OptimalPlan(K_Hg,K_Curve,K_Dist,K_CurveRate)
yf = 0;
y0 = 3.75;
tan0 =0 ;
tanf = 0;
tf = 8;
VehSpd = 50;
q = -0.002;
%y          = a0 + a1 * x + a2 * x^2 + a3 * x^3
%PhiAg      = a1 + 2 * a2 * x + 3 * a3 *x^2
%Curve      = 2 * a2 + 6 * a3 *x
%CurveRate  = 6 * a3
T = 0.02;
xf = VehSpd/3.6 * tf; %规划结束纵向距离

%求解多项式参数
% a0          = y0;
% a1          = tan0;
% SolveMatrix = [xf^2 xf^3 ; 2*xf 3*xf^2];
% MatrixB     = [yf - a0 - a1 * xf; tanf - a1];
% SolveRes    = SolveMatrix^(-1) * MatrixB;
H_Ag        = [0 0 0 0 0 0;0 xf xf^2 xf^3 xf^4 xf^5;0 xf^2 4*xf^3/3 6*xf^4/4 8*xf^5/5 10*xf^6/6;0 xf^3 6*xf^4/4 9*xf^5/5 12*xf^6/6 15*xf^7/7;0 xf^4 8*xf^5/5 2*xf^6 16*xf^7/7 20*xf^8/8;0 xf^5 10*xf^6/6 15*xf^7/7 20*xf^8/8 25*xf^9/9]* K_Hg;
%H_ = H * K_Hg + [0 0 0 0;0 0 0 0;0 0 4*xf 4*xf^3;0 0 6*xf^2 12*xf^3]* K_Curve;
H_Curve     = [0 0 0 0 0 0;0 xf 2*xf 3*xf^2 4*xf^3 5*xf^4;0 2*xf 4*xf 6*xf^2 8*xf^3 10*xf^4;0 3*xf^2  6*xf^2 12*xf^3 18*xf^4 24*xf^5; 0 4*xf^3 8*xf^3 18*xf^4 144*xf^5/5 40*xf^6;0 5*xf^4 10*xf^4 24*xf^5 40*xf^6 400*xf^7/7]* K_Curve;
H_dist      = [xf 0.5*xf^2 xf^3/3 xf^4/4 xf^5/5 xf^6/6; xf^2/2 xf^3/3 xf^4/4 xf^5/5 xf^6/6 xf^7/7;xf^3/3 xf^4/4 xf^5/5 xf^6/6 xf^7/7 xf^8/8;xf^4/4 xf^5/5 xf^6/6 xf^7/7 xf^8/8 xf^9/9;xf^5/5 xf^6/6 xf^7/7 xf^8/8 xf^9/9 xf^10/10;xf^6/6 xf^7/7 xf^8/8 xf^9/9 xf^10/10 xf^11/11] * K_Dist;
H_CurveRate = [0 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0;0 0 0 36*xf 72*xf^2 120*xf^3;0 0 0 72*xf^2 192*xf^3 360*xf^4;0 0 0 120*xf^3 360*xf^4 720*xf^5]*K_CurveRate;
f= [0;0;0;0;0;0];
Aeq = [1 0 0 0 0 0;1 xf xf^2 xf^3 xf^4 xf^5;0 1 0 0 0 0;0 1 2*xf 3*xf^2 4*xf^3 5*xf^4;0 0 2 3*xf 3*xf^2 2.5*xf^3];
beq =  [y0;yf;tan0;tanf;q];
H_  = H_Ag + H_Curve + H_dist + H_CurveRate;
[SolveRes,fval,exitflag,output,lambda] = quadprog(H_,f,[],[],Aeq,beq);


%多项式参数整理
a0 = SolveRes(1);
a1 = SolveRes(2)
a2 = SolveRes(3);
a3 = SolveRes(4);
a4 = SolveRes(5);
a5 = SolveRes(6);
x = 0;
y_Set      = [];
x_Set      = [];
PhiRes     = [];
CurveRes   = [];
while(x < xf || x == xf)
 %计算规划点
    y            = a0 + a1 * x + a2 * x^2 + a3 * x^3 + a4 * x^4 + a5 * x^5;
    PhiAg        = a1 + 2 * a2 * x + 3 * a3 * x^2 +4 * a4 * x^3  + 5 * a5 * x^4;
    Curve        = 2 * a2 + 6 * a3 * x + 12 * a4 * x^2 + 20 * a5 * x^3;
    CurveRate    = 6 * a3 + 24 * a4 * x +60 * a5 * x^2;
    x            = x + VehSpd*T/3.6;
    Spline5xLast = x;
    y_Set    = [y_Set y];
    x_Set    = [x_Set x];
    PhiRes   = [PhiRes PhiAg];
    CurveRes = [CurveRes Curve];
%     %输出轨迹点
% %     y_Set   =  y;
%     y_diff1 =  PhiAg;
%     y_diff2 =  Curve;
%     y_diff3 =  CurveRate;
%     y_Set = [y_Set y];
%     y_diff1 =  PhiAg;
%     y_diff2 =  Curve;
%     y_diff3 =  CurveRate;
    
end
%  plot(y_Set)

end