CN113276833A - 车辆的横向运动控制方法、控制终端及存储介质 - Google Patents

Abstract

本发明公开了一种车辆的横向运动控制方法、控制终端及存储介质，所述方法包括：根据当前时刻的车辆位置和车道线方程，获得车道线上与当前时刻车辆位置对应的初始位置；根据预设时间间隔、当前时刻车速、初始位置和车道线方程，获得车道线上沿车辆前进方向的多个预测位置；获得初始位置和多个预测位置分别对应的方向盘期望角度；将初始位置和多个预测位置分别对应的方向盘期望角度的平均值作为当前时刻车辆的方向盘期望角度；将当前时刻的方向盘实际角度和方向盘期望角度，输入至预设的闭环PID控制算法中，获得当前时刻的目标扭矩。本发明解决了现有横向运动控制技术存在车辆控制的稳定性差的问题。

Description

车辆的横向运动控制方法、控制终端及存储介质

技术领域

本发明涉及智能驾驶领域，尤其涉及一种车辆的横向运动控制方法、控制终端及计算机可读存储介质。

背景技术

智能驾驶的横向运动控制是该项技术的三大核心之一，横向运动控制方法是整个运动控制系统的核心，其优劣不仅会影响智能汽车对目标路径的跟踪精度，还会对整车的稳定性、舒适性等产生影响。

现有车辆主流的横向运动控制方法是由带前馈的偏差反馈PID控制算法构成，该方法中前馈量是由当前车速与道路转弯半径组成的二维插值表格得到；反馈量是由近点车辆中心偏离车道中心线的横向距离偏差量PID闭环控制与远点航向角偏差量PID闭环控制叠加得到；最终前馈量加上反馈量得到车道跟随所需要的力矩。但该方法中前馈量中二维表格需要标定的数据较多，对测试道路要求严格，远点的选择影响车辆控制的稳定性，容易出现车辆在直道内蛇形。

发明内容

本发明的主要目的在于提出一种车辆的横向运动控制方法、控制终端及计算机可读存储介质，旨在解决现有横向运动控制技术存在车辆控制的稳定性差的问题。

为实现上述目的，本发明提供一种车辆的横向运动控制方法，包括步骤：

根据当前时刻的车辆位置和车道线方程，获得车道线上与当前时刻车辆位置对应的初始位置；

根据预设时间间隔、当前时刻车速、初始位置和车道线方程，获得车道线上沿车辆前进方向的多个预测位置；

根据车辆固有参数、预设的初始状态矩阵计算公式、车道线方程、当前时刻车速、当前时刻车辆状态参数、当前时刻车辆位置信息、初始位置和多个预测位置，获得初始位置和多个预测位置分别对应的方向盘期望角度；

将初始位置和多个预测位置分别对应的方向盘期望角度的平均值作为当前时刻车辆的方向盘期望角度；

将当前时刻的方向盘实际角度和方向盘期望角度，输入至预设的闭环PID控制算法中，获得当前时刻的目标扭矩。

可选地，所述根据车辆固有参数、预设的初始状态矩阵计算公式、车道线方程、当前时刻车速、当前时刻车辆状态参数、当前时刻车辆位置信息、初始位置和多个预测位置，获得初始位置和多个预测位置分别对应的方向盘期望角度的步骤包括：

根据初始位置、预测位置和车道线方程，获取初始位置的道路曲率和预测位置的道路曲率；

根据车辆固有参数、车道线方程、当前时刻车速、当前时刻车辆状态参数、初始位置的道路曲率和预设的初始状态矩阵计算公式，计算获得当前时刻的初始状态矩阵；

根据车辆固有参数、当前时刻的车速、当前时刻的初始状态矩阵、当前时刻的车辆状态参数、初始位置的道路曲率和预设的LQR计算模型，计算初始位置对应的方向盘角度的前馈量和反馈量；

将初始位置对应的方向盘角度的前馈量和反馈量的总和作为初始位置对应的方向盘期望角度；

根据当前时刻的车辆位置、当前时刻的车速方向、初始位置对应的方向盘期望角度和预测位置，获得预测位置对应的状态矩阵偏差；

根据车辆固有参数、当前时刻的车速、当前时刻的车辆状态参数、当前时刻的初始状态矩阵、预测位置对应的状态矩阵偏差、预测位置的道路曲率和预设的LQR计算模型，计算预测位置对应的方向盘角度的前馈量和反馈量；

将预测位置对应的方向盘角度的前馈量和反馈量的总和作为预测位置对应的方向盘期望角度。

可选地，所述根据车辆固有参数、当前时刻的车速、当前时刻的初始状态矩阵、当前时刻的车辆状态参数、初始位置的道路曲率和预设的LQR计算模型，计算初始位置对应的方向盘角度的前馈量和反馈量的步骤之前还包括：

根据当前时刻车速和预设的修正系数与车速间的映射关系，获得当前时刻的修正系数；

所述根据车辆固有参数、当前时刻的车速、当前时刻的初始状态矩阵、当前时刻的车辆状态参数、初始位置的道路曲率和预设的LQR计算模型，计算初始位置对应的方向盘角度的前馈量和反馈量的步骤包括：

根据车辆固有参数、当前时刻的车速、当前时刻的初始状态矩阵、当前时刻的车辆状态参数、初始位置的道路曲率、当前时刻的修正系数和预设的LQR计算模型，计算初始位置对应的方向盘角度的前馈量和反馈量；

所述根据车辆固有参数、当前时刻的车速、当前时刻的车辆状态参数、当前时刻的初始状态矩阵、预测位置对应的状态矩阵偏差、预测位置的道路曲率和预设的LQR计算模型，计算预测位置对应的方向盘角度的前馈量和反馈量的步骤包括：

根据车辆固有参数、当前时刻的车速、当前时刻的车辆状态参数、当前时刻的初始状态矩阵、预测位置对应的状态矩阵偏差、预测位置的道路曲率、当前时刻的修正系数和预设的LQR计算模型，计算预测位置对应的方向盘角度的前馈量和反馈量。

可选地，所述将当前时刻的方向盘实际角度和方向盘期望角度，输入至预设的闭环PID控制算法中，获得当前时刻的目标扭矩的步骤包括：

根据当前时刻的方向盘实际角度和方向盘期望角度以及前一时刻的方向盘实际角度和方向盘期望角度，获得当前时刻的实际角度变化率、期望角度变化率、角度差值和角度变化率的差值；

根据当前时刻的方向盘实际角度、方向盘期望角度、实际角度变化率、期望角度变化率和角度差值，获得当前时刻的第一输出扭矩；

根据当前时刻的角度差值和前一时刻的第二输出扭矩，获得当前时刻的第二输出扭矩；

根据当前时刻的角度差值和角度变化率差值，获得当前时刻的第三输出扭矩；

将当前时刻的第一输出扭矩、第二输出扭矩和第三输出扭矩的总和，作为当前时刻的目标扭矩。

可选地，所述根据当前时刻的方向盘实际角度和方向盘期望角度以及前一时刻的方向盘实际角度和方向盘期望角度，获得当前时刻的实际角度变化率、期望角度变化率、角度差值和角度变化率的差值的步骤包括：

计算前一时刻的方向盘实际角度和当前时刻的方向盘实际角度的平均值作为当前时刻的实际角度滤波值；

根据当前时刻的实际角度滤波值、前一时刻的实际角度滤波值、相邻时刻间的时间差和预设的实际角度变化率阈值范围，获得当前时刻的第一角度变化率；

将当前时刻的第一角度变化率和前一时刻的第一角度变化率的平均值作为当前时刻的实际角度变化率；

根据当前时刻的方向盘期望角度、当前时刻的实际角度滤波值、预设的期望变化率阈值范围和相邻时刻间的时间差，获得当前时刻的第二角度变化率；

将当前时刻的第二角度变化率和前一时刻的第二角度变化率的平均值，作为当前时刻的期望角度变化率；

获取当前时刻的方向盘期望角度和方向盘实际角度的差值，作为当前时刻的方向盘角度偏差值；

获取当前时刻的方向盘角度偏差值和前一时刻的方向盘角度偏差值的平均值，作为当前时刻的角度差值；

获取当前时刻的期望角度变化率和实际角度变化率间差值，作为当前时刻的方向盘角度变化率偏差；

获取当前时刻的方向盘角度变化率偏差和前一时刻的方向盘角度变化率偏差的平均值，作为当前时刻的角度变化率差值。

可选地，所述根据当前时刻的方向盘实际角度、方向盘期望角度、实际角度变化率、期望角度变化率和角度差值，获得当前时刻的第一输出扭矩的步骤包括：

根据预设衰减系数计算公式、当前时刻的方向盘实际角度、方向盘期望角度、实际角度变化率、期望角度变化率和角度差值，获得当前时刻的衰减系数；

根据预设的第一输出扭矩计算公式、当前时刻的衰减系数和角度差值，获得当前时刻的第一输出扭矩。

可选地，所述根据当前时刻的角度差值和前一时刻的第二输出扭矩，获得当前时刻的第二输出扭矩的步骤包括：

根据预设的介入因子计算公式和当前时刻的角度差值，获得当前时刻的介入因子；

根据预设的介入因子与重置因子映射关系和当前时刻的介入因子，获得当前时刻的重置因子；

根据预设的第二输出扭矩计算公式、前一时刻的第二输出扭矩、当前时刻的角度差值、介入因子和重置因子，获得当前时刻的第二输出扭矩。

可选地，所述根据当前时刻的角度差值和角度变化率差值，获得当前时刻的第三输出扭矩的步骤包括：

根据预设的介入因子计算公式和当前时刻的角度差值，获得当前时刻的介入因子；

根据预设的介入因子与重置因子映射关系和当前时刻的介入因子，获得当前时刻的重置因子；

根据预设的第三输出扭矩计算公式、当前时刻的角度变化率差值、介入因子和重置因子，获得当前时刻的第三输出扭矩。

为实现上述目的，本发明还提供一种控制终端，所述控制终端包括存储器、处理器和存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如上所述的车辆的横向运动控制方法的步骤。

为实现上述目的，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的车辆的横向运动控制方法的步骤。

本发明提出的一种车辆的横向运动控制方法、控制终端及计算机可读存储介质，通过根据当前时刻的车辆位置和车道线方程，获得车道线上与当前时刻车辆位置对应的初始位置；根据预设时间间隔、当前时刻车速、初始位置和车道线方程，获得车道线上沿车辆前进方向的多个预测位置；根据车辆固有参数、预设的初始状态矩阵计算公式、车道线方程、当前时刻车速、当前时刻车辆状态参数、当前时刻车辆位置信息、初始位置和多个预测位置，获得初始位置和多个预测位置分别对应的方向盘期望角度；将初始位置和多个预测位置分别对应的方向盘期望角度的平均值作为当前时刻车辆的方向盘期望角度；将当前时刻的方向盘实际角度和方向盘期望角度，输入至预设的闭环PID控制算法中，获得当前时刻的目标扭矩。本方案中不需要进行标定，减少车辆标定工作所需要的时间，减少反复调试次数，并且通过多点预瞄的角度规划以及方向盘角度的闭环与控制扭矩的闭环的双闭环控制，实现车辆精确控制，提高车辆控制稳定性。

附图说明

图1是本发明实施例方案涉及的硬件运行环境的结构示意图；

图2为本发明车辆的横向运动控制方法第一实施例的流程示意图；

图3为本发明车辆的横向运动控制方法第一实施例中步骤S50的细化流程示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

请参照图1，图1为本发明各个实施例中所提供的控制终端的硬件结构示意图。控制终端包括通信模块01、存储器02及处理器03等部件。本领域技术人员可以理解，图1中所示出的终端还可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。其中，所述处理器03分别与所述存储器02和所述通信模块01连接，所述存储器02上存储有计算机程序，所述计算机程序同时被处理器03执行。

通信模块01，可通过网络与外部设备连接。通信模块01可以接收外部设备发出的数据，还可发送数据、指令及信息至所述外部设备，所述外部设备可以是手机、平板电脑、笔记本电脑和台式电脑等电子设备。

存储器02，可用于存储软件程序以及各种数据。存储器02可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(获得初始位置和多个预测位置分别对应的方向盘期望角度)等；存储数据区可存储根据终端的使用所创建的数据或信息等。此外，存储器02可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

处理器03，是控制终端的控制中心，利用各种接口和线路连接整个终端的各个部分，通过运行或执行存储在存储器02内的软件程序和/或模块，以及调用存储在存储器02内的数据，执行终端的各种功能和处理数据，从而对终端进行整体监控。处理器03可包括一个或多个处理单元；优选的，处理器03可集成应用处理器和调制解调处理器，其中，应用处理器主要处理操作系统、用户界面和应用程序等，调制解调处理器主要处理无线通信。可以理解的是，上述调制解调处理器也可以不集成到处理器03中。

尽管图1未示出，但上述控制终端还可以包括电路控制模块，电路控制模块用于与市电连接，实现电源控制，保证其他部件的正常工作。

本领域技术人员可以理解，图1中示出的控制终端结构并不构成对终端的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。

根据上述硬件结构，提出本发明方法各个实施例。

参照图2，在本发明车辆的横向运动控制方法的第一实施例中，所述车辆的横向运动控制方法包括步骤：

步骤S10，根据当前时刻的车辆位置和车道线方程，获得车道线上与当前时刻车辆位置对应的初始位置；

在本方案中，控制终端会根据车辆的前视摄像头采集的前方道路图像，实时构建出车道线方程，该车道线方程为三次方程y＝C₃x³+C₂x²+C₁x+C₀，该方程中x和y分别代表车道线上任一点位置的x坐标和y坐标，C₀、C₁、C2和C₃为系数。控制终端会根据当前时刻的车辆位置(x_车，y_车)和车道线方程，获得车道线上与当前时刻车辆位置对应的初始位置(x₀，y₀)，具体可以是，将车道线上与当前时刻的车辆位置具有相同的横坐标的位置作为初始位置，又或者，将车道线上与当前时刻的车辆位置的距离最小的位置作为初始位置。

步骤S20，根据预设时间间隔、当前时刻车速、初始位置和车道线方程，获得车道线上沿车辆前进方向的多个预测位置；

控制终端会获取当前时刻车辆的车速，然后根据将当前时刻车速乘以预设时间间隔，获得一个距离值，然后以初始位置为起始点，沿车辆前进方向，依次选择多个预测位置，起始位置和多个预测位置中沿车辆前进方向的依次相邻的两个位置的横坐标的差值为该距离值。本实施例中对预测位置的数量不作限定，预测位置数量越多，其最终获得的结果越有利于车辆的控制，但整个分析过程数据量比较大，其处理时间会比较长，一般优选六个预测位置。步骤S30，根据车辆固有参数、预设的初始状态矩阵计算公式、车道线方程、当前时刻车速、当前时刻车辆状态参数、当前时刻车辆位置信息、初始位置和多个预测位置，获得初始位置和多个预测位置分别对应的方向盘期望角度；

本实施例中，车辆固有参数为车辆固有的属性参数，包括车辆轴距、车辆前轮至车辆质心的纵向距离、车辆后轮至质心的纵向距离，车辆质量、车辆前轮侧偏刚度、车辆后轮侧偏刚度、车辆各挡位的传动比等。车辆状态参数为车辆中会发生变化的参数包括车辆的运行挡位、车辆的前轮转角、车辆质心的纵向速度和车辆质心处的横向方向惯性加速度等。本实施例中，是基于二自由度单车模型建立的车辆状态控制方程，然后根据车辆状态控制方程和车道线方程建立了预设的初始状态矩阵计算公式，该公式如下所示：

其中，L为车辆轴距，V_el为车速，θ_fw为前轮转角，ρ_des为初始位置处的道路曲率半径。控制终端。

控制终端会根据车辆固有参数、预设的初始状态矩阵计算公式、车道线方程、当前时刻车速、当前时刻车辆状态参数、当前时刻车辆位置信息、初始位置和多个预测位置，获得初始位置的方向盘期望角度和多个预测位置分别对应的方向盘期望角度。该过程具体如下。

步骤S30包括：

步骤S31，根据初始位置、预测位置和车道线方程，获取初始位置的道路曲率和预测位置的道路曲率；

步骤S32，根据车辆固有参数、车道线方程、当前时刻车速、当前时刻车辆状态参数、初始位置的道路曲率和预设的初始状态矩阵计算公式，计算获得当前时刻的初始状态矩阵；

控制终端会根据车道线方程和初始位置，计算初始位置处的道路曲率，根据车道线方程和预测位置，计算各预测位置处的道路曲率。获取初始位置处的道路曲率后，根据初始位置处的道路曲率会获得初始位置处的曲率半径，控制终端会将车辆固有参数中的车辆轴距、当前时刻的车速、当前时刻的车辆状态参数中的车辆前轮转角、初始位置处的曲率半径以及车道线方程中的系数C₁和C₀输入至预设的初始状态矩阵计算公式，计算获得当前时刻的初始状态矩阵X₀。

步骤S33，根据车辆固有参数、当前时刻的车速、当前时刻的初始状态矩阵、当前时刻的车辆状态参数、初始位置的道路曲率和预设的LQR计算模型，计算初始位置对应的方向盘角度的前馈量和反馈量；

步骤S34，将初始位置对应的方向盘角度的前馈量和反馈量的总和作为初始位置对应的方向盘期望角度；

预设的LQR计算模型中的参数Q设置为4x4的主对角矩阵，参数R设置为1x1的矩阵；矩阵R中元素的数值设置为矩阵Q的10倍至100倍，更容易获得能量函数的收敛，减少迭代次数。控制终端会采用当前时刻的初始状态矩阵和预设的LQR计算模型计算获得K矩阵，然后采用K矩阵、车辆固有参数、当前时刻的车速、当前时刻的车辆状态参数和初始位置的道路曲率，计算初始位置对应的方向盘角度的前馈量，其前馈量计算公式为

其中，R为初始位置的道路曲率半径，l_f为前轮至质心的纵向距离，l_r为后轮至质心的纵向距离，L＝l_f+l_r为车辆轴距；m为整车质量，V_x为车辆质心的纵向速度，k_v为不足转向率，a_y为车辆质心处横向方向的惯性加速度，k₃为矩阵K＝[k₁，k₂，k₃，k₄]中第三项值，C_ar为后轮侧偏刚度，C_af为前轮侧偏刚度，m_f＝ml_f/L为汽车作用在前轴上的质量，m_r＝ml_r/L为汽车作用在后轴上的质量，C_传动为当前时刻的车辆运行挡位对应的传动比。

控制终端计算K矩阵和初始状态矩阵的乘积作为初始位置对应的方向盘角度的反馈量。控制终端在获得初始位置对应的方向盘角度的前馈量和反馈量后，将两者的总和作为初始位置对应的方向盘期望角度θ_des,0。

步骤S35，根据当前时刻的车辆位置、当前时刻的车速方向、初始位置对应的方向盘期望角度和预测位置，获得预测位置对应的状态矩阵偏差；

控制终端会以当前时刻的车辆位置、车辆车速的方向为切向和当前初始位置对应的方向盘期望角度θ_des,0计算获得一个圆心坐标(O_a,O_b)和转弯半径R₀的圆。分别将该圆的圆心与各预测位置连接，获得各连线分别与该圆的交点(x_n，y_n)，然后根据交点与对应的预测位置，计算各预测位置的状态矩阵偏差ΔX_n＝[横向误差，航向误差，横向误差率，航向误差率]^T，其中横向误差为计算交点和对应的预测位置在当前位置坐标系下的横向的偏差，通常这个偏差较小，也可以用两点间的距离代替；航向误差为计算交点在该圆上的切线方向与交点对应的预测位置在车道线上的切线方向的角度差；横向误差率为计算当前时刻的车速乘以航向误差；航向误差率为当前时刻车速乘以交点处的曲率和交点对应的预测位置处的曲率间的差值。

步骤S36，根据车辆固有参数、当前时刻的车速、当前时刻的车辆状态参数、当前时刻的初始状态矩阵、预测位置对应的状态矩阵偏差、预测位置的道路曲率和预设的LQR计算模型，计算预测位置对应的方向盘角度的前馈量和反馈量；

步骤S37，将预测位置对应的方向盘角度的前馈量和反馈量的总和作为预测位置对应的方向盘期望角度。

控制终端在计算获得各预测位置的状态矩阵偏差ΔX_n，将初始状态矩阵X0和预测位置对应的状态矩阵偏差的总和作为各预测位置对应的状态矩阵。然后根据车辆固有参数、当前时刻的车速、当前时刻的车辆状态参数、各预测位置对应的状态矩阵、预测位置的道路曲率和预设的LQR计算模型，计算预测位置对应的方向盘角度的前馈量和反馈量，其前馈量和反馈量的具体计算过程与初始位置对应的方向盘角度的前馈量和反馈量的计算过程相同，在此不再赘述。将两者的总和作为预测位置对应的方向盘期望角度。

步骤S40，将初始位置和多个预测位置分别对应的方向盘期望角度的平均值作为当前时刻车辆的方向盘期望角度；

控制终端在计算获得初始位置和各预测位置对应的方向盘期望角度后，会对这些方向盘期望角度进行取平均，将其平均值作为当前时刻车辆的方向盘期望角度。

步骤S50，将当前时刻的方向盘实际角度和方向盘期望角度，输入至预设的闭环PID控制算法中，获得当前时刻的目标扭矩。

控制终端会将计算获得的当前时刻车辆的方向盘期望角度作为闭环PID控制算法的输入参数，输入至闭环PID控制算法中进行计算，输出当前时刻车辆跟随车道线的目标扭矩。

具体地的，请参照图3，图3为本申请一实施例中步骤S50的流程细化示意图，基于上述实施例，步骤S50包括：

步骤S51，根据当前时刻的方向盘实际角度和方向盘期望角度以及前一时刻的方向盘实际角度和方向盘期望角度，获得当前时刻的实际角度变化率、期望角度变化率、角度差值和角度变化率的差值；

控制终端计算前一时刻的方向盘实际角度和当前时刻的方向盘实际角度的平均值作为当前时刻的实际角度滤波值

其中，

为当前时刻的实际角度滤波值，θ_act,k为当前时刻的方向盘实际角度，θ_act,k-1为前一时刻的方向盘实际角度。

控制终端将计算获得的当前时刻的实际角度滤波值以及前一时刻的实际角度滤波值、相邻时刻间的时间差和预设的实际角度变化率阈值范围输入至第一角度变化率计算公式中，计算获得当前时刻的第一角度变化率

其中，第一角度变化率计算公式为：

为当前时刻的第一角度变化率，

为前一时刻的实际角度滤波值，ΔT为当前时刻与前一时刻的时间差。

需要说明的时，为了防止计算出的当前时刻的第一角度变化率的数值偏大，会采用一个预设第一角度变化率阈值范围对计算出的第一角度变化率进行限制，将限制后的第一角度变化率作为最终的当前时刻的第一角度变化率，例如预设第一角度变化率阈值范围为-90deg/s～90deg/s，若计算出的第一角度变化率处于该预设第一角度变化率阈值范围内，则直接将计算出的第一角度变化率作为当前时刻的最终第一角度变化率，若计算出的第一角度变化率小于该预设第一角度变化率阈值范围的下限值，则将该预设第一角度变化率阈值范围的下限值作为最终的当前时刻的第一角度变化率，若计算出的第一角度变化率大于该预设第一角度变化率阈值范围的上限值，则将该预设第一角度变化率阈值范围的上限值作为最终的当前时刻的第一角度变化率。

控制终端会将当前时刻的第一角度变化率和前一时刻的第一角度变化率的平均值作为当前时刻的实际角度变化率

其中，

为当前时刻的实际角度变化率，

为前一时刻的实际角度变化率。

控制终端将当前时刻的方向盘期望角度、当前时刻的实际角度滤波值、预设的期望变化率阈值范围和相邻时刻间的时间差输入至第二角度变化率计算公式中，计算获得当前时刻的第二角度变化率

其中，第二角度变化率计算公式为：

为当前时刻的第二角度变化率，θ_des,k为当前时刻的方向盘期望角度。

需要说明的时，为了防止计算出的当前时刻的第二角度变化率的数值偏大，会采用一个预设第二角度变化率阈值范围对计算出的第二角度变化率进行限制，将限制后的第二角度变化率作为最终的当前时刻的第二角度变化率，例如预设第二角度变化率阈值范围为-180deg/s～180deg/s，若计算出的第二角度变化率处于该预设第二角度变化率阈值范围内，则直接将计算出的第二角度变化率作为当前时刻的最终第二角度变化率，若计算出的第二角度变化率小于该预设第二角度变化率阈值范围的下限值，则将该预设第二角度变化率阈值范围的下限值作为最终的当前时刻的第二角度变化率，若计算出的第二角度变化率大于该预设第二角度变化率阈值范围的上限值，则将该预设第二角度变化率阈值范围的上限值作为最终的当前时刻的第二角度变化率。

控制终端将当前时刻的第二角度变化率和前一时刻的第二角度变化率的平均值，作为当前时刻的期望角度变化率

其中，

为当前时刻的期望角度变化率，

为前一时刻的第二角度变化率。

控制终端将当前时刻的方向盘期望角度和方向盘实际角度的差值，作为当前时刻的方向盘角度偏差值

控制终端将当前时刻的方向盘角度偏差值和前一时刻的方向盘角度偏差值的平均值，作为当前时刻的角度差值θ_diff,k，

其中，θ_diff,k为当前时刻的角度差值，

为前一时刻的方向盘角度偏差值。

控制终端将当前时刻的期望角度变化率和实际角度变化率间差值，作为当前时刻的方向盘角度变化率偏差

控制终端获取当前时刻的方向盘角度变化率偏差和前一时刻的方向盘角度变化率偏差的平均值，作为当前时刻的角度变化率差值，

其中，

为当前时刻的方向盘角度变化率差值，

为前一时刻的方向盘角度变化率偏差。

步骤S52，根据当前时刻的方向盘实际角度、方向盘期望角度、实际角度变化率、期望角度变化率和角度差值，获得当前时刻的第一输出扭矩；

控制终端会先判断当前时刻的角度差值的绝对值是否小于预设阈值，例如0.01，若θ_diff,k的绝对值小于预设阈值，则确定当前时刻的衰减系数为第一预设衰减值，例如0.5，若θ_diff,k的绝对值大于或等于预设阈值，则判断当前时刻的角度差值θ_diff,k的绝对值与当前时刻的方向盘期望角度θ_des,k的比值是否大于预设比值阈值，例如预设比值阈值为0.2，又或者判断当前时刻的角度差值θ_diff,k与当前时刻的期望角度变化率

是否同为正或同为负，若该比值大于或等于预设比值阈值，或者θ_diff,k与

同为正或同为负，则确定当前时刻的衰减系数为第二预设衰减值，例如第二预设衰减值取1，第二预设衰减值大于第一预设衰减值，若θ_diff,k的绝对值大于或等于预设阈值、θ_diff,k的绝对值与θ_des,k的比值大于或等于预设比值阈值并且θ_diff,k与

中一个为正数一个为负数，则将当前时刻的方向盘实际角度θ_act,k、方向盘期望角度θ_des,k、实际角度变化率

期望角度变化率

和角度差值θ_diff,k输入至预设衰减系数计算公式中，计算获得第三预设衰减值，作为当前时刻的衰减系数，其中，预设衰减系数计算公式为：

控制终端获得当前时刻的衰减系数后，会将当前时刻的衰减系数和角度差值输入至预设的第一输出扭矩计算公式中，计算获得当前时刻的第一输出扭矩，其中预设的第一输出扭矩计算公式为：

T_p,k＝f_Bpf_vpf_rθ_diff,k，T_p,k为当前时刻的第一输出扭矩，f_Bp为第一预设系数，取值范围可以为0.1～2.0，f_vp为第二预设系数，取值范围可以为0.1～2.0。

步骤S53，根据当前时刻的角度差值和前一时刻的第二输出扭矩，获得当前时刻的第二输出扭矩；

控制终端会将当前时刻的角度差值θ_diff,k输入至预设的介入因子计算公式中，计算获得当前时刻的介入因子α_i，其中，预设的介入因子计算公式为：

θ_l为预设下限值，θ_h为预设上限值。

控制终端计算获得当前时刻的介入因子后，会根据预设的介入因子与重置因子映射关系，获得当前时刻的介入因子对应的重置因子，例如当当前时刻的介入因子α_i为0时，对应的重置因子α_r为0，当当前时刻的介入因子α_i不为0时，对应的重置因子α_r为1。

控制终端将前一时刻的第二输出扭矩、当前时刻的角度差值θ_diff,k、介入因子α_i和重置因子α_r输入至预设的第二输出扭矩计算公式中，计算获得当前时刻的第二输出扭矩，其中预设的第二输出扭矩计算公式为：。

T_i,k＝α_r(f_Biθ_diff,k+α_iT_i,k-1)，T_i,k为当前时刻的第二输出扭矩，T_i,k-1为前一时刻的第二输出扭矩，f_Bi为第三预设系数，取值范围可以为0.01～0.1。

步骤S54，根据当前时刻的角度差值和角度变化率差值，获得当前时刻的第三输出扭矩；

控制终端在计算第三输出扭矩前，仍然会采用当前时刻的角度差值计算当前时刻的介入因子与重置因子，其计算过程与计算第二输出扭矩过程中的介入因子与重置因子的计算过程相同，在此不再赘述。

控制终端将当前时刻的角度变化率差值

介入因子和重置因子输入至预设的第三输出扭矩计算公式中，计算获得当前时刻的第三输出扭矩，其中预设的第三输出扭矩计算公式为

T_d,k为当前时刻的第三输出扭矩，f_Bd为第四预设系数，取值范围可以为0.1～2.0，f_vd为第五预设系数，取值范围可以为0.01～0.1。

步骤S55，将当前时刻的第一输出扭矩、第二输出扭矩和第三输出扭矩的总和，作为当前时刻的目标扭矩。

控制终端将最终获得的当前时刻的第一输出扭矩、第二输出扭矩和第三输出扭矩的总和，作为当前时刻的目标扭矩。

需要说明的时，为了防止最终计算处的目标扭矩的数值偏大，会采用一个预设扭矩阈值范围对其目标扭矩进行限制，将限制后的目标扭矩作为当前时刻的最终目标扭矩，例如预设扭矩阈值范围为-3Nm～3Nm，若计算出的目标扭矩处于该预设扭矩阈值范围内，则直接将计算出的目标扭矩作为当前时刻的最终目标扭矩，若计算出的目标扭矩小于该预设扭矩阈值范围的下限值，则将该预设扭矩阈值范围的下限值作为当前时刻的最终目标扭矩，若计算出的目标扭矩大于该预设扭矩阈值范围的上限值，则将该预设扭矩阈值范围的上限值作为当前时刻的最终目标扭矩。

本实例通过根据当前时刻的车辆位置和车道线方程，获得车道线上与当前时刻车辆位置对应的初始位置；根据预设时间间隔、当前时刻车速、初始位置和车道线方程，获得车道线上沿车辆前进方向的多个预测位置；根据车辆固有参数、预设的初始状态矩阵计算公式、车道线方程、当前时刻车速、当前时刻车辆状态参数、当前时刻车辆位置信息、初始位置和多个预测位置，获得初始位置和多个预测位置分别对应的方向盘期望角度；将初始位置和多个预测位置分别对应的方向盘期望角度的平均值作为当前时刻车辆的方向盘期望角度；将当前时刻的方向盘实际角度和方向盘期望角度，输入至预设的闭环PID控制算法中，获得当前时刻的目标扭矩。本方案中不需要进行标定，减少车辆标定工作所需要的时间，减少反复调试次数，并且通过多点预瞄的角度规划以及方向盘角度的闭环与控制扭矩的闭环的双闭环控制，实现车辆精确控制，提高车辆控制稳定性。

进一步地，根据本申请车辆的横向运动控制方法的第一实施例提出本申请车辆的横向运动控制方法的第二实施例，在本实施例中，步骤S33之前还包括：

步骤S38，根据当前时刻车速和预设的修正系数与车速间的映射关系，获得当前时刻的修正系数；

所述步骤S33包括：

步骤S331，根据车辆固有参数、当前时刻的车速、当前时刻的初始状态矩阵、当前时刻的车辆状态参数、初始位置的道路曲率、当前时刻的修正系数和预设的LQR计算模型，计算初始位置对应的方向盘角度的前馈量和反馈量；

所述步骤S36包括：

步骤S361，根据车辆固有参数、当前时刻的车速、当前时刻的车辆状态参数、当前时刻的初始状态矩阵、预测位置对应的状态矩阵偏差、预测位置的道路曲率、当前时刻的修正系数和预设的LQR计算模型，计算预测位置对应的方向盘角度的前馈量和反馈量。

在本实施例中，考虑到不同车速下车辆的稳定性，会对LQR模型中预设的矩阵Q乘以一个随车速变化而变化的系数进行修正。

故控制终端在采用预设的LQR计算模型计算初始位置对应的方向盘角度的前馈量和反馈量前，会根据当前时刻车速和预设的修正系数与车速间的映射关系，获得当前时刻的修正系数。

在计算初始位置对应的方向盘角度的前馈量和反馈量过程中，控制终端根据车辆固有参数、当前时刻的车速、当前时刻的车辆状态参数、当前时刻的修正系数、初始位置对应的初始状态矩阵、初始位置的道路曲率和预设的LQR计算模型，计算初始位置对应的方向盘角度的前馈量和反馈量。

在计算预测位置对应的方向盘角度的前馈量和反馈量过程中，控制终端根据车辆固有参数、当前时刻的车速、当前时刻的车辆状态参数、当前时刻的修正系数、当前时刻的初始状态矩阵、预测位置对应的状态矩阵偏差、预测位置的道路曲率和预设的LQR计算模型，计算预测位置对应的方向盘角度的前馈量和反馈量。

本实施例通过考虑到不同车速下车辆的稳定性，矩阵Q的元素乘以一个随车速变化而变化的系数，从而促进车辆的稳定控制。

本发明还提出一种计算机可读存储介质，其上存储有计算机程序。所述计算机可读存储介质可以是图1的终端中的存储器02，也可以是如ROM(Read-Only Memory，只读存储器)/RAM(Random Access Memory，随机存取存储器)、磁碟、光盘中的至少一种，所述计算机可读存储介质包括若干信息用以使得控制终端执行本发明各个实施例所述的方法。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种车辆的横向运动控制方法，其特征在于，包括步骤：

根据当前时刻的车辆位置和车道线方程，获得车道线上与当前时刻车辆位置对应的初始位置；

根据预设时间间隔、当前时刻车速、初始位置和车道线方程，获得车道线上沿车辆前进方向的多个预测位置；

根据车辆固有参数、预设的初始状态矩阵计算公式、车道线方程、当前时刻车速、当前时刻车辆状态参数、当前时刻车辆位置信息、初始位置和多个预测位置，获得初始位置和多个预测位置分别对应的方向盘期望角度；

将初始位置和多个预测位置分别对应的方向盘期望角度的平均值作为当前时刻车辆的方向盘期望角度；

将当前时刻的方向盘实际角度和方向盘期望角度，输入至预设的闭环PID控制算法中，获得当前时刻的目标扭矩。

2.根据权利要求1所述的车辆的横向运动控制方法，其特征在于，所述根据车辆固有参数、预设的初始状态矩阵计算公式、车道线方程、当前时刻车速、当前时刻车辆状态参数、当前时刻车辆位置信息、初始位置和多个预测位置，获得初始位置和多个预测位置分别对应的方向盘期望角度的步骤包括：

根据初始位置、预测位置和车道线方程，获取初始位置的道路曲率和预测位置的道路曲率；

根据车辆固有参数、车道线方程、当前时刻车速、当前时刻车辆状态参数、初始位置的道路曲率和预设的初始状态矩阵计算公式，计算获得当前时刻的初始状态矩阵；

根据车辆固有参数、当前时刻的车速、当前时刻的初始状态矩阵、当前时刻的车辆状态参数、初始位置的道路曲率和预设的LQR计算模型，计算初始位置对应的方向盘角度的前馈量和反馈量；

将初始位置对应的方向盘角度的前馈量和反馈量的总和作为初始位置对应的方向盘期望角度；

根据当前时刻的车辆位置、当前时刻的车速方向、初始位置对应的方向盘期望角度和预测位置，获得预测位置对应的状态矩阵偏差；

根据车辆固有参数、当前时刻的车速、当前时刻的车辆状态参数、当前时刻的初始状态矩阵、预测位置对应的状态矩阵偏差、预测位置的道路曲率和预设的LQR计算模型，计算预测位置对应的方向盘角度的前馈量和反馈量；

将预测位置对应的方向盘角度的前馈量和反馈量的总和作为预测位置对应的方向盘期望角度。

3.根据权利要求2所述的车辆的横向运动控制方法，其特征在于，所述根据车辆固有参数、当前时刻的车速、当前时刻的初始状态矩阵、当前时刻的车辆状态参数、初始位置的道路曲率和预设的LQR计算模型，计算初始位置对应的方向盘角度的前馈量和反馈量的步骤之前还包括：

根据当前时刻车速和预设的修正系数与车速间的映射关系，获得当前时刻的修正系数；

所述根据车辆固有参数、当前时刻的车速、当前时刻的初始状态矩阵、当前时刻的车辆状态参数、初始位置的道路曲率和预设的LQR计算模型，计算初始位置对应的方向盘角度的前馈量和反馈量的步骤包括：

根据车辆固有参数、当前时刻的车速、当前时刻的初始状态矩阵、当前时刻的车辆状态参数、初始位置的道路曲率、当前时刻的修正系数和预设的LQR计算模型，计算初始位置对应的方向盘角度的前馈量和反馈量；

所述根据车辆固有参数、当前时刻的车速、当前时刻的车辆状态参数、当前时刻的初始状态矩阵、预测位置对应的状态矩阵偏差、预测位置的道路曲率和预设的LQR计算模型，计算预测位置对应的方向盘角度的前馈量和反馈量的步骤包括：

根据车辆固有参数、当前时刻的车速、当前时刻的车辆状态参数、当前时刻的初始状态矩阵、预测位置对应的状态矩阵偏差、预测位置的道路曲率、当前时刻的修正系数和预设的LQR计算模型，计算预测位置对应的方向盘角度的前馈量和反馈量。

4.根据权利要求1至3中任一项所述的车辆的横向运动控制方法，其特征在于，所述将当前时刻的方向盘实际角度和方向盘期望角度，输入至预设的闭环PID控制算法中，获得当前时刻的目标扭矩的步骤包括：

根据当前时刻的方向盘实际角度和方向盘期望角度以及前一时刻的方向盘实际角度和方向盘期望角度，获得当前时刻的实际角度变化率、期望角度变化率、角度差值和角度变化率的差值；

根据当前时刻的方向盘实际角度、方向盘期望角度、实际角度变化率、期望角度变化率和角度差值，获得当前时刻的第一输出扭矩；

根据当前时刻的角度差值和前一时刻的第二输出扭矩，获得当前时刻的第二输出扭矩；

根据当前时刻的角度差值和角度变化率差值，获得当前时刻的第三输出扭矩；

将当前时刻的第一输出扭矩、第二输出扭矩和第三输出扭矩的总和，作为当前时刻的目标扭矩。

5.根据权利要求4所述的车辆的横向运动控制方法，其特征在于，所述根据当前时刻的方向盘实际角度和方向盘期望角度以及前一时刻的方向盘实际角度和方向盘期望角度，获得当前时刻的实际角度变化率、期望角度变化率、角度差值和角度变化率的差值的步骤包括：

计算前一时刻的方向盘实际角度和当前时刻的方向盘实际角度的平均值作为当前时刻的实际角度滤波值；

根据当前时刻的实际角度滤波值、前一时刻的实际角度滤波值、相邻时刻间的时间差和预设的实际角度变化率阈值范围，获得当前时刻的第一角度变化率；

将当前时刻的第一角度变化率和前一时刻的第一角度变化率的平均值作为当前时刻的实际角度变化率；

根据当前时刻的方向盘期望角度、当前时刻的实际角度滤波值、预设的期望变化率阈值范围和相邻时刻间的时间差，获得当前时刻的第二角度变化率；

将当前时刻的第二角度变化率和前一时刻的第二角度变化率的平均值，作为当前时刻的期望角度变化率；

获取当前时刻的方向盘期望角度和方向盘实际角度的差值，作为当前时刻的方向盘角度偏差值；

获取当前时刻的方向盘角度偏差值和前一时刻的方向盘角度偏差值的平均值，作为当前时刻的角度差值；

获取当前时刻的期望角度变化率和实际角度变化率间差值，作为当前时刻的方向盘角度变化率偏差；

获取当前时刻的方向盘角度变化率偏差和前一时刻的方向盘角度变化率偏差的平均值，作为当前时刻的角度变化率差值。

6.根据权利要求5所述的车辆的横向运动控制方法，其特征在于，所述根据当前时刻的方向盘实际角度、方向盘期望角度、实际角度变化率、期望角度变化率和角度差值，获得当前时刻的第一输出扭矩的步骤包括：

根据预设衰减系数计算公式、当前时刻的方向盘实际角度、方向盘期望角度、实际角度变化率、期望角度变化率和角度差值，获得当前时刻的衰减系数；

根据预设的第一输出扭矩计算公式、当前时刻的衰减系数和角度差值，获得当前时刻的第一输出扭矩。

7.根据权利要求6所述的车辆的横向运动控制方法，其特征在于，所述根据当前时刻的角度差值和前一时刻的第二输出扭矩，获得当前时刻的第二输出扭矩的步骤包括：

根据预设的介入因子计算公式和当前时刻的角度差值，获得当前时刻的介入因子；

根据预设的介入因子与重置因子映射关系和当前时刻的介入因子，获得当前时刻的重置因子；

根据预设的第二输出扭矩计算公式、前一时刻的第二输出扭矩、当前时刻的角度差值、介入因子和重置因子，获得当前时刻的第二输出扭矩。

8.根据权利要求7所述的车辆的横向运动控制方法，其特征在于，所述根据当前时刻的角度差值和角度变化率差值，获得当前时刻的第三输出扭矩的步骤包括：

根据预设的介入因子计算公式和当前时刻的角度差值，获得当前时刻的介入因子；

根据预设的介入因子与重置因子映射关系和当前时刻的介入因子，获得当前时刻的重置因子；

根据预设的第三输出扭矩计算公式、当前时刻的角度变化率差值、介入因子和重置因子，获得当前时刻的第三输出扭矩。

9.一种控制终端，其特征在于，所述控制终端包括存储器、处理器和存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如权利要求1至8中任一项所述的车辆的横向运动控制方法的步骤。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至8中任一项所述的车辆的横向运动控制方法的步骤。

Images