本发明属于无人驾驶车辆轨迹跟踪，具体涉及一种基于fhs-lqr算法的智能车辆轨迹跟踪方法。
背景技术：
1、随着城市交通的发展，交通拥堵现象日益严重，作为智能驾驶关键技术的轨迹跟踪控制可确保车辆在实际驾驶过程中能够有效地跟踪预定轨迹，因此非常重要。
2、目前对轨迹跟踪控制的研究主要集中在横向控制上，横向控制是在保证车辆行驶安全性和舒适性的前提下实时调节车辆的运动轨迹。横向控制常用的算法包括：滑模控制smc、模型预测控制mpc和线性二次型调节器lqr。这些算法被广泛应用于车辆控制中。其中，mpc基于对未来系统状态预测进行最优控制，是使用最多的一种控制方法，但该算法对未来状态的预测需要进行多次计算，计算量大，导致控制器缺乏实时性，从而最终难以应用于复杂场景。smc轨迹跟踪控制器对系统噪声非常敏感，容易产生振动，不能及时保证系统稳定。lqr控制器相比前两者通过最小化性能指标来实现最优控制，提高了系统的稳定性和控制精度，该算法得到了广泛引用，但lqr控制通常应用于线性系统，只能处理简单的问题，对于随机系统，由于不确定性导致lqr控制器并不能取得好的效果。
3、此为现有技术的不足，因此，针对现有技术中的上述缺陷，提供一种基于fhs-lqr算法的智能车辆轨迹跟踪方法，是非常有必要的。
技术实现思路
1、针对现有技术的上述轨迹跟踪控制是智能驾驶的关键技术，现有的轨迹跟踪控制主要采用smc、mpc及lqr控制器进行，mpc算法计算量大，smc算法容易产生振动，lqr算法不适用于随机系统的缺陷，本发明提供一种基于fhs-lqr算法的智能车辆轨迹跟踪方法，以解决上述技术问题。
2、本发明提供一种基于fhs-lqr算法的智能车辆轨迹跟踪方法，包括如下步骤：
3、s1.建立车辆跟踪误差动力学模型；
4、s2.以转向机构扰动作为随机噪声构建fhs-lqr控制器，根据riccati-zxl方程计算fhs-lqr控制器的最优控制率；
5、s3.使用车辆跟踪误差动力模型对预测的车辆轨迹参数计算运行误差，基于运行误差和最优控制率生成车辆转角控制车辆运动；
6、s4.基于道路曲率构建速度规划模块，将速度规划模块添加到预测模型，控制车辆转向时的精度。
7、进一步地，步骤s1具体步骤如下：
8、s11.将整车进行等效简化，构建二自由度的车辆动力学模型；
9、s12.引入车辆运动中的横向误差和横摆角误差，并基于二自由度的车辆动力学模型构建车辆跟踪误差动力学模型。
10、进一步地，步骤s11具体步骤如下：
11、s111.构建车辆动力学模型；
12、s112.设定车辆前后轮的侧偏刚度相同，省略车辆悬架，将整车等效简化为两个车轮；
13、s113.基于等效简化后的整车及基础车辆动力学模型构建二自由度的车辆动力学模型。
14、进一步地，步骤s111中，根据车辆质量、车辆横向加速度、转动惯量、车辆横摆角、车辆前轮转角、车辆质心到前轴的距离、车辆质心到后轴的距离以及作用在车辆前后轮胎上的侧向力构建车辆动力学模型如下：
15、
16、其中，为车辆质量，为车辆横向加速度，为转动惯量，为车辆横摆角加速度，分别为车辆质心到前后轴的距离，为车辆前轮转角，分别为作用在车辆前后轮胎上的侧向力；
17、步骤s113中，以等效简化后车辆及车辆动力学模型为基础，根据车辆质量、转动管理、车辆横摆加速度、车辆质心到前轴的距离、车辆质心到后轴的距离、车辆前轮转角、车辆前后轮胎的横向侧偏刚度、纵向速度及横向速度构建二自由度的车辆动力学模型如下：
18、
19、其中，表示车辆质量，表示转动惯量，表示车辆横摆加速度，和分别表示车辆质心到前后车轴的距离，表示车辆前轮转角，和分别是车辆前后轮胎的横向侧偏刚度，和分别为车辆的纵向速度和横向速度。
20、进一步地，步骤s12中，根据车辆前后轮胎的横向偏刚度、车辆质量、车辆的纵向速度、车辆质心到投影点的速度、车辆横摆角误差及误差变化率、车辆质心到前轴的距离、车辆质心到后轴的距离构建以车辆质心到投影点的加速度为目标函数的车辆跟踪误差模型方程如下：
21、
22、其中，为车辆质心到投影点的加速度，为车辆质心到投影点的速度，为车辆横摆角误差，为车辆横摆角的误差变化率，为车辆前轮胎的横向侧偏刚度，车辆后轮胎的横向侧偏刚度，a为车辆质心到前轴的距离，b为车辆质心到前后轴的距离，m为车辆质量，为车辆的纵向速度；
23、根据车辆质心到前轴的距离、车辆质心到后轴的距离、车辆前后轮胎的横向偏刚度转动惯量、车辆的纵向速度、车辆质心到投影点的速度、车辆横摆角误差变化率构建以车辆横摆角误差加速度为目标函数的车辆跟踪误差模型方程如下：
24、
25、其中，为车辆横摆角误差变化率构，为车辆质心到投影点的速度，为车辆横摆角误差，为车辆横摆角的误差变化率，为车辆前轮胎的横向侧偏刚度，车辆后轮胎的横向侧偏刚度，a为车辆质心到前轴的距离，b为车辆质心到前后轴的距离，为车辆的纵向速度，i为车辆前后轮胎的横向偏刚度转动惯量。
26、进一步地，步骤s2具体步骤如下：
27、s21.引入均值和方差已知的转向机构扰动作为标量随机白噪声构建轨迹跟踪的fhs-lqr控制器；
28、s22.基于成本函数，以轨迹跟踪成本最低为目标计算fhs-lqr控制器的最优控制率。
29、进一步地，步骤s3具体步骤如下：
30、s31.使用预测模型对车辆的横向位移、纵向位移、横摆角、横向速度、纵向速度及横摆角变化率进行预测；
31、s32.计算横向误差、横摆角误差、横向误差变化率及横摆角误差变化率；
32、s33.fhs-lqr控制器基于最优控制率，并根据横向速度、横向误差、横摆角误差、横向误差变化率及横摆角误差变化率计算车辆车轮转角，并使用车辆车轮转角对车辆进行轨迹跟踪控制。
33、进一步地，步骤s32具体步骤如下：
34、s321.设定参考位置点，寻找与车辆实际位置距离最近的参考位置点，计算参考位置点的横向位置、纵向位置以及横摆角；
35、s322.根据参考位置点的横摆角、参考位置点的纵向位置、参考位置点的横向位置、预测横向位置、预测纵向位置计算横向误差、投影点与参考位置点的误差；
36、s323.根据横摆角及投影点横摆角计算横摆角误差；
37、s324.根据预测纵向速度、预测横向速度、横摆角、投影点横摆角计算横向误差变化率；
38、s325.根据横摆角变化率、投影点位置以及参考位置斜率计算横摆角误差变化率。
39、进一步地，步骤s4具体步骤如下：
40、s41.根据道路摩擦系数和道路曲率构建速度规划模块；
41、s42.以预测横向位置、预测纵向位置、预测横摆角、预测纵向速度、预测横向速度、预测横摆角加速度为目标函数构建预测模型；
42、s43.将速度规划模块添加到预测模型。
43、进一步地，步骤s42具体步骤如下：
44、s421.基于车辆横向速度得到预测横向速度，基于车辆纵向速度得到预测纵向速度，并使用速度规划模块的道路曲率对预测纵向速度进行修正，基于车辆横摆角速度得到预测横摆角速度；
45、s422.基于车辆横摆角、预测横摆角速度及预测时间计算预测横摆角；
46、s423.基于横向位置、预测横向速度、车辆纵向速度、车辆横摆角及预测时间计算预测横向位置；
47、s424.基于纵向位置、预测纵向速度、车辆横向速度、车辆横摆角及预测时间计算预测纵向位置。
48、本发明的有益效果在于：
49、本发明提供的基于fhs-lqr算法的智能车辆轨迹跟踪方法，将具有乘性噪声的fhs-lqr模型应用于车辆轨迹跟踪控制，同时引入道路曲率对速度进行修正，跟踪精度高，转向稳定性好，跟踪效果好。
50、此外，本发明设计原理可靠，结构简单，具有非常广泛的应用前景。
51、由此可见，本发明与现有技术相比，具有突出的实质性特点和显著的进步，其实施的有益效果也是显而易见的。
1.一种基于fhs-lqr算法的智能车辆轨迹跟踪方法，其特征在于，包括如下步骤：
2.如权利要求1所述的基于fhs-lqr算法的智能车辆轨迹跟踪方法，其特征在于，步骤s1具体步骤如下：
3.如权利要求2所述的基于fhs-lqr算法的智能车辆轨迹跟踪方法，其特征在于，步骤s11具体步骤如下：
4.如权利要求3所述的基于fhs-lqr算法的智能车辆轨迹跟踪方法，其特征在于，步骤s111中，根据车辆质量、车辆横向加速度、转动惯量、车辆横摆角、车辆前轮转角、车辆质心到前轴的距离、车辆质心到后轴的距离以及作用在车辆前后轮胎上的侧向力构建车辆动力学模型如下：
5.如权利要求4所述的基于fhs-lqr算法的智能车辆轨迹跟踪方法，其特征在于，步骤s12中，根据车辆前后轮胎的横向偏刚度、车辆质量、车辆的纵向速度、车辆质心到投影点的速度、车辆横摆角误差及误差变化率、车辆质心到前轴的距离、车辆质心到后轴的距离构建以车辆质心到投影点的加速度为目标函数的车辆跟踪误差模型方程如下：
6.如权利要求5所述的基于fhs-lqr算法的智能车辆轨迹跟踪方法，其特征在于，步骤s2具体步骤如下：
7.如权利要求6所述的基于fhs-lqr算法的智能车辆轨迹跟踪方法，其特征在于，步骤s3具体步骤如下：
8.如权利要求7所述的基于fhs-lqr算法的智能车辆轨迹跟踪方法，其特征在于，步骤s32具体步骤如下：
9.如权利要求7所述的基于fhs-lqr算法的智能车辆轨迹跟踪方法，其特征在于，步骤s4具体步骤如下：
10.如权利要求9所述的基于fhs-lqr算法的智能车辆轨迹跟踪方法，其特征在于，步骤s42具体步骤如下：
