CN118144780A - 一种自适应权重系数mpc的路径跟踪控制方法和系统 - Google Patents

Abstract

本发明公开了一种自适应权重系数MPC的路径跟踪控制方法和系统。基于三自由度动力学模型和MPC算法建立车辆运行状态参数预测模型；根据从车辆获取的车辆状态参数的测量值采用滤波算法对所述车辆运行状态参数预测模型输出的下一时刻的车辆运行状态参数进行优化，得到优化后的车辆运行状态参数预测模型；求解MPC算法中的目标函数，得到下一时刻的前轮转角增量，将其作为调整量发送至被控车辆，从而完成路径跟踪控制。本发明抗干扰性强，降低外界噪声对传感器的影响；可以根据行驶状况自适应修正MPC控制器目标函数的权重，跟踪效果更加精准。

Description

一种自适应权重系数MPC的路径跟踪控制方法和系统

技术领域

本发明属于智能驾驶技术领域，具体涉及一种自适应权重系数MPC的路径跟踪控制方法和系统。

背景技术

随着智能驾驶技术的进步，越来越多的乘用车带有辅助驾驶功能，在这其中，路径跟踪是保证车辆按照预定轨迹行驶的核心功能，路径跟踪主要通过车辆的横向控制完成，而传统的PID(Proportional-Integral-Derivative)控制不依赖精确的车辆系统模型，计算速度快但参数整定困难，且受到复杂多变的道路环境以及工况改变的影响，其稳定性和跟踪效果较差。目前针对路径跟踪问题，常采用预瞄控制、LQR控制、模型预测控制等智能控制算法。要实现精确的跟踪控制，需要考虑车辆数学模型与实际系统中的约束、干扰等问题，对于解决多目标优化和复杂约束问题，具有其独特的优势。

现有技术之一为基于MPC算法，通过建立车辆模型，并预测模型-滚动优化-反馈校正，得到最优控制序列输入车辆中控制车辆对目标轨迹进行跟踪。但该技术方案不能避免因传感器、执行器等部件的测量噪声与过程噪声而产生的外部干扰；而且在不同路况和工况下，无法进行自适应调节，不具有鲁棒性。

发明内容

为了能解决传统MPC(Model Predictive Control，模型预测控制)不能自适应精确跟随和传感器、执行器等部件的噪声干扰问题，本发明提出一种自适应权重系数MPC的路径跟踪控制方法和系统。

实现本发明目的之一的一种自适应权重系数MPC的路径跟踪控制方法，包括：

S1、基于三自由度动力学模型和MPC算法建立车辆运行状态参数预测模型，所述车辆运行状态参数预测模型用于根据当前时刻的车辆运行状态参数和控制量预测下一时刻的车辆运行状态参数；所述车辆运行状态参数包括：纵向速度、横向速度、车辆横摆角、横摆角速度、横向距离、纵向距离；所述控制量包括前轮转角；

S2、根据从车辆获取的车辆状态参数的测量值采用滤波算法对所述车辆运行状态参数预测模型输出的下一时刻的车辆运行状态参数进行优化，得到优化后的车辆运行状态参数预测模型；所述车辆获取的车辆状态参数的测量值可以来自于车辆底盘系统；

S3、求解MPC算法中的目标函数，得到下一时刻的前轮转角增量，将其作为调整量发送至被控车辆，从而完成路径跟踪控制。

进一步地，所述步骤S3前，还包括：

采用模糊推理系统对MPC算法中的目标函数中的第一权重系数矩阵Q_y和第二权重系数矩阵R_u进行优化；所述第一权重系数矩阵Q_y为车辆预测状态参数到期望轨迹对应参数的偏差值的权重系数矩阵；第二权重系数矩阵R_u为前轮转角控制增量的权重系数矩阵。

进一步地，对第一权重系数矩阵Q_y和第二权重系数矩阵R_u进行优化的方法包括：

选取车辆的横向偏差e_y和路径曲率ρ_c作为模糊推理系统的输入变量；

对所述模糊推理系统进行计算，得到ΔQ_y和ΔR_u；

根据下式对ΔQ_y和ΔR_u进行计算，得到第一权重系数矩阵Q_y和第二权重系数矩阵R_u；

式中：

Q_yo为设定的系数矩阵；

R_yo为设定值。

进一步地，所述步骤S2包括：

根据上一时刻的车辆运行状态参数的最优估计值和车辆前轮转角得到当前时刻的车辆运行状态参数的先验估计值；

根据当前时刻的车辆运行状态参数的先验估计值得到当前时刻的车辆运行状态参数的最优估计值，所述最优估计值作为优化后的车辆运行状态参数预测模型的输出值。

进一步地，得到当前时刻的车辆运行状态参数的最优估计值的方法包括：

根据上一时刻的车辆运行状态参数的最优估计值、车辆底盘系统输出的当前时刻车辆状态参数的测量值、当前时刻的车辆运行状态参数的先验估计值得到当前时刻的车辆运行状态参数的最优估计值。

更进一步地，最优估计值的计算方法包括：

η(K)＝C_tξ(k)+v(k)

式中：

和/>分别为上一时刻和当前时刻车辆运行状态参数的最优估计值；

P_k表示当前时刻车辆运行状态参数的真实值与先验估计值之间的协方差；

P_k表示当前时刻车辆运行状态参数的真实值与最优估计值之间的协方差；

为当前时刻的车辆运行状态参数的先验估计值；

C_t为车辆状态观测矩阵；

L是车辆底盘系统输出的前一时刻的车辆状态参数的测量噪声的方差；

ξ(k)为不考虑噪声的车辆运行状态参数；

v(k)为当前时刻的测量噪声。

实现本发明目的之二的一种自适应权重系数MPC的路径跟踪控制系统，包括：

车辆运行状态参数预测模型构建模块、车辆运行状态参数预测模型优化模块和路径跟踪控制模块；

所述车辆运行状态参数预测模型构建模块用于基于三自由度动力学模型和MPC算法建立车辆运行状态参数预测模型，所述车辆运行状态参数预测模型根据当前时刻的车辆运行状态参数和控制量预测下一时刻的车辆运行状态参数；

所述车辆运行状态参数预测模型优化模块用于根据从车辆获取的车辆状态参数的测量值采用滤波算法对所述车辆运行状态参数预测模型输出的下一时刻的车辆运行状态参数进行优化，得到优化后的车辆运行状态参数预测模型；

所述路径跟踪控制模块用于求解MPC算法中的目标函数，得到下一时刻的前轮转角增量，将其作为调整量发送至被控车辆，从而完成路径跟踪控制。

进一步地，还包括权重优化模块，用于采用模糊推理系统对MPC算法中的目标函数中的第一权重系数矩阵Q_y和第二权重系数矩阵R_u进行优化；所述第一权重系数矩阵Q_y为车辆预测状态参数到期望轨迹对应参数的偏差值的权重系数矩阵；第二权重系数矩阵R_u为前轮转角控制增量的权重系数矩阵。

进一步地，还包括先验估计值计算模块，用于根据上一时刻的车辆运行状态参数的最优估计值和车辆前轮转角得到当前时刻的车辆运行状态参数的先验估计值。

进一步地，还包括最优估计值计算模块，用于根据当前时刻的车辆运行状态参数的先验估计值得到当前时刻的车辆运行状态参数的最优估计值，所述最优估计值作为优化后的车辆运行状态参数预测模型的输出值。

本发明所包括的有益效果包括：

1、抗干扰性强，降低外界噪声对传感器的影响；

2自适应性强，可以根据行驶状况自适应修正MPC控制器目标函数的权重，跟踪效果更加精准。

附图说明

图1是本发明所述方法的流程示意图；

图2是本发明所述的基于三自由度的车辆动力学模型；

图3是本发明所述方法与常规MPC算法的控制效果对比图。

具体实施方式

下列具体实施方式用于对本发明权利要求技术方案的解释，以便本领域的技术人员理解本权利要求书。本发明的保护范围不限于下列具体的实施结构。本领域的技术人员做出的包含有本发明权利要求书技术方案而不同于下列具体实施方式的也是本发明的保护范围。

本发明针对常规基于MPC算法的路径跟踪方法进行优化。用于路径跟踪的MPC算法主要包括模型预测、滚动优化、反馈校正。

本发明实施例提供一种自适应权重系数MPC的路径跟踪控制方法；如图1所示，包括如下步骤：

S101、基于三自由度动力学模型得到车辆非线性动力学状态方程，通过Taylor展开的方式转化为线性状态方程，再采用前向欧拉法，得到离散化后的线性状态方程，也即如下式(1)所示的车辆运行状态参数预测模型公式，其通过k-1时刻的车辆运行状态参数ξ(k-1)与k-1时刻的控制量u(k-1)来预测k时刻车辆运行状态参数ξ(k)；

ξ(k)＝A_t(k-1)ξ(k-1)+B_t(k-1)u(k-1) 式(1)

式中：

矩阵中参数依次分别为k时刻的如下参数：车辆的纵向速度、车辆的横向速度、车辆横摆角、车辆横摆角速度、车辆横向距离和车辆纵向距离，所述横向距离与纵向距离的原点位置参考大地坐标系，见图2；

ξ(k-1)为k-1时刻车辆运行状态参数，其物理意义同ξ(k)；

u(k-1)表示k-1时刻车辆控制量-前轮转角δ_f(k-1)，其值为：u(k-1)＝δ_f(k-1)；

A_t(k-1)＝I+TA(k-1)；I为单位矩阵，T为控制器采样周期；

B_t(k-1)＝TB(k-1)；

其中：

C_f为前轮的侧偏刚度，C_r为后轮的侧偏刚度，I_z为绕z轴转动惯量，l_f为前轴距，l_r为后轴距。

S102、假设本路径跟踪方法通过传感器信号得到的带有过程噪声与测量噪声的离散线性系统的状态方程(下式(2中)的分式1)与观测方程(下式(2)中的第二个公式)如下式所示：

ξ(k)＝A_t(k-1)ξ(k-1)+B_t(k-1)u(k-1)+w(k-1)

η(k)＝C_tξ(k)+v(k) 式(2)

式中：

A_t(k-1)为k-1时刻车辆运行状态参数ξ(k-1)的系数矩阵；

B_t(k-1)为k-1时刻车辆控制变量u(k-1)的系数矩阵；

w(k-1)为过程噪声，也即因环境因素造成的车辆状态量的偏差，根据传感器信号和计算预测车辆状态参数做差后得到；

C_t为车辆状态观测矩阵，

v(k)为测量噪声，也即观测车辆状态参数数据的偏差，根据传感器信号和理论数据做差后得到；

η(k)为k时刻底盘系统输出的测量值,η(k)＝[v_x,k,v_y,k,Y_k,X_k]^T。

过程噪声w(k-1)和测量噪声v(k)分别是符合p(w)∈N(0,M)，p(v)∈N(0,L)的高斯白噪声；其中N指的是高斯函数，M是w(k-1)的方差，L是v(k-1)的方差。

S103、对上述车辆运行状态参数预测模型进行优化，本发明实施例采用卡尔曼滤波算法对车辆传感器信号重新估计，卡尔曼滤波算法主要包括预测和更新两个部分，卡尔曼滤波的预测方程根据前一时刻车辆状态参数的最优估计值预测当前时刻车辆状态参数的先验估计值，预测计算式如下式(3)所示：

式中：

为k时刻的车辆运行状态参数的先验估计值，即通过上一时刻车辆运行状态参数最优估计值/>预测的当前时刻的车辆运行状态参数的值；

为k-1时刻的车辆运行状态参数的最优估计值；

A_t(k)为k时刻车辆运行状态参数的系数矩阵；

B_t(k)为k时刻车辆控制变量u(k)的系数矩阵；

u(k-1)表示k时刻车辆控制量-前轮转角δ_f(k-1)，其值为：u(k-1)＝δ_f(k-1)；

表示k时刻车辆运行状态参数的真实值与先验估计值/>之间的协方差，本发明中称先验估计协方差；

P_k-1表示k-1时刻车辆运行状态参数的真实值与最优估计值之间的协方差，本发明中称后验估计协方差；

M表示w(k-1)的方差；

用如下式(4)所示的测量更新方程对k时刻的车辆运行状态参数的先验估计值进行校正，得到最优估计值/>

式中：

K为卡尔曼增益；

P_k表示k时刻的车辆运行状态参数的真实值与最优估计值之间的协方差，本发明中称后验估计协方差；

η(k)为底盘系统输出的车辆状态参数的测量值，

为上一时刻车辆运行状态参数的最优估计值。

由上式(4)得到当前时刻的车辆运行状态参数的最优估计值该值用于作为MPC控制，将最优估计值/>作为车辆状态参数的预测值。由此便完成了本方法优化后的MPC算法第一步，模型预测。

S104、本发明实施例在完成了MPC算法中的模型预测之后即可得到一段时域内的车辆运行状态参数预测值，接下来需要设计一个目标函数(车辆预测状态参数到期望轨迹对应参数的偏差值与前轮转角控制增量的加权和)，将预测值输入到目标函数中，通过matlab函数求解使得目标函数最小，便可得到最优前轮转角控制量，控制车辆按照此最优前轮转角控制，车辆就能准确跟踪期望轨迹。在目标函数中权重系数矩阵Q_y、R_u尤为重要，Q_y为车辆预测状态参数到期望轨迹对应参数的偏差值的权重系数矩阵，R_u为前轮转角控制增量的权重系数矩阵，Q_y矩阵越大，表示目标函数越看重跟踪精度，R_u矩阵越大表示越看重前轮转角控制增量对系统的影响，其对车辆跟踪精度和稳定性有很大影响。本发明实施例设计一种自适应权重控制器，设计模糊控制系统，设定自适应权重规则，根据路径跟踪过程中车辆与期望路径的横向距离偏差e_y与期望路径曲率ρ_c的变化来自适应改变Q_y、R_u权重值。

横向偏差e_y真实论域取为[0,1.5]，单位为m。

路径曲率ρ_c计算应该取理论值ρ_c1和转角限制值ρ_c2中的最小值；

理论值：曲线r(t)＝(X(t),Y(t))

前轮转角限制值：

确定路径曲率的论域时，取为[0，ρ_cmax]，其中ρ_cmax＝min{ρ_c1,ρ_c2}。

将上述横向偏差真实论域与曲率真实论域归一化，横向偏差e_y的论域为[0,1]，曲率ρ_c的论域为[0,1]。

自适应权重控制器的中间输出变量为ΔQ_y和ΔR_u，论域取为[-2,2]。选择包含五个元素的模糊集，为{负大(NB)，负小(NS)，零(ZO)，正小(PS)，正大(PB)}。制定模糊规则，如下所示：

表1ΔQ_y模糊规则表

表2ΔR_u模糊规则表

选择输入变量(横向偏差e_y和曲率ρ_c)与输出变量(权重系数矩阵Q_y和R_u)的隶属度函数为三角型隶属度，模糊推理机制使用Mamdani法，解模糊方法为重心法。向自适应权重控制器输入归一化后的横向偏差e_y和路径曲率ρ_c，即可求解得到精确值ΔQ_y和ΔR_u，这两个矩阵是精确值；由于精确值直接输入matlab中会影响控制效果，因此根据下式(5)和(6)对ΔQ_y和ΔR_u进行数值改变得到最终结果Q_y和R_u，并将所述最终结果输入MPC控制器当中的目标函数中。

自此便完成了模糊自适应权重系数的设计。

S105、本发明在完成了自适应权重的设计后，即可通过目标函数求解最优控制量。

根据车辆系统状态参数ξ(k)、前轮转角控制量u(k)和控制增量Δu(k)＝u(k)-u(k-1)等对目标函数进行设计，目标函数J(k)可以表示为输出状态偏差值和控制增量的加权求和，如下式所示：

式中：

η(k+i|t)为从k+1时刻到k+N_P时刻的车辆系统实际输出状态参数；

η_ref(k+i|t)为从k+1时刻到k+NP时刻的参考轨迹对应输出状态参数；

Δu(k+i-1|t)为从k时刻到k+N_P-1时刻的前轮转角控制增量；

Q_y、R_u分别为优化后的车辆状态量偏差和前轮转角控制增量的权重系数矩阵；

ε，ρ分别为松弛因子和其权重系数；目标函数的收敛速度和收敛条件随的大小变化而发生改变，ε>1，加快收敛速度；ε<1，改善收敛条件。

N_p为预测时域；N_c为控制时域；根据控制需求来取，如果跟踪轨迹较短，取N_P＝20，N_c＝5即可。

将目标函数的求解转化为二次规划的求解问题，再通过matlab中的QP函数求解器求解出最优前轮转角控制量增量序列如下：

ΔU＝[Δu(k) Δu(k+1) … Δu(k+N_c-1)]^T 式(8)

控制器将所述增量序列中k时刻的增量Δu(k)添加至上一时刻的车辆控制量u(k-1)中作为当前时刻的控制量u(k)输出至被控车辆，即如下式(9)所示：

u(k)＝u(k-1)+Δu(k) 式(9)

式中u(k)中为k时刻前轮转角，u(k-1)为k-1时刻前轮转角，Δu(k)为k时刻前轮转角增量；

被控车辆收到每一时刻的最优控制量-前轮转角后即可控制车辆不断接近参考路径，自适应MPC与一般MPC的控制效果比较如图3所示。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

本发明实施例还提供一种自适应权重系数MPC的路径跟踪控制系统，包括：

车辆运行状态参数预测模型构建模块、车辆运行状态参数预测模型优化模块和路径跟踪控制模块；

车辆运行状态参数预测模型构建模块用于基于三自由度动力学模型和MPC算法建立车辆运行状态参数预测模型，所述车辆运行状态参数预测模型根据当前时刻的车辆运行状态参数和控制量预测下一时刻的车辆运行状态参数；

车辆运行状态参数预测模型优化模块用于根据从车辆获取的车辆状态参数的测量值采用滤波算法对所述车辆运行状态参数预测模型输出的下一时刻的车辆运行状态参数进行优化，得到优化后的车辆运行状态参数预测模型；

路径跟踪控制模块用于求解MPC算法中的目标函数，得到下一时刻的前轮转角增量，将其作为调整量发送至被控车辆，从而完成路径跟踪控制。

另一个实施例中，还包括权重优化模块，用于采用模糊推理系统对MPC算法中的目标函数中的第一权重系数矩阵Q_y和第二权重系数矩阵R_u进行优化；所述第一权重系数矩阵Q_y为车辆预测状态参数到期望轨迹对应参数的偏差值的权重系数矩阵；第二权重系数矩阵R_u为前轮转角控制增量的权重系数矩阵。

另一个实施例中，还包括先验估计值计算模块，用于根据上一时刻的车辆运行状态参数的最优估计值和车辆前轮转角得到当前时刻的车辆运行状态参数的先验估计值。

另一个实施例中，还包括最优估计值计算模块，用于根据当前时刻的车辆运行状态参数的先验估计值得到当前时刻的车辆运行状态参数的最优估计值，所述最优估计值作为优化后的车辆运行状态参数预测模型的输出值。

本说明书未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (10)

1.一种自适应权重系数MPC的路径跟踪控制方法，其特征在于，包括：

S1、基于三自由度动力学模型和MPC算法建立车辆运行状态参数预测模型，所述车辆运行状态参数预测模型用于根据当前时刻的车辆运行状态参数和控制量预测下一时刻的车辆运行状态参数；

S2、根据从车辆获取的车辆状态参数的测量值采用滤波算法对所述车辆运行状态参数预测模型输出的下一时刻的车辆运行状态参数进行优化，得到优化后的车辆运行状态参数预测模型；

S3、求解MPC算法中的目标函数，得到下一时刻的前轮转角增量，将其作为调整量发送至被控车辆，从而完成路径跟踪控制。

2.如权利要求1所述的自适应权重系数MPC的路径跟踪控制方法，其特征在于，所述步骤S3前，还包括：

采用模糊推理系统对MPC算法中的目标函数中的第一权重系数矩阵Q_y和第二权重系数矩阵R_u进行优化；所述第一权重系数矩阵Q_y为车辆预测状态参数到期望轨迹对应参数的偏差值的权重系数矩阵；第二权重系数矩阵R_u为前轮转角控制增量的权重系数矩阵。

3.如权利要求2所述的自适应权重系数MPC的路径跟踪控制方法，其特征在于，对第一权重系数矩阵Q_y和第二权重系数矩阵R_u进行优化的方法包括：

选取车辆的横向偏差e_y和路径曲率ρ_c作为模糊推理系统的输入变量；

对所述模糊推理系统进行计算，得到ΔQ_y和ΔR_u；

根据下式对ΔQ_y和ΔR_u进行计算，得到第一权重系数矩阵Q_y和第二权重系数矩阵R_u；

式中：

Q_yo为设定的系数矩阵；

R_yo为设定值。

4.如权利要求1所述的自适应权重系数MPC的路径跟踪控制方法，其特征在于，所述步骤S2包括：

根据上一时刻的车辆运行状态参数的最优估计值和车辆前轮转角得到当前时刻的车辆运行状态参数的先验估计值；

根据当前时刻的车辆运行状态参数的先验估计值得到当前时刻的车辆运行状态参数的最优估计值，所述最优估计值作为优化后的车辆运行状态参数预测模型的输出值。

5.如权利要求4所述的自适应权重系数MPC的路径跟踪控制方法，其特征在于，得到当前时刻的车辆运行状态参数的最优估计值的方法包括：

根据上一时刻的车辆运行状态参数的最优估计值、车辆底盘系统输出的当前时刻车辆状态参数的测量值、当前时刻的车辆运行状态参数的先验估计值得到当前时刻的车辆运行状态参数的最优估计值。

6.如权利要求5所述的自适应权重系数MPC的路径跟踪控制方法，其特征在于，最优估计值的计算方法包括：

K＝P_kC_t ^T(C_tP_kC_t ^T+L)^-1

η(K)＝C_tξ(k)+v(k)

式中：

和/>分别为上一时刻和当前时刻车辆运行状态参数的最优估计值；

表示当前时刻车辆运行状态参数的真实值与先验估计值/>之间的协方差；

P_k表示当前时刻车辆运行状态参数的真实值与最优估计值之间的协方差；

为当前时刻的车辆运行状态参数的先验估计值；

C_t为车辆状态观测矩阵；

L是车辆底盘系统输出的前一时刻的车辆状态参数的测量噪声的方差；

ξ(k)为不考虑噪声的车辆运行状态参数；

v(k)为当前时刻的测量噪声。

7.一种自适应权重系数MPC的路径跟踪控制系统，其特征在于，包括车辆运行状态参数预测模型构建模块、车辆运行状态参数预测模型优化模块和路径跟踪控制模块；

所述车辆运行状态参数预测模型构建模块用于基于三自由度动力学模型和MPC算法建立车辆运行状态参数预测模型，所述车辆运行状态参数预测模型根据当前时刻的车辆运行状态参数和控制量预测下一时刻的车辆运行状态参数；

所述车辆运行状态参数预测模型优化模块用于根据从车辆获取的车辆状态参数的测量值采用滤波算法对所述车辆运行状态参数预测模型输出的下一时刻的车辆运行状态参数进行优化，得到优化后的车辆运行状态参数预测模型；

所述路径跟踪控制模块用于求解MPC算法中的目标函数，得到下一时刻的前轮转角增量，将其作为调整量发送至被控车辆，从而完成路径跟踪控制。

8.如权利要求7所述的自适应权重系数MPC的路径跟踪控制方法，其特征在于，还包括权重优化模块，用于采用模糊推理系统对MPC算法中的目标函数中的第一权重系数矩阵Q_y和第二权重系数矩阵R_u进行优化；所述第一权重系数矩阵Q_y为车辆预测状态参数到期望轨迹对应参数的偏差值的权重系数矩阵；第二权重系数矩阵R_u为前轮转角控制增量的权重系数矩阵。

9.如权利要求7所述的自适应权重系数MPC的路径跟踪控制方法，其特征在于，还包括先验估计值计算模块，用于根据上一时刻的车辆运行状态参数的最优估计值和车辆前轮转角得到当前时刻的车辆运行状态参数的先验估计值。

10.如权利要求9所述的自适应权重系数MPC的路径跟踪控制方法，其特征在于，还包括最优估计值计算模块，用于根据当前时刻的车辆运行状态参数的先验估计值得到当前时刻的车辆运行状态参数的最优估计值，所述最优估计值作为优化后的车辆运行状态参数预测模型的输出值。