CN108334086A - 一种基于软约束二次规划mpc的无人驾驶车辆路径跟踪控制方法 - Google Patents

Abstract

本发明涉及一种基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，该方法首先根据已知的参考路径点计算并建立新的参考路径点集P_rset，然后预测从第k采样时刻起到未来Np个采样时刻的输出，之后利用带软约束二次规划优化方法计算出未来N_c个采样时刻的控制增量，进而求得当前时刻的控制量，将当前时刻控制量作用于系统，如此循环直至路径跟踪结束。本方法通过滚动优化策略，以局部优化代替全局优化，利用实测信息进行反馈校正，可以在控制过程中实时调整控制参数，路径跟踪更加精确。

Description

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

技术领域

本发明涉及无人驾驶车辆路径跟踪控制领域，具体涉及一种基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法。

背景技术

随着世界汽车保有量的增加，城市道路环境日益复杂，为提高车辆驾驶的安全性与效率，智能化无人化成为汽车发展的大趋势。车辆的路径跟踪控制算法也成为技术研究热点。

现有算法能够根据已有的路径进行跟踪控制，准确来说是将路径分为若干个点并控制车辆逐一经过这些点，整个过程中不能根据泊车路径的变化趋势对当前控制量进行调整，并且目前大部分研究均考虑在车速恒定的情况下进行控制，但车辆在实际行驶过程中，车速不可能完全不变，这给此类算法带来困难。

发明内容

为了解决上述问题，本发明提供一种基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，通过滚动优化策略，以局部优化代替全局优化，利用实测信息进行反馈校正，可以在控制过程中实时调整控制参数，路径跟踪更加精确。

为了实现上述目的，本发明具体技术方案如下：一种基于软约束二次规划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个采样时刻的控制增量；

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

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

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

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

进一步地，上述步骤1.3)中，v_r(i)、δ_r(i)的计数公式如下：

L为车辆轴距。

进一步地，上述步骤1.4)中，控制量和控制增量满足如下约束条件：

其中u＝[v δ]^T为车辆控制量，v为控制车速，δ为控制前轮转角，Δu＝[Δv Δδ]^T为采样周期内控制增量，Δv为采样周期内控制车速增量，Δδ为采样周期内控制前轮转角增量，Δu_min＝[Δv_min Δδ_min]^T为采样周期内控制增量下限，Δu_max＝[Δv_max Δδ_max]^T为采样周期内控制增量上限，u_min＝[v_min δ_min]^T为最小控制量，u_max＝[v_max δ_max]^T为最大控制量。

进一步地，上述步骤4)中矩阵Y(k)的计算步骤如下：

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

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...。

相应参考输出为：

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

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

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

经过整理：

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

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

式中：

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

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

其中：

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

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

本发明的有益效果在于：本发明在路径跟踪控制过程中，控制器通过传感器实时获取车辆当前状态量χ并结合上一时刻控制量u预测将来一段时间内的输出状态量，将预测状态量和参考状态量相减并利用带软约束的二次规划优化求解，最终可求出当前控制量。通过滚动优化策略，以局部优化代替全局优化，利用实测信息进行反馈校正，可以在控制过程中实时调整控制参数，路径跟踪更加精确。

附图说明

图1基于软约束二次规划MPC的泊车控制策略

图2路径跟踪仿真结果图。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，需要指出的是，下面仅以一种最优化的技术方案对本发明的技术方案以及设计原理进行详细阐述，但本发明的保护范围并不限于此。

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

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

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)为参考路径点对应的控制前轮转角，计数公式如下：

L为车辆轴距，

1.4)设置车辆控制量u和控制增量△u的约束条件，在采样周期内控制量和控制增量满足如下约束条件：

其中u＝[v δ]^T为车辆控制量，v为控制车速，δ为控制前轮转角，Δu＝[Δv Δδ]^T为采样周期内控制增量，Δv为采样周期内控制车速增量，Δδ为采样周期内控制前轮转角增量，Δu_min＝[Δv_min Δδ_min]^T为采样周期内控制增量下限，Δu_max＝[Δv_max Δδ_max]^T为采样周期内控制增量上限，u_min＝[v_min δ_min]^T为最小控制量，u_max＝[v_max δ_max]^T为最大控制量；

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)的形式表达，具体步骤如下：

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

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...。

相应参考输出为：

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

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

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

经过整理：

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

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

式中：

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

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

其中：

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

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

6)对第k个参考点进行跟踪控制，根据模型预测控制的基本原理，将5.2.3)中控制序列中第一个元素作为实际的控制增量作用于系统，即为k时刻的控制量：

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

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

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

下面介绍一个仿真实例：

具体实施中，为验证本方法的性能效果，以自动泊车路径跟踪为例进行仿真实验，取轴距L＝2.807m时对规划路径进行跟踪控制仿真，

控制器设计参数如下：

ρ＝diag(200,100,200,100)

z_1min＝-0.01,z_2min＝-0.01,z_3min＝-0.1,z_4min＝-0.01

z_1max＝0.01,z_2max＝0.01,z_3max＝0.1,z_4max＝0.01

m₁＝0.001,m₂＝0.001,m₃＝0.001,m₄＝0.001

控制量约束为：

采样时间T＝0.02s时，控制增量约束为：

当N_p＝20，N_c＝3时，利用simulink和carsim联合仿真。图2为参考路径和跟踪路径对比图。

Claims (5)

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₃、m4；

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个采样时刻的控制增量；

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

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

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

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

2.根据权利要求1所述的基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，其特征在于，所述步骤1.3)中，v_r(i)、δ_r(i)的计数公式如下：

L为车辆轴距。

3.根据权利要求1所述的基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，其特征在于，所述步骤1.4)中，控制量和控制增量满足如下约束条件：

其中u＝[v δ]^T为车辆控制量，v为控制车速，δ为控制前轮转角，Δu＝[Δv Δδ]^T为采样周期内控制增量，Δv为采样周期内控制车速增量，Δδ为采样周期内控制前轮转角增量，Δu_min＝[Δv_min Δδ_min]^T为采样周期内控制增量下限，Δu_max＝[Δv_max Δδ_max]^T为采样周期内控制增量上限，u_min＝[v_min δ_min]^T为最小控制量，u_max＝[v_max δ_max]^T为最大控制量。

4.根据权利要求1所述的基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，其特征在于，所述步骤4)中矩阵Y(k)的计算步骤如下：

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

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个采样时刻的输出值进行预测，将系统未来采样时刻的输出以矩阵的形式表达：

式中：

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

相应参考输出为：

5.根据权利要求1所述的基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，其特征在于，所述步骤5)中计算未来N_c个采样时刻的控制增量包括如下步骤：

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

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

经过整理：

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

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

式中：

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

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

其中：

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

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