—— 系统简述——
车辆主动避撞系统采用信息与传感技术获取外界信息（如车速、行人或其他障碍物距离等）并判断车辆是否存在安全隐患，给驾驶员提供相应的报警与提示信息。在紧急情况下，车辆主动避撞系统将自动接管车辆，使车辆能够自动避开危险，保证车辆安全行驶，从而避免交通事故的发生。现有的车辆主动避撞系统大致分为以下几种类型。（1）纵向制动避撞系统。若驾驶员对前方的紧急状况未及时做出相应反应，该系统能够自动使车辆进行制动，避免追尾事故的发生或减轻追尾事故的碰撞程度，如前向碰撞预警系统和自动紧急制动系统等。（2）侧向转向避撞系统。该系统能够自动控制车辆转向，绕过前方障碍物，并且当车辆超过障碍物时，避免车辆发生侧面碰撞。（3）复合型智能避撞系统。当车辆前方遇到障碍物时，系统自动控制车辆绕过障碍物，并且当车辆不满足实施转向要求时能够自动控制车辆进行制动，避免交通事故的发生或减轻碰撞事故的程度。当车辆在紧急情况下依靠纵向制动和侧向转向无法避免碰撞事故发生时，就只能依靠换道避障来规避或减轻碰撞的危害。另一方面，不合理的换道行为引起的交通延误占总交通延误时间的10%以上，换道引起的交通事故占总交通事故的5%~10%，而在车辆换道引起的交通事故中，人为因素导致的事故约占75%。因此，自主换道避障系统是自动驾驶车辆必备的重要功能之一，如何提高车辆换道操作的安全性和可靠度，一直是智能车辆和交通安全研究领域的重要方向。自动驾驶车辆在获取周围环境信息后并进行分析，根据换道决策系统判断能否进行换道操作。当满足换道条件后，决策系统会自主选择换道的方向，然后进行换道运动规划，并生成从当前车道至目标车道的参考轨迹，而后，自动驾驶系统控制线控底盘实施相应的换道操作，保证车辆平稳、安全地行驶至目标车道。因此，自动驾驶车辆的主动换道可以分为换道意图产生、换道时机决策、换道轨迹规划和换道轨迹跟踪四个过程。主动换道系统控制逻辑的基本框架如下图所示：首先利用传感器检测车辆周围行驶环境，在保证车辆行驶安全的前提下进行换道时机决策和目标车道选择；然后利用摄像头检测车道线，根据车道线选择参考线，根据车辆行驶状态和参考线进行换道路径规划，将规划后的引导线作为主动换道的目标轨迹，车道保持系统跟踪该目标运动轨迹；基于车辆横向动力学实现主动换道系统的闭环控制。图中主动换道系统控制策略的逻辑图，采用分层控制架构，上层控制器根据车道线和本车的运动状态计算车辆所需方向盘转角，底层控制器为线控转向系统来响应上层控制器的方向盘转角控制请求。
图1 主动换道系统架构
通过以上分析，可将自动驾驶车辆换道行为划分为3个部分，即：自主换道行为的决策、自主换道行为的运动规划、自主换道行为的轨迹跟随控制，其中换道轨迹的跟随控制策略与LKA的控制策略相类似，只不过目标轨迹由LKA的车道中心线变成主动换道系统规划的期望换道轨迹，车辆的横向动力学控制策略完全一样。主动换道系统具备以下特点：
（1）多重约束：车辆的运动存在几何约束和物理约束，其中，几何约束是指车辆的形状制约，而物理约束是指车辆受其自身加速性能和转向性能的限制。
（2）多目标优化：路径规划的要求存在多种目标，如规划路径最短、完成时间最短、安全性能最好、能源消耗最小等，这些目标往往存在冲突，应该考虑在它们之间寻求某种折中策略。
多项式采样法是非常重要的一种车辆运动规划方法，在智能驾驶车辆的运动规划中应用较为成熟，该方法可以应用于各类决策与规划问题中。因此，本文将重点从决策和运动规划这两个方面进行详细讲解，了解车辆运动规划和运动控制相关的理论方法，熟悉利用多项式采样法解决各类规划决策问题的流程，最后在prescan仿真环境下搭建仿真场景，并完成整个系统的闭环仿真。—— 换道避障决策——
在车辆的实际避障过程中，决策规划是自动驾驶最重要的部分之一。规划决策系统在融合多传感器信息之后，在基于车辆动力学的同时结合驾驶需求进行车辆行为的实时决策规划，主要分为行为决策和运动规划两部分，行为决策在满足交通规则、行驶安全等约束条件下进行驾驶意图规划；而运动规划主要是根据车辆当前运动状态和已知的环境信息，在考虑多重动态和多个约束条件下，确保车辆的安全性、舒适性和稳定性，实时规划求解车辆的期望运动轨迹。详细内容可参考公众号文章：
优化思想及KKT原理在换道决策中的应用。行为决策方法如下图所示。
图2 决策规划动作图
完成整个换道过程所需时间或车辆纵向行驶里程，是换道路径规划问题中非常重要的一个参数。换道时间太短，规划路径太急促，车辆换道过程行驶不安全；换道时间太长，换道效率太低，时效性差，因此，在完成换道时机决策的同时还需要规划求解出最优的换道时间或纵向行驶距离。详细内容可参考公众号文章：
基于KKT原理和五次多项式的换道策略和
基于五次多项式实现考虑驾驶风格的主动换道轨迹规划。—— 换道路径规划 ——
路径规划是指在一定的环境模型的基础上，给定汽车起始点和目标点后，按照性能指标规划出一条无碰撞、能安全到达目标点的有效路径。路径规划主要包含两个步骤：建立环境模型，即将现实的环境进行抽象后建立路径规划相关的模型；路径寻优，即寻找符合条件的最优换道路径。
——Frenét坐标系
在车辆横向控制中，应用最多的坐标系是Frenét坐标系。Frenét坐标系是由宝马公司的Moritz Werling提出的，因此，基于Frenét坐标系的车辆横向运动规划和控制方法也被称为Werling方法。基于Frenét坐标系，将车辆运动轨迹分解成两个方向的运动，即沿着车道向前前进方向（纵向）与在车道内左右偏离（横向）的两个运动，可以简化车辆横向运动规划模型，提高计算效率。详细内容可参考公众号文章：
无人驾驶常用坐标系和
路径规划中的Frenet坐标系简介。
忽略车辆行驶区域内坡度与海拔的变化，将车辆行驶区域假想为一个二维平面，建立车辆全局笛卡尔坐标系，将车辆位置映射到该坐标系下，设车辆坐标为(x,y)。假设车辆沿着车道线行驶，且车辆始终行驶在车道内，则此时的道路参考线可假设为车道中心线，建立如下图所示的Frenét坐标系，车辆到参考线的投影距离为d，投影点到起点的行驶里程为s，则车辆在全局笛卡尔坐标系下的坐标(x,y)与Frenét坐标系下的坐标(s,d)是相对应的。
图3 Frenét坐标系
——五次多项式路径规划
以目标车道中心线为参考线建立frenet坐标系，以车辆相对规划后的期望运动轨迹的横向运动状态作为反馈，利用LKA控制器计算期望方向盘转角，从而实现主动换道系统的闭环控制，其基本原理与LKA完全相同。
首先，在frenet坐标系下，基于环境信息和本车运动状态实时规划换道轨迹。图4中d、s表示车辆在frenet坐标系下的横向与纵向运动。采用五次多项式规划换道轨迹，其横纵向方程为：
其中，
为待定的多项式系数。不同系数下的横纵向运动轨迹如下图所示，需要在满足一定约束条件的基础上寻找一条最优的换道轨迹，接下来就对轨迹规划问题详细进行说明。
图4 换道轨迹规划示意图
—— 优化目标 ——
横纵向轨迹规划的优化目标主要由换道过程中的状态误差、加速度、冲击度以及换道完成后的状态误差组成：
其中，
为多目标优化的权重系数，T为运动规划的时间窗宽度，
为期望的换道距离。则在换道场景下，选取的代价函数为：
定义优化变量为
，可得
针对纵向规划，可得
则优化目标为：
其中
则纵向轨迹规划的优化目标等价于
其中
与纵向的分析过程类似，针对横向，可得
其中
则横向轨迹规划的优化目标等价于
其中，
—— 约束条件 ——
假设车辆实际轨迹一直沿着规划的路径向前运动，则根据坐标系映射关系可得换道轨迹满足边界约束条件：
其中，k0为本车相对目标车道中心线的航向角的正切值，y0为本车相对目标车道中心线的横向距离，κ为道路曲率。则车辆轨迹规划的等式约束等价于
其中，
为了保证换道的安全性，横纵向轨迹需满足约束条件：
其中，Vlim为安全极限车速，车辆偏离车道中心的允许偏差
，其中Lw为车道宽度。曲率约束（非完整性约束）：由车辆 Ackerman 转向几何带来的曲率约束是最主要的非完整性约束
其中，
为由车辆转向系统确定的最大曲率。根据上式所建立的曲率约束是最主要的非完整性约束。考虑到在Frenet坐标系下曲率计算的非线性特点，假设在一个规划窗内车辆的动力学状态变化较小且可以忽略，则简化后的曲率为
由于车辆横向速度远小于纵向速度，则简化后的曲率满足
即得可应用于实时规划的非完整约束条件
根据线性约束条件(4)，非完整性约束主要根据车辆纵向运动状态对车辆横向运动轨迹进行约束。可见，根据曲率简化模型(3)，将非线性的非完整约束转化为线性约束条件(4)。
横纵向轨迹需满足舒适性约束条件
其中，amax为车辆所能提供的最大加速度，jmax为最大冲击度。则安全性和舒适性约束条件等价于
其中，
由于对任意的t∈[0,T]，约束条件（5）都需满足，这对实时求解规划问题带来一定的困难。考虑到优化空间和约束条件的连续性以及多项式采样的特点，因此，采用离散点约束法将其简化，由于该方法能非常方便的处理各类非线性约束条件，且有效保证求解稳定性与收敛性。假设离散采样点为t=kT,k=0,0.1,0.2,...,1，则简化后的性能约束条件为
其中，
—— 二次规划 ——
综上分析，在Frenét坐标系下，将车辆轨迹规划问题转化为如下两个解耦的标准二次规划（QP）问题：
纵向轨迹规划的QP问题：
横向轨迹规划的QP问题：
由于引导线为一个5阶多项式，共含有6个求解变量，根据等式约束（2）可得，该优化模型的横纵向QP问题中其实各只有一个自由变量，这就降低了优化的难度，从而保证了优化质量和实时性。QP求解算法属于比较成熟的算法，可参阅公众号文章：
无人驾驶控制算法之QP算法，在此直接采用Karush-Kuhn-Tucker（KKT）条件方法，KKT方法可参考公众号文章：
基于KKT原理和五次多项式的换道策略和
优化思想及KKT原理在换道决策中的应用。
（1）横向轨迹
（2）纵向轨迹
图5 换道轨迹规划结果
在求解横纵向二次规划问题（6）和（7）时可能会出现无解的情况，这往往是由于约束条件太苛刻导致的，因此，需要引入松弛因子，将QP模型的硬约束变成软约束。以纵向QP问题（6）为例：
其中，
分别为权重系数和松弛因子，构造权重矩阵和约束松弛矩阵为66×66的正定对角矩阵：
为了将优化问题(8)转化为标准QP问题，首先定义优化变量为
根据(8)可得转化后的标准QP问题为
其中，
同理，针对横向QP问题（7），可得转化后的标准QP问题为：
其中，
以及优化变量为
其中，
为横向轨迹的松弛因子。通过求解QP问题(9)和(10)即可求得xs和xd，进而在考虑避障软约束的前提下实现车辆横纵向运动轨迹(1)的规划求解。—— 系统仿真 ——主动换道系统与LKA控制原理相同，其控制器设计方法与LKA相同。在Frenét坐标系下，以车辆相对规划后的期望换道轨迹的运动状态作为反馈，采用LKA控制器去跟踪目标车道的中心线，实现换道控制。而在LKA功能中，以车辆相对本车道中心线的运动状态作为反馈，这也是主动换道系统与LKA的主要区别。根据相关标准，主动换道避障系统有多种测试场景，如：在直道上（或者弯道上），本车道的前方有静止车辆（或者有正在减速的车辆）等。为了展示如何通过Prescan与Matlab/Simulink来进行主动换道避障系统的设计及仿真，本小节仅以最简单的测试场景作为本次仿真的场景，即：在直道上，在本车道的前方有一静止的前车(障碍物)存在，且相邻车道无干扰车辆。在上述说明的仿真场景下，车辆若不进行相应的制动或者避障动作，则本车将与前车发生碰撞，采用换道避障的策略来避免碰撞的发生。当本车道前方不存在障碍物时，主动换道避障系统采取与车道保持系统相同的控制原理，此时其控制器设计方法与LKA相同，可参考公众号文章：
车道保持辅助系统仿真及程序。当本车道前方存在障碍物时，主动换道避障系统将通过传感器所感知到的障碍物信息并结合本车的状态进行换道决策，并在做出决策后进行相应的避障路径规划，在完成避障路径规划之后，最后，将在Frenét坐标系下以车辆相对规划后的期望换道轨迹的运动状态作为反馈，采用LKA控制器去跟踪规划出来的避障路线，实现换道控制。首先，打开Prescan GUI，在GUI中进行上述仿真场景的搭建。具体操作步骤如下所示：1.路网的搭建。从Infrastructure的Road Segments中找到Straight Road模块，将其新建到Build Area中，并对其Object configuration进行相应的参数设置，主要的设置内容包括道路的长度及车道数，分别设置为600m及2车道，具体如下图所示：
图6 搭建路网
2.交通参与者的搭建。本次仿真中只涉及两个交通参与者，分别为本车及位于本车所在车道前方的静止车辆，所以需要在Actors中的Cars&Motors选择两辆模型，将之新建到Build Area中。本次仿真所选的车型分别为Audi A8 Sedan（本车）及Ford_Focus_Stationwagon（本车道前方静止车辆），具体如下图所示：
图7 交通参与者搭建
3.路径、速度及相关动力学模型设置。在完成上述操作后，即可为本车创建一条路径并且添加动力学模型及设定期望的行驶速度。所创建的路径为沿着中间车道的直线，本车的动力学模型选用2D Simple（三自由度）模型，同时为其添加Path Follow的Driver Model，期望速度设置为15m/s（匀速行驶）。前方静止车辆（BMW X5 SUV）则不用进行任何设置。具体内容如下图所示：
图8 车辆动力学参数设置
4.为本车添加传感器。本次仿真中，换道避障的判断指标采用的是前文中提到的碰撞时间（TTC），即：当车辆与目标车辆的TTC小于设定阈值时，系统做出换道决策。上述策略需要得到本车与前方静止车辆的相对距离、相对速度等信息，因此本次仿真选用Sensor中Detailed列表下的Camera、Radar及Ground Truth列表下的Analytical Lane Marker，将其安装在本车的车头及风挡玻璃位置，并设置相应的传感器参数。
图9 添加传感器
5.编译并启动Simulink。在完成上述操作后，Matlab/Simulink将被启动，在其中将可以编写相应的主动换道避障算法。仿真采用TTC作为换道决策的判断指标，即：当计算得到的TTC值小于所设定的阈值时，则进行换道，否则仍进行车道保持。
首先，根据TTC进行决策，当存在碰撞风险时进行换道避障。本次仿真示例中轨迹规划的方法采用的是基于五次多项式的轨迹规划方法。所搭建的MATLAB求解程序如下所示：
function [s,exitflag_s,dt,d,exitflag_d,AES_END_Flag,PlanFlag,t_1,s2]=
PathOptimization(PathSwitch,ModeSelection,PlanFlag,AES_END_Flag,T,s0,T_lc,V_set,
V_ego,a_ego,y0,y_delta,d_obs,k0,k1,V_lim, w,L_w,a_ymax,TTC,kappa_max,period,t_1,
s_delay,d_delay,s1)
coder.extrinsic('quadprog')
%% 五次多项式的轨迹优化
% T_lc为换道的时距，s0为换道时的距离常数
% T为运动规划的时间窗宽度，V_set为驾驶员设定的期望车速
% w1、w2、w3、w4是多目标优化的权重系数
% V_ego为本车车速，a_ego为本车加速度
% y0为车道线横向偏差
% y_delta为避障模式下参考线为车道中心线向左或者向右偏离一定的量，即障碍物在本车道右边时向左偏移y_delta，障碍物在本车道左边时向右偏移y_delta
%d_obs为障碍物的横向距离，delta_min为障碍物的横向安全距离
% delta_d为车辆偏离车道中心线允许的偏差
% delta_CA0为车道内转向避障的横向安全阈值
% delta_safe为横向安全阈值
% k0为航向角，k1为曲率，V_lim为车辆安全车速
% L_w为车道宽度，TTC为碰撞时间
% a_ymax为侧向加速度阈值，j_ymax为侧向冲击度阈值
% kappa_max为转向系统决定的最大曲率
% x_s=[a0,a1,a2,a3,a4,a5]';
% x_d=[b0,b1,b2,b3,b4,b5]';
if PathSwitch==1&&AES_END_Flag==0
if PlanFlag==1
s_T=s0+T_lc*V_set;
a_max=20;
j_max=200;
w1=w(1,1);
w2=w(2,1);
w3=w(3,1);
w4=w(4,1);
%% s方向，纵向
H_s1=[zeros(3,6);0, 0, 0, 2*T, 3/2*T^2, 2/3*T^3;
0, 0, 0, 3/2*T^2, 4/3*T^3, 5/8*T^4;0, 0, 0, 2/3*T^3, 5/8*T^4, 3/10*T^5];
H_s2=[zeros(2,6);0, 0, T, 1/2*T^2, 1/6*T^3, 1/24*T^4;
0, 0, 1/2*T^2, 1/3*T^3, 1/8*T^4, 1/30*T^5;
0, 0, 1/6*T^3, 1/8*T^4, 1/20*T^5, 1/72*T^6;
0, 0, 1/24*T^4, 1/30*T^5,1/72*T^6, 1/252*T^7];
H_s3=[zeros(1,6);0, T, 1/2*T^2, 1/6*T^3, 1/24*T^4, 1/120*T^5;
0, 1/2*T^2, 1/3*T^3, 1/8*T^4, 1/30*T^5, 1/144*T^6;
0, 1/6*T^3, 1/8*T^4, 1/20*T^5, 1/72*T^6, 1/336*T^7;
0, 1/24*T^4, 1/30*T^5, 1/72*T^6, 1/252*T^7, 1/1152*T^8;
0, 1/120*T^5, 1/144*T^6, 1/336*T^7, 1/1152*T^8, 1/5184*T^9];
H_s4=[T, T^2, 1/2*T^3, 1/6*T^4, 1/24*T^5, 1/120*T^6;
T^2, T^3, 1/2*T^4, 1/6*T^5, 1/24*T^6, 1/120*T^7;
1/2*T^3, 1/2*T^4, 1/4*T^5, 1/12*T^6, 1/48*T^7, 1/240*T^8;
1/6*T^4, 1/6*T^5, 1/12*T^6, 1/36*T^7, 1/144*T^8, 1/720*T^9;
1/24*T^5, 1/24*T^6, 1/48*T^7, 1/144*T^8, 1/576*T^9, 1/2880*T^10;
1/120*T^6, 1/120*T^7, 1/240*T^8, 1/720*T^9, 1/2880*T^10, 1/14400*T^11];
f_s=-(2*w3*V_set+2*w4*T*s_T)*[1, T, 1/2*T^2, 1/6*T^3, 1/24*T^4, 1/120*T^5];
H_s=w1*H_s1+w2*H_s2+w3*H_s3+w4*H_s4;
J_s0=w4*T*s_T^2+w3*T*V_set^2;
%J_s=x_s'*H_s*x_s+f_s*x_s+J_s0;
%%%%%%% 边界约束 %%%%%%
Aeq_s=[1, 0, 0, 0, 0, 0;0, 1, 0, 0, 0, 0;0, 0, 1, 0, 0, 0;
0, 1, T, 1/2*T^2, 1/6*T^3, 1/24*T^4;0, 0, 1, T, 1/2*T^2, 1/6*T^3];
Beq_s=[0; V_ego; a_ego; V_set; 0];
%%%%%%% 安全性约束、舒适性约束、非完整性约束 %%%%%%%
A_s=[0, -1, 0, 0, 0, 0; 0, 1, 0, 0, 0, 0; 0, 0, -1, 0, 0, 0; 0, 0, 1, 0, 0, 0; 0, 0, 0, -1, 0, 0; 0, 0, 0, 1, 0, 0; % t=0时
0, -1, -0.1*T, -1/2*(0.1*T)^2, -1/6*(0.1*T)^3, -1/24*(0.1*T)^4; 0, 1, 0.1*T, 1/2*(0.1*T)^2, 1/6*(0.1*T)^3, 1/24*(0.1*T)^4;
0, 0, -1, -0.1*T, -1/2*(0.1*T)^2, -1/6*(0.1*T)^3; 0, 0, 1, 0.1*T, 1/2*(0.1*T)^2, 1/6*(0.1*T)^3;
0, 0, 0, -1, -0.1*T, -1/2*(0.1*T)^2; 0, 0, 0, 1, 0.1*T, 1/2*(0.1*T)^2;% t=0.1T时
0, -1, -0.2*T, -1/2*(0.2*T)^2, -1/6*(0.2*T)^3, -1/24*(0.2*T)^4; 0, 1, 0.2*T, 1/2*(0.2*T)^2, 1/6*(0.2*T)^3, 1/24*(0.2*T)^4;
0, 0, -1, -0.2*T, -1/2*(0.2*T)^2, -1/6*(0.2*T)^3; 0, 0, 1, 0.2*T, 1/2*(0.2*T)^2, 1/6*(0.2*T)^3;
0, 0, 0, -1, -0.2*T, -1/2*(0.2*T)^2; 0, 0, 0, 1, 0.2*T, 1/2*(0.2*T)^2; % t=0.2T时
0, -1, -0.3*T, -1/2*(0.3*T)^2, -1/6*(0.3*T)^3, -1/24*(0.3*T)^4; 0, 1, 0.3*T, 1/2*(0.3*T)^2, 1/6*(0.3*T)^3, 1/24*(0.3*T)^4;
0, 0, -1, -0.3*T, -1/2*(0.3*T)^2, -1/6*(0.3*T)^3; 0, 0, 1, 0.3*T, 1/2*(0.3*T)^2, 1/6*(0.3*T)^3;
0, 0, 0, -1, -0.3*T, -1/2*(0.3*T)^2; 0, 0, 0, 1, 0.3*T, 1/2*(0.3*T)^2;% t=0.3T时
0, -1, -0.4*T, -1/2*(0.4*T)^2, -1/6*(0.4*T)^3, -1/24*(0.4*T)^4; 0, 1, 0.4*T, 1/2*(0.4*T)^2, 1/6*(0.4*T)^3, 1/24*(0.4*T)^4;
0, 0, -1, -0.4*T, -1/2*(0.4*T)^2, -1/6*(0.4*T)^3; 0, 0, 1, 0.2*T, 1/2*(0.4*T)^2, 1/6*(0.4*T)^3;
0, 0, 0, -1, -0.4*T, -1/2*(0.4*T)^2; 0, 0, 0, 1, 0.4*T, 1/2*(0.4*T)^2;% t=0.4T时
0, -1, -0.5*T, -1/2*(0.5*T)^2, -1/6*(0.5*T)^3, -1/24*(0.5*T)^4; 0, 1, 0.5*T, 1/2*(0.5*T)^2, 1/6*(0.5*T)^3, 1/24*(0.5*T)^4;
0, 0, -1, -0.5*T, -1/2*(0.5*T)^2, -1/6*(0.5*T)^3; 0, 0, 1, 0.5*T, 1/2*(0.5*T)^2, 1/6*(0.5*T)^3;
0, 0, 0, -1, -0.5*T, -1/2*(0.5*T)^2; 0, 0, 0, 1, 0.5*T, 1/2*(0.5*T)^2;% t=0.5T时
0, -1, -0.6*T, -1/2*(0.6*T)^2, -1/6*(0.6*T)^3, -1/24*(0.6*T)^4; 0, 1, 0.6*T, 1/2*(0.6*T)^2, 1/6*(0.6*T)^3, 1/24*(0.6*T)^4;
0, 0, -1, -0.6*T, -1/2*(0.6*T)^2, -1/6*(0.6*T)^3; 0, 0, 1, 0.6*T, 1/2*(0.6*T)^2, 1/6*(0.6*T)^3;
0, 0, 0, -1, -0.6*T, -1/2*(0.6*T)^2; 0, 0, 0, 1, 0.6*T, 1/2*(0.6*T)^2;% t=0.6T时
0, -1, -0.7*T, -1/2*(0.7*T)^2, -1/6*(0.7*T)^3, -1/24*(0.7*T)^4; 0, 1, 0.7*T, 1/2*(0.7*T)^2, 1/6*(0.7*T)^3, 1/24*(0.7*T)^4;
0, 0, -1, -0.7*T, -1/2*(0.7*T)^2, -1/6*(0.7*T)^3; 0, 0, 1, 0.7*T, 1/2*(0.7*T)^2, 1/6*(0.7*T)^3;
0, 0, 0, -1, -0.7*T, -1/2*(0.7*T)^2; 0, 0, 0, 1, 0.7*T, 1/2*(0.7*T)^2;% t=0.7T时
0, -1, -0.8*T, -1/2*(0.8*T)^2, -1/6*(0.8*T)^3, -1/24*(0.8*T)^4; 0, 1, 0.8*T, 1/2*(0.8*T)^2, 1/6*(0.8*T)^3, 1/24*(0.8*T)^4;
0, 0, -1, -0.8*T, -1/2*(0.8*T)^2, -1/6*(0.8*T)^3; 0, 0, 1, 0.8*T, 1/2*(0.8*T)^2, 1/6*(0.8*T)^3;
0, 0, 0, -1, -0.8*T, -1/2*(0.8*T)^2; 0, 0, 0, 1, 0.8*T, 1/2*(0.8*T)^2;% t=0.8T时
0, -1, -0.9*T, -1/2*(0.9*T)^2, -1/6*(0.9*T)^3, -1/24*(0.9*T)^4; 0, 1, 0.9*T, 1/2*(0.9*T)^2, 1/6*(0.9*T)^3, 1/24*(0.9*T)^4;
0, 0, -1, -0.9*T, -1/2*(0.9*T)^2, -1/6*(0.9*T)^3; 0, 0, 1, 0.9*T, 1/2*(0.9*T)^2, 1/6*(0.9*T)^3;
0, 0, 0, -1, -0.9*T, -1/2*(0.9*T)^2; 0, 0, 0, 1, 0.2*T, 1/2*(0.9*T)^2;% t=0.9T时
0, -1, -T, -1/2*T^2, -1/6*T^3, -1/24*T^4; 0, 1, T, 1/2*T^2, 1/6*T^3, 1/24*T^4;
0, 0, -1, -T, -1/2*T^2, -1/6*T^3; 0, 0, 1, T, 1/2*T^2, 1/6*T^3;
0, 0, 0, -1, -T, -1/2*T^2; 0, 0, 0, 1, T, 1/2*T^2];% t=T时
B_s=[0; V_lim; a_max; a_max; j_max; j_max; % t=0时
0; V_lim; a_max; a_max; j_max; j_max; % t=0.1T时
0; V_lim; a_max; a_max; j_max; j_max; % t=0.2T时
0; V_lim; a_max; a_max; j_max; j_max; % t=0.3T时
0; V_lim; a_max; a_max; j_max; j_max; % t=0.4T时
0; V_lim; a_max; a_max; j_max; j_max; % t=0.5T时
0; V_lim; a_max; a_max; j_max; j_max; % t=0.6T时
0; V_lim; a_max; a_max; j_max; j_max; % t=0.7T时
0; V_lim; a_max; a_max; j_max; j_max; % t=0.8T时
0; V_lim; a_max; a_max; j_max; j_max; % t=0.9T时
0; V_lim; a_max; a_max; j_max; j_max]; % t=T时
[s0,~,exitflag_S]=quadprog(H_s,f_s,A_s,B_s,Aeq_s,Beq_s); % 二次规划函数
%% d方向，横向
H_d1=H_s1;
H_d2=[zeros(2,6);0, 0, 2*T, 3/2*T^2, 2/3*T^3, 5/24*T^4;
0, 0, 3/2*T^2, 4/3*T^3, 5/8*T^4, 1/5*T^5;
0, 0, 2/3*T^3, 5/8*T^4, 3/10*T^5, 7/72*T^6;
0, 0, 5/24*T^4, 1/5*T^5, 7/72*T^6, 2/63*T^7];
H_d3=H_s3;
H_d4=[T, 1/2*T^2, 1/6*T^3, 1/24*T^4, 1/120*T^5, 1/720*T^6;
1/2*T^2, 1/3*T^3, 1/8*T^4, 1/30*T^5, 1/144*T^6, 1/840*T^7;
1/6*T^3, 1/8*T^4, 1/20*T^5, 1/72*T^6, 1/336*T^7, 1/1920*T^8;
1/24*T^4, 1/30*T^5, 1/72*T^6, 1/252*T^7, 1/1152*T^8, 1/6480*T^9;
1/120*T^5, 1/144*T^6, 1/336*T^7, 1/1152*T^8, 1/5184*T^9, 1/28800*T^10;
1/720*T^6, 1/840*T^7, 1/1920*T^8, 1/6480*T^9, 1/28800*T^10, 1/158400*T^11];
H_d=w1*H_d1+w2*H_d2+w3*H_d3+w4*H_d4;
% J_d=x_d'*H_d*x_d;
%%%%% 参数设置 %%%%%
if ModeSelection==0 || ModeSelection==2%ModeSelection=0跟随模式，ModeSelection=1避障模式，ModeSelection=2换道模式
b_d2=0;delta_d=0.15*L_w;
b_d1=delta_d-y0/2+y0/2*sign(y0);b_d3=delta_d+y0/2+y0/2*sign(y0);
else
y_0=y_delta+y0;b_d2=y_0;delta_d=0.3*L_w;
b_d1=delta_d-y_0/2+y_0/2*sign(y_0);
b_d3=delta_d+y_0/2+y_0/2*sign(y_0);
end
if ModeSelection==1 || ModeSelection==2
eta=1; %换道模式和避障模式
else
eta=0; %跟随模式
end
j_ymax=200;delta_safe=2;delta_safe0=2;
%%%%% 边界约束 %%%%%%
Aeq_d=[1, 0, 0, 0, 0, 0;0, 1, 0, 0, 0, 0;0, 0, 1, 0, 0, 0;
1, T, 1/2*T^2, 1/6*T^3, 1/24*T^4, 1/120*T^5;0, 1, T, 1/2*T^2, 1/6*T^3, 1/24*T^4];
Beq_d=[y0; V_ego*k0; a_ego*k0+(V_ego^2)*k1; b_d2; 0];
%%%%%%% 安全性约束、舒适性约束、非完整性约束 %%%%%%%
A_d=[sign(y0)*eta, TTC*sign(y0)*eta, 1/2*(TTC^2)*sign(y0)*eta, 1/6*(TTC^3)*sign(y0)*eta, 1/24*(TTC^4)*sign(y0)*eta, 1/120*(TTC^5)*sign(y0)*eta; %sign为取y0的正负号，y0为正数时等于1，y0为负数时等于-1，y0为0是等于0
-1, 0, 0, 0, 0, 0; 1, 0, 0, 0, 0, 0; 0, 0, -1, 0, 0, 0; 0, 0, 1, 0, 0, 0; 0, 0, 0, -1, 0, 0; 0, 0, 0, 1, 0, 0; % t=0时
-1, -0.1*T, -1/2*(0.1*T)^2, -1/6*(0.1*T)^3, -1/24*(0.1*T)^4, -1/120*(0.1*T)^5; 1, 0.1*T, 1/2*(0.1*T)^2, 1/6*(0.1*T)^3, 1/24*(0.1*T)^4, 1/120*(0.1*T)^5;
0, 0, -1, -0.1*T, -1/2*(0.1*T)^2, -1/6*(0.1*T)^3; 0, 0, 1, 0.1*T, 1/2*(0.1*T)^2, 1/6*(0.1*T)^3;
0, 0, 0, -1, -0.1*T, -1/2*(0.1*T)^2; 0, 0, 0, 1, 0.1*T, 1/2*(0.1*T)^2;% t=0.1T时
-1, -0.2*T, -1/2*(0.2*T)^2, -1/6*(0.2*T)^3, -3/24*(0.2*T)^4, -1/120*(0.2*T)^5 ; 1, 0.2*T, 1/2*(0.2*T)^2, 1/6*(0.2*T)^3, 1/24*(0.2*T)^4, 1/120*(0.2*T)^5;
0, 0, -1, -0.2*T, -1/2*(0.2*T)^2, -1/6*(0.2*T)^3; 0, 0, 1, 0.2*T, 1/2*(0.2*T)^2, 1/6*(0.2*T)^3;
0, 0, 0, -1, -0.2*T, -1/2*(0.2*T)^2; 0, 0, 0, 1, 0.2*T, 1/2*(0.2*T)^2;% t=0.2T时
-1, -0.3*T, -1/2*(0.3*T)^2, -1/6*(0.3*T)^3, -1/24*(0.3*T)^4, -1/120*(0.3*T)^5; 1, 0.3*T, 1/2*(0.3*T)^2, 1/6*(0.3*T)^3, 1/24*(0.3*T)^4, 1/120*(0.3*T)^5;
0, 0, -1, -0.3*T, -1/2*(0.3*T)^2, -1/6*(0.3*T)^3; 0, 0, 1, 0.3*T, 1/2*(0.3*T)^2, 1/6*(0.3*T)^3;
0, 0, 0, -1, -0.3*T, -1/2*(0.3*T)^2; 0, 0, 0, 1, 0.3*T, 1/2*(0.3*T)^2;% t=0.3T时
-1, -0.4*T, -1/2*(0.4*T)^2, -1/6*(0.4*T)^3, -1/24*(0.4*T)^4, -1/120*(0.4*T)^5; 1, 0.4*T, 1/2*(0.4*T)^2, 1/6*(0.4*T)^3, 1/24*(0.4*T)^4, 1/120*(0.4*T)^5;
0, 0, -1, -0.4*T, -1/2*(0.4*T)^2, -1/6*(0.4*T)^3; 0, 0, 1, 0.4*T, 1/2*(0.4*T)^2, 1/6*(0.4*T)^3;
0, 0, 0, -1, -0.4*T, -1/2*(0.4*T)^2; 0, 0, 0, 1, 0.4*T, 1/2*(0.4*T)^2;% t=0.4T时
-1, -0.5*T, -1/2*(0.5*T)^2, -1/6*(0.5*T)^3, -1/24*(0.5*T)^4, -1/120*(0.5*T)^5; 1, 0.5*T, 1/2*(0.5*T)^2, 1/6*(0.5*T)^3, 1/24*(0.5*T)^4, 1/120*(0.5*T)^5;
0, 0, -1, -0.5*T, -1/2*(0.5*T)^2, -1/6*(0.5*T)^3; 0, 0, 1, 0.5*T, 1/2*(0.5*T)^2, 1/6*(0.5*T)^3;
0, 0, 0, -1, -0.5*T, -1/2*(0.5*T)^2; 0, 0, 0, 1, 0.5*T, 1/2*(0.5*T)^2;% t=0.5T时
-1, -0.6*T, -1/2*(0.6*T)^2, -1/6*(0.6*T)^3, -1/24*(0.6*T)^4, -1/120*(0.6*T)^5; 1, 0.6*T, 1/2*(0.6*T)^2, 1/6*(0.6*T)^3, 1/24*(0.6*T)^4, 1/120*(0.6*T)^5;
0, 0, -1, -0.6*T, -1/2*(0.6*T)^2, -1/6*(0.6*T)^3; 0, 0, 1, 0.6*T, 1/2*(0.6*T)^2, 1/6*(0.6*T)^3;
0, 0, 0, -1, -0.6*T, -1/2*(0.6*T)^2; 0, 0, 0, 1, 0.6*T, 1/2*(0.6*T)^2;% t=0.6T时
-1, -0.7*T, -1/2*(0.7*T)^2, -1/6*(0.7*T)^3, -1/24*(0.7*T)^4, -1/120*(0.7*T)^5; 1, 0.7*T, 1/2*(0.7*T)^2, 1/6*(0.7*T)^3, 1/24*(0.7*T)^4, 1/120*(0.7*T)^5;
0, 0, -1, -0.7*T, -1/2*(0.7*T)^2, -1/6*(0.7*T)^3; 0, 0, 1, 0.2*T, 1/2*(0.7*T)^2, 1/6*(0.7*T)^3;
0, 0, 0, -1, -0.7*T, -1/2*(0.7*T)^2; 0, 0, 0, 1, 0.7*T, 1/2*(0.7*T)^2;% t=0.7T时
-1, -0.8*T, -1/2*(0.8*T)^2, -1/6*(0.8*T)^3, -1/24*(0.8*T)^4, -1/120*(0.8*T)^5; 1, 0.8*T, 1/2*(0.8*T)^2, 1/6*(0.8*T)^3, 1/24*(0.8*T)^4, 1/120*(0.8*T)^5;
0, 0, -1, -0.8*T, -1/2*(0.8*T)^2, -1/6*(0.8*T)^3; 0, 0, 1, 0.8*T, 1/2*(0.8*T)^2, 1/6*(0.8*T)^3;
0, 0, 0, -1, -0.8*T, -1/2*(0.8*T)^2; 0, 0, 0, 1, 0.8*T, 1/2*(0.8*T)^2;% t=0.8T时
-1, -0.9*T, -1/2*(0.9*T)^2, -1/6*(0.9*T)^3, -1/24*(0.9*T)^4, -1/120*(0.9*T)^5; 1, 0.9*T, 1/2*(0.9*T)^2, 1/6*(0.9*T)^3, 1/24*(0.9*T)^4, 1/120*(0.9*T)^5;
0, 0, -1, -0.9*T, -1/2*(0.9*T)^2, -1/6*(0.9*T)^3; 0, 0, 1, 0.9*T, 1/2*(0.9*T)^2, 1/6*(0.9*T)^3;
0, 0, 0, -1, -0.9*T, -1/2*(0.9*T)^2; 0, 0, 0, 1, 0.9*T, 1/2*(0.9*T)^2;% t=0.9T时
-1, -T, -1/2*T^2, -1/6*T^3, -1/24*T^4, -1/120*T^5; 1, T, 1/2*T^2, 1/6*T^3, 1/24*T^4, 1/120*T^5;
0, 0, -1, -T, -1/2*T^2, -1/6*T^3; 0, 0, 1, 0.2*T, 1/2*T^2, 1/6*T^3;
0, 0, 0, -1, -T, -1/2*T^2; 0, 0, 0, 1, T, 1/2*T^2;% t=T时
0, a_ego, -V_ego, 0, 0, 0;% t=0时
0, -a_ego, V_ego, 0, 0, 0;0, a_ego, a_ego*0.1*T-V_ego, 1/2*a_ego*(0.1*T)^2-V_ego*(0.1*T), 1/6*a_ego*(0.1*T)^3-1/2*V_ego*(0.1*T)^2, 1/24*a_ego*(0.1*T)^4-1/6*V_ego*(0.1*T)^3;% t=0.1T时
0, -a_ego, -(a_ego*0.1*T-V_ego), -(1/2*a_ego*(0.1*T)^2-V_ego*(0.1*T)), -(1/6*a_ego*(0.1*T)^3-1/2*V_ego*(0.1*T)^2), -(1/24*a_ego*(0.1*T)^4-1/6*V_ego*(0.1*T)^3);
0, a_ego, a_ego*0.2*T-V_ego, 1/2*a_ego*(0.2*T)^2-V_ego*(0.2*T), 1/6*a_ego*(0.2*T)^3-1/2*V_ego*(0.2*T)^2, 1/24*a_ego*(0.2*T)^4-1/6*V_ego*(0.2*T)^3;% t=0.2T时
0, -a_ego,-( a_ego*0.2*T-V_ego), -(1/2*a_ego*(0.2*T)^2-V_ego*(0.2*T)), -(1/6*a_ego*(0.2*T)^3-1/2*V_ego*(0.2*T)^2), -(1/24*a_ego*(0.2*T)^4-1/6*V_ego*(0.2*T)^3);
0, a_ego, a_ego*0.3*T-V_ego, 1/2*a_ego*(0.3*T)^2-V_ego*(0.3*T), 1/6*a_ego*(0.3*T)^3-1/2*V_ego*(0.3*T)^2, 1/24*a_ego*(0.3*T)^4-1/6*V_ego*(0.3*T)^3;% t=0.3T时
0, -a_ego, -(a_ego*0.3*T-V_ego), -(1/2*a_ego*(0.3*T)^2-V_ego*(0.3*T)), -(1/6*a_ego*(0.3*T)^3-1/2*V_ego*(0.3*T)^2), -(1/24*a_ego*(0.3*T)^4-1/6*V_ego*(0.3*T)^3);
0, a_ego, a_ego*0.4*T-V_ego, 1/2*a_ego*(0.4*T)^2-V_ego*(0.4*T), 1/6*a_ego*(0.4*T)^3-1/2*V_ego*(0.4*T)^2, 1/24*a_ego*(0.4*T)^4-1/6*V_ego*(0.4*T)^3;% t=0.4T时
0, -a_ego, -(a_ego*0.4*T-V_ego), -(1/2*a_ego*(0.4*T)^2-V_ego*(0.4*T)), -(1/6*a_ego*(0.4*T)^3-1/2*V_ego*(0.4*T)^2), -(1/24*a_ego*(0.4*T)^4-1/6*V_ego*(0.4*T)^3);
0, a_ego, a_ego*0.5*T-V_ego, 1/2*a_ego*(0.5*T)^2-V_ego*(0.5*T), 1/6*a_ego*(0.5*T)^3-1/2*V_ego*(0.5*T)^2, 1/24*a_ego*(0.5*T)^4-1/6*V_ego*(0.5*T)^3;% t=0.5T时
0, -a_ego, -(a_ego*0.5*T-V_ego), -(1/2*a_ego*(0.5*T)^2-V_ego*(0.5*T)), -(1/6*a_ego*(0.5*T)^3-1/2*V_ego*(0.5*T)^2), -(1/24*a_ego*(0.5*T)^4-1/6*V_ego*(0.5*T)^3);
0, a_ego, a_ego*0.6*T-V_ego, 1/2*a_ego*(0.6*T)^2-V_ego*(0.6*T), 1/6*a_ego*(0.6*T)^3-1/2*V_ego*(0.6*T)^2, 1/24*a_ego*(0.6*T)^4-1/6*V_ego*(0.6*T)^3;% t=0.6T时
0, -a_ego, -(a_ego*0.6*T-V_ego), -(1/2*a_ego*(0.6*T)^2-V_ego*(0.6*T)), -(1/6*a_ego*(0.6*T)^3-1/2*V_ego*(0.6*T)^2), -(1/24*a_ego*(0.6*T)^4-1/6*V_ego*(0.6*T)^3);
0, a_ego, a_ego*0.7*T-V_ego, 1/2*a_ego*(0.7*T)^2-V_ego*(0.7*T), 1/6*a_ego*(0.7*T)^3-1/2*V_ego*(0.7*T)^2, 1/24*a_ego*(0.7*T)^4-1/6*V_ego*(0.7*T)^3;% t=0.7T时
0, -a_ego, -(a_ego*0.7*T-V_ego), -(1/2*a_ego*(0.7*T)^2-V_ego*(0.7*T)), -(1/6*a_ego*(0.7*T)^3-1/2*V_ego*(0.7*T)^2), -(1/24*a_ego*(0.7*T)^4-1/6*V_ego*(0.7*T)^3);
0, a_ego, a_ego*0.8*T-V_ego, 1/2*a_ego*(0.8*T)^2-V_ego*(0.8*T), 1/6*a_ego*(0.8*T)^3-1/2*V_ego*(0.8*T)^2, 1/24*a_ego*(0.8*T)^4-1/6*V_ego*(0.8*T)^3;% t=0.8T时
0, -a_ego, -(a_ego*0.8*T-V_ego), -(1/2*a_ego*(0.8*T)^2-V_ego*(0.8*T)), -(1/6*a_ego*(0.8*T)^3-1/2*V_ego*(0.8*T)^2), -(1/24*a_ego*(0.8*T)^4-1/6*V_ego*(0.8*T)^3);
0, a_ego, a_ego*0.9*T-V_ego, 1/2*a_ego*(0.9*T)^2-V_ego*(0.9*T), 1/6*a_ego*(0.9*T)^3-1/2*V_ego*(0.9*T)^2, 1/24*a_ego*(0.9*T)^4-1/6*V_ego*(0.9*T)^3;% t=0.9T时
0, -a_ego, -(a_ego*0.9*T-V_ego), -(1/2*a_ego*(0.9*T)^2-V_ego*(0.9*T)), -(1/6*a_ego*(0.9*T)^3-1/2*V_ego*(0.9*T)^2), -(1/24*a_ego*(0.9*T)^4-1/6*V_ego*(0.9*T)^3);
0, a_ego, a_ego*T-V_ego, 1/2*a_ego*T^2-V_ego*T, 1/6*a_ego*T^3-1/2*V_ego*T^2, 1/24*a_ego*T^4-1/6*V_ego*T^3;% t=T时
0, -a_ego, -(a_ego*T-V_ego), -(1/2*a_ego*T^2-V_ego*T), -(1/6*a_ego*T^3-1/2*V_ego*T^2), -(1/24*a_ego*T^4-1/6*V_ego*T^3)];
B_d=[eta*d_obs*sign(y0)-eta*delta_safe;b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax;
b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax;
b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax;
b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax;
b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax;
b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax;
b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax;
b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax;
b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax;
b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax;
b_d1; b_d3; a_ymax; a_ymax; j_ymax; j_ymax;
kappa_max*V_ego^3; kappa_max*V_ego^3;
kappa_max*V_ego^3; kappa_max*V_ego^3;
kappa_max*V_ego^3; kappa_max*V_ego^3;
kappa_max*V_ego^3; kappa_max*V_ego^3;
kappa_max*V_ego^3; kappa_max*V_ego^3;
kappa_max*V_ego^3; kappa_max*V_ego^3;
kappa_max*V_ego^3; kappa_max*V_ego^3;
kappa_max*V_ego^3; kappa_max*V_ego^3;
kappa_max*V_ego^3; kappa_max*V_ego^3;
kappa_max*V_ego^3; kappa_max*V_ego^3;
kappa_max*V_ego^3; kappa_max*V_ego^3];
[d0,~,exitflag_D]=quadprog(H_d,zeros(6,1),A_d,B_d,Aeq_d,Beq_d); %二次规划函数
%% 验证二次规划函数是否有解
s5=zeros(6,1); exitflag_s5=0;exitflag_s5= exitflag_S; %定义数据类型，避免mxArrays数据类型
d5=zeros(6,1); exitflag_d5=0;exitflag_d5= exitflag_D;
if exitflag_s5==1 && exitflag_d5==1
exitflag_s=1;s5=s0;s=s5;%求解s方向的多项式系数
exitflag_d=1;d5=d0;d=d5;%求解d方向的多项式系数
PlanFlag=0;%规划成功之后，不再对多项式系数进行求解
else
exitflag_s=0;%路径规划失败
s=s5;exitflag_d=0;d=d5;
PlanFlag=1;%规划失败之后，继续对多项式系数进行求解
end
else
s=s_delay;exitflag_s=1;%规划成功之后，系数保持不变，继续维持上一时刻
d=d_delay;exitflag_d=1;
end
%%%%根据避障过程中车辆的行驶里程，计算时间T
s2=s1+V_ego*period+0.5*a_ego*(period^2);t_1=t_1+period;
dt=[1,t_1,1/2*(t_1^2),1/6*(t_1^3),1/24*(t_1^4),1/120*(t_1^5)]*d;
if t_1>=T-0.5
AES_END_Flag=1; %避障结束标志位
else
AES_END_Flag=0;
end
else
s=zeros(6,1);exitflag_s=-1;d=zeros(6,1);exitflag_d=-1;
t_1=0;dt=0;AES_END_Flag=0;s2=0;
end
在完成上述轨迹规划之后，即可在Frenét坐标系下，以车辆相对规划后的期望换道轨迹的运动状态作为反馈，采用LKA控制器去跟踪规划出来的避障路线，实现换道过程控制。设置本车初始车速为15m/s时，利用QP算法求解出换道避障过程中最优横向误差、航向误差以及LKA的控制效果如下图所示。
图10 换道避障相关参数
图11 车辆换道避障的方向盘转角
图12 车辆换道过程的航向角和横摆角速度
