[发明专利]一种基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法

权利要求书

1.一种基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，其特征在于包括如下步骤：

1)系统初始化，包括：

1.1)设置系统采样周期T；

1.2)从路径规划模块获取世界坐标系XOY及参考路径点集P_r＝{(x_r(i),y_r(i))i＝0,1,2...}，其中x_r(i)表示参考路径第i个点横坐标，y_r(i)表示参考路径第i个点纵坐标；

1.3)建立新的参考路径点集其中为参考路径点对应的航向角、v_r(i)为参考路径点对应的控制车速、δ_r(i)为参考路径点对应的控制前轮转角；

1.4)设置车辆控制量u和控制增量△u的约束条件；

1.5)设置控制器参数：Q、R、F、ρ、z_1min、z_2min、z_3min、z_4min、z_1max、z_2max、z_3max、z_4max、m₁、m₂、m₃、m₄；

1.6)设置预测时域N_p和控制时域N_c；

2)初始化k＝0，其中k表示采样计数变量，用于对每一个采样进行计数；

3)路径跟踪控制器通过高精度卫星差分定位实时采样车辆状态量χ，其中x、y分别为车辆后轴中点横坐标和纵坐标，为车辆航向角；

4)根据车辆状态量χ(k)、u(k-1)、Y_r(k)以及u_r(k)预测从第k采样时刻起到未来N_p个采样时刻的输出，并以矩阵Y(k)的形式表示；

5)利用带软约束二次规划优化方法计算出未来N_c个采样时刻的控制增量；

所述步骤5)中计算未来N_c个采样时刻的控制增量包括如下步骤：

5.1)在约束条件中加入松弛因子，并设置目标函数，采用如下目标函数：

式中：Q为预测状态误差权重矩阵，R为预测控制增量权重矩阵，F为预测控制误差权重矩阵，ε＝[ε₁ ε₂ ε₃ ε₄]^T为松弛因子向量，ρ为松弛因子权重矩阵；

经过整理：

5.2)将5.1)中带软约束目标函数整理为可以用二次规划方法求解的形式，过程为：

5.2.1)经过相应的矩阵计算，可以将步骤5.1)中目标函数调整为：

式中：

E_U(k)＝U_current(k-1)-U_r(k)，

5.2.2)这种带约束的优化问题可转化为如下的二次规划问题：

其中：

M＝[m₁ m₂ m₃ m₄]^T，

其中Δu_min为采样周期内控制增量下限，Δu_max为采样周期内控制增量上限，u_min为最小控制量，u_max为最大控制量；

5.2.3)完成对5.2.2)求解后，得到未来N_c个时刻的一系列控制增量：

6)对第k个参考点进行跟踪控制，跟踪控制量为上一时刻的控制量加上当前时刻的控制增量，即：

u(k)＝u(k-1)+Δu^*(k);

7)采样计数加1，即k＝k+1；

8)重复步骤3)到步骤6)，直至完成路径跟踪过程；

所述步骤4)中矩阵Y(k)的计算步骤如下：

4.1)建立车辆运动学模型：

其中，v为控制车速，δ为控制前轮转角，L为车辆轴距；

4.2)离散化车辆运动学模型，方法如下：

4.2.1)将步骤4.1)中车辆运动学方程表示为并将此式在参考点(χ_r,u_r)处进行泰勒展开，消去高阶项可得：

式中：A为f(χ,u)相对χ的雅可比矩阵，B为f(χ,u)相对u的雅可比矩阵，u_r＝[v_r δ_r]^T；

4.2.2)令并离散化，其中则

χ(k+1)＝A_kχ(k)+B_ku(k)+g(k)

其中：

4.2.3)整理上式可得离散化车辆运动学方程：

C＝diag(1,1,1)；

4.3)计算从第k采样时刻起到未来N_p个采样时刻的输出，分为如下过程：

4.3.1)取新的状态空间方程为：

经过整理可得：

式中：

其中I为单位矩阵；

4.3.2)做出如下假设：

其中，下标k+j,k表示在k采样时刻对第k+j时刻的预测值，j＝1,2...；

4.3.3)在k时刻，对未来N_p个采样时刻的输出值进行预测，将系统未来采样时刻的输出以矩阵的形式表达：

Y(k)＝ψ_kξ(k)+Θ_kΔU(k)+Φ_kG(k)

式中：

其中，(k+j|k)表示在k采样时刻对第k+j时刻的预测值，j＝1,2...；

相应参考输出为：