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

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

Info

Publication number
CN114185272A
CN114185272A CN202111469816.5A CN202111469816A CN114185272A CN 114185272 A CN114185272 A CN 114185272A CN 202111469816 A CN202111469816 A CN 202111469816A CN 114185272 A CN114185272 A CN 114185272A
Authority
CN
China
Prior art keywords
vehicle
formula
control
speed
preview
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202111469816.5A
Other languages
English (en)
Inventor
蒋涛
陈谢天
黄小燕
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chengdu University of Information Technology
Original Assignee
Chengdu University of Information Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Chengdu University of Information Technology filed Critical Chengdu University of Information Technology
Priority to CN202111469816.5A priority Critical patent/CN114185272A/zh
Publication of CN114185272A publication Critical patent/CN114185272A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

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

Description

一种基于模型预测控制的具有预览特性的自适应速度智能车 辆路径跟踪方法
技术领域
本发明涉及无人驾驶车辆路径跟踪控制技术领域，具体为一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法。
背景技术
随着电控技术的快速发展和对汽车主动安全功能的需求日益增长，自动驾驶汽车被认为是未来汽车工业发展的重点。对于自动驾驶汽车来说，要解决的最重要的问题就是如何控制车辆沿着期望的路径行驶。轨迹跟踪采用视觉伺服控制算法、滑模控制算法、模糊控制算法、LQR算法和模型预测控制算法。虽然轨迹跟踪的研究很多，但转向控制仍然是一个具有挑战性的领域，涉及如何平衡车辆的横向稳定性和跟踪精度。与其他控制方法相比，模型预测控制在轨迹跟踪方面具有优势，它可以在多变量控制框架下同时处理状态变量的软硬约束和控制输入与输出，提高了性能，实现了稳定性。
模型预测控制的原理主要是在每一个采样时刻，根据获得的当前测量信息，在线求解一个有限时间开环优化问题，然后将得到的控制序列的第一个元素作用于被控对象，并在接下来的每个采样时刻重复上述操作。虽然模型预测控制算法在路径跟踪上表现较好，但依然存在一定的跟踪误差和车辆稳定性问题，为了解决上述问题，本文提出了一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法，提出的方法主要分为车辆纵向速度控制算法和基于车辆纵向速度和横向误差的MPC预览算法两部分，MPC预览算法提升了跟踪性能，车辆纵向速度控制算法在保证跟踪性能情况下提高了车辆的安全性和通过性。车辆纵向速度控制算法主要是基于模糊控制器自适应调整车辆转向速度，减速距离算法作为补充。
发明内容
本发明的一个目的是解决至少上述问题和/或缺陷，并提供至少后面将说明的优点。为了实现根据本发明的这些目的和其它优点，提供了一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法，包括：
步骤一、计算车辆的预览距离和参考航向角；
步骤二、基于预览距离计算车辆的减速距离，且根据道路的目标转向速度，得到车辆在入弯前后实时应被作用的纵向速度与车辆实时位置之间的关系判定式；
步骤三、基于参考航向角和纵向速度建立车辆运动学模型，得到预测时域方程，优化后得到MPC问题并求解，实现对无人驾驶车辆的路径跟踪。
优选的是，其中，步骤一中，其包括：S11、根据当前的车辆纵向速度和横向误差，代入预览距离计算公式得到预览距离，将预览距离映射到参考路径上得到对应参考点，其过程为：根据预览距离计算公式，即公式一，得到预览距离Nz：
Tmax＝0.04V
T＝Tmax(1-β)
Nz＝VT
Figure BDA0003391219550000021
其中：V为当前车辆纵向速度，Tmax为最大预览时间，T为预览时间，yE为横向误差，
Figure BDA0003391219550000022
为能接受的最大横向误差；将得到的预览距离Nz投影到参考路径上得到对应参考点；
S12、根据计算对应参考点的道路曲率得到理想的转弯半径，得到参考航向角，其过程为：根据曲率计算公式，即公式二，得到对应参考点的曲率K：
Figure BDA0003391219550000023
其中：y″为道路函数的二阶导数，y′为道路函数的一阶导数；
根据曲率与转弯半径成反比的关系，即公式三，得到对应参考点的理想转弯半径R：
Figure BDA0003391219550000031
根据公式四，得到参考航向角
Figure BDA0003391219550000032
Figure BDA0003391219550000033
其中：L为车辆前轴和后轴之间的距离；
优选的是，其中，步骤二中，其包括：
S21、通过当前车速和预览距离内的道路的最大曲率计算出减速距离，其过程为：根据公式五，得到减速距离Na：
Figure BDA0003391219550000034
其中：Ao为最大减速距离，r0为减速系数，ρm为普通道路最大曲率，
Figure BDA0003391219550000035
为Na内道路曲率最大值；减速距离的终点是道路中曲率大于0.005的第一个点；
S22、将预览距离内的平均曲率和平均曲率变化率作为模糊推理系统的输入量，目标转向速度作为模糊推理系统的输出量，其过程为：根据实际道路最小转弯半径，计算得到道路平均曲率
Figure BDA0003391219550000036
和道路曲率变化率
Figure BDA0003391219550000037
的取值范围；通过地面附着力限制公式
Figure BDA0003391219550000038
可知|ay|≤μg，其中：ay为车辆y方向的加速度，且
Figure BDA0003391219550000039
μ为地面附着系数，g为车体重量，Vr为目标转向速度；根据车辆操控安全的ay规定值确定目标转向速度Vr的最小值；根据城市道路转弯路口设计车速确定目标转向速度Vr的最大值，从而确定了目标转向速度Vr的取值范围；根据模糊推理系统的输出结果，使车辆在预览距离Nz内对目标转向速度Vr在取值范围内进行实时调整；
S23、建立车辆在入弯前后实时应被作用的纵向速度与当前车辆位置情况之间的判定关系式，其过程为：
建立车辆的纵向速度Vk的关系判定式：
Figure BDA0003391219550000041
其中：Va为参考车速，Vr为目标转向速度，
Figure BDA0003391219550000042
为前方道路平均曲率，LOC为车辆在行驶过程中不断地计算当前车辆位置与减速距离的终点的距离；确定车辆的纵向速度Vk后，作用于车辆。
优选的是，其中，步骤三中，其包括：
S31、建立无人驾驶车辆的运动学模型，其过程为：在惯性坐标系下，建立智能车辆的车辆运动学模型为：
Figure BDA0003391219550000043
其中：(x，y)为车辆后轴轴心坐标，
Figure BDA0003391219550000044
为步骤S12中的参考航向角，δf为车辆前轮偏角，l为车辆轴距，Vk为步骤S23中的车辆纵向速度，且在每次MPC计算前更新，参数上的符号“·”表示该参数的一阶导数；
该模型可被表达为更一般的形式，即公式八：
Figure BDA0003391219550000045
其中：系统的状态变量可表示为
Figure BDA0003391219550000046
系统的控制变量可表示为u＝[δf]，f(·)为车辆运动学模型函数；
S32、将车辆的非线性运动学模型进行线性离散化，得到预测时域方程，其过程为：参考系统的任意时刻的状态和控制量满足公式九：
Figure BDA0003391219550000051
在任意参考路径点(ξr，ur)处，对公式九进行泰勒展开，忽略高阶项，只保留一阶项得公式十：
ξ＝f(ξ，u)+At(ξ-ξr)+Bt(u-ur)
Figure BDA0003391219550000052
其中At为f(ξ，u)相对于状态量ξ的雅各比矩阵，Bt为f(ξ，u)相对于控制量u的雅各比矩阵，ξr表示参考点的状态量，此处ξr中的
Figure BDA0003391219550000053
的值为步骤S12所求得的
Figure BDA0003391219550000054
的值，ur表示参考点的控制量；
将公式十与公式九相减得到车辆的线性误差模型，即公式十一：
Figure BDA0003391219550000055
其中，
Figure BDA0003391219550000056
表示当前状态量与参考状态量的差，
Figure BDA0003391219550000057
表示当前控制量与参考控制量的差。连续的状态方程不适用于设计模型预测控制器，采用单步欧拉法对线性误差模型离散化后，离散线性误差模型，即公式十二为：
Figure BDA0003391219550000058
其中At，k＝I+TAt，Bt，k＝TBt，Ak，t表示k时刻的f(ξ，u)相对于状态量ξ的雅各比矩阵，Bk，t表示k时刻的f(ξ，u)相对于控制量u的雅各比矩阵，I为单位矩阵，T为离散时间，
Figure BDA0003391219550000059
表示当前状态量与参考状态量的误差，
Figure BDA00033912195500000510
表示当前控制量与参考控制量的误差；
令公式十三：
Figure BDA00033912195500000511
其中
Figure BDA00033912195500000512
表示人为设置的状态参数在离散线性模型中引入的控制增量，
Figure BDA00033912195500000513
表示当前状态量的误差，
Figure BDA00033912195500000514
表示上一时刻的控制量的误差；
将所述公式十二表述为它的等价形式，即公式十四：
Figure BDA0003391219550000061
Figure BDA0003391219550000062
其中η(k)为k时刻的输出量，
Figure BDA0003391219550000063
控制量的误差的增量，
Figure BDA0003391219550000064
Figure BDA0003391219550000065
分别为At，k，Bt，k的增广矩阵，通过设置矩阵Ct，k使输出量只包含
Figure BDA0003391219550000066
假设预测时域为Np，控制时域为Nc，得到预测状态误差参数
Figure BDA0003391219550000067
和输出η(k+Np)的表达式分别为
公式十五：
Figure BDA0003391219550000068
公式十六：
Figure BDA0003391219550000069
令公式十七：
Figure BDA0003391219550000071
Figure BDA0003391219550000072
Figure BDA0003391219550000073
其中
Figure BDA0003391219550000074
表示预测时域内的输出矩阵，ψt表示预测时域内的状态系数矩阵，ΔU(t)表示预测时域内的控制序列，Θt表示预测时域内的控制增量矩阵，则预测时域方程的紧凑形式表示为公式十八：
Figure BDA0003391219550000075
S23、对预测时域方程进行优化，得到MPC问题，其过程为：根据预测时域方程，得到整个预测时域的期望输出
Figure BDA0003391219550000076
为公式十九：
Figure BDA0003391219550000077
其中ηref(t+1)表示下一时刻的参考输出量，根据整个预测时域的期望输出得到MPC问题表示为公式二十、公式二十一：
Figure BDA0003391219550000078
Figure BDA0003391219550000081
其中，ΔUmin为控制增量的最小值，ΔUmax为控制增量的最大值。Umin为控制总量的最小值，Umax为控制总量的最大值。
Figure BDA0003391219550000083
为大小为Nc×Nc的单位矩阵，E为单位向量。Q表示输出量误差的权重矩阵，R表示控制增量的权重矩阵。ε为防止目标函数在控制范围内无解而添加的松弛因子，ρ为松弛因子的权重系数；
S24、求解MPC问题模型，实现对无人驾驶车辆的路径跟踪，其过程为：将所述公式二十转换为二次规划问题，使用内点法可以快速完成二次规划问题的求解，在每个控制周期会得到一系列的最优控制序列，即公式二十二：
Figure BDA0003391219550000082
最优控制序列只有第一个矢量作为实际输入，并在每一个控制周期重复如上步骤进行计算，一直循环直到完成跟踪控制任务。
本发明至少包括以下有益效果：
本发明提出了一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法(PADC)，首先根据当前车辆纵向速度和当前车辆横向误差计算预览距离，其次由预览距离投影到参考路径上得到对应的参考点并计算该点的道路曲率，根据道路曲率得到理想的转弯半径由提出的公式得到当前MPC的参考航向角。接下来根据计算预览距离内的平均曲率和平均曲率变化率得到目标转向速度，并且由提出的公式得到减速距离再建立车辆纵向速度与车辆当前所处位置之间的关系，将目标车辆纵向速度施加在车辆上。本发明用于解决路径跟踪过程中，车辆在高速行驶过弯状况下由于速度过快且转向不足导致在弯道处与目标轨迹相差过大的情况，并且解决了传统的具有预览特性的MPC算法的提前转向过早的问题。提升了车辆的跟踪性能并保证了通过性和安全性。
附图说明
图1为本发明的算法流程图；
图2为本发明的车辆预览距离参考点示意图；
图3为本发明的车辆减速过程示意图；
图4为本发明的
Figure BDA0003391219550000091
和Vr的隶属度函数图；
图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、根据当前的车辆纵向速度和横向误差，代入预览距离计算公式得到预览距离，将预览距离映射到参考路径上得到对应参考点，其过程为：根据预览距离计算公式，即公式一，得到预览距离Nz：
Tmax＝0.04V
T＝Tmax(1-β)
Nz＝VT
Figure BDA0003391219550000111
其中：V为当前车辆纵向速度，Tmax为最大预览时间，T为预览时间，yE为横向误差，
Figure BDA0003391219550000112
为能接受的最大横向误差；将得到的预览距离Nz投影到参考路径上得到对应参考点；若当前横向误差yE大于或等于
Figure BDA0003391219550000113
对，控制器的首要任务应为尽量减小车辆与目标跟踪路径的误差，此时转向预览距离Nz应为0。
S12、根据计算对应参考点的道路曲率得到理想的转弯半径，得到参考航向角，其过程为：根据曲率计算公式，即公式二，得到对应参考点的曲率K：
Figure BDA0003391219550000114
其中：y″为道路函数的二阶导数，y′为道路函数的一阶导数；
根据曲率与转弯半径成反比的关系，即公式三，得到对应参考点的理想转弯半径R：
Figure BDA0003391219550000115
根据公式四，得到参考航向角
Figure BDA0003391219550000116
Figure BDA0003391219550000121
其中：L为车辆前轴和后轴之间的距离；
(如图3)步骤二、基于预览距离计算车辆的减速距离，且根据道路的目标转向速度，得到车辆在入弯前后实时应被作用的纵向速度与车辆实时位置之间的关系判定式，其包括：
S21、通过当前车速和预览距离内的道路的最大曲率计算出减速距离，其过程为：根据公式五，得到减速距离Na：
Figure BDA0003391219550000122
其中：Ao为最大减速距离，r0为减速系数，ρm为普通道路最大曲率，为0.166，
Figure BDA0003391219550000123
为Na内道路曲率最大值；减速距离的终点是道路中曲率大于0.005的第一个点；由于驾驶员在通过弯道时，不会在进入弯道后才减速，而是会在距离弯道一定距离时开始减速，当进入弯道时车速已经减速至目标转弯车速。在实际中，车辆通过曲率较小的弯道时为保证通过效率一般不减速，通常设计时速80km/h的道路转弯半径R最小为220m，而城市中最高行驶速度为80km/h，因此曲率小于0.005的道路不进行减速操作，将弯道内第一个曲率大于等于0.005的点作为减速距离的终点。根据当前车速V和预览距离Na内的道路的最大曲率
Figure BDA0003391219550000124
通过公式五计算出减速距离；
S22、将预览距离内的平均曲率和平均曲率变化率作为模糊推理系统的输入量，目标转向速度作为模糊推理系统的输出量，其过程为：根据实际城市道路最小转弯半径为6m，计算得到道路平均曲率
Figure BDA0003391219550000125
的取值范围[0，0.166]，和道路曲率变化率
Figure BDA0003391219550000126
的取值范围[-0.166，0.166]；通过地面附着力限制公式
Figure BDA0003391219550000131
可知|ay|≤μg，其中：ay为车辆y方向的加速度，且
Figure BDA0003391219550000132
μ为地面附着系数，范围为[0.4，0.85]，g为车体重量，Vr为目标转向速度；根据车辆操控安全的ay规定最大值6m/s2确定目标转向速度Vr的最小值为6m/s；根据城市道路转弯路口设计车速最大值45km/h(12.5m/s)确定目标转向速度Vr的最大值为12.5m/s，从而确定了目标转向速度Vr的取值范围[6，12.5]；根据模糊推理系统的输出结果，对车辆的目标转向速度Vr在取值范围[6，12.5]内进行实时调整；
Figure BDA0003391219550000133
和
Figure BDA0003391219550000134
越小，说明前方道路弯道越平缓，因此的Vr值越大。当
Figure BDA0003391219550000135
较小，
Figure BDA0003391219550000136
增大时，说明前方道路弯道有变陡峭的可能，因此Vr值应适当减小。
Figure BDA0003391219550000137
和Vr的隶属度函数和Vr的模糊控制调节规则(如图4-5)。
S23、建立车辆在入弯前后实时应被作用的纵向速度与当前车辆位置情况之间的判定关系式，其过程为：
建立车辆的纵向速度Vk的关系判定式：
Figure BDA0003391219550000138
其中：Va为参考车速，Vr为目标转向速度，
Figure BDA0003391219550000139
为前方道路平均曲率，LOC为车辆在行驶过程中不断地计算当前车辆位置与减速距离的终点的距离；确定车辆的纵向速度Vk后，作用于车辆；当LOC≤Na时，车辆开始减速，在车辆到达减速距离的终点时，车速已降至目标转向速度。车辆在弯道内按照目标转向速度Vr行驶，直到前方道路平均曲率为0时车速恢复至参考车速Va。通过样的分阶段速度自适应调配，使车辆在过弯前后进行更平滑的减速，以最适当该弯道的转向速度进行转向，提升了车辆的跟踪性能并保证了通过性和安全性。
步骤三、基于参考航向角和纵向速度建立车辆运动学模型，得到预测时域方程，优化后得到MPC问题并求解，实现对无人驾驶车辆的路径跟踪，其包括：
(如图6)S31、建立无人驾驶车辆的运动学模型，其过程为：在惯性坐标系下，建立智能车辆的车辆运动学模型为：
Figure BDA0003391219550000141
其中：(x，y)为车辆后轴轴心坐标，
Figure BDA0003391219550000142
为步骤S12中的参考航向角，δf为车辆前轮偏角，l为车辆轴距，Vk为步骤S23中的车辆纵向速度，δf与Vk在每次MPC计算前更新，参数上的符号“·”表示该参数的一阶导数；
该模型可被表达为更一般的形式，即公式八：
Figure BDA0003391219550000143
其中：系统的状态变量可表示为
Figure BDA0003391219550000144
系统的控制变量可表示为u＝[δf]，f(·)为车辆运动学模型函数；步骤S31中无人驾驶车辆的车辆运动学模型基于以下假设：1.忽略车辆垂直于地面的运动。2.左前轮和右前轮角度相同。3.车身和悬架系统是刚性的。4.只考虑车辆前轮转动；
S32、将车辆的非线性运动学模型进行线性离散化，得到预测时域方程，其过程为：参考系统的任意时刻的状态和控制量满足公式九：
Figure BDA0003391219550000145
在任意参考路径点(ξr，ur)处，对公式九进行泰勒展开，忽略高阶项，只保留一阶项得公式十：
Figure BDA0003391219550000146
Figure BDA0003391219550000147
其中At为f(ξ，u)相对于状态量ξ的雅各比矩阵，Bt为f(ξ，u)相对于控制量u的雅各比矩阵，ξr表示参考点的状态量，此处ξr中的
Figure BDA0003391219550000151
的值为步骤S12所求得的
Figure BDA0003391219550000152
的值，ur表示参考点的控制量；
将公式十与公式九相减得到车辆的线性误差模型，即公式十一：
Figure BDA0003391219550000153
其中，
Figure BDA0003391219550000154
表示当前状态量与参考状态量的差，
Figure BDA0003391219550000155
表示当前控制量与参考控制量的差。连续的状态方程不适用于设计模型预测控制器，采用单步欧拉法对线性误差模型离散化后，离散线性误差模型，即公式十二为：
Figure BDA0003391219550000156
其中At，k＝I+TAt，Bt，k＝TBt，Ak，t表示k时刻的f(ξ，u)相对于状态量ξ的雅各比矩阵，Bk，t表示k时刻的f(ξ，u)相对于控制量u的雅各比矩阵，I为单位矩阵，T为离散时间，
Figure BDA0003391219550000157
表示当前状态量与参考状态量的误差，
Figure BDA0003391219550000158
表示当前控制量与参考控制量的误差；
令公式十三：
Figure BDA0003391219550000159
其中
Figure BDA00033912195500001510
表示人为设置的状态参数在离散线性模型中引入的控制增量，
Figure BDA00033912195500001511
表示当前状态量的误差，
Figure BDA00033912195500001512
表示上一时刻的控制量的误差；
将所述公式十二表述为它的等价形式，即公式十四：
Figure BDA00033912195500001513
Figure BDA00033912195500001514
其中η(k)为k时刻的输出量，
Figure BDA00033912195500001515
控制量的误差的增量，
Figure BDA0003391219550000161
Figure BDA0003391219550000162
分别为At，k，Bt，k的增广矩阵，通过设置矩阵Ct，k使输出量只包含
Figure BDA0003391219550000163
假设预测时域为Np，控制时域为Nc，得到预测状态误差参数
Figure BDA0003391219550000164
和输出η(k+Np)的表达式分别为
公式十五：
Figure BDA0003391219550000165
公式十六：
Figure BDA0003391219550000166
令公式十七：
Figure BDA0003391219550000167
Figure BDA0003391219550000168
Figure BDA0003391219550000169
其中
Figure BDA00033912195500001610
表示预测时域内的输出矩阵，ψt表示预测时域内的状态系数矩阵，ΔU(t)表示预测时域内的控制序列，Θt表示预测时域内的控制增量矩阵，则预测时域方程的紧凑形式表示为公式十八：
Figure BDA0003391219550000171
S23、对预测时域方程进行优化，得到MPC问题，其过程为：根据预测时域方程，得到整个预测时域的期望输出
Figure BDA0003391219550000172
为公式十九：
Figure BDA0003391219550000173
其中ηref(t+1)表示下一时刻的参考输出量，根据整个预测时域的期望输出得到MPC问题表示为公式二十、公式二十一：
Figure BDA0003391219550000174
Figure BDA0003391219550000175
其中，ΔUmin为控制增量的最小值，ΔUmax为控制增量的最大值。Umin为控制总量的最小值，Umax为控制总量的最大值。
Figure BDA0003391219550000176
为大小为Nc×Nc的单位矩阵，E为单位向量。Q表示输出量误差的权重矩阵，R表示控制增量的权重矩阵。ε为防止目标函数在控制范围内无解而添加的松弛因子，ρ为松弛因子的权重系数；
S24、求解MPC问题模型，实现对无人驾驶车辆的路径跟踪，其过程为：将所述公式二十转换为二次规划问题，使用内点法可以快速完成二次规划问题的求解，在每个控制周期会得到一系列的最优控制序列，即公式二十二：
Figure BDA0003391219550000177
最优控制序列只有第一个矢量作为实际输入，并在每一个控制周期重复如上步骤进行计算，一直循环直到完成跟踪控制任务；模型预测控制的原理主要是在每一个采样时刻，根据获得的当前测量信息，在线求解一个有限时间开环优化问题，然后将得到的控制序列的第一个元素作用于被控对象，并在接下来的每个采样时刻重复上述操作，与其他控制方法相比，模型预测控制在轨迹跟踪方面具有优势，它可以在多变量控制框架下同时处理状态变量的软硬约束和控制输入与输出，提高了性能，实现了稳定性。
实施例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 (4)

1.一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法，其特征在于，包括：
步骤一、计算车辆的预览距离和参考航向角；
步骤二、基于预览距离计算车辆的减速距离，且根据道路的目标转向速度，得到车辆在入弯前后实时应被作用的纵向速度与车辆实时位置之间的关系判定式；
步骤三、基于参考航向角和纵向速度建立车辆运动学模型，得到预测时域方程，优化后得到MPC问题并求解，实现对无人驾驶车辆的路径跟踪。
2.如权利要求1所述的一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法，其特征在于，步骤一中，其包括：
S11、根据当前的车辆纵向速度和横向误差，代入预览距离计算公式得到预览距离，将预览距离映射到参考路径上得到对应参考点，其过程为：根据预览距离计算公式，即公式一，得到预览距离Nz：
Tmax＝0.04V
T＝Tmax(1-β)
Nz＝VT
Figure FDA0003391219540000011
其中：V为当前车辆纵向速度，Tmax为最大预览时间，T为预览时间，yE为横向误差，
Figure FDA0003391219540000012
为能接受的最大横向误差；将得到的预览距离Nz投影到参考路径上得到对应参考点；
S12、根据计算对应参考点的道路曲率得到理想的转弯半径，得到参考航向角，其过程为：根据曲率计算公式，即公式二，得到对应参考点的曲率K：
Figure FDA0003391219540000013
其中：y″为道路函数的二阶导数，y′为道路函数的一阶导数；
根据曲率与转弯半径成反比的关系，即公式三，得到对应参考点的理想转弯半径R：
Figure FDA0003391219540000021
根据公式四，得到参考航向角
Figure FDA0003391219540000029
Figure FDA0003391219540000022
其中：L为车辆前轴和后轴之间的距离。
3.如权利要求1所述的一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法，其特征在于，步骤二中，其包括：
S21、通过当前车速和预览距离内的道路的最大曲率计算出减速距离，其过程为：根据公式五，得到减速距离Na：
Figure FDA0003391219540000023
其中：Ao为最大减速距离，r0为减速系数，ρm为普通道路最大曲率，
Figure FDA0003391219540000024
为Nz内道路曲率最大值；减速距离的终点是道路中曲率大于0.005的第一个点；
S22、将预览距离内的平均曲率和平均曲率变化率作为模糊推理系统的输入量，目标转向速度作为模糊推理系统的输出量，其过程为：根据实际道路最小转弯半径，计算得到道路平均曲率
Figure FDA0003391219540000025
和道路曲率变化率
Figure FDA0003391219540000026
的取值范围；通过地面附着力限制公式
Figure FDA0003391219540000027
可知|ay|≤μg，其中：ay为车辆y方向的加速度，且
Figure FDA0003391219540000028
μ为地面附着系数，g为车体重量，Vr为目标转向速度；根据车辆操控安全的ay规定值确定目标转向速度Vr的最小值；根据城市道路转弯路口设计车速确定目标转向速度Vr的最大值，从而确定了目标转向速度Vr的取值范围；根据模糊推理系统的输出结果，对车辆的目标转向速度Vr在取值范围内进行实时调整；
S23、建立车辆在入弯前后实时应被作用的纵向速度与当前车辆位置情况之间的判定关系式，其过程为：
建立车辆的纵向速度Vk的关系判定式：
Figure FDA0003391219540000031
其中：Va为参考车速，Vr为目标转向速度，
Figure FDA0003391219540000032
为前方道路平均曲率，LOC为车辆在行驶过程中不断地计算当前车辆位置与减速距离的终点的距离；确定车辆的纵向速度Vk后，作用于车辆。
4.如权利要求1所述的一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法，其特征在于，步骤三中，其包括：
S31、建立无人驾驶车辆的运动学模型，其过程为：在惯性坐标系下，建立智能车辆的车辆运动学模型为：
Figure FDA0003391219540000033
其中：(x，y)为车辆后轴轴心坐标，
Figure FDA0003391219540000034
为步骤S12中的参考航向角，δf为车辆前轮偏角，l为车辆轴距，Vk为步骤S23中的车辆纵向速度，
Figure FDA0003391219540000035
与Vk在每次MPC计算前更新，参数上的符号“·”表示该参数的一阶导数；
该模型可被表达为更一般的形式，即公式八：
Figure FDA0003391219540000036
其中：系统的状态变量可表示为
Figure FDA0003391219540000037
系统的控制变量可表示为u＝[δf]，f(·)为车辆运动学模型函数；
S32、将车辆的非线性运动学模型进行线性离散化，得到预测时域方程，其过程为：参考系统的任意时刻的状态和控制量满足公式九：
Figure FDA0003391219540000041
在任意参考路径点(ξr，ur)处，对公式九进行泰勒展开，忽略高阶项，只保留一阶项得公式十：
Figure FDA0003391219540000042
Figure FDA0003391219540000043
其中At为f(ξ，u)相对于状态量ξ的雅各比矩阵，Bt为f(ξ，u)相对于控制量u的雅各比矩阵，ξr表示参考点的状态量，此处ξr中的
Figure FDA0003391219540000044
的值为步骤S12所求得的
Figure FDA0003391219540000045
的值，ur表示参考点的控制量；
将公式十与公式九相减得到车辆的线性误差模型，即公式十一：
Figure FDA0003391219540000046
其中，
Figure FDA00033912195400000412
表示当前状态量与参考状态量的差，
Figure FDA0003391219540000047
表示当前控制量与参考控制量的差；连续的状态方程不适用于设计模型预测控制器，采用单步欧拉法对线性误差模型离散化后，离散线性误差模型，即公式十二为：
Figure FDA0003391219540000048
其中At，k＝I+TAt，Bt，k＝TBt，Ak，t表示k时刻的f(ξ，u)相对于状态量ξ的雅各比矩阵，Bk，t表示k时刻的f(ξ，u)相对于控制量u的雅各比矩阵，I为单位矩阵，T为离散时间，
Figure FDA0003391219540000049
表示当前状态量与参考状态量的误差，
Figure FDA00033912195400000410
表示当前控制量与参考控制量的误差；
令公式十三：
Figure FDA00033912195400000411
其中
Figure FDA0003391219540000051
表示人为设置的状态参数在离散线性模型中引入的控制增量，
Figure FDA00033912195400000511
表示当前状态量的误差，
Figure FDA0003391219540000052
表示上一时刻的控制量的误差；
将所述公式十二表述为它的等价形式，即公式十四：
Figure FDA0003391219540000053
Figure FDA0003391219540000054
其中η(k)为k时刻的输出量，
Figure FDA00033912195400000512
控制量的误差的增量，
Figure FDA0003391219540000055
Figure FDA0003391219540000056
分别为At，k，Bt，k的增广矩阵，通过设置矩阵Ct，k使输出量只包含
Figure FDA0003391219540000057
假设预测时域为Np，控制时域为Nc，得到预测状态误差参数
Figure FDA0003391219540000058
和输出η(k+Np)的表达式分别为
公式十五：
Figure FDA0003391219540000059
公式十六：
Figure FDA00033912195400000510
令公式十七：
Figure FDA0003391219540000061
Figure FDA0003391219540000062
Figure FDA0003391219540000063
其中
Figure FDA0003391219540000064
表示预测时域内的输出矩阵，ψz表示预测时域内的状态系数矩阵，ΔU(t)表示预测时域内的控制序列，Θt表示预测时域内的控制增量矩阵，则预测时域方程的紧凑形式表示为公式十八：
Figure FDA0003391219540000065
S23、对预测时域方程进行优化，得到MPC问题，其过程为：根据预测时域方程，得到整个预测时域的期望输出
Figure FDA0003391219540000066
为公式十九：
Figure FDA0003391219540000067
其中ηref(t+1)表示下一时刻的参考输出量，根据整个预测时域的期望输出得到MPC问题表示为公式二十、公式二十一：
Figure FDA0003391219540000071
s.t.MΔU≤Ψ
ΔUmin≤ΔUt≤ΔUmax
Figure FDA0003391219540000072
其中，ΔUmin为控制增量的最小值，ΔUmax为控制增量的最大值；Umin为控制总量的最小值，Umax为控制总量的最大值；
Figure FDA0003391219540000074
为大小为Nc×Nc的单位矩阵，E为单位向量；Q表示输出量误差的权重矩阵，R表示控制增量的权重矩阵；ε为防止目标函数在控制范围内无解而添加的松弛因子，ρ为松弛因子的权重系数；
S24、求解MPC问题模型，实现对无人驾驶车辆的路径跟踪，其过程为：将所述公式二十转换为二次规划问题，使用内点法可以快速完成二次规划问题的求解，在每个控制周期会得到一系列的最优控制序列，即公式二十二：
Figure FDA0003391219540000073
最优控制序列只有第一个矢量作为实际输入，并在每一个控制周期重复如上步骤进行计算，一直循环直到完成跟踪控制任务。
CN202111469816.5A 2021-12-03 2021-12-03 一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法 Pending CN114185272A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111469816.5A CN114185272A (zh) 2021-12-03 2021-12-03 一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111469816.5A CN114185272A (zh) 2021-12-03 2021-12-03 一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法

Publications (1)

Publication Number Publication Date
CN114185272A true CN114185272A (zh) 2022-03-15

Family

ID=80542212

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111469816.5A Pending CN114185272A (zh) 2021-12-03 2021-12-03 一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法

Country Status (1)

Country Link
CN (1) CN114185272A (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114987549A (zh) * 2022-06-22 2022-09-02 小米汽车科技有限公司 车辆控制方法、装置、存储介质及车辆
CN115373287A (zh) * 2022-08-11 2022-11-22 中国农业大学 一种折腰转向拖拉机的自适应参数模型预测路径跟踪控制方法
CN115432009A (zh) * 2022-10-09 2022-12-06 海南大学 一种自动驾驶车辆轨迹跟踪控制系统
CN117048639A (zh) * 2023-10-12 2023-11-14 华东交通大学 一种车辆自适应路径控制方法、存储介质及计算机
CN117311153A (zh) * 2023-10-24 2023-12-29 酷哇科技有限公司 自适应模型预测非线性控制延边清扫控制跟踪方法和系统
CN117360544A (zh) * 2023-11-14 2024-01-09 海南大学 一种基于drl-mpc的自动驾驶车辆横向控制方法
CN115373287B (zh) * 2022-08-11 2024-07-05 中国农业大学 一种折腰转向拖拉机的自适应参数模型预测路径跟踪控制方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130054128A1 (en) * 2011-08-31 2013-02-28 GM Global Technology Operations LLC System and method for collision avoidance maneuver path determination with jerk limit
CN109318905A (zh) * 2018-08-22 2019-02-12 江苏大学 一种智能汽车路径跟踪混合控制方法
CN113359757A (zh) * 2021-06-30 2021-09-07 湖北汽车工业学院 一种改进型混合a*算法的无人驾驶车辆路径规划与轨迹跟踪方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130054128A1 (en) * 2011-08-31 2013-02-28 GM Global Technology Operations LLC System and method for collision avoidance maneuver path determination with jerk limit
CN109318905A (zh) * 2018-08-22 2019-02-12 江苏大学 一种智能汽车路径跟踪混合控制方法
CN113359757A (zh) * 2021-06-30 2021-09-07 湖北汽车工业学院 一种改进型混合a*算法的无人驾驶车辆路径规划与轨迹跟踪方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
CHANGHUA DAI: "Path Tracking Control Based on Model Predictive Control With Adaptive Preview Characteristics and Speed-Assisted Constraint", 《IEEE ACCESS》, 21 October 2020 (2020-10-21), pages 2 - 3 *
QIANGQIANG YAO: "A Model Predictive Controller with Longitudinal Speed Compensation for Autonomous Vehicle Path Tracking", 《APPLIED SCIENCES》, 6 November 2019 (2019-11-06) *
孟宇;甘鑫;白国星;: "基于预瞄距离的地下矿用铰接车路径跟踪预测控制", 工程科学学报, no. 05, 18 April 2019 (2019-04-18) *
牛前: "轮式移动机器人的动力学建模及路径跟踪控制", 《现代机械》, 31 December 2020 (2020-12-31) *
黄小燕: "Correntropy based model predictive controller with multi-constraints for robust path trajectory tracking of self-driving vehicle", 《SCIENCEDIRECT》, 17 May 2023 (2023-05-17) *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114987549A (zh) * 2022-06-22 2022-09-02 小米汽车科技有限公司 车辆控制方法、装置、存储介质及车辆
CN115373287A (zh) * 2022-08-11 2022-11-22 中国农业大学 一种折腰转向拖拉机的自适应参数模型预测路径跟踪控制方法
CN115373287B (zh) * 2022-08-11 2024-07-05 中国农业大学 一种折腰转向拖拉机的自适应参数模型预测路径跟踪控制方法
CN115432009A (zh) * 2022-10-09 2022-12-06 海南大学 一种自动驾驶车辆轨迹跟踪控制系统
CN115432009B (zh) * 2022-10-09 2023-09-05 海南大学 一种自动驾驶车辆轨迹跟踪控制系统
CN117048639A (zh) * 2023-10-12 2023-11-14 华东交通大学 一种车辆自适应路径控制方法、存储介质及计算机
CN117048639B (zh) * 2023-10-12 2024-01-23 华东交通大学 一种车辆自适应路径控制方法、存储介质及计算机
CN117311153A (zh) * 2023-10-24 2023-12-29 酷哇科技有限公司 自适应模型预测非线性控制延边清扫控制跟踪方法和系统
CN117360544A (zh) * 2023-11-14 2024-01-09 海南大学 一种基于drl-mpc的自动驾驶车辆横向控制方法

Similar Documents

Publication Publication Date Title
CN114185272A (zh) 一种基于模型预测控制的具有预览特性的自适应速度智能车辆路径跟踪方法
CN111890951B (zh) 智能电动汽车轨迹跟踪与运动控制方法
CN111413966B (zh) 一种递进式模型预测无人驾驶规划跟踪协同控制方法
CN111497826B (zh) 一种电动汽车横摆稳定性协调控制方法及系统
CN106372758B (zh) 一种辅助泊车系统的路径跟随方法及装置
CN112622903B (zh) 一种车辆跟随驾驶环境下自主车辆的纵向和横向控制方法
CN111688704B (zh) 一种基于驾驶状态预测的人机力矩协同控制方法
Wurts et al. Collision imminent steering at high speed using nonlinear model predictive control
CN113753080B (zh) 一种自动驾驶汽车横向运动自适应参数控制方法
Xu et al. Model predictive control for lane keeping system in autonomous vehicle
CN110262229B (zh) 基于mpc的车辆自适应路径追踪方法
CN111959527B (zh) 一种基于转角优化序列的汽车路径跟踪控制方法
Vivek et al. A comparative study of Stanley, LQR and MPC controllers for path tracking application (ADAS/AD)
CN111123701B (zh) 基于管道预测模型的自动驾驶路径跟踪抗干扰控制方法
CN112904849A (zh) 一体式自动驾驶汽车换道轨迹规划和跟踪控制方法及系统
CN112578672B (zh) 基于底盘非线性的无人驾驶汽车轨迹控制系统及其轨迹控制方法
CN116560371A (zh) 基于自适应模型预测控制的自动驾驶车辆路径跟踪方法
CN113911106A (zh) 基于博弈论的商用车横向轨迹跟随与稳定性协同控制方法
CN115421499A (zh) 具有转向补偿功能的智能车辆路径跟踪方法
CN114779641A (zh) 基于新航向误差定义的环境自适应mpc路径追踪控制方法
CN111845738B (zh) 基于双模型复合的车辆路径跟踪控制方法
CN114326709A (zh) 一种基于模型预测控制的具有预览特性的智能车辆路径跟踪方法
CN111413979B (zh) 一种基于快速模型预测的汽车轨迹跟踪控制方法
CN116834754A (zh) 一种自动驾驶车辆自适应调速的横纵协同控制方法
CN115933662A (zh) 一种基于自适应模型预测控制的智能汽车轨迹跟踪和稳定性控制系统及方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination