CN112590802B - 车辆驾驶控制方法、装置、车辆以及计算机可读存储介质 - Google Patents

Abstract

本发明提供一种基于LQR的车道居中驾驶控制方法，所述方法包括：根据车道线计算车道中心线以使车辆大致位于车道中心线上；根据车辆坐标系沿车头航向角方向预瞄一段距离X₀计算横向偏移量D₀、预瞄一段距离X₁计算横向偏移量D₁、预瞄一段距离X₂计算横向偏移量D₂，根据X₁、X₂、D₁、D₂计算相对航向角ε，通过LQR横向控制算法计算方向转角以对车辆进行横向控制。本发明能够提高车辆自动驾驶的稳定性。

Description

车辆驾驶控制方法、装置、车辆以及计算机可读存储介质

技术领域

本发明涉及自动驾驶技术领域，尤其涉及一种车辆驾驶控制方法、装置、车辆以及计算机可读存储介质。

背景技术

自动驾驶车辆行驶在车载摄像头可识别车道线的结构化道路上，当驾驶员激活车道居中系统时，汽车的摄像头传感器会跟踪车道标记，并通过使用汽车的主动转向系统来保持航向，帮助司机保持前方的稳定路径，使车辆控制在车道中心行驶，在车速过快或者曲率变化率过大的情况下运动学模型的方法无法满足车辆的稳定性控制要求。

发明内容

鉴于此，为了在一定程度上解决相关技术中的技术问题之一，有必要提供一种车辆驾驶控制方法、装置、车辆以及计算机可读存储介质，提高车辆自动驾驶的稳定性。

本发明第一方面提供一种基于LQR的车道居中驾驶控制方法，所述方法包括：

根据车道线计算车道中心线以使车辆大致位于车道中心线上；

根据车辆坐标系沿车头航向角方向预瞄一段距离X₀计算横向偏移量D₀、预瞄一段距离X₁计算横向偏移量D₁、预瞄一段距离X₂计算横向偏移量D₂，其中：X₁≠X₂；

根据X₁、X₂、D₁、D₂计算相对航向角ε，ε的计算公式为：

其中，D₀为横向误差，D_{0_last}为上一帧的横向误差，D₀-D_{0_last}为横向误差率，ε为航向误差，ε_last为上一帧的航向误差，ε-ε_last为航向误差率；

通过LQR横向控制算法计算方向转角以对车辆进行横向控制。

进一步的，所述根据车道线计算车道中心线，包括：

根据摄像头输入的车道线信息，计算车辆与左边车道线的距离以及车辆与右边车道线的距离；

车辆与左边车道线的距离以及车辆与右边车道线的距离之和为车道宽度值。

进一步的，当道路只有一边车道线时，计算车辆与该车道线的距离；

若该距离在不合理范围内，则将该距离数据剔除并使用上一帧距离值计算车道中心线。

进一步的，在行驶过程中，存储左右车道线距离值的多帧数据以计算车道宽度；

根据车道宽度和视觉识别到的车道线坐标平移获得车道中心点；

对离散的车道中心点进行三次样条拟合；

对拟合的在车辆坐标系下的车道中心线计算横向误差、横向误差率、航向误差、航向误差率。

本发明第二方面提供一种基于LQR的车道居中驾驶控制装置，所述装置包括：

第一计算模块，用于根据车道线计算车道中心线以使车辆大致位于车道中心线上；

预瞄模块，用于根据车辆坐标系沿车头航向角方向预瞄一段距离X₀计算横向偏移量D₀、预瞄一段距离X₁计算横向偏移量D₁、预瞄一段距离X₂计算横向偏移量D₂，其中：X₁≠X₂；

第二计算模块，用于根据X₁、X₂、D₁、D₂计算相对航向角ε，ε的计算公式为：

其中，D₀为横向误差，D_{0_last}为上一帧的横向误差，D₀-D_{0_last}为横向误差率，ε为航向误差，ε_last为上一帧的航向误差，ε-ε_last为航向误差率；

第三计算模块，用于通过LQR横向控制算法计算方向转角以对车辆进行横向控制。

进一步的，所述第一计算模块具体用于：

根据摄像头输入的车道线信息，计算车辆与左边车道线的距离以及车辆与右边车道线的距离；

车辆与左边车道线的距离以及车辆与右边车道线的距离之和为车道宽度值。

进一步的，当道路只有一边车道线时，计算车辆与该车道线的距离；

若该距离在不合理范围内，则将该距离数据剔除并使用上一帧距离值计算车道中心线。

进一步的，在行驶过程中，存储左右车道线距离值的多帧数据以计算车道宽度；

根据车道宽度和视觉识别到的车道线坐标平移获得车道中心点；

对离散的车道中心点进行三次样条拟合；

对拟合的在车辆坐标系下的车道中心线计算横向误差、横向误差率、航向误差、航向误差率。

本发明第三方面提供一种车辆，所述车辆包括：

处理器；以及

存储装置，该存储装置存储有计算机程序，所述计算机程序在由处理器执行时用于执行所述的基于LQR的车道居中驾驶控制方法的步骤。

本发明第四方面提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现所述的基于LQR的车道居中驾驶控制方法的步骤。

通过以上方案可知，本发明根据车道线计算车道中心线以使车辆大致位于车道中心线上，根据车辆坐标系沿车头航向角方向预瞄一段距离X₀计算横向偏移量D₀、预瞄一段距离X₁计算横向偏移量D₁、预瞄一段距离X₂计算横向偏移量D₂，根据X₁、X₂、D₁、D₂计算相对航向角ε，通过LQR横向控制算法计算方向转角以对车辆进行横向控制，因此，本发明基于视觉方法得到LQR计算所需的状态偏差，从而得到期望方向盘转角，在不同速度的情况下以及在弯道场景下横向偏离距离小，车道居中驾驶稳定性高。

附图说明

图1为车辆上的作用力示意图。

图2为单车模型示意图。

图3为车辆轮胎受到的横向压力示意图。

图4为本发明的具体实施方式的方法流程图。

图5为车辆居中驾驶示意图。

图6为本发明的车辆原理图。

图7为本发明的一实施方式的基于LQR的车道居中驾驶控制装置的结构示意图。

如下具体实施方式将结合上述附图进一步说明本发明。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得所有其他实施例，都属于本发明的保护范围。可以理解的是，附图仅仅提供参考与说明用，并非用来对本发明加以限制。

在本发明中，车辆线性二自由度动力学模型如下：

正常情况下，如图1所示，车辆上的作用力沿着三个不同的轴分布：

(1)纵轴上的力包括驱动力和制动力，以及滚动阻力和拖拽阻力作滚摆运动；

(2)横轴上的力包括转向力、离心力和侧风力，汽车绕横轴作俯仰运动；

(3)立轴上的力包括车辆上下振荡施加的力，汽车绕立轴作偏摆或转向运动。

在单车模型假设的前提下，仅考虑纯侧偏轮胎特性，忽略轮胎力的纵横向耦合关系，用单车模型来描述车辆的运动，不考虑载荷的左右转移，忽略横纵向空气动力学，即可构建车辆的动力学模型。

如图2所示，oxyz为固定于车身的车辆坐标系，OXY为固定于地面的惯性坐标系。单车模型的车辆具有2个自由度：绕z轴的横摆运动，和沿x轴的纵向运动。纵向指沿物体前进方向，横向(或侧向)指垂直纵向方向。

横向运动：出自横向的风力，以及曲线行驶时的离心力等。

纵向运动：受总驱动阻力、加速、减速等的影响。总驱动阻力由滚动阻力、拖拽阻力和坡度阻力等构成。

滑移角(slip-angle)：轮胎方向和轮胎速度方向的夹角。滑移角的产生主要是由于车轮所受合力方向并非朝向车轮行进方向，但车轮的偏移角通常较小。

根据牛顿第二定律，分别沿x轴、y轴和z轴作受力分析：

在x轴方向上：

ma_x＝F_xf+F_xr (1)

在y轴方向上：

ma_y＝F_yf+F_yr (2)

在z轴方向上：

其中，m为整车质量，Iz为车辆绕z轴转动的转动惯量。x轴方向的运动(绕纵轴的滚动运动)可暂不用考虑。

y轴方向加速度ay由两部分构成：y轴方向的位移相关的加速度和向心加速度：

公式2可变为：

如图3所示，由于轮胎受到的横向压力，轮胎会有一个很小的滑移角，前轮滑移角为：

α_f＝δ-θ_Vf (5)

其中，θ_Vf为前轮速度方向，δ为前轮转角。

后轮滑移角，因为为前驱，后轮转角为零α_r＝-θ_Vr(6)，其中，θ_Vr为后轮速度方向。

则前轮所受的横向力为F_yf＝2C_αf(δ-θ_Vf)(7),后轮所受的横向力为F_yr＝2C_αr(-θ_Vr)(8),其中，C_αf，C_αr分别为前后轮的侧偏刚度(cornering stiffness)，由于车辆前后各两个轮，所以受力要乘以2。

结合图2，θ_Vf和θ_Vr可用下式计算

其中，l_f为前悬长度，l_r为后悬长度。

又因为

则公式9、公式10可近似转换为：

将公式5、公式6、公式11、公式12代入公式2、公式3中可得动力学模型：

横向控制主要通过控制轮胎转角实现，而对于驾驶员来说，可直接操控的是方向盘角度，因此在搭建车辆动力学模型时，可以以相对于道路的方向和距离误差为状态变量的动力学模型，亦即车辆路径跟踪偏差状态方程。

假设，e₁为横向误差，车辆质心距车道中心线的距离，e₂为航向误差，车辆纵向速度为V_x，车辆转弯半径为R，结合图1和图2，则：

车身转过期望角度所需转角速度：

所需横向加速度：

则横向加速度误差：

横行速度误差为：

航向误差：

将公式17、公式18带入公式3、公式4可得：

横向控制中，系统的状态变量有四个：横向误差e₁、横向误差率

航向误差e₂、航向误差率

综上，可得方向盘控制的动力学模型：

假设车辆系统的状态空间方程为：

由于

只对横摆角角度偏差变化率的导数产生影响，在横向控制中主要是控制横向偏差、横向偏差变化率、横摆角角度偏差、横摆角角度偏差变化率，因而忽略公式中(22)的

车辆系统的状态空间方程表示为：

上述连续的状态空间方程用于计算机控制需要对连续的状态空间方程进行离散化，其中A_t采用双线性变换得到：

同理则有：

其中T为控制周期。

通过上述介绍我们得到了车辆离散状态空间方程(21)中的A_t，B_t，B_1t，则系统的最优前轮转角为多元线性表达式：

LQR算法的核心就是定义和求解如下目标函数：

其中Q为状态权重系数，R为控制量权重系数。

当上述目标函数(27)最小时就得到最优的状态反馈矩阵K：

同时矩阵P满足黎卡提方程：

将公式(26)的控制量带入公式(23)得到系统状态反馈后的状态空间方程如下：

车辆沿固定曲率的轨迹运行时

不为零，因此通过LQR调节(A-BK)的特征值使系统趋于稳定，但是系统的稳态偏差并不为0。

为此在原有最优控制量的基础上增加一个前馈环节，使系统趋于稳定的同时系统的横向稳态偏差为0。

u(t)＝δ＝-Kx(t)+δ_ff (31)

公式中为前馈环节提供的前轮转角。将公式(31)带入公式(23)得到：

在此设初始条件为0，对公式(29)进行拉普拉斯变换得到：

假设汽车以固定纵向速度V_x沿某一固定曲率的弯道行驶，则通过纵向车速V_x和道路的半径R可以计算出期望汽车横摆角速度：

公式(34)可知横摆角速度的拉普拉斯变换结果为：

假设δ_ff为固定值，则其拉普拉斯变换结果为

根据终值定理，系统的稳态误差为：

将A,B,B₁,K带入公式(33)得到：

观察公式(38)中的第一项和第三项。可知道对横摆角角度偏差无影响，选择合适的可将横向偏差稳态值趋向于0。

要想横向偏差的稳态值趋于零，则：

因此由(38)可知：

令

不足转向梯度系数

前馈控制前轮转角公式(34)可以简化为：

最终的前轮转角的控制量为前馈控制前轮转角与最优状态反馈控制量之和，由公式(24)和(35)可知：δ_des＝δ_ff+u(t)(45)。

本发明主要提供一种基于LQR的车道居中驾驶控制方法，如图4所示，所述方法包括以下步骤：

S110：根据车道线计算车道中心线以使车辆大致位于车道中心线上。

具体的，如图5所示，车道中心线位于左右车道的中间处，在本实施方式中，可以根据摄像头输入的车道线信息，计算车辆与左边车道线的距离以及车辆与右边车道线的距离，车辆与左边车道线的距离以及车辆与右边车道线的距离之和为车道宽度值。

需要说明的是，当道路只有一边车道线时，计算车辆与该车道线的距离；若该距离在不合理范围内，则将该距离数据剔除并使用上一帧距离值计算车道中心线。

S120：根据车辆坐标系沿车头航向角方向预瞄一段距离X₀计算横向偏移量D₀、预瞄一段距离X₁计算横向偏移量D₁、预瞄一段距离X₂计算横向偏移量D₂。

D₀为横向误差，D_{0_last}为上一帧的横向误差，D₀-D_{0_last}为横向误差率。

S130：根据X₁、X₂、D₁、D₂计算相对航向角ε。

ε为航向误差，ε_last为上一帧的航向误差，ε-ε_last为航向误差率。ε的计算公式为：

S140：通过LQR横向控制算法计算方向转角以对车辆进行横向控制。

结合计算公式23-45。选择参数矩阵Q、R，求解黎卡提方程得到矩阵P，并可以得到最优的状态反馈矩阵K。根据车辆相关参数求解车辆动力学的A和B矩阵，再由A和B矩阵以及状态偏差(横向误差、横向误差率、航向误差以及航向误差率)通过LQR求解最优状态反馈方程。

在行驶过程中，存储左右车道线距离值的多帧数据以计算车道宽度，根据车道宽度和视觉识别到的车道线坐标平移获得车道中心点，对离散的车道中心点进行三次样条拟合，对拟合的在车辆坐标系下的车道中心线计算横向误差、横向误差率、航向误差、航向误差率。

在离散的车道中心点进行三次样条拟合得到车道中心线后，可以得到车道中心线的曲率，可以一并结合LQR求解得到的方向转角，综合得到期望方向盘转角值。

根据本发明提供的实施方式，在直车道上居中自动驾驶进行测试，测试如下：

在弯道车道上居中自动驾驶进行测试，测试如下：

本发明基于视觉方法得到LQR计算所需的状态偏差，从而得到期望方向盘转角，在不同速度的情况下以及在弯道场景下横向偏离距离小，车道居中驾驶稳定性高。

图6为本发明提供的车辆100的一种实施例的结构图，所述车辆100可包括处理器10、存储装置20，还包括存储在所述存储装置20中并可向所述处理器10上运行的计算机程序，例如控制程序等等。

所述处理器10可以是中央处理单元(CPU)，还可以是其他通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述车辆100的控制中心，利用各种接口和线路连接整个车辆100的各个部分。

所述存储装置20可用于存储所述计算机程序和/或模块，所述处理器10通过运行或执行存储在所述存储装置20内的计算机程序和/或模块，以及调用存储在存储装置20内的数据，实现所述车辆的各种功能。所述存储装置20可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作车辆、至少一个功能所需的程序等；存储数据区可存储根据手机的使用所创建的数据等。此外，存储装置20可以包括高速随机存取存储装置，还可以包括非易失性存储装置，例如硬盘、内存、插接式硬盘，智能存储卡，安全数字卡，闪存卡、至少一个磁盘存储装置件、闪存器件、或其他非易失性固态存储装置件。

图7为发明提供的基于LQR的车道居中驾驶控制装置200的结构示意图，所述基于LQR的车道居中驾驶控制装置200可以包括第一计算模块21、预瞄模块22、第二计算模块23、第三计算模块24。

第一计算模块21，用于根据车道线计算车道中心线以使车辆大致位于车道中心线上；

预瞄模块22，用于根据车辆坐标系沿车头航向角方向预瞄一段距离X₀计算横向偏移量D₀、预瞄一段距离X₁计算横向偏移量D₁、预瞄一段距离X₂计算横向偏移量D₂，其中：X₁≠X₂；

第二计算模块23，用于根据X₁、X₂、D₁、D₂计算相对航向角ε，ε的计算公式为：

其中，D₀为横向误差，D_{0_last}为上一帧的横向误差，D₀-D_{0_last}为横向误差率，ε为航向误差，ε_last为上一帧的航向误差，ε-ε_last为航向误差率；

第三计算模块24，用于通过LQR横向控制算法计算方向转角以对车辆进行横向控制。

进一步的，所述第一计算模块21具体用于：

根据摄像头输入的车道线信息，计算车辆与左边车道线的距离以及车辆与右边车道线的距离；

车辆与左边车道线的距离以及车辆与右边车道线的距离之和为车道宽度值。

进一步的，当道路只有一边车道线时，计算车辆与该车道线的距离；

若该距离在不合理范围内，则将该距离数据剔除并使用上一帧距离值计算车道中心线。

进一步的，在行驶过程中，存储左右车道线距离值的多帧数据以计算车道宽度；

根据车道宽度和视觉识别到的车道线坐标平移获得车道中心点；

对离散的车道中心点进行三次样条拟合；

对拟合的在车辆坐标系下的车道中心线计算横向误差、横向误差率、航向误差、航向误差率。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包括在本发明的保护范围之内。

Claims (10)

1.一种基于LQR的车道居中驾驶控制方法，其特征在于，所述方法包括：

根据车道线计算车道中心线以使车辆大致位于车道中心线上；

根据车辆坐标系沿车头航向角方向预瞄一段距离X₀计算横向偏移量D₀、预瞄一段距离X₁计算横向偏移量D₁、预瞄一段距离X₂计算横向偏移量D₂，其中：X₁≠X₂；

根据X₁、X₂、D₁、D₂计算相对航向角ε，ε的计算公式为：

其中，D₀为横向误差，D_{0_last}为上一帧的横向误差，D₀-D_{0_last}为横向误差率，ε为航向误差，ε_last为上一帧的航向误差，ε-ε_last为航向误差率；

通过LQR横向控制算法计算方向转角以对车辆进行横向控制。

2.根据权利要求1所述的基于LQR的车道居中驾驶控制方法，其特征在于，所述根据车道线计算车道中心线，包括：

根据摄像头输入的车道线信息，计算车辆与左边车道线的距离以及车辆与右边车道线的距离；

车辆与左边车道线的距离以及车辆与右边车道线的距离之和为车道宽度值。

3.根据权利要求2所述的基于LQR的车道居中驾驶控制方法，其特征在于：

当道路只有一边车道线时，计算车辆与该车道线的距离；

若该距离在不合理范围内，则将该距离数据剔除并使用上一帧距离值计算车道中心线。

4.根据权利要求2所述的基于LQR的车道居中驾驶控制方法，其特征在于：

在行驶过程中，存储左右车道线距离值的多帧数据以计算车道宽度；

根据车道宽度和视觉识别到的车道线坐标平移获得车道中心点；

对离散的车道中心点进行三次样条拟合；

对拟合的在车辆坐标系下的车道中心线计算横向误差、横向误差率、航向误差、航向误差率。

5.一种基于LQR的车道居中驾驶控制装置，其特征在于，所述装置包括：

第一计算模块，用于根据车道线计算车道中心线以使车辆大致位于车道中心线上；

预瞄模块，用于根据车辆坐标系沿车头航向角方向预瞄一段距离X₀计算横向偏移量D₀、预瞄一段距离X₁计算横向偏移量D₁、预瞄一段距离X₂计算横向偏移量D₂，其中：X₁≠X₂；

第二计算模块，用于根据X₁、X₂、D₁、D₂计算相对航向角ε，ε的计算公式为：

其中，D₀为横向误差，D_{0_last}为上一帧的横向误差，D₀-D_{0_last}为横向误差率，ε为航向误差，ε_last为上一帧的航向误差，ε-ε_last为航向误差率；

第三计算模块，用于通过LQR横向控制算法计算方向转角以对车辆进行横向控制。

6.根据权利要求5所述的基于LQR的车道居中驾驶控制装置，其特征在于，所述第一计算模块具体用于：

根据摄像头输入的车道线信息，计算车辆与左边车道线的距离以及车辆与右边车道线的距离；

车辆与左边车道线的距离以及车辆与右边车道线的距离之和为车道宽度值。

7.根据权利要求6所述的基于LQR的车道居中驾驶控制装置，其特征在于：

当道路只有一边车道线时，计算车辆与该车道线的距离；

若该距离在不合理范围内，则将该距离数据剔除并使用上一帧距离值计算车道中心线。

8.根据权利要求6所述的基于LQR的车道居中驾驶控制装置，其特征在于：

在行驶过程中，存储左右车道线距离值的多帧数据以计算车道宽度；

根据车道宽度和视觉识别到的车道线坐标平移获得车道中心点；

对离散的车道中心点进行三次样条拟合；

对拟合的在车辆坐标系下的车道中心线计算横向误差、横向误差率、航向误差、航向误差率。

9.一种车辆，其特征在于，所述车辆包括：

处理器；以及

存储装置，该存储装置存储有计算机程序，所述计算机程序在由处理器执行时用于执行如权利要求1-4任一项所述的基于LQR的车道居中驾驶控制方法的步骤。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-4任一项所述的基于LQR的车道居中驾驶控制方法的步骤。

Images