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

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

【技术实现步骤摘要】
一种基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法
本专利技术涉及无人驾驶车辆路径跟踪控制领域，具体涉及一种基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法。
技术介绍
随着世界汽车保有量的增加，城市道路环境日益复杂，为提高车辆驾驶的安全性与效率，智能化无人化成为汽车发展的大趋势。车辆的路径跟踪控制算法也成为技术研究热点。现有算法能够根据已有的路径进行跟踪控制，准确来说是将路径分为若干个点并控制车辆逐一经过这些点，整个过程中不能根据泊车路径的变化趋势对当前控制量进行调整，并且目前大部分研究均考虑在车速恒定的情况下进行控制，但车辆在实际行驶过程中，车速不可能完全不变，这给此类算法带来困难。
技术实现思路
为了解决上述问题，本专利技术提供一种基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，通过滚动优化策略，以局部优化代替全局优化，利用实测信息进行反馈校正，可以在控制过程中实时调整控制参数，路径跟踪更加精确。为了实现上述目的，本专利技术具体技术方案如下：一种基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，包括如下步骤：1)系统初始化，包括：1.1)设置系统采样周期T；1.2)从路径规划模块获取世界坐标系XOY及参考路径点集Pr＝{(xr(i),yr(i))|i＝0,1,2...}，其中xr(i)表示参考路径第i个点横坐标，yr(i)表示参考路径第i个点纵坐标；1.3)建立新的参考路径点集其中为参考路径点对应的航向角、vr(i)为参考路径点对应的控制车速、δr(i)为参考路径点对应的控制前轮转角；1.4)设置车辆控制量u和控制增量△u的约束条件；1.5)设置控制器参数：Q、R、F、ρ、z1min、z2min、z3min、z4min、z1max、z2max、z3max、z4max、m1、m2、m3、m4；1.6)设置预测时域Np和控制时域Nc；2)初始化k＝0，其中k表示采样计数变量，用于对每一个采样进行计数；3)路径跟踪控制器通过高精度卫星差分定位实时采样车辆状态量χ，其中x、y分别为车辆后轴中点横坐标和纵坐标，为车辆航向角；4)根据车辆状态量χ(k)、u(k-1)、Yr(k)以及ur(k)预测从第k采样时刻起到未来Np个采样时刻的输出，并以矩阵Y(k)的形式表示；5)利用带软约束二次规划优化方法计算出未来Nc个采样时刻的控制增量；6)对第k个参考点进行跟踪控制，跟踪控制量为上一时刻的控制量加上当前时刻的控制增量，即：u(k)＝u(k-1)+Δu*(k)；7)采样计数加1，即k＝k+1；8)重复步骤3)到步骤6)，直至完成路径跟踪过程。进一步地，上述步骤1.3)中，vr(i)、δr(i)的计数公式如下：L为车辆轴距。进一步地，上述步骤1.4)中，控制量和控制增量满足如下约束条件：其中u＝[vδ]T为车辆控制量，v为控制车速，δ为控制前轮转角，Δu＝[ΔvΔδ]T为采样周期内控制增量，Δv为采样周期内控制车速增量，Δδ为采样周期内控制前轮转角增量，Δumin＝[ΔvminΔδmin]T为采样周期内控制增量下限，Δumax＝[ΔvmaxΔδmax]T为采样周期内控制增量上限，umin＝[vminδmin]T为最小控制量，umax＝[vmaxδmax]T为最大控制量。进一步地，上述步骤4)中矩阵Y(k)的计算步骤如下：4.1)建立车辆运动学模型：4.2)离散化车辆运动学模型，方法如下：4.2.1)将步骤4.1)中车辆运动学方程表示为并将此式在参考点(χr,ur)处进行泰勒展开，消去高阶项可得：式中：A为f(χ,u)相对χ的雅可比矩阵，B为f(χ,u)相对u的雅可比矩阵，ur＝[vrδr]T。4.2.2)令并离散化，其中则χ(k+1)＝Akχ(k)+Bku(k)+g(k)其中：4.2.3)整理上式可得离散化车辆运动学方程：C＝diag(1,1,1)。4.3)计算从第k采样时刻起到未来Np个采样时刻的输出，分为如下过程：4.3.1)取新的状态空间方程为：经过整理可得：式中：其中I为单位矩阵。4.3.2)做出如下假设：其中，下标k+j,k表示在k采样时刻对第k+j时刻的预测值，j＝1,2...；4.3.3)在k时刻，对未来Np个采样时刻的输出值进行预测，将系统未来采样时刻的输出以矩阵的形式表达：Y(k)＝ψkξ(k)+ΘkΔU(k)+ΦkG(k)式中：其中，(k+j|k)表示在k采样时刻对第k+j时刻的预测值，j＝1,2...。相应参考输出为：进一步地，上述步骤5)中计算未来Nc个采样时刻的控制增量包括如下步骤：5.1)在约束条件中加入松弛因子，并设置目标函数，采用如下目标函数：式中：Q为预测状态误差权重矩阵，R为预测控制增量权重矩阵，F为预测控制误差权重矩阵，ε＝[ε1ε2ε3ε4]T为松弛因子向量，ρ为松弛因子权重矩阵；经过整理：5.2)将5.1)中带软约束目标函数整理为可以用二次规划方法求解的形式，过程为：5.2.1)经过相应的矩阵计算，可以将步骤6)中目标函数调整为：式中：EU(k)＝Ucurrent(k-1)-Ur(k)5.2.2)这种带约束的优化问题可转化为如下的二次规划问题：其中：M＝[m1m2m3m4]T5.2.3)完成对5.2.2)求解后，得到未来Nc个时刻的一系列控制增量：本专利技术的有益效果在于：本专利技术在路径跟踪控制过程中，控制器通过传感器实时获取车辆当前状态量χ并结合上一时刻控制量u预测将来一段时间内的输出状态量，将预测状态量和参考状态量相减并利用带软约束的二次规划优化求解，最终可求出当前控制量。通过滚动优化策略，以局部优化代替全局优化，利用实测信息进行反馈校正，可以在控制过程中实时调整控制参数，路径跟踪更加精确。附图说明图1基于软约束二次规划MPC的泊车控制策略图2路径跟踪仿真结果图。具体实施方式下面结合附图以及具体实施例对本专利技术作进一步的说明，需要指出的是，下面仅以一种最优化的技术方案对本专利技术的技术方案以及设计原理进行详细阐述，但本专利技术的保护范围并不限于此。所述实施例为本专利技术的优选的实施方式，但本专利技术并不限于上述实施方式，在不背离本专利技术的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本专利技术的保护范围。一种基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，如图1所示，包括以下步骤：1)系统初始化，包括：1.1)设置系统采样周期T；1.2)从路径规划模块获取世界坐标系XOY及参考路径点集Pr＝{(xr(i),yr(i))|i＝0,1,2...}，其中xr(i)表示参考路径第i个点横坐标，yr(i)表示参考路径第i个点纵坐标；1.3)建立新的参考路径点集其中为参考路径点对应的航向角、vr(i)为参考路径点对应的控制车速、δr(i)为参考路径点对应的控制前轮转角，计数公式如下：L为车辆轴距，1.4)设置车辆控制量u和控制增量△u的约束条件，在采样周期内控制量和控制增量满足如下约束条件：其中u＝[vδ]T为车辆控制量，v为控制车速，δ为控制前轮转角，Δu＝[ΔvΔδ]T为采样周期内控制增量，Δv为采样周期内控制车速增量，Δδ为采样周期内控制前轮转角增量，Δumin＝[ΔvminΔδmin]T为采样周期内控制增量下限，Δumax＝[ΔvmaxΔ本文档来自技高网...

【技术保护点】
1.一种基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，其特征在于包括如下步骤：1)系统初始化，包括：1.1)设置系统采样周期T；1.2)从路径规划模块获取世界坐标系XOY及参考路径点集Pr＝{(xr(i),yr(i))|i＝0,1,2...}，其中xr(i)表示参考路径第i个点横坐标，yr(i)表示参考路径第i个点纵坐标；1.3)建立新的参考路径点集

【技术特征摘要】
1.一种基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，其特征在于包括如下步骤：1)系统初始化，包括：1.1)设置系统采样周期T；1.2)从路径规划模块获取世界坐标系XOY及参考路径点集Pr＝{(xr(i),yr(i))|i＝0,1,2...}，其中xr(i)表示参考路径第i个点横坐标，yr(i)表示参考路径第i个点纵坐标；1.3)建立新的参考路径点集其中为参考路径点对应的航向角、vr(i)为参考路径点对应的控制车速、δr(i)为参考路径点对应的控制前轮转角；1.4)设置车辆控制量u和控制增量△u的约束条件；1.5)设置控制器参数：Q、R、F、ρ、z1min、z2min、z3min、z4min、z1max、z2max、z3max、z4max、m1、m2、m3、m4；1.6)设置预测时域Np和控制时域Nc；2)初始化k＝0，其中k表示采样计数变量，用于对每一个采样进行计数；3)路径跟踪控制器通过高精度卫星差分定位实时采样车辆状态量χ，其中x、y分别为车辆后轴中点横坐标和纵坐标，为车辆航向角；4)根据车辆状态量χ(k)、u(k-1)、Yr(k)以及ur(k)预测从第k采样时刻起到未来Np个采样时刻的输出，并以矩阵Y(k)的形式表示；5)利用带软约束二次规划优化方法计算出未来Nc个采样时刻的控制增量；6)对第k个参考点进行跟踪控制，跟踪控制量为上一时刻的控制量加上当前时刻的控制增量，即：u(k)＝u(k-1)+Δu*(k)；7)采样计数加1，即k＝k+1；8)重复步骤3)到步骤6)，直至完成路径跟踪过程。2.根据权利要求1所述的基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，其特征在于，所述步骤1.3)中，vr(i)、δr(i)的计数公式如下：L为车辆轴距。3.根据权利要求1所述的基于软约束二次规划MPC的无人驾驶车辆路径跟踪控制方法，其特征在于，所述步骤1.4)中，控制量和控制增量满足如下约束条件：其中u＝[vδ]T为车辆控制量，v为控制车速，δ为控制前轮转角，Δu＝[ΔvΔδ]T为采样周期内控制增量，Δv为采样周期内控制车速增量，Δδ为采样周期内控制前轮转角增量，Δumin＝[ΔvminΔδmin]T为采样周期内控制增量下限，Δumax＝[Δvmax...

【专利技术属性】
技术研发人员：江浩斌，叶浩，马世典，王俊娴，张旭培，王成雨，江绍康，
申请(专利权)人：江苏大学，
类型：发明
国别省市：江苏,32