基于车辆运动学模型及MPC车辆自动轨迹控制算法学习与实现

By-小黄同学

<文章属于个人学习性文本，如果有错误，麻烦大家告诉小黄，小黄及时学习改正 :-）     >

引用大佬：小黎的Ally:【路径规划与轨迹跟踪系列算法学习_第9讲_车辆运动学和动力学模型】https://www.bilibili.com/video/BV19X4y1G7BE?vd_source=369d383c90729d8c8b513367020c1468

基于车辆运动学模型及MPC车辆自动轨迹控制算法学习与实现

摘要

本文探讨了基于车辆运动学模型及模型预测控制（MPC）算法的车辆自动轨迹控制学习与实现。首先，详细介绍了车辆运动学模型的基本原理，涵盖了车辆在空间中的运动描述以及相关的数学表示。其次，系统地分析了模型预测控制算法在车辆轨迹控制中的应用，阐述了MPC算法如何通过优化问题求解来实现对车辆轨迹的精确控制。进一步，探讨了学习过程中的关键步骤，包括数据收集、模型训练以及参数调优等方面的重要性。最后，通过实验和仿真验证了所提出算法的有效性和性能，也发现其在测试过程中存在的滞后性的问题。通过这项技术，了解车辆自动驾驶技术的发展。本文的内容仅建立在研究学习的基础上，不做他用。

关键词：MPC算法、车辆运动学模型、自动驾驶、车辆轨迹控制

 
引言
随着自动驾驶技术的快速发展，车辆自动轨迹控制成为了自动驾驶系统中的关键技术之一。在实现车辆自动轨迹控制的过程中，车辆运动学模型和模型预测控制（Model Predictive Control，MPC）算法被广泛应用并取得了显著的成果。本文旨在基于车辆运动学模型及 MPC 算法展开研究，探讨其在车辆自动轨迹控制中的应用与实现。
车辆运动学建模
车辆运动学模型是在车辆几何特征的基础上的理想化模型。当不考虑车辆行驶过程中路面的不良情况，如凹凸不平、材质不同的路面等情况，同时不考虑车辆的操作过程中的行为损失等因素时，利用车辆的运动学模型能够较好地反应车辆当前的状态和控制信息。
车辆运动学模型参数
状态参数
在车辆运动学模型中，车辆的状态由当前车辆的坐标\left(x,y\right)、车辆朝向角\varphi，即车辆的中轴线与横坐标轴正半轴的夹角、车身轴距长度l。其中，认为车辆在运动过程中视作刚体，因此在本文中，车身轴距恒为常量。
控制参数
车辆的未来动态由当前状态下的前轮偏角，即前轮的朝向与车辆中轴线的夹角\delta
车辆运动学建模及其离散化
坐标系的选择
在本文的车辆运动学模型的建立使基于笛卡尔坐标系的。笛卡尔坐标系下的数学运算相对简单，例如两点之间的距离计算、角度变换等。这些数学运算在车辆运动学模型和控制算法的实现中经常需要用到，因此使用笛卡尔坐标系可以简化相关计算过程。
车辆运动学模型
车辆的水平位置即横坐标的变化量\dot{x}，即为速度沿x轴的正半轴的分量，而纵坐标变化量\dot{y}，即为速度沿y轴的分量(1)。
         (1)
当车轮的前轮偏角不为0时，会改变车辆的运动方向。在本文中认为研究车辆为前轮驱动，因此在车辆转向时，可以以后轮车轮连线中点为圆心，如图 1所示。
 
图 1:车辆角度变化建模
车辆的朝向角的变化量，即车辆的朝向变化的角速度有如公式(2)关系
         (2)
因此可以等到车辆运动学模型的矩阵形式，如公式(3)所示
         (3)
状态量与控制量建模
为简便表示，本文将模型的参数区分为控制量与状态量，并给出如公式(4)的符号表示。
         (4)
并根据公式(3)，对状态量的变化有如(5)公式表示。
         (5)
为了将状态量表示成如(6)的线性形式，我们需要对状态量进行泰勒展开处理
         (6)
可以发现\dot{\chi}是关于\chi、U的二元函数，可表示成如公式(7)的形式。
         (7)
而二元函数的泰勒展开公式如下：
     (8)
    因此，对\dot{\chi}进行泰勒展开并忽略高阶小量后可以表示为公式(9)
     (9)
因此只需要求解两部分偏导即可以线性方程的形式进行表示。根据雅可比行列式可以得到二元变量的偏导可表示为公式(10)(11)。
     (10)
     (11)
在此基础上即可表示\dot{\chi}的变化量\dot{\widetilde{\chi}}表示为如下(12)公式。
     (12)
    对于公式(12)在物理意义上可以表述为状态量导数的误差是关于状态量误差变化和控制量误差变化的线性函数。
模型离散化
对于公式(12)中的\dot{\widetilde{\chi}}我们可以利用欧拉向前差分的格式将其表示为如下形式：
         (13)
那么可以得到方程(14)的形式。
         (14)
整理可以得到下一个步长下的状态量的变化表示成公式。
         (15)
至此，车辆的运动学建模及其离散化求解完成。
MPC算法原理及其实现
在车辆运动学模型中，车辆的信息可以由其状态量、控制量进行表述，同时在笛卡尔坐标系下确定唯一的空间与趋势信息。因此在轨道控制问题中只要能够求解出这些变量的改变方式，就能实现轨迹的跟踪。
状态向量的构建
为了简化模型的表示，本文将模型的状态量和控制量构建为新的向量矩阵。
         (16)
那么下一个步长下的状态向量则可被表示为如公式的形式
         (17)
为了将当前的状态向量表示成与前一时刻的状态向量的线性关系，将矩阵做拆分处理。
                                             (18)
在此基础上可以得到系统的输出，即状态量的输出结果，表达式为状态向量的前三行元素，记作公式(19)。
         (19)
模型多步预测
预测表示
基于上述模型，当已知当前所处的状态向量时，通过改变 项时，就能实现对车辆轨迹的多步模拟预测。
假设对现在处于状态时刻为k时，可以得到如下的步骤状态向量的显式公式(20)。
                                             (20)
那么我们对输出量也能进行多步的预测，如下文公式(21)所示，值得注意的是当预测的步长过长时，只取前几项作为模型的重要参数以构成矩阵。                                             (21)
多步预测的向量形式
预测对于N步长的预测可以将其预测的结果以向量的形式进行表示，则可以得到如下的矩阵表示。
         (22)
其中多步预测的向量Y与系数矩阵\Psi可以表示成如下形式。
         (23)
而造成系统预测差异的矩阵参数则有公式的表示(24)(25)。
     (24)
         (25)
目标函数构建
由于我们的目标函数表示的是与参考轨迹的误差，因此在确定目标函数和执行优化策略的过程中肯定将误差项作为目标函数，以寻求最小化误差的策略。
那么我们就能构造如下的目标函数，如(26)所示（此处还未完全理解，但思想应该就是以误差最小化为目标）。
         (26)
其中 的值为一个常数，可以去掉。
综上所示，目标函数可以化简为如(27)公式所示
         (27)
    那么此时优化的目标函数就构建完成了。
约束条件的构建
在模拟控制的过程中，需要考虑到现实中车辆转角等因素的影响，这会形成模拟过程中的约束条件。依据(20)的公式递推形式，我们可以给出控制量的变化量的预测递推式
         (28)
将其优化成矩阵形式则有如下表示
             (29)
运动过程中的车辆控制能力存在机械限制，那么存在以下约束：
                 (30)
可以将其化简成不等式约束表达：
         (31)
完整模型
综上，基于车辆轨迹控制能得到以下的完整模型。这将车辆轨迹控制转化成了一个二次型优化的问题。此类问题已有现成的求解算法。
          (32)
其中
         (33)
到此为止基于车辆运动学模型和MPC的简单算法部分已经完成。
模型实现与结果分析
基于上述模型与求解过程，本文使用实现利用车辆运动学模型和mpc轨迹控制算法进行轨迹跟踪。
分布使用直线、正弦、指数、双曲线轨迹对模型进行测试，如所示，可见mpc算法在轨迹控制中具有较高的准确性，但是当目标轨迹变化速度过大时体现出反应滞后的特点。
  
  
图 2:mpc轨迹控制结果图
但是在模型求解的过程中，若车辆的初始点距离目标轨迹过远时可能难以规划最优的曲线以靠近目标轨迹，因此可以在后期的过程中添加长距离目标的路线自主规划策略来避免由初始状态所带来的风险。
其次，本文为学习性文本，并没有考虑复杂的路况，如障碍物干扰、视野盲区等问题，以待后续深入。
参考文献
[1]蔡志鑫,古永鹏,董浩,等.基于MPC的无人驾驶汽车轨迹控制研究[J].汽车实用技术,2022,47(01):24-27.DOI:10.16638/j.cnki.1671-7988.2022.001.006.