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

1.BackGround

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

2.Algorithm

3.Reference

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

============20230609对上述引入目标函数，使用QP优化

附：

对上述的使用QP二次规划对生成曲线优化，
考虑使用目标函数如下（n = 1，只处理一段轨迹）：

其中下述的代码中 K_Hg = 1表示W1,K_Curve = 1表示W2 K_CurveRate表示W3;且n = 1；注意上述的“曲线中点曲率变化率为0”的条件去掉。

Coding

%Function：基于五次多项式的QP优化的换道曲线规划
%Create by：juchunyu@qq.com
%Date;2023-06-06 17:20:09
%Last modifed:juchunyu@qq.com
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

Results

Reference

appolo–二次规划（QP）样条路径优化
【MPC】①二次规划问题MATLAB求解器quadprog
二次规划（QP）样条路径优化

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

自动驾驶仿真(五)—— 基于Carsim、Prescan、Simulink的联合仿真
自动驾驶仿真五--基于Carsim.Prescan.Simulink的联合仿真 1. 联合仿真流程 2. CarSim联合仿真配置要点 3. Prescan联合仿真配置要点 4. Carsim.Pre ...
自动驾驶（五十二）---------惯性导航原理
惯性导航一般集成在GPS设备中,都是由供应商集成,那在这里有什么讨论的必要呢,要知道在车辆行驶中,我们可以拿到GPS的yawrate和speed信号,而且车辆本身还有一套传感器获取yawrate和sp ...
自动驾驶（五十七）---------车身航迹推算
自动驾驶中kalman滤波随处可见,这中间不可避免的要知道预测模型,而预测模型很大程度上依赖于本车姿态变化的推算,俗称航迹推算,尤其在控制中,pid调节也是以此为基础. 之前的文章有零散的讨论过这方面 ...
自动驾驶（五十一）---------ICCV 2019之无人驾驶
ICCV和CVPR是两大顶尖国际计算机视觉会议,ICCV2019有哪些研究成果呢?ICCV每两年举行一次,今年是在韩国首尔举行,共提交了1076篇论文.下面是关于感知.规划.建图&定位.自动驾 ...
使用QP方法解基于五次多项式形式的cost function minimization问题
我们在这里使用二次规划(QP)的方法解一个五次多项式形式的曲线的cost function minimization的问题.二次规划的标准形式如下: 在之前的讨论的五次多项式的方法中,我们在采用空间上 ...
博弈论在自动驾驶方向的应用（Ⅰ）：变道决策的综述
今天发生的许多事故都是人类驾驶员错误的感知和决策造成的.由于准确的风险评估对于防止碰撞至关重要,因此,自动驾驶被认为可以大大减少此类错误.虽然目前的系统已经成功地应用于在真实环境中寻找路径和探测障 ...
Apollo自动驾驶入门-地图、定位、感知、预测、规划、控制
目录高精地图定位 GNSS RTK定位惯性导航定位激光雷达定位视觉定位感知检测与分类跟踪分割预测规划路径规划轨迹生成 Frenet坐标路径速度解耦规划 ST 图生成路径的 ...
自动驾驶（五）---------车辆运动学模型
车辆运动学模型我的定义:正常直线行驶,位置变化等于速度*时间,车辆不是直线行驶,已知速度和方向盘转角等,求新的位置. 1.单车模型(Bicycle Model) 其中将左/右前轮合并为一个点,位于A ...
【OpenAI】Python：基于 Gym-CarRacing 的自动驾驶项目（3） | 路径训练功能的实现 | 规划与决策 | 路径平滑 | 利用公式进行目标速度预测
猛戳订阅!