[发明专利]基于MPC的自适应位姿修正的路径跟踪控制方法

权利要求书

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时刻的车辆航向角。