CN114185272B - 一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法 - Google Patents

Abstract

本发明公开了一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法，包括：步骤一、计算车辆的预览距离和参考航向角；步骤二、基于预览距离计算车辆的减速距离，且根据道路的目标转向速度，得到车辆在入弯前实时应被作用的纵向速度与车辆实时位置之间的关系判定式；步骤三、基于参考航向角和纵向速度建立车辆运动学模型，得到预测时域方程，优化后得到MPC问题并求解，实现对无人驾驶车辆的路径跟踪。本发明自适应预览特性的模型预测控制方法使车辆面对即将的转向动作能提前做好准备，并且通过获取道路曲率信息从而计算出合适的减速距离和目标转向速度，使车辆平滑的减速，以适当的转向速度进行转向，提升车辆的跟踪性能并保证通过性和安全性。

Description

一种基于模型预测控制的具有预览特性的自适应速度智能车 辆路径跟踪方法

技术领域

本发明涉及无人驾驶车辆路径跟踪控制技术领域，具体为一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法。

背景技术

随着电控技术的快速发展和对汽车主动安全功能的需求日益增长，自动驾驶汽车被认为是未来汽车工业发展的重点。对于自动驾驶汽车来说，要解决的最重要的问题就是如何控制车辆沿着期望的路径行驶。轨迹跟踪采用视觉伺服控制算法、滑模控制算法、模糊控制算法、LQR算法和模型预测控制算法。虽然轨迹跟踪的研究很多，但转向控制仍然是一个具有挑战性的领域，涉及如何平衡车辆的横向稳定性和跟踪精度。与其他控制方法相比，模型预测控制在轨迹跟踪方面具有优势，它可以在多变量控制框架下同时处理状态变量的软硬约束和控制输入与输出，提高了性能，实现了稳定性。

模型预测控制的原理主要是在每一个采样时刻，根据获得的当前测量信息，在线求解一个有限时间开环优化问题，然后将得到的控制序列的第一个元素作用于被控对象，并在接下来的每个采样时刻重复上述操作。虽然模型预测控制算法在路径跟踪上表现较好，但依然存在一定的跟踪误差和车辆稳定性问题，为了解决上述问题，本文提出了一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法，提出的方法主要分为车辆纵向速度控制算法和基于车辆纵向速度和横向误差的MPC预览算法两部分，MPC预览算法提升了跟踪性能，车辆纵向速度控制算法在保证跟踪性能情况下提高了车辆的安全性和通过性。车辆纵向速度控制算法主要是基于模糊控制器自适应调整车辆转向速度，减速距离算法作为补充。

发明内容

本发明的一个目的是解决至少上述问题和/或缺陷，并提供至少后面将说明的优点。为了实现根据本发明的这些目的和其它优点，提供了一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法，包括：

步骤一、计算车辆的预览距离和参考航向角；

步骤二、基于预览距离计算车辆的减速距离，且根据道路的目标转向速度，得到车辆在入弯前后实时应被作用的纵向速度与车辆实时位置之间的关系判定式；

步骤三、基于参考航向角和纵向速度建立车辆运动学模型，得到预测时域方程，优化后得到MPC问题并求解，实现对无人驾驶车辆的路径跟踪。

优选的是，其中，步骤一中，其包括：S11、根据当前的车辆纵向速度和横向误差，代入预览距离计算公式得到预览距离，将预览距离映射到参考路径上得到对应参考点，其过程为：根据预览距离计算公式，即公式一，得到预览距离N_z：

T_max＝0.04V

T＝T_max(1-β)

N_z＝VT

其中：V为当前车辆纵向速度，T_max为最大预览时间，T为预览时间，y^E为横向误差，为能接受的最大横向误差；将得到的预览距离N_z投影到参考路径上得到对应参考点；

S12、根据计算对应参考点的道路曲率得到理想的转弯半径，得到参考航向角，其过程为：根据曲率计算公式，即公式二，得到对应参考点的曲率K：

其中：y″为道路函数的二阶导数，y′为道路函数的一阶导数；

根据曲率与转弯半径成反比的关系，即公式三，得到对应参考点的理想转弯半径R：

根据公式四，得到参考航向角

其中：L为车辆前轴和后轴之间的距离；

优选的是，其中，步骤二中，其包括：

S21、通过当前车速和预览距离内的道路的最大曲率计算出减速距离，其过程为：根据公式五，得到减速距离N_a：

其中：A_o为最大减速距离，r₀为减速系数，ρ_m为普通道路最大曲率，为N_a内道路曲率最大值；减速距离的终点是道路中曲率大于0.005的第一个点；

S22、将预览距离内的平均曲率和平均曲率变化率作为模糊推理系统的输入量，目标转向速度作为模糊推理系统的输出量，其过程为：根据实际道路最小转弯半径，计算得到道路平均曲率和道路曲率变化率的取值范围；通过地面附着力限制公式可知|a_y|≤μg，其中：a_y为车辆y方向的加速度，且μ为地面附着系数，g为车体重量，V_r为目标转向速度；根据车辆操控安全的a_y规定值确定目标转向速度V_r的最小值；根据城市道路转弯路口设计车速确定目标转向速度V_r的最大值，从而确定了目标转向速度V_r的取值范围；根据模糊推理系统的输出结果，使车辆在预览距离N_z内对目标转向速度V_r在取值范围内进行实时调整；

S23、建立车辆在入弯前后实时应被作用的纵向速度与当前车辆位置情况之间的判定关系式，其过程为：

建立车辆的纵向速度V_k的关系判定式：

其中：V_a为参考车速，V_r为目标转向速度，为前方道路平均曲率，LOC为车辆在行驶过程中不断地计算当前车辆位置与减速距离的终点的距离；确定车辆的纵向速度V_k后，作用于车辆。

优选的是，其中，步骤三中，其包括：

S31、建立无人驾驶车辆的运动学模型，其过程为：在惯性坐标系下，建立智能车辆的车辆运动学模型为：

其中：(x，y)为车辆后轴轴心坐标，为步骤S12中的参考航向角，δ_f为车辆前轮偏角，l为车辆轴距，V_k为步骤S23中的车辆纵向速度，且在每次MPC计算前更新，参数上的符号“·”表示该参数的一阶导数；

该模型可被表达为更一般的形式，即公式八：

其中：系统的状态变量可表示为系统的控制变量可表示为u＝[δ_f]，f(·)为车辆运动学模型函数；

S32、将车辆的非线性运动学模型进行线性离散化，得到预测时域方程，其过程为：参考系统的任意时刻的状态和控制量满足公式九：

在任意参考路径点(ξ_r，u_r)处，对公式九进行泰勒展开，忽略高阶项，只保留一阶项得公式十：

ξ＝f(ξ，u)+A_t(ξ-ξ_r)+B_t(u-u_r)

其中A_t为f(ξ，u)相对于状态量ξ的雅各比矩阵，B_t为f(ξ，u)相对于控制量u的雅各比矩阵，ξ_r表示参考点的状态量，此处ξ_r中的的值为步骤S12所求得的的值，u_r表示参考点的控制量；

将公式十与公式九相减得到车辆的线性误差模型，即公式十一：

其中，表示当前状态量与参考状态量的差，表示当前控制量与参考控制量的差。连续的状态方程不适用于设计模型预测控制器，采用单步欧拉法对线性误差模型离散化后，离散线性误差模型，即公式十二为：

其中A_t，k＝I+TA_t，B_t，k＝TB_t，A_k，t表示k时刻的f(ξ，u)相对于状态量ξ的雅各比矩阵，B_k，t表示k时刻的f(ξ，u)相对于控制量u的雅各比矩阵，I为单位矩阵，T为离散时间，表示当前状态量与参考状态量的误差，表示当前控制量与参考控制量的误差；

令公式十三：

其中表示人为设置的状态参数在离散线性模型中引入的控制增量，表示当前状态量的误差，表示上一时刻的控制量的误差；

将所述公式十二表述为它的等价形式，即公式十四：

其中η(k)为k时刻的输出量，控制量的误差的增量， 分别为A_t，k，B_t，k的增广矩阵，通过设置矩阵C_t，k使输出量只包含

假设预测时域为N_p，控制时域为N_c，得到预测状态误差参数和输出η(k+N_p)的表达式分别为

公式十五：

公式十六：

令公式十七：

其中表示预测时域内的输出矩阵，ψ_t表示预测时域内的状态系数矩阵，ΔU(t)表示预测时域内的控制序列，Θ_t表示预测时域内的控制增量矩阵，则预测时域方程的紧凑形式表示为公式十八：

S23、对预测时域方程进行优化，得到MPC问题，其过程为：根据预测时域方程，得到整个预测时域的期望输出为公式十九：

其中η_ref(t+1)表示下一时刻的参考输出量，根据整个预测时域的期望输出得到MPC问题表示为公式二十、公式二十一：

其中，ΔU_min为控制增量的最小值，ΔU_max为控制增量的最大值。U_min为控制总量的最小值，U_max为控制总量的最大值。为大小为N_c×N_c的单位矩阵，E为单位向量。Q表示输出量误差的权重矩阵，R表示控制增量的权重矩阵。ε为防止目标函数在控制范围内无解而添加的松弛因子，ρ为松弛因子的权重系数；

S24、求解MPC问题模型，实现对无人驾驶车辆的路径跟踪，其过程为：将所述公式二十转换为二次规划问题，使用内点法可以快速完成二次规划问题的求解，在每个控制周期会得到一系列的最优控制序列，即公式二十二：

最优控制序列只有第一个矢量作为实际输入，并在每一个控制周期重复如上步骤进行计算，一直循环直到完成跟踪控制任务。

本发明至少包括以下有益效果：

本发明提出了一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法(PADC)，首先根据当前车辆纵向速度和当前车辆横向误差计算预览距离，其次由预览距离投影到参考路径上得到对应的参考点并计算该点的道路曲率，根据道路曲率得到理想的转弯半径由提出的公式得到当前MPC的参考航向角。接下来根据计算预览距离内的平均曲率和平均曲率变化率得到目标转向速度，并且由提出的公式得到减速距离再建立车辆纵向速度与车辆当前所处位置之间的关系，将目标车辆纵向速度施加在车辆上。本发明用于解决路径跟踪过程中，车辆在高速行驶过弯状况下由于速度过快且转向不足导致在弯道处与目标轨迹相差过大的情况，并且解决了传统的具有预览特性的MPC算法的提前转向过早的问题。提升了车辆的跟踪性能并保证了通过性和安全性。

附图说明

图1为本发明的算法流程图；

图2为本发明的车辆预览距离参考点示意图；

图3为本发明的车辆减速过程示意图；

图4为本发明的和V_r的隶属度函数图；

图5为本发明的模糊控制规则图；

图6为本发明的车辆运动学模型图；

图7为本发明提供的算法(PADC)与传统MPC算法以及传统具有预览特性的MPC算法(Preview-MPC)跟踪结果对比图；

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。应当理解，本文所使用的诸如“具有”、“包含”以及“包括”术语并不排除一个或多个其它元件或其组合的存在或添加。

工作原理：(如图1)

(1)实际的人为驾驶操作中，驾驶员会根据当前车辆情况以及前方道路情况来调整行驶速度、减速距离和转向行为。驾驶员在跟踪路径时，会将车辆的跟踪精度控制在一个安全的范围内。对于提高跟踪精度和乘客舒适性，合适的转向角度和转向速度是关键，因此预览距离应该是自适应的，因为驾驶员在自身车速较快时预览距离会适度增加，同时为了保证控制精度，当与跟踪路径误差较大时，预览距离会适度减小。因此提出了一种基于车辆纵向速度和横向误差的自适应预览距离公式。所以根据当前车辆纵向速度和横向误差得到预览距离。

(2)将预览距离映射到参考路径上得到对应参考点，根据曲率公式计算该点的道路曲率，并根据曲率与半径成反比的关系得到理想转弯半径，根据公式得到当前采样时刻MPC控制器的参考航向角。因为传统的模型预测控制中的参考航向角不为0的点是处于弯道内的，而实际驾驶操作中，驾驶员并不会在进入弯道后才进行转向操作，相反，驾驶员在接近弯道时便已经开始转向。得到的参考航向角会提升车辆在弯道时的跟踪精度和转角稳定性。

(3)将弯道内第一个曲率大于等于0.005的点作为减速距离的终点。根据当前车速和预览距离内的道路的最大曲率通过提出的公式计算出减速距离，车辆在行驶过程中不断地计算当前车辆位置与减速距离的终点的距离，当时，车辆开始减速，在车辆到达减速距离的终点时，车速已降至目标转向速度。车辆在弯道内按照目标转向速度行驶，直到前方道路平均曲率为0时车速恢复至参考车速，车辆纵向速度在每次MPC计算前更新。

(4)以道路平均曲率和道路曲率变化率作为模糊控制器的输入变量，目标转向速度为输出变量。和越小，说明前方道路弯道越平缓，因此的值越大。当较小，增大时，说明前方道路弯道有变陡峭的可能，因此值应适当减小。结合步骤三建立车辆纵向速度与车辆当前所处位置情况的关系，确定车辆的纵向速度并作用于车辆。

(5)建立无人驾驶车辆的运动学模型。车辆运动学模型是实现路径跟踪和模型预测的基础，复杂的模型仿真时间较长且收敛速度较慢。运动学模型作为一个适当简化的模型，不仅能够获得良好的控制精度，还能进一步降低计算复杂度，提高实时性能。

(6)将车辆的非线性运动学模型进行线性离散化，得到预测时域方程。因为要使用二次规划算法进行优化所以需要对运动学模型进行线性化，并且由于连续的预测方程不适用于设计模型预测控制器，故采用单步欧拉法对线性误差模型离散化，得到离散线性误差模型。并经过数学推导得到预测时域方程。

(7)对预测时域方程进行优化，得到MPC问题。通过步骤五所得的预测方程，设计相应的目标函数并考虑系统的约束问题，并结合预测时域的期望输出，将其转换为多目标实时优化的MPC问题模型。

(8)将MPC问题模型转换为二次规划问题，使用内点法可以快速完成二次规划问题的求解，在每个控制周期会得到一系列的最优控制序列，选取第一个作为实际控制量，然后在新的时刻重新进行预测求解，一直循环直至完成路径跟踪控制任务。

实施例1：

一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法，其过程包括：

(如图2)步骤一、计算车辆的预览距离和参考航向角；其包括：

S11、根据当前的车辆纵向速度和横向误差，代入预览距离计算公式得到预览距离，将预览距离映射到参考路径上得到对应参考点，其过程为：根据预览距离计算公式，即公式一，得到预览距离N_z：

T_max＝0.04V

T＝T_max(1-β)

N_z＝VT

其中：V为当前车辆纵向速度，T_max为最大预览时间，T为预览时间，y^E为横向误差，为能接受的最大横向误差；将得到的预览距离N_z投影到参考路径上得到对应参考点；若当前横向误差y^E大于或等于对，控制器的首要任务应为尽量减小车辆与目标跟踪路径的误差，此时转向预览距离N_z应为0。

S12、根据计算对应参考点的道路曲率得到理想的转弯半径，得到参考航向角，其过程为：根据曲率计算公式，即公式二，得到对应参考点的曲率K：

其中：y″为道路函数的二阶导数，y′为道路函数的一阶导数；

根据曲率与转弯半径成反比的关系，即公式三，得到对应参考点的理想转弯半径R：

根据公式四，得到参考航向角

其中：L为车辆前轴和后轴之间的距离；

(如图3)步骤二、基于预览距离计算车辆的减速距离，且根据道路的目标转向速度，得到车辆在入弯前后实时应被作用的纵向速度与车辆实时位置之间的关系判定式，其包括：

S21、通过当前车速和预览距离内的道路的最大曲率计算出减速距离，其过程为：根据公式五，得到减速距离N_a：

其中：A_o为最大减速距离，r₀为减速系数，ρ_m为普通道路最大曲率，为0.166，为N_a内道路曲率最大值；减速距离的终点是道路中曲率大于0.005的第一个点；由于驾驶员在通过弯道时，不会在进入弯道后才减速，而是会在距离弯道一定距离时开始减速，当进入弯道时车速已经减速至目标转弯车速。在实际中，车辆通过曲率较小的弯道时为保证通过效率一般不减速，通常设计时速80km/h的道路转弯半径R最小为220m，而城市中最高行驶速度为80km/h，因此曲率小于0.005的道路不进行减速操作，将弯道内第一个曲率大于等于0.005的点作为减速距离的终点。根据当前车速V和预览距离N_a内的道路的最大曲率通过公式五计算出减速距离；

S22、将预览距离内的平均曲率和平均曲率变化率作为模糊推理系统的输入量，目标转向速度作为模糊推理系统的输出量，其过程为：根据实际城市道路最小转弯半径为6m，计算得到道路平均曲率的取值范围[0，0.166]，和道路曲率变化率的取值范围[-0.166，0.166]；通过地面附着力限制公式可知|a_y|≤μg，其中：a_y为车辆y方向的加速度，且μ为地面附着系数，范围为[0.4，0.85]，g为车体重量，V_r为目标转向速度；根据车辆操控安全的a_y规定最大值6m/s²确定目标转向速度V_r的最小值为6m/s；根据城市道路转弯路口设计车速最大值45km/h(12.5m/s)确定目标转向速度V_r的最大值为12.5m/s，从而确定了目标转向速度V_r的取值范围[6，12.5]；根据模糊推理系统的输出结果，对车辆的目标转向速度V_r在取值范围[6，12.5]内进行实时调整；和越小，说明前方道路弯道越平缓，因此的V_r值越大。当较小，增大时，说明前方道路弯道有变陡峭的可能，因此V_r值应适当减小。和V_r的隶属度函数和V_r的模糊控制调节规则(如图4-5)。

S23、建立车辆在入弯前后实时应被作用的纵向速度与当前车辆位置情况之间的判定关系式，其过程为：

建立车辆的纵向速度V_k的关系判定式：

其中：V_a为参考车速，V_r为目标转向速度，为前方道路平均曲率，LOC为车辆在行驶过程中不断地计算当前车辆位置与减速距离的终点的距离；确定车辆的纵向速度V_k后，作用于车辆；当LOC≤N_a时，车辆开始减速，在车辆到达减速距离的终点时，车速已降至目标转向速度。车辆在弯道内按照目标转向速度V_r行驶，直到前方道路平均曲率为0时车速恢复至参考车速V_a。通过样的分阶段速度自适应调配，使车辆在过弯前后进行更平滑的减速，以最适当该弯道的转向速度进行转向，提升了车辆的跟踪性能并保证了通过性和安全性。

步骤三、基于参考航向角和纵向速度建立车辆运动学模型，得到预测时域方程，优化后得到MPC问题并求解，实现对无人驾驶车辆的路径跟踪，其包括：

(如图6)S31、建立无人驾驶车辆的运动学模型，其过程为：在惯性坐标系下，建立智能车辆的车辆运动学模型为：

其中：(x，y)为车辆后轴轴心坐标，为步骤S12中的参考航向角，δ_f为车辆前轮偏角，l为车辆轴距，V_k为步骤S23中的车辆纵向速度，δ_f与V_k在每次MPC计算前更新，参数上的符号“·”表示该参数的一阶导数；

该模型可被表达为更一般的形式，即公式八：

其中：系统的状态变量可表示为系统的控制变量可表示为u＝[δ_f]，f(·)为车辆运动学模型函数；步骤S31中无人驾驶车辆的车辆运动学模型基于以下假设：1.忽略车辆垂直于地面的运动。2.左前轮和右前轮角度相同。3.车身和悬架系统是刚性的。4.只考虑车辆前轮转动；

S32、将车辆的非线性运动学模型进行线性离散化，得到预测时域方程，其过程为：参考系统的任意时刻的状态和控制量满足公式九：

在任意参考路径点(ξ_r，u_r)处，对公式九进行泰勒展开，忽略高阶项，只保留一阶项得公式十：

其中A_t为f(ξ，u)相对于状态量ξ的雅各比矩阵，B_t为f(ξ，u)相对于控制量u的雅各比矩阵，ξ_r表示参考点的状态量，此处ξ_r中的的值为步骤S12所求得的的值，u_r表示参考点的控制量；

将公式十与公式九相减得到车辆的线性误差模型，即公式十一：

其中，表示当前状态量与参考状态量的差，表示当前控制量与参考控制量的差。连续的状态方程不适用于设计模型预测控制器，采用单步欧拉法对线性误差模型离散化后，离散线性误差模型，即公式十二为：

其中A_t，k＝I+TA_t，B_t，k＝TB_t，A_k，t表示k时刻的f(ξ，u)相对于状态量ξ的雅各比矩阵，B_k，t表示k时刻的f(ξ，u)相对于控制量u的雅各比矩阵，I为单位矩阵，T为离散时间，表示当前状态量与参考状态量的误差，表示当前控制量与参考控制量的误差；

令公式十三：

其中表示人为设置的状态参数在离散线性模型中引入的控制增量，表示当前状态量的误差，表示上一时刻的控制量的误差；

将所述公式十二表述为它的等价形式，即公式十四：

其中η(k)为k时刻的输出量，控制量的误差的增量， 分别为A_t，k，B_t，k的增广矩阵，通过设置矩阵C_t，k使输出量只包含

假设预测时域为N_p，控制时域为N_c，得到预测状态误差参数和输出η(k+N_p)的表达式分别为

公式十五：

公式十六：

令公式十七：

其中表示预测时域内的输出矩阵，ψ_t表示预测时域内的状态系数矩阵，ΔU(t)表示预测时域内的控制序列，Θ_t表示预测时域内的控制增量矩阵，则预测时域方程的紧凑形式表示为公式十八：

S23、对预测时域方程进行优化，得到MPC问题，其过程为：根据预测时域方程，得到整个预测时域的期望输出为公式十九：

其中η_ref(t+1)表示下一时刻的参考输出量，根据整个预测时域的期望输出得到MPC问题表示为公式二十、公式二十一：

其中，ΔU_min为控制增量的最小值，ΔU_max为控制增量的最大值。U_min为控制总量的最小值，U_max为控制总量的最大值。为大小为N_c×N_c的单位矩阵，E为单位向量。Q表示输出量误差的权重矩阵，R表示控制增量的权重矩阵。ε为防止目标函数在控制范围内无解而添加的松弛因子，ρ为松弛因子的权重系数；

S24、求解MPC问题模型，实现对无人驾驶车辆的路径跟踪，其过程为：将所述公式二十转换为二次规划问题，使用内点法可以快速完成二次规划问题的求解，在每个控制周期会得到一系列的最优控制序列，即公式二十二：

最优控制序列只有第一个矢量作为实际输入，并在每一个控制周期重复如上步骤进行计算，一直循环直到完成跟踪控制任务；模型预测控制的原理主要是在每一个采样时刻，根据获得的当前测量信息，在线求解一个有限时间开环优化问题，然后将得到的控制序列的第一个元素作用于被控对象，并在接下来的每个采样时刻重复上述操作，与其他控制方法相比，模型预测控制在轨迹跟踪方面具有优势，它可以在多变量控制框架下同时处理状态变量的软硬约束和控制输入与输出，提高了性能，实现了稳定性。

实施例2：

(如图7)，通过对本算法(PADC)1、传统MPC算法2(MPC-Method)、传统具有预览特性的MPC算法(Preview-MPC)3分别对标准路径跟踪曲线4(Expected trajectory)进行路径跟踪结果对比，可得：

传统MPC算法2与标准路径跟踪曲线4相比，误差较大；

传统具有预览特性的MPC算法3(与本算法1相比，未采用所述步骤S21的减速算法)，与标准路径跟踪曲线4相比，误差中等；

本算法1与标准路径跟踪曲线4相比，误差较小；

可见，本发明提供的算法解决了路径跟踪过程中，车辆在高速行驶过弯状况下由于速度过快且转向不足导致在弯道处与目标轨迹相差过大的情况，并且解决了传统的具有预览特性的MPC算法的提前转向过早的问题。提升了车辆的跟踪性能并保证了通过性和安全性。

这里说明的设备数量和处理规模是用来简化本发明的说明的。对本发明的应用、修改和变化对本领域的技术人员来说是显而易见的。尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用，它完全可以被适用于各种适合本发明的领域，对于熟悉本领域的人员而言，可容易地实现另外的修改，因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。

Claims (2)

1.一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法，其特征在于，包括：

步骤一、计算车辆的预览距离和参考航向角；

步骤二、基于预览距离计算车辆的减速距离，且根据道路的目标转向速度，得到车辆在入弯前后实时应被作用的纵向速度与车辆实时位置之间的关系判定式；

步骤三、基于参考航向角和纵向速度建立车辆运动学模型，得到预测时域方程，优化后得到MPC问题并求解，实现对无人驾驶车辆的路径跟踪；

其中，步骤一中，其包括：

S11、根据当前的车辆纵向速度和横向误差，代入预览距离计算公式得到预览距离，将预览距离映射到参考路径上得到对应参考点，其过程为：根据预览距离计算公式，即公式一，得到预览距离N_z：

T_max＝0.04V

T＝T_max(1-β)

N_z＝VT

其中：V为当前车辆纵向速度，T_max为最大预览时间，T为预览时间，y^E为横向误差，为能接受的最大横向误差；将得到的预览距离N_z投影到参考路径上得到对应参考点；

S12、根据计算对应参考点的道路曲率得到理想的转弯半径，得到参考航向角，其过程为：根据曲率计算公式，即公式二，得到对应参考点的曲率K：

其中：y″为道路函数的二阶导数，y′为道路函数的一阶导数；

根据曲率与转弯半径成反比的关系，即公式三，得到对应参考点的理想转弯半径R：

根据公式四，得到参考航向角

其中：L为车辆前轴和后轴之间的距离；

步骤二中，其包括：

S21、通过当前车速和预览距离内的道路的最大曲率计算出减速距离，其过程为：根据公式五，得到减速距离N_a：

其中：A_o为最大减速距离，r₀为减速系数，ρ_m为普通道路最大曲率，为N_z内道路曲率最大值；减速距离的终点是道路中曲率大于0.005的第一个点；

S22、将预览距离内的平均曲率和平均曲率变化率作为模糊推理系统的输入量，目标转向速度作为模糊推理系统的输出量，其过程为：根据实际道路最小转弯半径，计算得到道路平均曲率和道路曲率变化率的取值范围；通过地面附着力限制公式可知|a_y|≤μg，其中：a_y为车辆y方向的加速度，且μ为地面附着系数，g为车体重量，V_r为目标转向速度；根据车辆操控安全的a_y规定值确定目标转向速度V_r的最小值；根据城市道路转弯路口设计车速确定目标转向速度V_r的最大值，从而确定了目标转向速度V_r的取值范围；根据模糊推理系统的输出结果，对车辆的目标转向速度V_r在取值范围内进行实时调整；

S23、建立车辆在入弯前后实时应被作用的纵向速度与当前车辆位置情况之间的判定关系式，其过程为：

建立车辆的纵向速度V_k的关系判定式：

其中：V_a为参考车速，V_r为目标转向速度，为前方道路平均曲率，LOC为车辆在行驶过程中不断地计算当前车辆位置与减速距离的终点的距离；确定车辆的纵向速度V_k后，作用于车辆。

2.如权利要求1所述的一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法，其特征在于，步骤三中，其包括：

S31、建立无人驾驶车辆的运动学模型，其过程为：在惯性坐标系下，建立智能车辆的车辆运动学模型为：

其中：(x，y)为车辆后轴轴心坐标，为步骤S12中的参考航向角，δ_f为车辆前轮偏角，l为车辆轴距，V_k为步骤S23中的车辆纵向速度，与V_k在每次MPC计算前更新，参数上的符号“”表示该参数的一阶导数；

该模型可被表达为更一般的形式，即公式八：

其中：系统的状态变量可表示为系统的控制变量可表示为u＝[δ_f]，f(·)为车辆运动学模型函数；

S32、将车辆的非线性运动学模型进行线性离散化，得到预测时域方程，其过程为：参考系统的任意时刻的状态和控制量满足公式九：

在任意参考路径点(ξ_r，u_r)处，对公式九进行泰勒展开，忽略高阶项，只保留一阶项得公式十：

其中A_t为f(ξ，u)相对于状态量ξ的雅各比矩阵，B_t为f(ξ，u)相对于控制量u的雅各比矩阵，ξ_r表示参考点的状态量，此处ξ_r中的的值为步骤S12所求得的的值，u_r表示参考点的控制量；

将公式十与公式九相减得到车辆的线性误差模型，即公式十一：

其中，表示当前状态量与参考状态量的差，表示当前控制量与参考控制量的差；连续的状态方程不适用于设计模型预测控制器，采用单步欧拉法对线性误差模型离散化后，离散线性误差模型，即公式十二为：

其中A_k,t＝I+TA_t,B_k,t＝TB_t，A_k，t表示k时刻的f(ξ，u)相对于状态量ξ的雅各比矩阵，B_k，t表示k时刻的f(ξ，u)相对于控制量u的雅各比矩阵，I为单位矩阵，T为离散时间，表示当前状态量与参考状态量的误差，表示当前控制量与参考控制量的误差；

令公式十三：

其中表示人为设置的状态参数在离散线性模型中引入的控制增量，表示当前状态量的误差，表示上一时刻的控制量的误差；

将所述公式十二表述为它的等价形式，即公式十四：

其中η(k)为k时刻的输出量，控制量的误差的增量， 分别为A_k,t,B_k,t的增广矩阵，通过设置矩阵C_t，k使输出量只包含

假设预测时域为N_p，控制时域为N_c，得到预测状态误差参数和输出η(k+N_p)的表达式分别为

公式十五：

公式十六：

令公式十七：

其中表示预测时域内的输出矩阵，ψ_t表示预测时域内的状态系数矩阵，ΔU(t)表示预测时域内的控制序列，Θ_t表示预测时域内的控制增量矩阵，则预测时域方程的紧凑形式表示为公式十八：

S23、对预测时域方程进行优化，得到MPC问题，其过程为：根据预测时域方程，得到整个预测时域的期望输出为公式十九：

其中η_ref(t+1)表示下一时刻的参考输出量，根据整个预测时域的期望输出得到MPC问题表示为公式二十、公式二十一：

s.t.MΔU≤Ψ

ΔU_min≤ΔU_t≤ΔU_max

其中，ΔU_min为控制增量的最小值，ΔU_max为控制增量的最大值；U_min为控制总量的最小值，U_max为控制总量的最大值；为大小为N_c×N_c的单位矩阵，E为单位向量；Q表示输出量误差的权重矩阵，R表示控制增量的权重矩阵；ε为防止目标函数在控制范围内无解而添加的松弛因子，ρ为松弛因子的权重系数；

S24、求解MPC问题模型，实现对无人驾驶车辆的路径跟踪，其过程为：将所述公式二十转换为二次规划问题，使用内点法可以快速完成二次规划问题的求解，在每个控制周期会得到一系列的最优控制序列，即公式二十二：

最优控制序列只有第一个矢量作为实际输入，并在每一个控制周期重复如上步骤进行计算，一直循环直到完成跟踪控制任务。