CN114047722A - 基于mpc的自适应位姿修正的路径跟踪控制方法 - Google Patents

Abstract

本发明公开了一种基于MPC的自适应位姿修正的路径跟踪控制方法，包括以下步骤：步骤一，在路径跟踪控制过程中，通过监督控制器来判断当前定位状态是否发生漂移，则使用预测状态值替换当前漂移的位置状态值传入MPC控制器，否则，直接将当前位置状态值传入MPC控制器；步骤二，MPC控制器基于监督控制器输出的结果以确定当前定位位姿状态，并得到预测时域方程；步骤三，通过对预测时域方程优化，得到MPC问题并求解，实现对无人驾驶车辆的路径跟踪。本发明提出了一种基于MPC的自适应位姿修正的路径跟踪控制方法，极大地提高了算法的鲁棒性，使得无人车辆在路径跟踪过程中具有更高的精度和稳定性。

Description

基于MPC的自适应位姿修正的路径跟踪控制方法

技术领域

本发明属于无人驾驶车辆路径跟踪控制技术领域，具体涉及一种基于MPC的自适应位姿修正的路径跟踪方法的设计。

背景技术

无人驾驶系统主要分为三个部分，环境感知与定位、决策规划和路径跟踪控制。路径跟踪控制作为无人驾驶系统的关键环节，主要目标就是让无人车能够按照上层规划的路径行驶，让路径跟踪误差收敛到零。

模型预测控制(MPC)是一种广泛应用于工业领域的一种优化控制算法，随着算法的完善，现在在许多领域都有其应用。传统的路径跟踪控制方法都是假设计算的输入不会达到执行器的饱和极限，这在实际应用中是不正确的，实际中应该要考虑到无人驾驶车辆的物理约束。由于模型预测控制算法在复杂耦合系统、多约束控制和多输入多输出问题等方面具有显著的优势，成为了解决上述问题的关键，现已被广泛应用于无人驾驶车辆路径跟踪控制领域。

虽然模型预测控制能够减少一些模型不确定性和外部扰动的负面影响，但是由于传感器噪声或传感器异常导致的定位漂移会在一段时间内持续对控制系统造成影响，车辆位姿状态无法及时恢复，最终会导致控制性能下降，跟踪误差增加。

发明内容

本发明的一个目的是解决至少上述问题和/或缺陷，并提供至少后面将说明的优点。

为了实现根据本发明的这些目的和其它优点，提供了一种基于MPC的自适应位姿修正的路径跟踪控制方法，以解决定位漂移带来的控制稳定性下降的问题，通过在路径跟踪控制中设计了一个监督控制器，用于判断当前定位状态是否发生漂移，并对下一时刻的位置状态进行了预测；进一步为了实现多目标实时优化，设计了一个线性时变的模型预测控制器(LTV-MPC)来计算期望的控制量，以提高路径跟踪控制的精度和稳定性，具体包括包括以下步骤：

步骤一，在路径跟踪控制过程中，通过监督控制器来判断当前定位状态是否发生漂移，则使用预测状态值替换当前漂移的位置状态值传入MPC控制器，否则，直接将当前位置状态值传入MPC控制器；

步骤二，MPC控制器基于监督控制器输出的结果以确定当前定位位姿状态，并得到预测时域方程；

步骤三，通过对预测时域方程优化，得到MPC问题并求解，实现对无人驾驶车辆的路径跟踪。

优选的是，在步骤一中，所述监督控制器的获得方式被配置为包括：

S10，基于当前位置的车身坐标投影判断当前时刻k位置是否在上一时刻k-1前方，如果不在则认定为定位信息发生漂移，则进入步骤S13，否则进入到S11；

S11，若未发生漂移，则再次判断当前位置是否在上一时刻的近似范围内，如果在近似范围内，如果落入则进入步骤S12；

S12，如果车辆的当前位置落入近似范围，则进一步判断车辆的姿态是否正确，以使每个时刻的航向变化落入到车辆的物理约束条件下，进而确定当前时刻定位状态；

S13，通过实时判断每个时刻的定位状态，如果发生漂移则通过如下的赋值等式完成修正：

X_current＝X_predict；

其中：X_current表示当前状态，X_predict表示从上一个时刻k-1的位姿状态X(k-1)进行状态预测得到的下一个时刻k的位姿状态。

优选的是，在S10中，所述车身坐标投影由公式一得到：

在公式一中，x_{k_local}，y_{k_local}分别表示车身坐标系下的坐标，

表示在k-1时刻车辆的航向角，x_k表示当前时刻的纵向位置；x_k-1表示上一时刻时刻的纵向位置，y_k表示当前时刻的横向位置，y_k-1表示上一时刻的横向位置；

所述定位信息发生漂移的第一判断式为：

ifx_{k_local}＜0；

若第一判断式成立，则认定当前时刻车辆位置不在上一时刻的前方，定位信息发生漂移。

优选的是，在S11中，判断当前位置是否在上一时刻估计的近似范围内的第二判断式为：

x′(k)-ε_x，y≤x(k)≤x′(k)+ε_x，y

y′(k)-ε_x，y≤y(k)≤y′(k)+ε_x，y；

在第二判断式中，x′(k)，y′(k)表示上一时刻估计的近似位置，ε_x，y表示的是表示距离松弛因子；

所述近似位置的获取步骤被配置为包括：

S110，在两个时刻采样间隔时间内，假设速度v恒定不变，结合车辆的前轮转向模型和运动学模型，基于公式三计算出前后两个时刻车辆的近似距离d：

d≈vT：

S111，基于近似距离通过如下的公式四，以得到车辆在k+1时刻的近似位置：

其中，在公式三中，T表示离散时间；

在公式四中，x′(k+1)，y′(k+1)分别近似地表示在k+1时刻车辆在全局惯性坐标系下的纵向和横向位置，

表示当前k时刻车辆的航向角，x(k)，y(k)分别表示k时刻车辆在全局惯性坐标系下的纵向和横向位置。

优选的是，在S12中，所述物理约束条件为满足以下第三判断式：

在第三判断式中，W表示车辆的偏航角速度，

表示在k时刻的车辆航向角。

优选的是，在S13中，X_current的获取方式被配置为包括：

S130，基于车辆线性化误差模型以得到如公式五的状态量微分形式：

S131，对公式五进行四阶龙格-库塔算法预测，以得到每个时刻对下一时刻的高精度的状态预测值：

其中：

优选的是，在步骤二中，所述预测时域方程的获得被配置为包括：

S20，基于监督控制器传入的当前时刻的车辆位置状态量，将车辆的离散线性误差模型转换成增量矩阵：

其中，ξ(k|k)表示在离散模型中人为设置的状态量，x(k|k)表示当前状态量，u(k-1|k)表示上一时刻的控制量；

S21，将离散线性误差模型表述为如下的等价形式：

其中，η为输出量，

为三阶单位矩阵，

分别为A_k，B_k，C_k的增广矩阵；

S22，令预测时域为N_p，控制时域为N_c，得到预测状态参数ξ(k+N_p|k)和输出η(k+N_p|k)的表达式分别为：

其中，Δu(k|k)为当前时刻控制增量，令：

其中，y表示预测时域内的输出矩阵，ψ_k表示预测时域内的状态系数矩阵，Δu_k表示预测时域内的控制增量序列，Θ_k表示预测时域内的控制增量系数矩阵，则预测时域方程的紧凑形式表示为：

y＝ψ_kξ(k|k)+Θ_kΔu_k。

优选的是，所述离散线性误差模型为：

x(k+1)＝A_kx(k)+B_ku(k)；

其中，A_k＝A(t)T+I，B_k＝B(t)T，I为单位矩阵，T为离散时间，x(k)表示当前状态量，u(k)表示当前控制量。

优选的是，在步骤三中，对预测时域方程优化是基于预测时域方程，以得到整个预测时域的期望输出y_ref：

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

其中，u_min表示控制增量的最小值，u_max表示控制增量的最大值，u_lb表示控制总量的最小值，u_ub表示控制总量的最大值，Q表示输出量误差的权重矩阵，R表示控制增量的权重矩阵。

优选的是，在步骤三中，求解MPC问题的步骤被配置为包括：

S30，将MPC问题转化成二次规划问题；

S31，通过内点法对二次规划问题进行求解，以在每个控制周期得到对应的控制增量序列；

S32，在每个控制时刻只取控制增量序列中的第一项作为实际控制输入，直到新的时刻重新预测；

S33,在下一个控制周期，重新进行S30-S32中的步骤进行优化求解，直到完成跟踪控制任务。

本发明至少包括以下有益效果：本发明提出了一种基于MPC的自适应位姿修正的路径跟踪控制方法，并建立了一个监督器用来判断当前位姿是否发生漂移，然后基于车辆的线性化模型使用了四阶龙格库塔算法对车辆的位姿进行了预测，并将其用于替换漂移的位姿数据，以此来解决车辆在路径跟踪过程中存在定位漂移和噪声的问题，极大地提高了算法的鲁棒性，使得无人车辆在路径跟踪过程中具有更高的精度和稳定性。

本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。

附图说明

图1为本发明的运动学模型；

图2为本发明的路径跟踪算法流程图；

图3本发明算法与传统算法的效果对比图。

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

本发明的整体设计思路是：该发明在目前主流的路径跟踪控制算法模型预测控制(Model Predictive Control，MPC)算法的基础上进行改进，传统的模型预测控制算法几乎没有考虑由于传感器噪声或传感器异常导致定位位姿数据漂移从而出现车辆控制不稳定的情况。本发明针对这一问题，提出了一种基于MPC的自适应位姿修正的路径跟踪控制方法，首先建立了一个监督控制器，用于判断当前时刻的定位位姿数据是否发生漂移，再基于车辆的线性误差模型，使用四阶的龙格库塔算法对车辆的下一时刻的位姿状态进行预测，根据监督器判断的结果来决定是否对位姿数据进行修正，最后设计了一个线性时不变的模型预测控制器(LTV-MPC)根据由监督器传入的当前时刻的位姿数据，来进行多目标的实时优化，计算出期望的控制量，从而更加稳定的完成车辆的控制任务。

在介绍本发明的实施步骤前，先介绍本发明的技术原理。该技术原理包括如下步骤：

步骤一：建立无人驾驶车辆的运动学模型。无人驾驶车辆是一个多自由度的、复杂的非线性时变系统，建立一个能完全反映车辆特性的高精度模型是非常困难的，而且这也会增加系统的复杂性和计算负担，降低实时性能。运动学模型作为一个适当简化的模型，不仅能够获得良好的控制精度，还能进一步降低计算复杂度，提高实时性能。

步骤二：将车辆的非线性运动学模型进行线性离散化，得到离散线性误差模型。为了使用模型预测控制的二次优化算法，需要将步骤一中得到的非线性模型进行线性化，又因为连续的状态方程不能直接用于模型预测控制器的设计，所以采用一阶差商的方式对其进行离散化，最终得到一个离散线性误差模型。

步骤三：建立一个监督控制器，用于判断当前时刻位姿是否发生漂移并对下一时刻位姿进行预测。通过将当前时刻实际所得的车辆的位置以及航向与理论所得的车辆位置以及航向来对比，判断当前时刻的位姿状态是否在可接受的阈值范围内，进一步再判断是否发生漂移，并且基于车辆线性误差模型采用四阶龙格库塔算法对下一个时刻的位姿状态进行预测，如果判断当前时刻的位姿发生漂移则使用上个时刻预测的位姿状态对其进行替换修正，否则就不进行处理。

步骤四：根据监督器的结果确定当前定位位姿状态，并得到预测时域方程。将步骤三实际所得的车辆位姿状态带入到MPC基本框架中，推导得到相应的预测时域方程。

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

步骤六：求解MPC问题模型，实现对无人驾驶车辆的路径跟踪。MPC问题可以很容易的转换成一个标准的二次规划问题，使用优化求解工具箱或者内点优化算法可以很快速的完成对二次规划问题的优化求解，最终会求解得到一系列的控制量，选取第一个作为实际控制量，然后在新的时刻重新进行预测求解，一直循环直至完成路径跟踪控制任务。

具体来说，本发明的目的是为了解决路径跟踪过程中，由于传感器噪声或传感器异常导致的GPS定位漂移问题，提出了一种基于MPC的自适应位姿修正的路径跟踪控制方法，能够有效的对产生漂移的定位位姿数据进行修正，以提高路径跟踪控制的精度和稳定性。

本发明的技术方案处理流程如图2所示，具体包括以下步骤：

S1、建立如图1所示无人驾驶车辆的运动学模型。

S2、将车辆的非线性运动学模型进行线性离散化，得到离散线性误差模型。

S3、建立一个监督控制器，用于判断当前时刻位姿是否发生漂移并对下一时刻位姿进行预测。

S4、根据监督器的结果确定当前定位位姿状态，并得到预测时域方程。

S5、对预测时域方程进行优化，得到MPC问题。

S6、求解MPC问题模型，实现对无人驾驶车辆的路径跟踪。

进一步地，步骤S1中无人驾驶车辆的车辆运动学模型基于以下假设：

假设一：忽略车辆的垂向移动。

假设二：车辆和悬架系统是刚性的。

假设三：只考虑车辆的前轮转向。

假设四：用单轨模型来描述车辆的运动，即左右前轮转向角度一致。

进一步地，步骤S1具体为：

在惯性坐标系下，建立智能车辆的车辆运动学模型为：

其中(X_r，Y_r)为车辆后轴轴心坐标，

为车辆的航向角，δ_f为车辆前轮偏角，l为车辆轴距，v_r为车辆后轴中心速度，参数上的符号“.”表示该参数的一阶导数。

将车辆运动学模型表示为一般形式：

其中状态量

控制量u＝[v_r，δ_f]^T，f(·)表示车辆运动学模型函数。

进一步地，步骤S2具体为：

参考系统的任意时刻的状态和控制量满足：

在任意参考点(X_r，u_r)处，对公式(2)进行泰勒展开，只保留一阶项，得到：

其中J_f(X)为f(X，u)相对于状态量X的雅各比矩阵，J_f(u)为f(X，u)相对于控制量u的雅各比矩阵，u_r表示参考点的控制量。

将公式(4)与公式(3)相减得到车辆的线性误差模型：

其中

表示当前状态量与参考状态量的差，

表示当前控制量与参考控制量的差，A(t)＝J_f(x)表示状态量X的雅各比矩阵，B(t)＝J_f(u)表示控制量u的雅各比矩阵。

连续的状态方程不能直接用于模型预测控制器的设计，采用一阶差商的方法将上式进行离散化，即可得到车辆的离散线性误差模型：

x(k+1)＝A_kx(k)+B_ku(k) (6)

其中，A_k＝A(t)T+I，B_k＝B(t)T，I为单位矩阵，T为离散时间，x(k)表示当前状态量，u(k)表示当前控制量。

进一步地，步骤S3具体为：

因为两个时刻采样间隔时间很短，我们可以假设速度v恒定不变，再结合车辆的前轮转向模型和运动学模型，可以计算出前后两个时刻车辆的近似距离。

θ＝wT (9)

其中，R表示车辆转弯半径，W表示车辆偏航角速度，θ表示车辆前后两个时刻转动的角度。

由三角定理可得：

d即为前后两个时刻车辆的近似距离，结合式(7)(8)(9)可得：

由三角函数的小角度近似可得：

d≈vT (12)

因此可得车辆在k+1时刻的近似位置为：

其中：x′(k+1)，y′(k+1)分别近似地表示在k+1时刻车辆在全局惯性坐标系下的纵向和横向位置，

表示当前k时刻车辆的航向角，x(k)，y(k)分别表示k时刻车辆在全局惯性坐标系下的纵向和横向位置。

要自适应的修正车辆的位姿，首先要判断车辆是否处于定位偏移状态，如果车辆未发生偏移则不需要进行位姿修正，继续按照当前位姿状态进行优化求解，如果车辆发生定位偏移，则需要修正车辆位姿后再进行优化求解。具体的判断策略如下：

首先，需要判断当前时刻k位置是否在上一时刻k-1前方，我们通过对当前位置进行车身坐标投影可得：

其中：x_{k_local}，y_{k_local}分别表示车身坐标系下的坐标。

if x_{k_local}＜0 (15)

如果上式成立可以判断出当前时刻车辆位置不在上一时刻的前方，定位信息发生漂移。

若未漂移，我们再继续判断当前位置是否在上一时刻计算出的近似范围内，即：

x′(k)-ε_x，y≤x(k)≤x′(k)+ε_x，y

y′(k)-ε_x，y≤y(k)≤y′(k)+ε_x，y (16)

其中：x′(k)，y′(k)表示由式(13)计算出来的近似位置，ε_x，y表示距离松弛因子，用于缩放近似定位区域的范围，具体值由实际场景而定。如果上式成立，则我们可以认为车辆的当前位置处于我们可容忍的精度范围内，但是这并不能说明车辆定位未发生漂移，因为车辆的姿态也可能会出现偏移，这也将会影响控制的稳定性，所以我们还要考虑车辆的姿态是否正确，每个时刻的航向变化应该在车辆的物理约束之下，即：

结合条件表达式(15)式(16)式(17)，即为所设计的监督器，通过它可以确定当前时刻定位状态。

由监督器可以实时判断每个时刻的定位状态，如果发生漂移则进行修正策略，即令：

X_current＝X_predict (18)

其中：X_current表示当前状态，X_predict表示从上一个时刻(k-1)的位姿状态X(k-1)进行状态预测得到的下一个时刻(k)的位姿状态。

由此就可以将车辆的位姿自适应的进行修正，使其限制在一定的可接受区域范围内。为了获得单步的高精度预测状态X_predict，本发明使用了四阶龙格-库塔算法对车辆的位姿状态信息进行预测。四阶龙格-库塔算法，是一种在工业上广泛使用的高精度算法，能够较好的控制预测精度，主要用于数值求解微分方程，因为其能够采取措施对误差进行抑制，所以其单步精度非常高。由车辆线性化误差模型(5)可得状态量微分的形式：

再将式(19)进行四阶龙格-库塔算法预测得：

其中：

由(20)式就可以得到每个时刻对下一时刻的高精度的状态预测值，下一时刻再根据监督器的判断结果，决定是否修正当前车辆位置状态值，如果需要修正，则使用预测的状态值替换当前的漂移的位置状态值，替换后再由MPC进行优化求解，得到期望的控制量，否则，直接将当前位置状态值传入MPC控制器，进行优化求解。

进一步地，步骤S4具体为：

根据当前时刻监督器传入的位置状态量，再将式(6)转换成增量矩阵的形式，定义：

其中ξ(k|k)表示在离散模型中人为设置的状态量，x(k|k)表示当前状态量，u(k-1|k)表示上一时刻的控制量。

将公式(6)表述为它的等价形式：

其中η为输出量，

为三阶单位矩阵，

分别为A_k，B_k，C_k的增广矩阵。

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

其中Δu(k|k)为当前时刻控制增量，令：

其中，y表示预测时域内的输出矩阵，ψ_k表示预测时域内的状态系数矩阵，Δu_k表示预测时域内的控制增量序列，Θ_k表示预测时域内的控制增量系数矩阵，则预测时域方程的紧凑形式表示为：

y＝ψ_kξ(k|k)+Θ_kΔu_k (27)

进一步地，步骤S5具体为：

根据预测时域方程，得到整个预测时域的期望输出y′_ref为：

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

其中，u_min表示控制增量的最小值，u_max表示控制增量的最大值，u_lb表示控制总量的最小值，u_ub表示控制总量的最大值，Q表示输出量误差的权重矩阵，R表示控制增量的权重矩阵。

进一步地，步骤S6具体为：

式(29)可以很容易的转化成二次规划(Quadratic Programming，QP)问题：

其中：

E＝ψ_kξ(k|k)-y_ref(k)表示在预测时域里的输出的偏差。

使用内点法可以快速完成二次规划问题的求解，在每个控制周期会得到一系列的最优控制增量：

最终，在每个控制时刻只取该最优控制序列中的第一项作为实际控制输入，直到新的时刻重新预测：

在下一个控制周期，系统会重新进行上述步骤进行优化求解，一直循环直到完成跟踪控制任务。

如图3所示，本发明的算法相对于现有传统的MPC算法来说，其在实际运用时，能使实际行驶路径与规划路径基本重合，有效提高无人车辆在路径跟踪过程中的精度和稳定性。

以上方案只是一种较佳实例的说明，但并不局限于此。在实施本发明时，可以根据使用者需求进行适当的替换和/或修改。

这里说明的设备数量和处理规模是用来简化本发明的说明的。对本发明的应用、修改和变化对本领域的技术人员来说是显而易见的。

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

Claims (10)

1.一种基于MPC的自适应位姿修正的路径跟踪控制方法，其特征在于，包括以下步骤：

步骤一，在路径跟踪控制过程中，通过监督控制器来判断当前定位状态是否发生漂移，则使用预测状态值替换当前漂移的位置状态值传入MPC控制器，否则，直接将当前位置状态值传入MPC控制器；

步骤二，MPC控制器基于监督控制器输出的结果以确定当前定位位姿状态，并得到预测时域方程；

步骤三，通过对预测时域方程优化，得到MPC问题并求解，实现对无人驾驶车辆的路径跟踪。

2.如权利要求1所述的基于MPC的自适应位姿修正的路径跟踪控制方法，其特征在于，在步骤一中，所述监督控制器的获得方式被配置为包括：

S10，基于当前位置的车身坐标投影判断当前时刻k位置是否在上一时刻k-1前方，如果不在则认定为定位信息发生漂移，则进入步骤S13，否则进入到S11；

S11，若未发生漂移，则再次判断当前位置是否在上一时刻的近似范围内，如果在近似范围内，如果落入则进入步骤S12；

S12，如果车辆的当前位置落入近似范围，则进一步判断车辆的姿态是否正确，以使每个时刻的航向变化落入到车辆的物理约束条件下，进而确定当前时刻定位状态；

S13，通过实时判断每个时刻的定位状态，如果发生漂移则通过如下的赋值等式完成修正：

X_current＝X_predict；

其中：X_current表示当前状态，X_predict表示从上一个时刻k-1的位姿状态X(k-1)进行状态预测得到的下一个时刻k的位姿状态。

3.如权利要求2所述的基于MPC的自适应位姿修正的路径跟踪控制方法，其特征在于，在S10中，所述车身坐标投影由公式一得到：

在公式一中，x_{k_local}，y_{k_local}分别表示车身坐标系下的坐标，

表示在k-1时刻车辆的航向角，x_k表示当前时刻的纵向位置；x_k-1表示上一时刻时刻的纵向位置，y_k表示当前时刻的横向位置，y_k-1表示上一时刻的横向位置；

所述定位信息发生漂移的第一判断式为：

if x_{k_local}＜0；

若第一判断式成立，则认定当前时刻车辆位置不在上一时刻的前方，定位信息发生漂移。

4.如权利要求2所述的基于MPC的自适应位姿修正的路径跟踪控制方法，其特征在于，在S11中，判断当前位置是否在上一时刻估计的近似范围内的第二判断式为：

x′(k)-ε_x，y≤x(k)≤x′(k)+ε_x，y

y′(k)-ε_x，y≤y(k)≤y′(k)+ε_x，y；

在第二判断式中，x′(k)，y′(k)表示上一时刻估计的近似位置，ε_x，y表示的是表示距离松弛因子；

所述近似位置的获取步骤被配置为包括：

S110，在两个时刻采样间隔时间内，假设速度v恒定不变，结合车辆的前轮转向模型和运动学模型，基于公式三计算出前后两个时刻车辆的近似距离d：

d≈vT；

S111，基于近似距离通过如下的公式四，以得到车辆在k+1时刻的近似位置：

其中，在公式三中，T表示离散时间；

在公式四中，x′(k+1)，y′(k+1)分别近似地表示在k+1时刻车辆在全局惯性坐标系下的纵向和横向位置，

表示当前k时刻车辆的航向角，x(k)，y(k)分别表示k时刻车辆在全局惯性坐标系下的纵向和横向位置。

5.如权利要求2所述的基于MPC的自适应位姿修正的路径跟踪控制方法，其特征在于，在S12中，所述物理约束条件为满足以下第三判断式：

在第三判断式中，w表示车辆的偏航角速度，

表示在k时刻的车辆航向角。

6.如权利要求2所述的基于MPC的自适应位姿修正的路径跟踪控制方法，其特征在于，在S13中，X_current的获取方式被配置为包括：

S130，基于车辆线性化误差模型以得到如公式五的状态量微分形式：

S131，对公式五进行四阶龙格-库塔算法预测，以得到每个时刻对下一时刻的高精度的状态预测值：

其中：

7.如权利要求1所述的基于MPC的自适应位姿修正的路径跟踪控制方法，其特征在于，在步骤二中，所述预测时域方程的获得被配置为包括：

S20，基于监督控制器传入的当前时刻的车辆位置状态量，将车辆的离散线性误差模型转换成增量矩阵：

其中，ξ(k|k)表示在离散模型中人为设置的状态量，x(k|k)表示当前状态量，u(k-1|k)表示上一时刻的控制量；

S21，将离散线性误差模型表述为如下的等价形式：

其中，η为输出量，

为三阶单位矩阵，

分别为A_k，B_k，C_k的增广矩阵；

S22，令预测时域为N_p，控制时域为N_c，得到预测状态参数ξ(k+N_p|k)和输出η(k+N_p|k)的表达式分别为：

其中，Δu(k|k)为当前时刻控制增量，令：

其中，y表示预测时域内的输出矩阵，ψ_k表示预测时域内的状态系数矩阵，Δu_k表示预测时域内的控制增量序列，Θ_k表示预测时域内的控制增量系数矩阵，则预测时域方程的紧凑形式表示为：

y＝ψ_kξ(k|k)+Θ_kΔu_k。

8.如权利要求7所述的基于MPC的自适应位姿修正的路径跟踪控制方法，其特征在于，所述离散线性误差模型为：

x(k+1)＝A_kx(k)+B_ku(k)；

其中，A_k＝A(t)T+I，B_k＝B(t)T，I为单位矩阵，T为离散时间，x(k)表示当前状态量，u(k)表示当前控制量。

9.如权利要求2所述的基于MPC的自适应位姿修正的路径跟踪控制方法，其特征在于，在步骤三中，对预测时域方程优化是基于预测时域方程，以得到整个预测时域的期望输出y_ref：

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

其中，u_min表示控制增量的最小值，u_max表示控制增量的最大值，u_lb表示控制总量的最小值，u_ub表示控制总量的最大值，Q表示输出量误差的权重矩阵，R表示控制增量的权重矩阵。

10.如权利要求2所述的基于MPC的自适应位姿修正的路径跟踪控制方法，其特征在于，在步骤三中，求解MPC问题的步骤被配置为包括：

S30，将MPC问题转化成二次规划问题；

S31，通过内点法对二次规划问题进行求解，以在每个控制周期得到对应的控制增量序列；

S32，在每个控制时刻只取控制增量序列中的第一项作为实际控制输入，直到新的时刻重新预测；

S33，在下一个控制周期，重新进行S30-S32中的步骤进行优化求解，直到完成跟踪控制任务。

Images