CN114312848A - 基于双层mpc的智能驾驶汽车轨迹规划与跟踪控制方法 - Google Patents

Abstract

一种基于双层MPC的智能驾驶汽车轨迹规划与跟踪控制方法，属于自动化控制技术领域。本发明的目的是针对智能驾驶汽车驾驶在可能存在局部积雪的偶发性混合路况，设计不仅考虑乘车的舒适性和安全性，又能够处理局部积雪的偶发性问题，扩大规划方法对驾驶场景覆盖的MPC轨迹规划层的基于双层MPC的智能驾驶汽车轨迹规划与跟踪控制方法。本发明首先根据对轨迹规划的需求，建立全局坐标系下的车辆运动学模型；然后，引入轨迹规划问题的代价函数和约束，包括了构建具有防止碰撞功能代价函数、状态偏差跟踪代价函数以及保证驾驶舒适性、安全性约束的避撞换道轨迹规划层。本发明控制性能、安全性和稳定性都得到了提升，更加适合混合路况场景的避撞换道研究。

Description

基于双层MPC的智能驾驶汽车轨迹规划与跟踪控制方法

技术领域

本发明属于自动驾驶轨迹规划与运动控制技术领域。

背景技术

智能驾驶汽车在减少交通事故以及提高出行效率方面有着具有巨大优势，也一直是学术界和产业界的研究热点。当智能驾驶汽车在道路上行驶时，我们期望它们能处理人类日常驾驶挑战，比如智能驾驶汽车在驾驶过程中必须避开可能跑到街上的儿童，或避开动态/静态的障碍，或在遇到结冰表面时安全驾驶。在控制工程中，这个概念被称为自适应性—即当系统或环境参数发生意外变化时，控制器安全运行的能力。目前，国内外有关智能驾驶汽车避撞的研究普遍将轨迹规划与跟踪控制分开单独进行研究，将控制器设计为分层式架构。上层设计轨迹规划算法，是环境感知任务决策与跟踪运动控制模块之间的重要桥梁，用于规划出能够避开障碍物的可行驶的轨迹；下层设计跟踪控制算法，实现对上层规划轨迹的跟踪控制。

针对这种分层式智能驾驶汽车的避障换道轨迹规划与跟踪控制方案，目前存在以下几个方面的问题：

1.目前关于智能驾驶汽车的避撞换道控制技术的研究，通常先是根据当前驾驶环境进行轨迹规划，再对规划的轨迹做跟踪控制的分层式技术框架。传统的方案，先采用多项式等方法对轨迹进行规划，再利用LQR、PID等控制方法实现对轨迹的跟踪，智能驾驶车辆中高速驾驶时，尤其在低附路面容易发生打滑，传统的方案仅利用反馈的补偿控制来调整偏差，这样控制会比较被动，会存在一定的滞后，对偶发性紧急情况的鲁棒性较差。

2.对于自动控制系统来说，偶发性仍然是一个根本性的问题，目前换道轨迹规划方法研究中，较少关注到其功能对驾驶场景的覆盖能力。当车辆在我国北方地区驾驶时，路面可能会存在局部积雪情况，这种偶发性混合路况很容易发生交通事故。

3.对于混合路况中的偶发性紧急路况，不只规划层需要相应的调整动态规划策略，跟踪控制层若不能够充分考虑车辆的动力学特性、车辆轨迹跟踪性能和跟踪平滑性，则当系统或环境参数发生意外变化时，跟踪控制的自适应调整能力就会较差。

发明内容

本发明的目的是针对智能驾驶汽车驾驶在可能存在局部积雪的偶发性混合路况，设计不仅考虑乘车的舒适性和安全性，又能够处理局部积雪的偶发性问题，扩大规划方法对驾驶场景覆盖的MPC轨迹规划层；设计能够对系统或环境参数发生意外变化有很好的自适应能力，实现对上层规划轨迹实时、在线的稳定跟踪的MPC跟踪控制层，综合以上提出的基于双层MPC的智能驾驶汽车轨迹规划与跟踪控制方法。

本发明的步骤是：

首先根据对轨迹规划的需求，建立全局坐标系下的车辆运动学模型；然后，引入轨迹规划问题的代价函数和约束，包括了构建具有防止碰撞功能代价函数、状态偏差跟踪代价函数以及保证驾驶舒适性、安全性约束的避撞换道轨迹规划层：

S1.建立全局坐标系下的车辆的运动学模型

简化汽车运动，建立全局坐标系下的车辆的运动学模型，忽略掉了车辆在垂直方向的运动，给出其二维平面上的运动学方程如下：

其中，a_y代表车辆的侧向加速度，a_x代表车辆纵向加速度，

代表车辆的航向角，v_y代表车辆的侧向速度，v_x代表车辆的纵向速度，X、Y代表车辆在全局坐标系下的位置坐标；系统模型可以简化为：

以

为车辆的状态量，以u(t)＝[a_y a_x]^T为控制量；

S2、建立带有避免换道过程碰撞代价函数的轨迹规划方法

代价函数设计中的位置状态偏差的跟踪代价函数J₁，其表达形式如下：

其中，N_P代表预测步长；W_P代表系统状态的权重；Y代表换道轨迹中的自车实际侧向位置坐标；Y_ref代表目标车道中心线侧向位置坐标，当前道路单车道宽为W；

代价函数设计中的防止障碍车辆与自车碰撞的避撞代价函数J₂，其表达形式如下：

其中，X、Y代表自车全局坐标系下的纵、横向位置坐标；N_obs代表障碍物坐标点的数量；X_obsk、Y_obsk表第k个障碍物点在全局坐标系下的纵、横向位置坐标；W_R表示障碍物与自车碰撞威胁的权重；ε代表一个很小的标量；

设计了如下的控制问题代价函数J₃：

其中，N_C代表控制步长；u代表当前时刻的待优化控制量；W_C代表控制量的权重；

车辆的乘坐舒适性和驾驶的安全性约束如下：

u_min≤u(t+k)≤u_max,k＝0,1,…,N_C-1 (6)

控制量u_min＝[-μg,-μg]^T；u_max＝[μg,μg]^T，μ为路面的附着系数；

综合代价函数和约束条件：

根据下式求解当前控制量下的状态量，就获得所需的规划的轨迹信息：

ξ_k+1＝ξ_k+Δt f(ξ_k,u_k) (8)

2.将以上轨迹规划方案进行改进，扩展至混合路况，针对这种特殊的偶发性混合驾驶路况，在模型预测控制的框架下，构造了带有应急功能的MPC轨迹规划层：

建立带有紧急工况规划的代价函数J_∑作为常规规划代价函数和紧急规划代价函数的代价综合：

J_∑＝min(J_normal+J_urgency) (9)

各自的轨迹受制于自己的代价函数，常规工况规划代价函数J_normal和紧急工况规划代价函数J_normal，并保持各自的代价函数的独立性；

优化问题的约束如下

受等式约束：

受不等式约束：

3.根据研究车辆的转向性质和稳定性的关系，建立考虑车辆横摆和滑移的动力学模型；然后，对建立的模型进行线性化和离散化，给出保证车辆操纵稳定性的道路环境约束、车辆执行机构约束以及稳定行驶约束，利用模型预测控制算法能够处理约束的能力，设计基于代价函数和约束的轨迹跟踪控制层：

S1、根据研究车辆的转向性质和稳定性的关系，建立考虑车辆横摆和滑移的车辆动力学模型如下：

其中，m代表车辆的质量，v_x、v_y分别代表车身坐标系下车辆质心处的纵向速度、侧向速度，

代表车辆的横摆角，

代表车辆的横摆角速度，δ_f代表车辆的前轮转向角，I_z代表车辆的转动惯量，l_f和l_r分别代表车辆质心到前、后轴的距离，F_yf和F_yr分别代表车辆前、后轴上的轮胎侧向力的合力，F_xf和F_xr分别代表车辆前、后轴上的轮胎纵向力的合力；

假设忽略耦合关系并且忽略掉前轮纵向驱动力对对车辆横摆运动作用，将上式简化为：

运动控制中智能驾驶车辆的相对全局坐标表达式如下：

其中X、Y分别代表车辆在相对全局坐标系下的纵、横向位置坐标；

将轮胎力等价为轮胎侧偏角的线性函数表达式：

F＝C_αα (16)

其中，C_α代表轮胎线性区的侧偏刚度，α代表轮胎侧偏角；

得到的智能驾驶汽车前轮系统的轮胎侧向力表达式如下：

其中，C_αf和C_αr分别代表前、后轮胎的侧偏刚度，β为质心侧偏角，一般可以

同样车辆的纵向力表达式为：

其中，C_lf和C_lr分别代表前、后轮胎的纵向侧偏刚度，λ_f和λ_r分别代表前、后轮胎的滑移率；

S2、线性化与离散化

综合上式(13)-(18)，得到车辆在全局坐标系下的非线性模型表示为：

该车辆的连续系统状态方程表示如下：

其中，状态量为

控制量为u＝[δ_f]^T，

对上式进行离散化处理，得到离散的状态空间方程：

其中，A(k)＝I+TA(t)，B(k)＝TB(t)；

S3、安全驾驶约束

车速约束方式如下：

v_min≤v≤v_max (22)

其中，v_min＝0和v_max＝1.1*v_ref分别代表当前驾驶道路最小、最大的驾驶速度，v_ref代表期望的车辆驾驶速度；

执行器的执行区间的约束：

其中，δ_fmax代表车辆的最大前轮转向角，也就是车辆转向执行机构的最大约束量，Δδ_fmax代表每一个控制步最大的前轮转向角变化量；

侧向加速度约束：

|a_y|≤μg (24)

车辆的横摆角速度限制范围：

轮胎的侧偏角限制：

|α|≤5° (26)

S4、跟踪控制器

轨迹跟踪控制器在每个控制周期进行优化

其中，

为轨迹规划层给出的参考轨迹，N_P代表预测步长；N_C代表控制步长；ε代表松弛变量；W_P代表换道期望轨迹跟踪能力的加权；W_R代表控制输入的加权；W_S代表松弛变量的加权。

本发明的有益效果是：

1.发明的基于双层MPC的智能驾驶汽车换道轨迹规划与跟踪控制方法，与其他现有的方法相比，具有双重的前馈-反馈调节。从仿真可以看出，如果车辆以中高速驾驶进行避撞换道，尤其驾驶环境中存在附着系数较低的冰雪路面，系统经过两次对车辆状态的校正和优化，，有效地改善了车辆的横摆角和横摆角速度，系统的控制性能、安全性和稳定性都得到了提升，更加适合混合路况场景的避撞换道研究。

2.针对智能驾驶汽车的避撞换道问题，规划轨迹方案不能只关注常规工况下乘车的舒适性和安全性。若驾驶在可能存在局部积雪的偶发性混合路况场景中，对于这种特殊驾驶路况，发明了基于MPC的混合路况换道轨迹规划方法。该方案在模型预测控制的框架下，规划出常规路况下的换道轨迹，同时保持着一个可以应对混合路况下偶发性情况的紧急路况换道轨迹，扩大了规划方法对驾驶场景的覆盖。

3.基于MPC的跟踪控制方案的优势在于，通过构建考虑车辆横摆和滑移的动力学模型，并利用模型预测控制擅长解决带约束的优化问题，且具有预测功能、滚动优化和反馈校正的前馈-反馈特性，当系统或环境参数发生意外变化时，控制器有着更强的自适应能力，保证了车辆安全、稳定的行驶。

附图说明

图1是本发明所述的避免碰撞换道场景示意图；

图2是本发明所述的双层模型预测控制原理图；

图3是本发明所述的基于双层MPC的智能驾驶汽车轨迹规划与跟踪控制基于双层MPC的换道轨迹规划与运动控制方法流程框图；

图4是本发明所述的上层基于MPC的换道轨迹规划层采用的自行车模型的示意图；

图5是本发明所述的下层基于MPC的跟踪控制层采用的考虑滑移和横摆运动的车辆动力学模型的示意图；

图6是本发明所述的基于MPC规划层规划的常规路况换道轨迹和紧急路况下的应急换道轨迹图，纵坐标单位为m，横坐标为时间，单位为s；

图7是本发明所述的常规工况(高附路面)基于双层MPC避撞换道轨迹规划与跟踪控制仿真图；

图8是本发明所述的紧急工况(低附路面)基于双层MPC避撞换道轨迹规划与跟踪控制效果图。

具体实施方式

现有技术的问题需要系统的控制性能提升，从而驾驶安全性、稳定性才能得到更好的保障。要求规划层有着较高的实时性，且可以应对混合路况下的驾驶，处理智能驾驶过程中的偶发性紧急路况。对于混合路况中的偶发性紧急路况，当系统或环境参数发生意外变化时，不只规划层需要相应的调整动态规划策略，跟踪控制层也需要在充分考虑车辆的动力学特性、车辆轨迹跟踪能力和跟踪平滑性的情况下，对系统或环境参数发生意外变化时有更强的自适应能力。

本发明具体涉及一种模型预测控制框架下智能驾驶汽车的避撞换道规划与控制方法。更具体地讲，本发明主要针对局部积雪、结冰等偶发性混合路况下行驶的车辆，发明了具有应急功能的动态轨迹规划方法；在充分考虑车辆的动力学特性、轨迹跟踪能力以及驾驶安全性、稳定性的情况下，发明了可以自适应调整的跟踪控制方法。综上所述，本发明设计了双层MPC架构的方案，完成了智能驾驶汽车混合路况的避撞换道控制，属于智能汽车自动驾驶轨迹规划与运动控制技术领域。

本发明针对智能驾驶汽车驾驶在可能存在局部积雪的偶发性混合路况，且自车前方有障碍车辆的驾驶场景(如附图1：智能驾驶汽车有障碍换道场景)。在该场景下需要对车辆进行避免碰撞的换道，于是本发明提出了双层MPC的避障换道规划与控制架构，其中，上层为基于MPC轨迹规划层，规划的参考轨迹不仅考虑了乘车的舒适性和安全性，又针对局部积雪的偶发性问题改进规划方案，扩大规划方法对驾驶场景的覆盖；下层为基于MPC跟踪控制层，不仅能解决多约束的优化问题，还对系统或环境参数发生意外变化有很好的自适应能力，实现对上层规划轨迹实时、在线的稳定跟踪控制。

为解决研究中的技术问题，本发明将采用如下技术方案实现：

1、智能驾驶汽车的模型搭建与仿真工况的构建：利用专业的高保真车辆动力学仿真平台CarSim得到前轮转向的智能驾驶汽车模型，该仿真平台会实时为智能驾驶轨迹规划与运动控制系统提供车辆的状态信息。

2、上层轨迹规划层设计：

1)根据对轨迹规划的需求，建立全局坐标系下的车辆运动学模型；

2)引入轨迹规划问题的代价函数和约束，包括了构建具有防止碰撞功能代价函数、状态偏差跟踪代价函数以及保证驾驶舒适性、安全性约束的避撞换道轨迹规划系统；

3)针对局部积雪的偶发性混合路况，构造带有应急功能的上层MPC轨迹规划层，也就是基于模型预测控制的框架，规划出常规路况下的避撞换道轨迹，同时保持着一个为紧急路况做准备的更保守的换道轨迹；

3、下层跟踪控制层设计：

1)考虑智能驾驶汽车在中高速行驶的安全性，建立考虑车辆横摆和滑移的动力学模型；

2)模型进行线性化和离散化，给出保证车辆操纵稳定性的道路环境约束、车辆执行机构约束以及稳定行驶约束，利用模型预测控制算法能够处理约束的能力，设计基于代价函数和约束的轨迹跟踪控制层；

3)将优化问题转化为二次规划问题并求解控制序列，将得到的控制序列的第一个元素作用于被控对象。在下一个采样时刻，重复上述过程，用新的测量值作为此时预测系统未来动态的初始条件，刷新优化问题并重新求解，实现对规划轨迹的跟踪控制。

下面结合附图对本发明进行详细的说明：

本发明具体应用的避免碰撞换道场景如附图1所示，在我国北方地区，一年有近三分之一的时间路面可能存在局部积雪情况，这就造成了驾驶路况会出现局部积雪的偶发性混合路况，所以如果智能驾驶汽车行驶在北方地区，就要对这种特殊的驾驶情况做出应对。本发明设计了在模型预测控制框架下的双层MPC轨迹规划与跟踪控制方法。

双层MPC的换道轨迹规划与跟踪控制方法控制框图如附图2所示，图中上层MPC规划层的核心思想在于，将感知到的路况、目标参考道路等信息前馈到系统中，利用反馈的车辆状态信息，每一个时刻可以计算出一个相对于该时刻的局部优化性能指标，得到了动态的参考轨迹。规划层的输入是感知层获取的当前路况(路面附着系数)、障碍车辆位置信息、当前车辆的状态信息(自车在全局坐标系下的横纵向坐标、航向角、纵侧向驾驶速度)，输出为规划层规划的参考轨迹信息

下层MPC跟踪控制层根据规划层给出的期望换道轨迹信息与被控对象输出的实际状态信息(自车在全局坐标系下的横纵向坐标、横摆角、横摆角速度、纵侧向驾驶速度)，设计基于模型预测控制轨迹跟踪控制方案，结合目标函数、约束条件进行优化求解，得到在所需控制时域内的控制序列，将控制序列第一个前轮转向角值作为控制输入给被控智能驾驶汽车。

附图3给出了双层MPC换道轨迹规划与运动控制的控制原理图；上层MPC轨迹规划层和下层MPC跟踪控制层均是在MATLAB/Simulink中搭建的；被控对象是利用CarSim构造的汽车模型。

本发明根据以上运行原理和运行过程搭建了用于联合仿真的模型，其搭建以及运行过程如下：

1、软件选择

该控制系统的双层MPC轨迹规划与跟踪控制器和被控智能驾驶汽车的仿真模型分别通过软件MATLAB/Simulink和CarSim进行搭建，软件版本分别为MATLAB R2020b和CarSim2019。其中MATLAB/Simulink则是用于搭建智能驾驶汽车轨迹规划与运动控制系统，即通过Simulink编程来完成该控制系统中控制器的运算；CarSim软件是专门针对车辆动力学的一套商用的车辆模型仿真软件，它在本发明中的主要作用是为系统提供高保真的车辆动力学模型，在仿真实验中代替真实智能驾驶汽车作为控制系统中的控制对象，并提供仿真环境以及车辆驾驶在仿真环境中的状态量信息。

2、联合仿真

CarSim电动汽车整车模型主要由车身、传动系、转向系、制动系、轮胎、空气动力学、工况配置等系统构成。要实现MATLAB/Simulink和CarSim的联合仿真，首先根据对被控智能驾驶汽车的需求设置好车辆的模型，以及规划与控制系统的输入输出量，然后要把CarSim的工作路径设指定为Simulink Model，然后将在CarSim中把设置好的车辆模型Send到Simulink中，运行Simulink从而实现系统完整的联合仿真与通信。如果对CarSim中的模型结构或者参数设置进行了修改，则需要重新将新的模型发送到Simulink。

CarSim的控制输入设置为IMP_STEER_L1、IMP_STEER_R1、IMP_STEER_L2、IMP_STEER_R2(使用前轮转向的智能驾驶汽车作为被控对象，于是将后轮转向角在Simulink中输入恒定0度)；输出设置为控制和观测所需的车辆的状态量。汽车参数如表1所示。

表1电动汽车参数表

3、本发明基于双层MPC的换道轨迹规划与跟踪控制原理及目的智能驾驶汽车的控制是一项极具挑战性的任务，需要采用先进的控制方案来解决系统动力学要求，同时满足驾驶环境约束、车辆执行机构约束以及安全性约束等。模型预测控制是一种基于优化的技术，能够为上述的系统需求提供有效的解决方案。本发明的重点是实现双层MPC方案对智能驾驶汽车的规划与控制。

基于双层MPC的换道轨迹规划与运动控制原理如附图3所示，双层的MPC系统需要经过两次的前馈-反馈对车辆状态进行校正和优化，处理系统中偶发性和不确定性，从而实现应对因冰雪造成的偶发性混合路况下的避撞换道。第一层前馈-反馈的规划轨迹，构建一个基于车辆运动学的系统模型，以目标车道中心线为参考前馈到系统中，针对局部积雪的偶发性混合路况，规划方法上我们将常规路况换道规划和应急路况换道规划分开，每种情况定义相应的约束条件和代价函数，针对不同的路面，利用反馈的车辆状态信息，每一个时刻可以计算出一个相对于该时刻的局部优化性能指标，完成换道轨迹的动态规划；第二层前馈-反馈的跟踪控制，将规划得到的参考轨迹前馈进来，建立考虑自动驾驶车辆横摆和滑移的车辆动力学模型，利用模型预测控制擅长解决带约束的优化问题，还具有预测功能、滚动优化和反馈校正的特性，设计代价函数和约束并进行优化求解，得到在所需控制时域内的控制序列，将控制序列第一个值作为输入给被控智能驾驶汽车，完成对目标轨迹的跟踪控制。

以下介绍本发明关于双层MPC的换道轨迹规划与运动控制方法的具体步骤，包括以下步骤：

步骤一、智能驾驶汽车的模型搭建与仿真工况的构建：利用专业的高保真车辆动力学仿真平台CarSim得到前轮转向的智能驾驶汽车模型，仿真平台提供的汽车模型模拟真实的被控对象，会实时为智能驾驶轨迹规划与运动控制系统提供车辆的状态信息。

在本发明中，由于使用了联合仿真，因此在CarSim中首先选择典型的乘用车模型，之后对模型的相关参数进行修改并获取，将车辆模型参数添加到Simulink仿真模型中。车辆的主要模型参数有车辆质量、前后轴距轮胎侧偏刚度等。在选择相应的车辆模型和参数之后，需要构建相应的仿真工况，仿真工况中可以选择车辆的行驶路线，行驶环境等。

步骤二、设计基于MPC的轨迹规划层：为了克服现有规划方法中的不足，解决车辆避障换道的动态轨迹规划问题，采取基于模型预测控制的方案。首先建立全局坐标系下的车辆的运动学模型，给出相应的运动学方程；然后，为避免自车与障碍车辆碰撞、保证乘坐舒适性和安全性，设计轨迹规划问题的代价函数和约束；最后针对智能驾驶汽车在局部积雪的偶发性混合路况下避撞换道问题，构造带有应急功能的上层MPC轨迹规划层，也就是基于模型预测控制的框架下，规划出常规路况下的避撞换道轨迹，同时保持着一个为紧急路况做准备的更保守的换道轨迹。

①建立全局坐标系下的车辆的运动学模型

规划轨迹时为保证计算的快速性，需要简化汽车运动，其中运动学模型就是简单且有效的简化方式，忽略掉了车辆在垂直方向的运动，也就是说我们描述的车辆是一个二维平面上的运动物体。如附图4所示的全局坐标系下建立车辆的运动学模型，其方程如下：

其中，a_y代表车辆的侧向加速度，a_x代表车辆纵向加速度，

代表车辆的航向角，v_y代表车辆的侧向速度，v_x代表车辆的纵向速度，X、Y代表车辆在全局坐标系下的位置坐标。

系统可以简化描述为：

以

为车辆的状态量，以u(t)＝[a_y a_x]^T为控制量。

建立带有避免换道过程碰撞代价函数的轨迹规划方法

为解决智能驾驶汽车的避障换道轨迹规划问题，采取基于模型预测控制的方案，依据附图4建立的全局坐标系下的车辆的运动学模型，设计换道轨迹规划器。为了保证智能驾驶汽车能够平滑、稳定地规划出避免碰撞的期望换道轨迹，分别设计目标函数，包括目标车道中心线与车辆当前位置状态偏差的跟踪代价函数、控制问题代价函数以及防止障碍车辆与自车碰撞的避撞代价函数。同样为了保证平稳的换道路线，加入约束条件保证待优化控制量的连续性变化要求，避免加速度变化幅度过大而导致驾驶出现不安全的问题。

代价函数设计中的位置状态偏差的跟踪代价函数J₁，其表达形式如下：

其中，N_P代表预测步长；W_P代表系统状态的权重；Y代表换道轨迹中的自车实际侧向位置坐标；Y_ref代表目标车道中心线侧向位置坐标，当前道路单车道宽为W。通过调整该目标函数的权重W_P的大小，实现对换道轨迹规划的快速性要求，合适的权重可以牺牲掉一定的快速性，从而可以保证更好的舒适性和驾驶稳定性。

代价函数设计中的防止障碍车辆与自车碰撞的避撞代价函数J₂，其表达形式如下：

其中，X、Y代表自车全局坐标系下的纵、横向位置坐标；N_obs代表障碍物坐标点的数量；X_obsk、Y_obsk表第k个障碍物点在全局坐标系下的纵、横向位置坐标；W_R表示障碍物与自车碰撞威胁的权重；ε代表一个很小的标量，为了代价函数的可求解。

为保证规划换道轨迹的连续性，车辆转向更加平稳，以及减少能量的消耗。设计了如下的控制问题代价函数J₃：

其中，N_C代表控制步长；u代表当前时刻的待优化控制量；W_C代表控制量的权重。

为保证智能驾驶汽车平稳的换道，避免加速度变化幅度过大的甚至突变，需要进行以下约束，保证驾驶中乘坐舒适性和驾驶的安全性，约束如下：

u_min≤u(t+k)≤u_max,k＝0,1,…,N_C-1 (6)

控制量u_min＝[-μg,-μg]^T；u_max＝[μg,μg]^T，μ为路面的附着系数。

综合代价函数和约束条件：

轨迹规划器将感知到的路况、目标道路中心线等信息前馈到系统中，利用反馈的车辆状态信息，根据所构建的系统模型在线求解一个有限时间开环优化问题，每一个时刻可以计算出一个相对于该时刻的局部优化性能指标，然后根据下式求解当前控制量下的状态量，于是就可以获得我们所需的规划的轨迹信息：

ξ_k+1＝ξ_k+Δt f(ξ_k,u_k) (8)

规划层将模型预测控制的处理多约束的能力充分利用，在考虑时间因素、车辆状态和动力学约束的基础上实现智能驾驶车辆道行驶的动态规划。

③针对局部积雪的偶发性混合路况，构造带有应急功能的MPC轨迹规划器将上述模型预测控制的轨迹规划方案扩展至混合路况的避撞换道，发明了基于MPC的混合路况换道轨迹规划方法。该方案在模型预测控制的框架下，规划出常规路况下的避撞换道轨迹，同时保持着一个应对偶发性紧急路况更保守的换道轨迹。然后根据当前的感知层和决策层的路况信息来选择换道的轨迹，或常规工况规划的ξ_normal轨迹，或紧急工况规划的ξ_urgency轨迹。

接下来定义带有紧急工况规划的代价函数J_∑作为常规规划代价函数和紧急规划代价函数的代价综合：

J_∑＝min(J_normal+J_urgency) (9)

各自的轨迹受制于自己的代价函数，常规工况规划代价函数J_normal和紧急工况规划代价函数J_normal，并保持各自的代价函数的独立性。

而优化问题的约束问题如下，首先保证在换道开始时车辆的当前状态一样，然后在计算两条轨迹；其次，两种方案受到车辆模型的等式约束，由于方案应对的驾驶情况不同，最后规划出的轨迹也是不同的(即相同时间步的车辆计算状态不同

)；最后两方案也有着各自的不等式约束，综上所述，得到以下约束：

受等式约束：

受不等式约束：

就像典型的模型预测控制一样，当系统在时间上向前推进时，有两种可能性：当感知层感知到路况是高附沥青路面，没有紧急路况，按照常规路况规划避撞换道轨迹；或者，当感知到前方道路有冰雪低附路况，这样系统或环境参数发生变化，所以采用更加保守的紧急路况换道轨迹规划策略。方案不仅可以满足常规工况下的避撞换道，在紧急路况时也可以有效地改善了车辆的横摆角和横摆角速度等，使得系统的控制性能、安全性和稳定性都得到了提升，扩大了规划方法对驾驶场景的覆盖。

表2规划层仿真实验参数表

符号	定义	数值/单位
			W	单车道宽度	3.75/m
N<sub>P</sub>	规划层MPC预测步长	10
			N<sub>C</sub>	规划层MPC控制步长	2
N<sub>obs</sub>	障碍点数量	1
			W<sub>P</sub>	系统状态权重	100
W<sub>C</sub>	控制量权重	30
			W<sub>R</sub>	碰撞威胁权重	25

。

步骤三、基于MPC的跟踪控制层：由于上层轨迹规划是一个动态过程，具有时变特性，模型预测控制对系统不确定性有着很好的自适应能力。智能驾驶汽车在中高速行驶过程中，尤其在冰雪路况，很容易导致车辆侧滑和滑移等情况发生，通过研究车辆的转向性质和稳定性的关系，建立考虑智能驾驶汽车的横摆和滑移的车辆动力学模型；然后，对建立的模型进行线性化和离散化，给出保证车辆操纵稳定性的道路环境约束、车辆执行机构约束以及稳定行驶约束，利用模型预测控制算法能够处理约束的能力，建立带有代价函数和约束的换道跟踪控制器；将模型预测控制问题转化为二次规划问题求解控制序列，并将得到的控制序列的第一个元素作用于被控对象，实现对规划轨迹的跟踪控制。

①建立车辆动力学模型

实际的车辆系统的动力学行为模型是很复杂的，但是为了控制的可行性和实时性我们会在模型的准确性和计算成本之间进行权衡。为了更加准确地描述车辆动力学过程，我们进行了部分的简化并进行了如下的理想化假设：(1)假设忽略悬架运动，假设车辆在平坦的路面上进行驾驶；(2)对轮胎进行横、纵向轮胎力的解耦，忽略了其耦合关系；(3)忽略空气动力学对驾驶过程的影响；(4)假设忽略前轮驱动纵向力对车辆横摆运动的影响。于是我们根据车辆的受力平衡和力矩平衡的关系，如附图5所示的全局坐标系下建立考虑车辆横摆和滑移的车辆动力学模型，其方程可由下式表示：

其中，m代表车辆的质量，v_x、v_y分别代表车身坐标系下车辆质心处的纵向速度、侧向速度，

代表车辆的横摆角，

代表车辆的横摆角速度，δ_f代表车辆的前轮转向角，I_z代表车辆的转动惯量，l_f和l_r分别代表车辆质心到前、后轴的距离，F_yf和F_yr分别代表车辆前、后轴上的轮胎侧向力的合力，F_xf和F_xr分别代表车辆前、后轴上的轮胎纵向力的合力。

由于假设忽略耦合关系并且忽略掉前轮纵向驱动力对对车辆横摆运动作用，可以将上式简化为：

运动控制中智能驾驶车辆的相对全局坐标表达式如下：

其中X、Y分别代表车辆在相对全局坐标系下的纵、横向位置坐标。

车辆的运动过程中，轮胎所受到的纵向力、侧向力和垂向力等对汽车的操纵稳定性和安全性影响巨大，由于轮胎结构复杂且动力学性能呈现非线性，常见的关于车辆的轮胎模型，一般如果前轮转角较小或者说轮胎侧偏角较小的情况下，轮胎的侧向力与轮胎侧偏角的关系呈现线性关系，近似的将轮胎力等价为轮胎侧偏角的线性函数表达式：

F＝C_αα (16)

其中，C_α代表轮胎线性区的侧偏刚度，α代表轮胎侧偏角。

换道过程为了驾驶的安全性和乘坐舒适性，其前轮转角和轮胎侧偏角是一个小角度的变化过程，于是得到我们的智能驾驶汽车前轮系统的轮胎侧向力表达式如下：

其中，C_αf和C_αr分别代表前、后轮胎的侧偏刚度，β为质心侧偏角，一般可以

同样车辆的纵向力表达式为：

其中，C_lf和C_lr分别代表前、后轮胎的纵向侧偏刚度，λ_f和λ_r分别代表前、后轮胎的滑移率。

②线性化与离散化

综合上式(13)-(18)，得到车辆在全局坐标系下的非线性模型表示为：

该车辆的连续系统状态方程可以表示如下：

其中，状态量为

控制量为u＝[δ_f]^T，

对上式进行离散化处理，得到离散的状态空间方程：

其中，A(k)＝I+TA(t)，B(k)＝TB(t)。

③安全驾驶约束

模型预测控制的优点之一就是能够处理多约束条件，本文下层运动控制层的模型预测控制需要考虑的约束包括道路法规约束、车辆的稳定性约束、控制量约束、执行机构约束等。首先，车辆不能够违反当前驾驶条件下的驾驶速度要求，车速被认为约束方式如下：

v_min≤v≤v_max (22)

其中，v_min＝0和v_max＝1.1*v_ref分别代表当前驾驶道路最小、最大的驾驶速度，v_ref代表期望的车辆驾驶速度。

关于执行机构约束，首先为保证控制器输出的控制量能够被下层执行，需要对执行器的执行区间进行约束；其次，为了换道过程的平滑性和乘坐舒适性，也应该对相邻两控制量的控制增量进行约束，于是得到了以下的约束条件：

其中，δ_fmax＝25°代表车辆的最大前轮转向角，也就是车辆转向执行机构的最大约束量，Δδ_fmax＝0.85°代表每一个控制步最大的前轮转向角变化量。

保证车辆驾驶安全的状态约束，首先是车辆的侧向加速度约束，过大的侧向加速度会影响驾智能驾驶汽车的舒适性，因此其约束为：

|a_y|≤μg (24)

然后是为保证车辆的稳定性，对车辆的横摆角速度限制在合理的范围：

最后是为了保证车辆的轮胎力约束在线性区域内，因此对轮胎的侧偏角进行限制：

|α|≤5° (26)

④换道跟踪运动控制器设计

在设计基于模型预测控制的跟踪控制层控制器时，由于系统的控制增量是未知的，通常通过设定合适的优化目标，并对其求解进而得到控制时域内的最优控制序列。在利用模型预测控制器进行实际控制过程中，优化求解器在规定的计算时间内可能出现求解不出最优结果的情况。主要是因为车辆动力学模型比较复杂，而预测模型对其有所简化，同时也加入了比较多的约束条件，因此有必要考虑解决控制器无法得到最优解问题的策略。

一般解决无法得到最优解问题的方法就是使约束“软化”，也就是说对比把约束限定住，绝对不允许超越而言，允许其在有必要的时候，可以偶尔越过约束更有利于控制器。本文的控制输入是方向盘转角，所有其约束无法软化。对输出约束进行软化最简单的方法就是在以前约束上加上一个新的变量，一般称之为“松弛变量”，通常情况下被定义为非零量。

综合上述的换道控制过程中的约束条件和需要优化的目标，基于动力学模型的轨迹跟踪控制器在每个控制周期进行优化求解的问题如下：

其中，

为轨迹规划层给出的参考轨迹，N_P代表预测步长；N_C代表控制步长；_ε代表松弛变量，也就是代表了软约束对轮胎力的惩罚；W_P代表换道期望轨迹跟踪能力的加权；W_R代表控制输入的加权，保证控制输入的连续性和平滑性；W_S代表松弛变量的加权。可以通过调节不同的权重参数，体现优化目标的优先级。

将以上的优化问题转化为二次型问题，并通过在线的实时求解，生成有效避撞换道运动的控制序列。待优化的变量是车辆的前轮转向角和松弛因子，将控制序列中的第一个元素作为实际的控制输入增量作用于系统，即u(k)＝u(k-1)+Δu(k)。在下一个控制周期后，循环往复，求解最优控制量。方案利用模型预测控制擅长解决带约束的优化问题的特性，还具有预测功能、滚动优化和反馈校正的前馈-反馈特性，当系统或环境参数发生意外变化时，控制器有着更强的自适应能力，车辆能够安全、稳定、平滑地完成避撞换道。

仿真验证

为验证所提算法的可行性，利用MATLAB/Simulink和专业的高保真车辆动力学仿真平台CarSim进行联合仿真分析。

由于本发明研究的是智能驾驶汽车在混合路况下避撞换道轨迹规划与跟踪控制问题。所以我们针对常规路况(路面附着系数μ＝0.85)和紧急路况(路面附着系数μ＝0.25)进行仿真验证。当智能驾驶车辆以中高速72km/h驾驶时，发明的带有应急功能的上层MPC轨迹规划器，为应对局部积雪的偶发性混合路况下避撞换道问题，规划出常规路况下的避撞换道轨迹，同时保持着一个为紧急路况做准备的更保守的换道轨迹，不同路况下规划的轨迹如附图6所示。

针对不同的路况，对本发明的基于双层MPC的避撞换道轨迹规划与跟踪控制方法进行验证，其具有双重的前馈-反馈调节，车辆在中高速驾驶时，尤其是驾驶中存在附着系数较低的冰雪路面，系统经过两次的车辆状态的矫正和优化，通过仿真图(图7和图8)可以看出，在紧急路况避撞换道时有效地改善了车辆的横摆角和横摆角速度，系统的控制性能、安全性和稳定性都得到了提升。

Claims (3)

1.一种基于双层MPC的智能驾驶汽车轨迹规划与跟踪控制方法，其特征在于：其步骤是：首先根据对轨迹规划的需求，建立全局坐标系下的车辆运动学模型；然后，引入轨迹规划问题的代价函数和约束，包括了构建具有防止碰撞功能代价函数、状态偏差跟踪代价函数以及保证驾驶舒适性、安全性约束的避撞换道轨迹规划层：

S1.建立全局坐标系下的车辆的运动学模型

简化汽车运动，建立全局坐标系下的车辆的运动学模型，忽略掉了车辆在垂直方向的运动，给出其二维平面上的运动学方程如下：

其中，a_y代表车辆的侧向加速度，a_x代表车辆纵向加速度，

代表车辆的航向角，v_y代表车辆的侧向速度，v_x代表车辆的纵向速度，X、Y代表车辆在全局坐标系下的位置坐标；

系统模型可以简化为：

以

为车辆的状态量，以u(t)＝[a_y a_x]^T为控制量；

S2、建立带有避免换道过程碰撞代价函数的轨迹规划方法

代价函数设计中的位置状态偏差的跟踪代价函数J₁，其表达形式如下：

其中，N_P代表预测步长；W_P代表系统状态的权重；Y代表换道轨迹中的自车实际侧向位置坐标；Y_ref代表目标车道中心线侧向位置坐标，当前道路单车道宽为W；

代价函数设计中的防止障碍车辆与自车碰撞的避撞代价函数J₂，其表达形式如下：

其中，X、Y代表自车全局坐标系下的纵、横向位置坐标；N_obs代表障碍物坐标点的数量；X_obsk、Y_obsk表第k个障碍物点在全局坐标系下的纵、横向位置坐标；W_R表示障碍物与自车碰撞威胁的权重；ε代表一个很小的标量；

设计了如下的控制问题代价函数J₃：

其中，N_C代表控制步长；_u代表当前时刻的待优化控制量；W_C代表控制量的权重；

车辆的乘坐舒适性和驾驶的安全性约束如下：

u_min≤u(t+k)≤u_max,k＝0,1,…,N_C-1 (6)

控制量u_min＝[-μg,-μg]^T；u_max＝[μg,μg]^T，μ为路面的附着系数；

综合代价函数和约束条件：

s.t.u_min≤u(t+k)≤u_max,k＝1,2,…,N_C-1

根据下式求解当前控制量下的状态量，就获得所需的规划的轨迹信息：

ξ_k+1＝ξ_k+Δtf(ξ_k,u_k) (8)

2.根据权利要求1所述的基于双层MPC的智能驾驶汽车轨迹规划与跟踪控制方法，其特征在于：将以上轨迹规划方案进行改进，扩展至混合路况，针对这种特殊的偶发性混合驾驶路况，在模型预测控制的框架下，构造了带有应急功能的MPC轨迹规划层：

建立带有紧急工况规划的代价函数J_∑作为常规规划代价函数和紧急规划代价函数的代价综合：

J_∑＝min(J_normal+J_urgency) (9)

各自的轨迹受制于自己的代价函数，常规工况规划代价函数J_normal和紧急工况规划代价函数J_normal，并保持各自的代价函数的独立性；

优化问题的约束如下

受等式约束：

受不等式约束：

3.根据权利要求1所述的基于双层MPC的智能驾驶汽车轨迹规划与跟踪控制方法，其特征在于：根据研究车辆的转向性质和稳定性的关系，建立考虑车辆横摆和滑移的动力学模型；然后，对建立的模型进行线性化和离散化，给出保证车辆操纵稳定性的道路环境约束、车辆执行机构约束以及稳定行驶约束，利用模型预测控制算法能够处理约束的能力，设计基于代价函数和约束的轨迹跟踪控制层：

S1、根据研究车辆的转向性质和稳定性的关系，建立考虑车辆横摆和滑移的车辆动力学模型如下：

其中，m代表车辆的质量，v_x、v_y分别代表车身坐标系下车辆质心处的纵向速度、侧向速度，

代表车辆的横摆角，

代表车辆的横摆角速度，δ_f代表车辆的前轮转向角，I_z代表车辆的转动惯量，l_f和l_r分别代表车辆质心到前、后轴的距离，F_yf和F_yr分别代表车辆前、后轴上的轮胎侧向力的合力，F_xf和F_xr分别代表车辆前、后轴上的轮胎纵向力的合力；

假设忽略耦合关系并且忽略掉前轮纵向驱动力对对车辆横摆运动作用，将上式简化为：

运动控制中智能驾驶车辆的相对全局坐标表达式如下：

其中X、Y分别代表车辆在相对全局坐标系下的纵、横向位置坐标；

将轮胎力等价为轮胎侧偏角的线性函数表达式：

F＝C_αα (16)

其中，C_α代表轮胎线性区的侧偏刚度，α代表轮胎侧偏角；

得到的智能驾驶汽车前轮系统的轮胎侧向力表达式如下：

其中，C_αf和C_αr分别代表前、后轮胎的侧偏刚度，β为质心侧偏角，一般可以

同样车辆的纵向力表达式为：

F_xf＝-C_lfλ_f

F_xr＝-C_lrλ_r

(18)

其中，C_lf和C_lr分别代表前、后轮胎的纵向侧偏刚度，λ_f和λ_r分别代表前、后轮胎的滑移率；S2、线性化与离散化

综合上式(13)-(18)，得到车辆在全局坐标系下的非线性模型表示为：

该车辆的连续系统状态方程表示如下：

其中，状态量为

控制量为u＝[δ_f]^T，

对上式进行离散化处理，得到离散的状态空间方程：

其中，A(k)＝I+TA(t)，B(k)＝TB(t)；

S3、安全驾驶约束

车速约束方式如下：

v_min≤v≤v_max (22)

其中，v_min＝0和v_max＝1.1*v_ref分别代表当前驾驶道路最小、最大的驾驶速度，v_ref代表期望的车辆驾驶速度；

执行器的执行区间的约束：

其中，δ_fmax代表车辆的最大前轮转向角，也就是车辆转向执行机构的最大约束量，Δδ_fmax代表每一个控制步最大的前轮转向角变化量；

侧向加速度约束：

|a_y|≤μg (24)

车辆的横摆角速度限制范围：

轮胎的侧偏角限制：

|α|≤5° (26)

S4、跟踪控制器

轨迹跟踪控制器在每个控制周期进行优化

其中，

为轨迹规划层给出的参考轨迹，N_P代表预测步长；N_C代表控制步长；ε代表松弛变量；W_P代表换道期望轨迹跟踪能力的加权；W_R代表控制输入的加权；W_S代表松弛变量的加权。

Images