CN118092193A - 一种基于fhs-lqr算法的智能车辆轨迹跟踪方法 - Google Patents

Abstract

本发明提供一种基于FHS‑LQR算法的智能车辆轨迹跟踪方法，属于无人驾驶车辆轨迹跟踪技术领域，包括如下步骤：建立车辆跟踪误差动力学模型；以转向机构扰动作为随机噪声构建FHS‑LQR控制器，根据Riccati‑ZXL方程计算FHS‑LQR控制器的最优控制率；使用车辆跟踪误差动力模型对预测的车辆轨迹参数计算运行误差，基于运行误差和最优控制率生成车辆转角控制车辆运动；基于道路曲率构建速度规划模块，将速度规划模块添加到预测模型，控制车辆转向时的精度。本发明将具有乘性噪声的FHS‑LQR应用于车辆轨迹跟踪控制，同时引入道路曲率对速度进行修正，跟踪精度高，转向稳定性好，跟踪效果好。

Description

一种基于FHS-LQR算法的智能车辆轨迹跟踪方法

技术领域

本发明属于无人驾驶车辆轨迹跟踪技术领域，具体涉及一种基于FHS-LQR算法的智能车辆轨迹跟踪方法。

背景技术

随着城市交通的发展，交通拥堵现象日益严重，作为智能驾驶关键技术的轨迹跟踪控制可确保车辆在实际驾驶过程中能够有效地跟踪预定轨迹，因此非常重要。

目前对轨迹跟踪控制的研究主要集中在横向控制上，横向控制是在保证车辆行驶安全性和舒适性的前提下实时调节车辆的运动轨迹。横向控制常用的算法包括：滑模控制SMC、模型预测控制MPC和线性二次型调节器LQR。这些算法被广泛应用于车辆控制中。其中，MPC基于对未来系统状态预测进行最优控制，是使用最多的一种控制方法，但该算法对未来状态的预测需要进行多次计算，计算量大，导致控制器缺乏实时性，从而最终难以应用于复杂场景。SMC轨迹跟踪控制器对系统噪声非常敏感，容易产生振动，不能及时保证系统稳定。LQR控制器相比前两者通过最小化性能指标来实现最优控制，提高了系统的稳定性和控制精度，该算法得到了广泛引用，但LQR控制通常应用于线性系统，只能处理简单的问题，对于随机系统，由于不确定性导致LQR控制器并不能取得好的效果。

此为现有技术的不足，因此，针对现有技术中的上述缺陷，提供一种基于FHS-LQR算法的智能车辆轨迹跟踪方法，是非常有必要的。

发明内容

针对现有技术的上述轨迹跟踪控制是智能驾驶的关键技术，现有的轨迹跟踪控制主要采用SMC、MPC及LQR控制器进行，MPC算法计算量大，SMC算法容易产生振动，LQR算法不适用于随机系统的缺陷，本发明提供一种基于FHS-LQR算法的智能车辆轨迹跟踪方法，以解决上述技术问题。

本发明提供一种基于FHS-LQR算法的智能车辆轨迹跟踪方法，包括如下步骤：

S1.建立车辆跟踪误差动力学模型；

S2.以转向机构扰动作为随机噪声构建FHS-LQR控制器，根据Riccati-ZXL方程计算FHS-LQR控制器的最优控制率；

S3.使用车辆跟踪误差动力模型对预测的车辆轨迹参数计算运行误差，基于运行误差和最优控制率生成车辆转角控制车辆运动；

S4.基于道路曲率构建速度规划模块，将速度规划模块添加到预测模型，控制车辆转向时的精度。

进一步地，步骤S1具体步骤如下：

S11.将整车进行等效简化，构建二自由度的车辆动力学模型；

S12.引入车辆运动中的横向误差和横摆角误差，并基于二自由度的车辆动力学模型构建车辆跟踪误差动力学模型。

进一步地，步骤S11具体步骤如下：

S111.构建车辆动力学模型；

S112.设定车辆前后轮的侧偏刚度相同，省略车辆悬架，将整车等效简化为两个车轮；

S113.基于等效简化后的整车及基础车辆动力学模型构建二自由度的车辆动力学模型。

进一步地，步骤S111中，根据车辆质量、车辆横向加速度、转动惯量、车辆横摆角、车辆前轮转角、车辆质心到前轴的距离、车辆质心到后轴的距离以及作用在车辆前后轮胎上的侧向力构建车辆动力学模型如下：

其中，为车辆质量，/>为车辆横向加速度，/>为转动惯量，/>为车辆横摆角加速度，分别为车辆质心到前后轴的距离，/>为车辆前轮转角，/>分别为作用在车辆前后轮胎上的侧向力；

步骤S113中，以等效简化后车辆及车辆动力学模型为基础，根据车辆质量、转动管理、车辆横摆加速度、车辆质心到前轴的距离、车辆质心到后轴的距离、车辆前轮转角、车辆前后轮胎的横向侧偏刚度、纵向速度及横向速度构建二自由度的车辆动力学模型如下：

其中，表示车辆质量，/>表示转动惯量，/>表示车辆横摆加速度，/>和/>分别表示车辆质心到前后车轴的距离，/>表示车辆前轮转角，/>和/>分别是车辆前后轮胎的横向侧偏刚度，/>和/>分别为车辆的纵向速度和横向速度。

进一步地，步骤S12中，根据车辆前后轮胎的横向偏刚度、车辆质量、车辆的纵向速度、车辆质心到投影点的速度、车辆横摆角误差及误差变化率、车辆质心到前轴的距离、车辆质心到后轴的距离构建以车辆质心到投影点的加速度为目标函数的车辆跟踪误差模型方程如下：

其中，为车辆质心到投影点的加速度，/>为车辆质心到投影点的速度，/>为车辆横摆角误差，/>为车辆横摆角的误差变化率，/>为车辆前轮胎的横向侧偏刚度，/>车辆后轮胎的横向侧偏刚度，a为车辆质心到前轴的距离，b为车辆质心到前后轴的距离，m为车辆质量，/>为车辆的纵向速度；

根据车辆质心到前轴的距离、车辆质心到后轴的距离、车辆前后轮胎的横向偏刚度转动惯量、车辆的纵向速度、车辆质心到投影点的速度、车辆横摆角误差变化率构建以车辆横摆角误差加速度为目标函数的车辆跟踪误差模型方程如下：

其中，为车辆横摆角误差变化率构，/>为车辆质心到投影点的速度，/>为车辆横摆角误差，/>为车辆横摆角的误差变化率，/>为车辆前轮胎的横向侧偏刚度，/>车辆后轮胎的横向侧偏刚度，a为车辆质心到前轴的距离，b为车辆质心到前后轴的距离，/>为车辆的纵向速度，I为车辆前后轮胎的横向偏刚度转动惯量。

进一步地，步骤S2具体步骤如下：

S21.引入均值和方差已知的转向机构扰动作为标量随机白噪声构建轨迹跟踪的FHS-LQR控制器；

S22.基于成本函数，以轨迹跟踪成本最低为目标计算FHS-LQR控制器的最优控制率。

进一步地，步骤S3具体步骤如下：

S31.使用预测模型对车辆的横向位移、纵向位移、横摆角、横向速度、纵向速度及横摆角变化率进行预测；

S32.计算横向误差、横摆角误差、横向误差变化率及横摆角误差变化率；

S33.FHS-LQR控制器基于最优控制率，并根据横向速度、横向误差、横摆角误差、横向误差变化率及横摆角误差变化率计算车辆车轮转角，并使用车辆车轮转角对车辆进行轨迹跟踪控制。

进一步地，步骤S32具体步骤如下：

S321.设定参考位置点，寻找与车辆实际位置距离最近的参考位置点，计算参考位置点的横向位置、纵向位置以及横摆角；

S322.根据参考位置点的横摆角、参考位置点的纵向位置、参考位置点的横向位置、预测横向位置、预测纵向位置计算横向误差、投影点与参考位置点的误差；

S323.根据横摆角及投影点横摆角计算横摆角误差；

S324.根据预测纵向速度、预测横向速度、横摆角、投影点横摆角计算横向误差变化率；

S325.根据横摆角变化率、投影点位置以及参考位置斜率计算横摆角误差变化率。

进一步地，步骤S4具体步骤如下：

S41.根据道路摩擦系数和道路曲率构建速度规划模块；

S42.以预测横向位置、预测纵向位置、预测横摆角、预测纵向速度、预测横向速度、预测横摆角加速度为目标函数构建预测模型；

S43.将速度规划模块添加到预测模型。

进一步地，步骤S42具体步骤如下：

S421.基于车辆横向速度得到预测横向速度，基于车辆纵向速度得到预测纵向速度，并使用速度规划模块的道路曲率对预测纵向速度进行修正，基于车辆横摆角速度得到预测横摆角速度；

S422.基于车辆横摆角、预测横摆角速度及预测时间计算预测横摆角；

S423.基于横向位置、预测横向速度、车辆纵向速度、车辆横摆角及预测时间计算预测横向位置；

S424.基于纵向位置、预测纵向速度、车辆横向速度、车辆横摆角及预测时间计算预测纵向位置。

本发明的有益效果在于：

本发明提供的基于FHS-LQR算法的智能车辆轨迹跟踪方法，将具有乘性噪声的FHS-LQR模型应用于车辆轨迹跟踪控制，同时引入道路曲率对速度进行修正，跟踪精度高，转向稳定性好，跟踪效果好。

此外，本发明设计原理可靠，结构简单，具有非常广泛的应用前景。

由此可见，本发明与现有技术相比，具有突出的实质性特点和显著的进步，其实施的有益效果也是显而易见的。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明基于FHS-LQR算法的智能车辆轨迹跟踪方法一个实施例的流程示意图。

图2是本发明基于FHS-LQR算法的智能车辆轨迹跟踪方法另一个实施例的流程示意图。

图3是本发明车辆动力学模型示意图。

图4是本发明车辆跟踪误差动力学模型示意图。

图5是本发明车速36km/h时FHS-LQR控制器与LQR控制器和MPC控制器轨迹跟踪效果对比仿真图。

图6是本发明车速54km/h时FHS-LQR控制器与LQR控制器和MPC控制器轨迹跟踪效果对比仿真图。

图7是本发明车速70km/h时FHS-LQR控制器与LQR控制器和MPC控制器轨迹跟踪效果对比仿真图。

具体实施方式

为了使本技术领域的人员更好地理解本发明中的技术方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

FHS-LQR，是Finite-Horizon Stochastic Linear Quadratic Regulator简称，又简称为有限视界随机LQR，有限视界随机线性二次型调节器。

实施例1：

如图1所示，本发明提供一种基于FHS-LQR算法的智能车辆轨迹跟踪方法，包括如下步骤：

S1.建立车辆跟踪误差动力学模型；

S2.以转向机构扰动作为随机噪声构建FHS-LQR控制器，根据Riccati-ZXL方程计算FHS-LQR控制器的最优控制率；

S3.使用车辆跟踪误差动力模型对预测的车辆轨迹参数计算运行误差，基于运行误差和最优控制率生成车辆转角控制车辆运动；

S4.基于道路曲率构建速度规划模块，将速度规划模块添加到预测模型，控制车辆转向时的精度。

实施例2：

如图2所示，本发明提供一种基于FHS-LQR算法的智能车辆轨迹跟踪方法，包括如下步骤：

S1.建立车辆跟踪误差动力学模型；步骤S1具体步骤如下：

S11.将整车进行等效简化，构建二自由度的车辆动力学模型；步骤S11具体步骤如下：

S111.构建车辆动力学模型；如图3所示，步骤S111中，根据车辆质量、车辆横向加速度、转动惯量、车辆横摆角、车辆前轮转角、车辆质心到前轴的距离、车辆质心到后轴的距离以及作用在车辆前后轮胎上的侧向力构建车辆动力学模型如下：

其中，为车辆质量，/>为车辆横向加速度，/>为转动惯量，/>为车辆横摆角加速度，分别为车辆质心到前后轴的距离，/>为车辆前轮转角，/>分别为作用在车辆前后轮胎上的侧向力；

S112.设定车辆前后轮的侧偏刚度相同，省略车辆悬架，将整车等效简化为两个车轮；

S113.基于等效简化后的整车及基础车辆动力学模型构建二自由度的车辆动力学模型；步骤S113中，以等效简化后车辆及车辆动力学模型为基础，根据车辆质量、转动管理、车辆横摆加速度、车辆质心到前轴的距离、车辆质心到后轴的距离、车辆前轮转角、车辆前后轮胎的横向侧偏刚度、纵向速度及横向速度构建二自由度的车辆动力学模型如下：

其中，表示车辆质量，/>表示转动惯量，/>表示车辆横摆加速度，/>和/>分别表示车辆质心到前后车轴的距离，/>表示车辆前轮转角，/>和/>分别是车辆前后轮胎的横向侧偏刚度，/>和/>分别为车辆的纵向速度和横向速度；

S12.引入车辆运动中的横向误差和横摆角误差，并基于二自由度的车辆动力学模型构建车辆跟踪误差动力学模型；如图4所示，步骤S12中，根据车辆前后轮胎的横向偏刚度、车辆质量、车辆的纵向速度、车辆质心到投影点的速度、车辆横摆角误差及误差变化率、车辆质心到前轴的距离、车辆质心到后轴的距离构建以车辆质心到投影点的加速度为目标函数的车辆跟踪误差模型方程如下：

其中，为车辆质心到投影点的加速度，/>为车辆质心到投影点的速度，/>为车辆横摆角误差，/>为车辆横摆角的误差变化率，/>为车辆前轮胎的横向侧偏刚度，/>车辆后轮胎的横向侧偏刚度，a为车辆质心到前轴的距离，b为车辆质心到前后轴的距离，m为车辆质量，/>为车辆的纵向速度；

根据车辆质心到前轴的距离、车辆质心到后轴的距离、车辆前后轮胎的横向偏刚度转动惯量、车辆的纵向速度、车辆质心到投影点的速度、车辆横摆角误差变化率构建以车辆横摆角误差加速度为目标函数的车辆跟踪误差模型方程如下：

其中，为车辆横摆角误差变化率构，/>为车辆质心到投影点的速度，/>为车辆横摆角误差，/>为车辆横摆角的误差变化率，/>为车辆前轮胎的横向侧偏刚度，/>车辆后轮胎的横向侧偏刚度，a为车辆质心到前轴的距离，b为车辆质心到前后轴的距离，/>为车辆的纵向速度，I为车辆前后轮胎的横向偏刚度转动惯量；

S2.以转向机构扰动作为随机噪声构建FHS-LQR控制器，根据Riccati-ZXL方程计算FHS-LQR控制器的最优控制率；步骤S2具体步骤如下：

S21.引入均值和方差已知的转向机构扰动作为标量随机白噪声构建轨迹跟踪的FHS-LQR控制器；

对应FHS-LQR控制器引入包含乘性噪声的离散时间随机系统，具体如下：

其中，A、B、和、/>表示多维常数矩阵；/>表示为均值为零、方差为/>的标量随机白噪声，在车辆控制方面，本申请将/>定义为转向机构扰动；

S22.基于成本函数，以轨迹跟踪成本最低为目标计算FHS-LQR控制器的最优控制率；

构建成本函数如下：

其中，、/>、/>为半正定矩阵；

求解随机LQR控制的问题是找到一个最优的，并使上式最小化；

首先，将庞特里亚金最大值原理应用于上式，得到:

在上式中，是常数，/>，/>；

然后，定义三组矩阵，/>，/>，其中，/>，/>；通过定义,/>，递归反推，可得:

在这里必须是可逆的，否则递归将终止；

最后，令,/>，当/>时，结合上式，可得：

其中：

可得最优为:

；

S3.使用车辆跟踪误差动力模型对预测的车辆轨迹参数计算运行误差，基于运行误差和最优控制率生成车辆转角控制车辆运动；步骤S3具体步骤如下：

S31.使用预测模型对车辆的横向位移、纵向位移、横摆角、横向速度、纵向速度及横摆角变化率进行预测；

S32.计算横向误差、横摆角误差、横向误差变化率及横摆角误差变化率；步骤S32具体步骤如下：

S321.设定参考位置点，寻找与车辆实际位置距离最近的参考位置点，计算参考位置点的横向位置、纵向位置以及横摆角；

S322.根据参考位置点的横摆角、参考位置点的纵向位置、参考位置点的横向位置、预测横向位置、预测纵向位置计算横向误差、投影点与参考位置点的误差；

S323.根据横摆角及投影点横摆角计算横摆角误差；

横向误差和横摆角误差/>如下：

其中，为投影点与车辆实际位置最近参考位置之间的误差；/>为投影点处的横摆角；/>为距离车辆实际位置最近的参考位置，/>为参考位置的横摆角；

S324.根据预测纵向速度、预测横向速度、横摆角、投影点横摆角计算横向误差变化率；

S325.根据横摆角变化率、投影点位置以及参考位置斜率计算横摆角误差变化率；

横向误差的变化率和横摆角误差的变化率/>分别为：

；

S33.FHS-LQR控制器基于最优控制率，并根据横向速度、横向误差、横摆角误差、横向误差变化率及横摆角误差变化率计算车辆车轮转角，并使用车辆车轮转角对车辆进行轨迹跟踪控制；

S4.基于道路曲率构建速度规划模块，将速度规划模块添加到预测模型，控制车辆转向时的精度；步骤S4具体步骤如下：

S41.根据道路摩擦系数和道路曲率构建速度规划模块；速度规划模块具体如下：

其中，为道路摩擦系数，/>为重力加速度，/>为道路曲率。

由上式可以看出，当道路摩擦系数一定时，随着道路曲率的增大，车速会降低，以保证车辆的转向稳定；

S42.以预测横向位置、预测纵向位置、预测横摆角、预测纵向速度、预测横向速度、预测横摆角加速度为目标函数构建预测模型；步骤S42具体步骤如下：

S421.基于车辆横向速度得到预测横向速度，基于车辆纵向速度得到预测纵向速度，并使用速度规划模块的道路曲率对预测纵向速度进行修正，基于车辆横摆角速度得到预测横摆角速度；

S422.基于车辆横摆角、预测横摆角速度及预测时间计算预测横摆角；

S423.基于横向位置、预测横向速度、车辆纵向速度、车辆横摆角及预测时间计算预测横向位置；

S424.基于纵向位置、预测纵向速度、车辆横向速度、车辆横摆角及预测时间计算预测纵向位置；

预测模型具体如下：

；

S43.将速度规划模块添加到预测模型。

如图5、图6及图7所示，通过仿真展示了三种速度工况下，FHS-LQR控制器与LQR控制器和MPC控制器的性能对比，其中，图5对应的速度为36km/h，图6对应的速度为54km/h，图7对应的速度为70km/h；当车辆低速行驶时，从图5可以看出，FHS-LQR控制器控制的车辆在转向时生成的轨迹最接近参考轨迹，而LQR和MPC控制器的跟踪效果较差；图6中低速54km/h时，MPC控制器控制的车辆在完成第三处转向后逐渐偏离参考轨迹，LQR控制器在转向时也无法进行有效跟踪；然而，FHS-LQR控制器仍然能够有效地跟踪轨迹；当速度增加到70km/h时，从图7可以看出，MPC控制器和LQR控制器生成的真实轨迹与参考轨迹的偏差更为明显；其中，MPC控制器生成的轨迹有轻微的振荡，LQR控制器产生的轨迹在第三段转向处逐渐偏离参考轨迹。通过以上三种速度条件下的仿真比较，FHS-LQR控制器对双移线轨迹的跟踪性能最好，车辆更稳定。

本发明首次将具有乘性噪声的有限视界随机LQR理论应用于智能车辆轨迹跟踪控制领域；由于车辆系统是一个随机系统，采用随机LQR控制可以更准确地描述车辆的运动；首先，为了更好地呈现车辆横向运动时的状态误差变化，便于FHS-LQR轨迹跟踪控制器的计算，本文设计了基于跟踪误差的车辆动力学模型；然后，与一般的离散LQR控制不同，FHS-LQR控制考虑了系统噪声的影响，并定义了一组Riccati - ZXL方程；通过求解Riccati-ZXL方程得到最优解，并结合状态误差生成车轮转角，控制车辆运动；最后，考虑车辆转向稳定性，在FHS-LQR轨迹跟踪控制器中增加了基于曲率变化的速度规划模块，以保证车辆转向时保持跟踪精度。

尽管通过参考附图并结合优选实施例的方式对本发明进行了详细描述，但本发明并不限于此。在不脱离本发明的精神和实质的前提下，本领域普通技术人员可以对本发明的实施例进行各种等效的修改或替换，而这些修改或替换都应在本发明的涵盖范围内/任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (10)

1.一种基于FHS-LQR算法的智能车辆轨迹跟踪方法，其特征在于，包括如下步骤：

S1.建立车辆跟踪误差动力学模型；

S2.以转向机构扰动作为随机噪声构建FHS-LQR控制器，根据Riccati-ZXL方程计算FHS-LQR控制器的最优控制率；

S3.使用车辆跟踪误差动力模型对预测的车辆轨迹参数计算运行误差，基于运行误差和最优控制率生成车辆转角控制车辆运动；

S4.基于道路曲率构建速度规划模块，将速度规划模块添加到预测模型，控制车辆转向时的精度。

2.如权利要求1所述的基于FHS-LQR算法的智能车辆轨迹跟踪方法，其特征在于，步骤S1具体步骤如下：

S11.将整车进行等效简化，构建二自由度的车辆动力学模型；

S12.引入车辆运动中的横向误差和横摆角误差，并基于二自由度的车辆动力学模型构建车辆跟踪误差动力学模型。

3.如权利要求2所述的基于FHS-LQR算法的智能车辆轨迹跟踪方法，其特征在于，步骤S11具体步骤如下：

S111.构建车辆动力学模型；

S112.设定车辆前后轮的侧偏刚度相同，省略车辆悬架，将整车等效简化为两个车轮；

S113.基于等效简化后的整车及基础车辆动力学模型构建二自由度的车辆动力学模型。

4.如权利要求3所述的基于FHS-LQR算法的智能车辆轨迹跟踪方法，其特征在于，步骤S111中，根据车辆质量、车辆横向加速度、转动惯量、车辆横摆角、车辆前轮转角、车辆质心到前轴的距离、车辆质心到后轴的距离以及作用在车辆前后轮胎上的侧向力构建车辆动力学模型如下：

其中，为车辆质量，/>为车辆横向加速度，/>为转动惯量，/>为车辆横摆角加速度，分别为车辆质心到前后轴的距离，/>为车辆前轮转角，/>分别为作用在车辆前后轮胎上的侧向力；

步骤S113中，以等效简化后车辆及车辆动力学模型为基础，根据车辆质量、转动管理、车辆横摆加速度、车辆质心到前轴的距离、车辆质心到后轴的距离、车辆前轮转角、车辆前后轮胎的横向侧偏刚度、纵向速度及横向速度构建二自由度的车辆动力学模型如下：

其中，表示车辆质量，/>表示转动惯量，/>表示车辆横摆加速度，/>和/>分别表示车辆质心到前后车轴的距离，/>表示车辆前轮转角，/> 和 /> 分别是车辆前后轮胎的横向侧偏刚度，/> 和/> 分别为车辆的纵向速度和横向速度。

5.如权利要求4所述的基于FHS-LQR算法的智能车辆轨迹跟踪方法，其特征在于，步骤S12中，根据车辆前后轮胎的横向偏刚度、车辆质量、车辆的纵向速度、车辆质心到投影点的速度、车辆横摆角误差及误差变化率、车辆质心到前轴的距离、车辆质心到后轴的距离构建以车辆质心到投影点的加速度为目标函数的车辆跟踪误差模型方程如下：

其中，为车辆质心到投影点的加速度，/>为车辆质心到投影点的速度，/>为车辆横摆角误差，/>为车辆横摆角的误差变化率，/>为车辆前轮胎的横向侧偏刚度，/>车辆后轮胎的横向侧偏刚度，a为车辆质心到前轴的距离，b为车辆质心到前后轴的距离，m为车辆质量，/>为车辆的纵向速度；

根据车辆质心到前轴的距离、车辆质心到后轴的距离、车辆前后轮胎的横向偏刚度转动惯量、车辆的纵向速度、车辆质心到投影点的速度、车辆横摆角误差变化率构建以车辆横摆角误差加速度为目标函数的车辆跟踪误差模型方程如下：

其中，为车辆横摆角误差变化率构，/>为车辆质心到投影点的速度，/>为车辆横摆角误差，/>为车辆横摆角的误差变化率，/>为车辆前轮胎的横向侧偏刚度，/>车辆后轮胎的横向侧偏刚度，a为车辆质心到前轴的距离，b为车辆质心到前后轴的距离，/>为车辆的纵向速度，I为车辆前后轮胎的横向偏刚度转动惯量。

6.如权利要求5所述的基于FHS-LQR算法的智能车辆轨迹跟踪方法，其特征在于，步骤S2具体步骤如下：

S21.引入均值和方差已知的转向机构扰动作为标量随机白噪声构建轨迹跟踪的FHS-LQR控制器；

S22.基于成本函数，以轨迹跟踪成本最低为目标计算FHS-LQR控制器的最优控制率。

7.如权利要求6所述的基于FHS-LQR算法的智能车辆轨迹跟踪方法，其特征在于，步骤S3具体步骤如下：

S31.使用预测模型对车辆的横向位移、纵向位移、横摆角、横向速度、纵向速度及横摆角变化率进行预测；

S32.计算横向误差、横摆角误差、横向误差变化率及横摆角误差变化率；

S33.FHS-LQR控制器基于最优控制率，并根据横向速度、横向误差、横摆角误差、横向误差变化率及横摆角误差变化率计算车辆车轮转角，并使用车辆车轮转角对车辆进行轨迹跟踪控制。

8.如权利要求7所述的基于FHS-LQR算法的智能车辆轨迹跟踪方法，其特征在于，步骤S32具体步骤如下：

S321.设定参考位置点，寻找与车辆实际位置距离最近的参考位置点，计算参考位置点的横向位置、纵向位置以及横摆角；

S322.根据参考位置点的横摆角、参考位置点的纵向位置、参考位置点的横向位置、预测横向位置、预测纵向位置计算横向误差、投影点与参考位置点的误差；

S323.根据横摆角及投影点横摆角计算横摆角误差；

S324.根据预测纵向速度、预测横向速度、横摆角、投影点横摆角计算横向误差变化率；

S325.根据横摆角变化率、投影点位置以及参考位置斜率计算横摆角误差变化率。

9.如权利要求7所述的基于FHS-LQR算法的智能车辆轨迹跟踪方法，其特征在于，步骤S4具体步骤如下：

S41.根据道路摩擦系数和道路曲率构建速度规划模块；

S42.以预测横向位置、预测纵向位置、预测横摆角、预测纵向速度、预测横向速度、预测横摆角加速度为目标函数构建预测模型；

S43.将速度规划模块添加到预测模型。

10.如权利要求9所述的基于FHS-LQR算法的智能车辆轨迹跟踪方法，其特征在于，步骤S42具体步骤如下：

S421.基于车辆横向速度得到预测横向速度，基于车辆纵向速度得到预测纵向速度，并使用速度规划模块的道路曲率对预测纵向速度进行修正，基于车辆横摆角速度得到预测横摆角速度；

S422.基于车辆横摆角、预测横摆角速度及预测时间计算预测横摆角；

S423.基于横向位置、预测横向速度、车辆纵向速度、车辆横摆角及预测时间计算预测横向位置；

S424.基于纵向位置、预测纵向速度、车辆横向速度、车辆横摆角及预测时间计算预测纵向位置。