基于动力学模型的无人驾驶汽车模型预测控制器设计方法与流程

本发明属于无人驾驶汽车控制器设计领域，更具体的说，是涉及一种基于动力学模型的无人驾驶汽车模型预测控制器设计方法。

背景技术：

在过去的几年里，作为人工智能最重要的一部分，同时受益于全球定位系统(gps)、雷达系统和机器视觉的发展，无人驾驶获得了社会各界的广泛关注。无人驾驶可以分为图1所示的四部分：全局路径规划、环境感知、局部路径规划和轨迹追踪。由横向控制器和纵向控制器组成的轨迹追踪控制器是无人驾驶的重要组成部分。

起初，学者们采用比例-积分-微分(pid)控制器来完成轨迹追踪任务。但是，由于汽车是多输入多输出系统，并且周围环境和车辆本身物理结构的限制会带来多种约束，pid控制器的控制效果不能令人满意。随后，模型预测控制器(mpc)依靠其突出的优点被用于无人驾驶车辆的轨迹追踪。

mpc是一种处理多约束和多变量问题的有效方法。它在当前采样时刻使用控制对象的模型来预测系统在预测时域内每个采样时刻的状态。在预测状态的基础上，计算使预测状态和参考状态之间误差最小的控制序列，并且该控制序列需要满足所有的约束条件。最后将序列的第一个分量作为当前时刻的最优输入施加到系统上。在预测过程中使用的预测模型对于控制效果的好坏至关重要。目前，尚未出现一种十分有效的车辆模型来描述车辆的特性，且现有的动力学模型仅以前轮偏角作为控制量，使纵向控制和横向控制不能有机地结合起来。本发明提出的新型动力学模型与现有动力学模型相比，增加了车辆速度作为控制量，可以使车辆根据横向控制的需求改变速度。车辆模型的特性与车辆的实际特性之间的误差不可避免，此外模型的线性化和离散化也会带来状态误差。因此，本发明增加了误差补偿项来补偿预测过程中的累积误差。上述方法不仅具有明确的理论意义，而且具有很强的实际应用价值与现实意义。

技术实现要素：

本发明的目的是为了克服现有技术中的不足，提供一种基于动力学模型的无人驾驶汽车模型预测控制器设计方法，增加车辆速度作为控制量，同时引入误差补偿项以弥补模型精度、线性化和离散化在预测过程中带来的累积误差，从而使无人驾驶车辆具有更好的轨迹追踪效果。

本发明的目的是通过以下技术方案实现的。

本发明的基于动力学模型的无人驾驶汽车模型预测控制器设计方法，包括以下步骤：

第一步：建立新型车辆动力学模型

使用由pacejka提出的轮胎侧向力和牵引力模型，描述纵向力、翻转力矩、回正力矩、阻力矩、侧偏角和滑移率之间的数值关系；轮胎力表示为如下形式：

其中，fl为轮胎纵向力，cl为轮胎的纵向刚度，s表示滑移率，fc为轮胎侧向力，cc为轮胎的侧向刚度，α表示侧偏角；

忽略汽车在竖直方向的运动，汽车的结构和受力用单轨模型描述，根据牛顿第二定律，得到以下三个方向上的受力平衡方程：

其中，m表示汽车的质量，x为汽车重心在车载坐标系下的横坐标，y为汽车重心在车载坐标系下的纵坐标，为汽车的航向角，为汽车在车载坐标系下的纵向速度，为汽车在车载坐标系下的横向速度，为航向角的一阶导数，即汽车在竖直方向上的角速度，为汽车在车载坐标系下的纵向加速度，为汽车在车载坐标系下的横向加速度，为汽车在竖直方向上的角加速度，fxf为前轮在x轴方向所受的力，fxr为后轮在x轴方向所受的力，fyf为前轮在y轴方向所受的力，δ代表汽车的前轮偏角，fyr为后轮在y轴方向所受的力，i为汽车的转动惯量，a为汽车重心与前轴之间的距离，b为汽车重心与后轴之间的距离；

fxf、fxr、fyf和fyr通过以下公式使用轮胎的纵向力和侧向力计算得到：

其中，flf为前轮受到的纵向力，fcf为前轮受到的侧向力，flr为后轮受到的纵向力，fcr为后轮受到的侧向力；这几种力通过公式(1)计算得到，其中滑移率s和侧偏角α利用车速和轮速计算得到；

联立公式(1)、(2)、(3)，并经过线性化和euler离散化处理之后得到新型车辆动力学模型：

其中，k表示当前采样时刻，“k+n”,n＝1,2,3…表示当前采样时刻之后的第n个采样时刻，表示当前时刻的系统状态，y为汽车在惯性坐标系下的纵坐标，x为汽车在惯性坐标系下的横坐标，上角标“t”表示矩阵的转置，ξ(k+1)为下一采样时刻的系统状态，u(k)＝[v,δ]^t为当前时刻的控制输入，v表示车辆速度，为当前时刻的系统输出，ak为系统的状态转移矩阵，bk为系统的输入矩阵，c为系统的输出矩阵；

第二步：设计基于新型车辆动力学模型的无人驾驶汽车模型预测控制器

①利用在当前工作点线性化得到的新型车辆动力学模型对系统在预测时域内的输出状态进行预测：

y(k)＝ψkξ(k|k)+θku(k)+γkγ(k)(5)

其中，为系统在未来np个采样时刻的输出，np表示预测时域，“k+n|k”,n＝0,1,…,np表示在第k个采样时刻预测得到的第k+n个采样时刻的变量，ξe(k)＝f(ξ(k),u(k))-akξ(k)-bku(k)为线性化所引起的状态误差，nc表示控制时域，ξ(k+1)＝f(ξ(k),u(k))为离散的非线性车辆动力学模型；

②利用由式(5)得到的系统预测输出和给定的参考输出构建优化问题：

其中，j(ξ(k),u(k))为代价函数；

③求解优化问题，最优解u(k)满足各种物理约束、舒适度和控制精度的需求，将u(k)的第一个分量u(k|k)作为当前时刻的最优控制量作用到系统上。

第一步中侧偏角α和滑移率s由以下公式得到：：

其中，vc＝vycosδ-vxsinδ为轮胎的侧向速度，vx为汽车在车载坐标系下沿x轴方向的速度，vy为汽车在车载坐标系下沿y轴方向的速度，vl＝vysinδ+vxcosδ为轮胎的纵向速度，r为轮胎的半径，w为轮胎的角速度。

第二步中构建的优化问题具体为：

满足：ξ(k+1)＝akξ(k)+bku(k)

η(k)＝cξ(k)

u(k-1|k)＝u(k-1)

αmin≤α(k+i|k)≤αmax,i＝0,…,np

δu(k+i|k)＝u(k+i|k)-u(k+i-1|k),i＝0,…,np

δumin≤δu(k+i|k)≤δumax,i＝0,…,nc-1

δu(k+i|k)＝0,i＝nc,…,np

其中，ηr为给出的参考输出，“||*||²”表示矩阵的欧几里得范数，q为输出误差的权重矩阵，r为控制增量的权重矩阵。侧偏角α被限制在αmin和αmax之间，为了满足无人驾驶汽车对于稳定性和舒适性的要求，控制增量δu被严格限制在δumin和δumax之间。此外，在时间段内，假设控制量u是一个定值。

与现有技术相比，本发明的技术方案所带来的有益效果是：

(1)本发明增加了速度作为模型的控制输入，使无人驾驶汽车可以根据道路的实际情况以及横向控制的需求改变车辆的速度，因此可以达到更好地控制效果。

(2)本发明增加了误差补偿项，以补偿在状态预测过程中模型的线性化和离散化引起的累积误差，所以预测结果更加精准，进而可以达到更精确的轨迹追踪效果。

附图说明

图1是无人驾驶的分层结构图；

图2是不同地面摩擦系数下的轮胎纵向力和侧向力示意图；

图3是汽车的单轨模型图；

图4是在18km/h下的仿真结果图；

图5是在45km/h下的仿真结果图；

图6是在72km/h下的仿真结果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚，以下从新型车辆动力学模型的建立、模型预测控制器的设计原理和求解方法等方面作进一步说明，下述的具体设计方法用以解释本发明，但并不限于本发明。

本发明的基于动力学模型的无人驾驶汽车模型预测控制器设计方法，包括以下步骤：

第一步：建立新型车辆动力学模型

除了重力和空气阻力，汽车受到的所有重要的力几乎都直接作用在轮胎上。因为轮胎力的复杂性和非线性，选择一个合适的轮胎模型对于建立精确的车辆动力学模型至关重要。本发明使用由pacejka提出的轮胎侧向力和牵引力模型。该模型通过组合多个三角函数来拟合实验数据，它可以描述纵向力、翻转力矩、回正力矩、阻力矩、侧偏角和滑移率之间的数值关系。从图2所示的纵向力和侧向力随着滑移率和侧偏角的变化情况可以看出，如果滑移率和侧偏角被限定在一定范围内，纵向力和侧向力与滑移率和侧偏角之间的关系可以近似为线性关系。事实上，大部分车辆具有良好的防抱死系统(abs)，所以滑移率可以保持在较好的工作点。因此，轮胎力可以表示为如下形式：

其中，fl为轮胎纵向力，cl为轮胎的纵向刚度，s表示滑移率，fc为轮胎侧向力，cc为轮胎的侧向刚度，α表示侧偏角。

忽略汽车在竖直方向的运动，汽车的结构和受力可用图3所示的单轨模型描述。该模型可以表征与汽车稳定性相关的绝大部分动力学特性。根据牛顿第二定律，可以得到以下三个方向上的受力平衡方程：

其中，m表示汽车的质量，x为汽车重心在车载坐标系下的横坐标，y为汽车重心在车载坐标系下的纵坐标，为汽车的航向角，为汽车在车载坐标系下的纵向速度，为汽车在车载坐标系下的横向速度，为航向角的一阶导数，即汽车在竖直方向上的角速度，为汽车在车载坐标系下的纵向加速度，为汽车在车载坐标系下的横向加速度，为汽车在竖直方向上的角加速度，fxf为前轮在x轴方向所受的力，fxr为后轮在x轴方向所受的力，fyf为前轮在y轴方向所受的力，δ代表汽车的前轮偏角，fyr为后轮在y轴方向所受的力，i为汽车的转动惯量，a为汽车重心与前轴之间的距离，b为汽车重心与后轴之间的距离。

fxf、fxr、fyf和fyr可通过以下公式使用轮胎的纵向力和侧向力计算得到：

其中，flf为前轮受到的纵向力，fcf为前轮受到的侧向力，flr为后轮受到的纵向力，fcr为后轮受到的侧向力。这几种力可通过公式(1)计算得到，其中滑移率s和侧偏角α可利用车速和轮速计算得到：

其中，vc＝vycosδ-vxsinδ为轮胎的侧向速度，vx为汽车在车载坐标系下沿x轴方向的速度，vy为汽车在车载坐标系下沿y轴方向的速度，vl＝vysinδ+vxcosδ为轮胎的纵向速度，r为轮胎的半径，w为轮胎的角速度。

联立公式(1)、公式(2)和公式(3)，非线性动力学模型可以被简化为：

其中，为系统的状态变量，表示状态变量的导数，y为汽车在惯性坐标系下的纵坐标，x为汽车在惯性坐标系下的横坐标，上角标“t”表示向量的转置，u＝[v,δ]^t为模型的输入，v表示车辆的速度，为模型的输出。

将非线性动力学模型(4)在当前工作点线性化之后，可得到线性动力学模型：

其中，表示函数在t时刻对的偏微分，bk为系统的输入矩阵，c为系统的输出矩阵。

使用euler离散化方法将线性动力学模型(5)离散化可以得到离散线性动力学模型，即新型车辆动力学模型：

其中，k表示当前采样时刻，“k+n”,n＝1,2,3…表示当前采样时刻之后的第n个采样时刻，表示当前时刻的系统状态，y为汽车在惯性坐标系下的纵坐标，x为汽车在惯性坐标系下的横坐标，上角标“t”表示矩阵的转置，ξ(k+1)为下一采样时刻的系统状态，u(k)＝[v,δ]^t为当前时刻的控制输入，v表示车辆速度，为当前时刻的系统输出，ak为系统的状态转移矩阵，ak＝i+tat，i为合适维数的单位矩阵，t为系统的采样周期，bk为系统的输入矩阵，bk＝tbt，c为系统的输出矩阵。

第二步：设计基于新型车辆动力学模型的无人驾驶汽车模型预测控制器

①利用在当前工作点线性化得到的新型车辆动力学模型对系统在预测时域内的输出状态进行预测：

y(k)＝ψkξ(k|k)+θku(k)+γkγ(k)(7)

其中，为系统在未来np个采样时刻的输出，np表示预测时域，“k+n|k”,n＝0,1,…,np表示在第k个采样时刻预测得到的第k+n个采样时刻的变量，

ξe(k)＝f(ξ(k),u(k))-akξ(k)-bku(k)为线性化所引起的状态误差，nc表示控制时域，ξ(k+1)＝f(ξ(k),u(k))为离散的非线性车辆动力学模型。

②在考虑了各种物理约束、舒适度和控制精度的要求之后，利用由式(7)得到的系统预测输出和给定的参考输出构建优化问题：

其中，j(ξ(k),u(k))为代价函数。

优化问题具体为：

满足：ξ(k+1)＝akξ(k)+bku(k)

η(k)＝cξ(k)

u(k-1|k)＝u(k-1)

αmin≤α(k+i|k)≤αmax,i＝0,…,np

δu(k+i|k)＝u(k+i|k)-u(k+i-1|k),i＝0,…,np

δumin≤δu(k+i|k)≤δumax,i＝0,…,nc-1

δu(k+i|k)＝0,i＝nc,…,np

其中，ηr为给出的参考输出，“||*||²”表示矩阵的欧几里得范数，q为输出误差的权重矩阵，r为控制增量的权重矩阵。侧偏角α被限制在αmin和αmax之间，为了满足无人驾驶汽车对于稳定性和舒适性的要求，控制增量δu被严格限制在δumin和δumax之间。此外，在时间段内，假设控制量u是一个定值。

③优化问题(9)可以转化为标准二次规划的形式，之后便可使用多种求解器快速求解，最优解u(k)需满足各种物理约束、舒适度和控制精度的需求。问题(9)的解u(k)＝[u(k|k),u(k+1|k),…,u(k+nc-1|k)]是在控制时域内每个时刻的控制量的序列，模型预测控制器只将u(k)的第一个分量u(k|k)作为当前时刻的最优控制量作用到系统上，即：

u(k)＝u(k|k)(10)

图4、5和6分别展示了在18km/h、45km/h和72km/h三种情况下的仿真结果。使用原有动力学模型和新型动力学模型的控制器分别被命名为控制器a和控制器b。

从三种情况下的仿真结果可以看出，控制器b在弯道处降低了车辆速度，在出弯处提高了车辆速度，所以控制效果要远远好于控制器a。图4(e)、5(e)和6(e)展示了三种情况下包含误差补偿项和不包含误差补偿项的控制器b的追踪误差。可以看出，误差补偿项可以大幅减小追踪误差，在提高追踪精度中起到了重要作用。

另外，本发明中滑移率和侧偏角需要被限制在一定范围内来确保公式(1)中对轮胎力做线性化处理的合理性，轮胎滑移率s可以通过汽车自带的abs保证，滑移率α则需要通过约束控制量的范围来保证，所以在求解优化问题(9)的时候需要考虑到侧偏角α的变化范围。本发明的目的是提高控制器求解的速度以满足无人驾驶车辆对于快速性的需求，本发明所提出的新型车辆动力学模型和误差补偿项同样适用于非线性模型预测控制器。

尽管上面结合附图对本发明的功能及工作过程进行了描述，但本发明并不局限于上述的具体功能和工作过程，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。