CN118004275A - 一种交叉路口直行的车辆横向控制方法及结构 - Google Patents

Abstract

本发明提供一种交叉路口直行的车辆横向控制方法，其可以基于较低成本提高直行车辆经过交叉路口中心区域时的横向控制的精准度。其通过融合车道中心线轨迹、自车运动轨迹和前车运动轨迹，得到了更准确的车辆行驶参考轨迹，降低了驾驶辅助***在交叉路口直行的异常情况发生概率。同时，本申请还公开了一种交叉路口直行的车辆横向控制结构。

Description

一种交叉路口直行的车辆横向控制方法及结构

技术领域

本发明涉及驾驶辅助技术领域，具体为一种交叉路口直行的车辆横向控制方法及结构。

背景技术

自动驾驶技术是指在没有人类驾驶员干预的情况下，车辆能够自主进行加速、转向、刹车和避免障碍物等操作。目前，许多汽车制造商已经推出了配备L2级别（部分自动化）驾驶功能的家用汽车。这些车辆通常配备了先进的驾驶辅助***，如自适应巡航控制（Adaptive Cruise Control，ACC）、车道保持辅助***（Lane KeepingAssist，LKA）、交通拥堵辅助（Traffic Jam Assistant，TJA）、领航辅助（Navigateon Autopilot，NOA）等，能够在特定条件下实现一定程度的自动化驾驶。目前的驾驶辅助***都能够在通过使用摄像头、激光雷达等传感器来监测车辆在道路上的位置，并能够对车辆进行自动的方向调整，帮助驾驶员保持车辆在行驶过程中不偏离车道。通常情况下，驾驶辅助***能够在车辆的速度范围内工作，一般从低速到高速都可以发挥作用。这种技术可以提高驾驶的安全性，尤其是在长途高速行驶或在疲劳驾驶时具有一定的作用。

驾驶辅助***对车辆的控制可分为横向控制和纵向控制，纵向控制的控制目标为车辆的速度，横向控制用于保持车辆直线行驶、控制车辆转向、变换车辆所在的车道等方面。

然而，如图1所示，当驾驶辅助***的车辆，图中标记为自车2在城区道路的交叉路口直行时，进入到交叉路口的中心区域1后，由于中心区域1是不标记车道线的，即，对于车辆辅助***来说是车道线数据丢失的情况，但中心区域的车辆和道路情况却更复杂，所以极易出现异常情况。

针对图1所示场景，现有的L2级别的驾驶辅助***的横向控制通常采取三种方案来应对交叉路口直行的中车道线丢失的场景。第一种方案是将车道线丢失前的车道中心线信息锁存，作为车辆直线行驶的参考轨迹，直到车道线信息被重新获取。第二种方案是保持车辆的横向控制姿态不变，直到通过路口。第三种方案是使用高精地图或者激光雷达提供的道路车道线信息，规划车辆行驶的参考轨迹。

当前方案存在的问题是：基于第一个方案和第二个方案通过交叉路口时，车道线丢失前的信息与真实道路信息存在偏差，使用车道线丢失前的中心线信息作为参考轨迹进行横向控制，或者直接沿用车道线丢失前横向控制姿态信息，控制不够精准，都会导致车辆可能会偏离车道。而使用高精地图和激光雷达会增加整车成本，并且目前只有部分城市的高精地图数据，无法覆盖所有城市的道路场景。

发明内容

为了解决现有技术中驾驶辅助***在交叉路口直行车道线数据丢失时无法实现精准的横向控制的问题，本发明提供一种交叉路口直行的车辆横向控制方法，其可以基于较低成本提高直行车辆经过交叉路口中心区域时的横向控制的精准度。同时，本申请还公开了一种交叉路口直行的车辆横向控制结构。

本发明的技术方案是这样的：一种交叉路口直行的车辆横向控制方法，其特征在于，其包括以下步骤：

S1：基于车载摄像头，实时监控当前车辆行驶的场景；

所述摄像头安装在当前车辆的前方，检测车辆前方环境信息；所述环境信息包括：车道线、车辆、行人、障碍物；

S2：确认当前场景为是否为交叉路口直行场景；

如果是，则执行步骤S3，进入交叉口车辆横向控制流程；

否则，循环执行步骤S1~S2；

S3：采集车道线信息，计算车道中心线轨迹；

S4：根据VCU的自车运动信息计算当前车辆的预测运动轨迹；

S5：判断当前车辆是否存在前车；

如果存在，则根据前车历史运动信息计算前车历史运动轨迹后，执行步骤S6；

否则，将前车历史运动轨迹赋值为空后，执行步骤S6；

S6：判断所述前车历史运动轨迹是否为空；

如果是，则使用扩展卡尔曼滤波方法对所述车道中心线轨迹和所述预测运动轨迹进行融合，得到最终的车辆行驶参考路径轨迹；

否则，使用扩展卡尔曼滤波方法对所述车道中心线轨迹、所述预测运动轨迹和所述前车历史运动轨迹进行融合，得到最终的车辆行驶参考路径轨迹；

融合后的所述车辆行驶参考路径的轨迹方程为：

y_s=d_s+a_s×x_s+b_s×x² _s+c_s×x³ _s；

其中，轨迹计算基于笛卡尔车辆坐标系，原点O为轨迹计算开始时的车辆后轴中心，X轴与车辆前进方向一致，Y轴与车辆的右侧方向一致，y_s为参考路径轨迹点的Y轴坐标，x_s为参考路径轨迹点的X轴坐标，a_s、b_s、c_s和d_s为轨迹方程的系数；

S7：使用基于预瞄的横向轨迹跟踪PID计算目标方向盘角度；

Φ_ST=Φ_f+Φ_pi+Φ_Z；

其中，Φ_ST为目标方向盘角度，Φ_f为前馈方向盘转角，Φ_pi为反馈pid控制方向盘转角，Φ_Z为预先标定的方向盘安装的零位偏差修正角度；

Φ_f= K_f*i*(180/π)*arctan(L*b_F)；

其中，K _f 为前馈转角标定系数，由车速和综合偏差决定；i为转向***的传动比；L为车辆轴距，b_F为参考轨迹上参考点的曲率，参考点是参考轨迹上距离预瞄点最近的轨迹点；

；

其中，K_PY为横向偏离距离反馈控制比例项系数，d_Y为预瞄点与参考点之间的横向偏离距离；K_PH为航向角偏差反馈控制比例项系数，d_F为预瞄距离，由车速、参考点曲率和预瞄时间共同决定，a_Y为预瞄点与参考点之间的航向角偏差，K_i为反馈控制积分项系数；

S8：对所述目标方向盘角度进行干扰限制处理后，得到处理后方向盘角度，将所述处理后方向盘角度输出到车辆VCU控制器，控制车辆横向运动；

所述干扰限制处理包括：限幅处理、滤波处理和变化率限制处理。

其进一步特征在于：

其还包括以下步骤：

S9：根据结束条件判断是否结束本次横向控制；

所述结束条件包括：

条件1：当左右车道线质量均可用，并持续3s以上；

条件2：驾驶员手动接管方向盘操作；

当上述两个条件中的任意一个时，则判断为结束本次横向控制；

如果判断本次横向控制结束，则退出；

否则，保持使用所述处理后方向盘角控制车辆横向运动；

步骤S2中，当同时满足以下场景条件时，则判断当前车辆处于交叉路口直行场景：

场景条件1：左右两侧车道线质量均可不用；

或者单侧车道线质量可用并且曲率大于设定阈值SC_curv、且曲率变化率大于设定阈值SC_crate；

场景条件2：车辆左右转向灯均为熄灭状态；

场景条件3：车辆横摆角速度小于设定阈值SC_yaw；

场景条件4：车辆航向角小于设置阈值SC_head；

场景条件5：方向盘角度小于设定阈值SC_strag；

场景条件6：交通灯状态为绿灯或者黄灯；

场景条件7：车道居中保持功能为打开状态；

场景条件8：自适应巡航功能为打开状态；

步骤S3中，所述车道中心线轨迹的计算方法，具体包括以下步骤：

a1：采集当前车辆进入交叉路口直行场景的前一秒的车道线数据，采样周期为40ms；

a2：使用车辆纵向速度积分方法计算离散点的x轴坐标x_H：

，

其中， v_log为第i个离散点时刻的车辆纵向速度，x_Hi为第i个离散点的x轴坐标；t为离散点对应的时间；

a3：计算离散点的y轴坐标y_H：

当左右两侧车道线质量均可用的情况下：

y_Hi=0.5×(L_C0i+R_C0i)；

其中，L_C0i为第i个离散点对应的左车道线参考点y轴坐标；R_C0i为第i个离散点对应的右车道线参考点y轴坐标；

当左侧车道线可用，右侧车道线质量不可用的情况下：

y_Hi=L_C0i+H/2；

其中，H为左右两侧车道线质量均可用时锁存的自车车道宽度；

当右侧车道线可用，左侧车道线质量不可用的情况下：

y_Hi=R_C0i-H/2；

a4：使用离散点(x_H，y_H)拟合三次多项式曲线方程，得到车道中心线轨迹：

y_H=d_H+a_H×x_H+b_H×x² _H+c_H×x³ _H；

其中，y_H为拟合后车道中心线轨迹点的纵坐标，x_H为拟合后车道中心线轨迹横坐标， a_H为拟合后车道中心线轨迹点的斜率，b_H代表拟合后车道中心线轨迹点曲率的二分之一，c_H代表拟合后车道中心线轨迹点曲率变化率的六分之一，d_H为拟合后车道中心线轨迹起始点的横轴坐标；

步骤S4中，当前车辆的所述预测运动轨迹的计算方法包括以下步骤：

b1：基于二自由度车辆稳态运动学模型推算当前车辆的运动离散点(x_V，y_V)；

x_Vi= ( L(1+Kv²)/(K_s*S))*cos(yaw*t)；

y_Vi= ( L(1+Kv²)/(K_s*S))*[1-sin(yaw*t)]；

其中，L为车辆轴距，v为当前车辆的纵向速度，K_s为转向传动比，S为方向盘角度，yaw为横摆角速度，t为离散点的时间；K为车辆侧偏系数；

K =[(a/k₂)-(b/k₁)] *m/L²；

其中，m为整车质量，a为车辆质心到前轮的距离，b为车辆质心到后轮的距离，k₁为前轮侧偏刚度，k₂为后轮侧偏刚度；

b2：基于当前车辆的运动离散点(x_V，y_V) 拟合三次多项式曲线方程，得到当前车辆的预测运动轨迹方程：

y_V=d_V+a_V×x_V+b_V×x_V ²+c_V×x_V ³，

其中，y_V为拟合后车辆预测运动轨迹点的纵轴坐标，x_V为拟合后车辆预测运动轨迹点的横轴坐标， a_V预测运动轨迹点的斜率，b_V代表预测运动轨迹点的曲率的二分之一，c_V代表预测运动轨迹点的曲率变化率的六分之一，d_V为预测运动轨迹起始点的横轴坐标；

步骤S5中，基于毫米波雷达判断当前车辆是否存在前车，以及检测前车信息；

所述前车信息包括：前方车辆与当前车辆的距离和前方车辆的速度；

步骤S5中，所述前车运动轨迹的计算方法包括以下步骤：

c1：基于离散点选取计算瞬间之前1s的前车数据，采样周期为40ms，离散点（x_Ei，y_Ei）计算公式为：

x_Ei= OLog_i- OLog_i-1，

y_Ei= OLat_i；

其中，x_Ei和y_Ei表示第i个离散点的横纵坐标；OLog_i为第i个离散点对应的时刻前车距离自车的纵向距离，OLog_i-1为上个离散点时刻的前车距离自车的纵向距离；OLat_i为第i个离散点对应的时刻前车距离自车的横向距离；

c2：基于离散点（x_Ei，y_Ei）拟合出前车历史运动轨迹的3次多项式方程：

y_E=d_E+a_E×x_E+b_E×x² _E+c_E×x³ _E；

其中，y_E为前车历史运动轨迹点的纵轴坐标，x_E为前车历史运动轨迹点的横轴坐标，a_E前车历史运动轨迹点的斜率，b_E代表前车历史运动轨迹点曲率的二分之一，c_E代表前车历史运动轨迹点曲率变化率的六分之一，d_E为前车历史运动轨迹起始点的横轴坐标；

步骤S8中，所述干扰限制处理具体包括：

所述限幅处理为将方向盘角度限制在正负30度以内；

所述滤波处理使用低通滤波；

所述变化率限制处理将方向盘的方向角限制在20度每秒以内。

一种交叉路口直行的车辆横向控制结构，其包括：摄像头、毫米波雷达和控制模块，其特征在在于：所述控制模块包括：传感器信息处理模块、轨迹计算模块、轨迹融合模块和横向轨迹跟踪PID模块；所述摄像头和所述毫米微波雷达电连接所述控制模块，所述控制模块通信连接当前车辆的VCU车载控制器和ADU自动驾驶控制器；

所述传感器信息处理模块包括：摄像头信息处理模块、车辆信息处理模块、目标信息处理模块和场景判断模块；

所述摄像头信息处理模块接受所述摄像头采集处理的数据，主要操作是车道线质量判断，车道线数据低通滤波；所述车辆信息处理模块接受的车身控制器VCU的信号, 主要操作是数据有效性校验和数据滤波；所述目标信息处理模块接收ADU融合摄像头数据和毫米波雷达数据后的目标信息，主要操作是目标数据滤波，然后根据目标横向距离和纵向距离筛选出前车目标；当无有效前车目标时，前车目标有效性信号应置为0；所述场景判断模块接受处理后的摄像头信息、车辆信息和前车目标信息，主要操作是对于车辆当前所处当前场景为是否为交叉路口直行场景进行判断；

所述轨迹计算模块包括车道中心线轨迹计算模块、自车运动轨迹计算模块和前车运动轨迹计算模块，基于所述传感器信息处理模块采集到的数据，分别对车道中心线轨迹、自车运动轨迹计算和前车运动轨迹计算，并将结算结果送入所述轨迹融合模块；

所述轨迹融合模块中，轨迹融合模块使用扩展卡尔曼滤波对轨迹计算模块所计算出的轨迹数据进行融合，得到当前车辆对应的融合后的车辆行驶参考路径的轨迹方程；将得到的车辆行驶参考路径的轨迹方程送入到所述横向轨迹跟踪PID模块中；

所述横向轨迹跟踪PID模块包括：前馈控制、反馈pid和方向盘角度输出模块，所述前馈控制根据参考轨迹的曲率进行计算得到前馈方向盘转角；所述反馈pid控制为PI控制器，输入为参考轨迹横向偏离距离和航向角，输出反馈pid控制方向盘转角；

所述方向盘角度输出模块基于所述前馈方向盘转角、所述反馈pid控制方向盘转角和预先标定的方向盘安装的零位偏差修正角度计算得到目标方向盘角度；然后对所述目标方向盘角度进行干扰限制处理，得到处理后方向盘角度，将处理后方向盘角度输出到车辆VCU控制器，控制车辆横向运动。

其进一步特征在于：

其还包括：方法状态标记位F-LC，所述方法状态标记位F-LC的状态包括：激活状态和抑制状态；

当所述方法状态标记位F-LC为激活状态时，则启动所述控制模块，执行车辆横向控制方法；

当所述方法状态标记位F-LC为抑制状态时，则停止所述控制模块，结束车辆横向控制方法。

本申请提供的一种交叉路口直行的车辆横向控制方法，其通过融合车道中心线轨迹、自车运动轨迹和前车运动轨迹，得到了更准确的车辆行驶参考轨迹，降低了驾驶辅助***在交叉路口直行的异常情况发生概率。本方法无需使用高清地图，也无需增加更昂贵的设备，基于较低成本即可实时规划出车辆在交叉路口直行的参考轨迹，提高车辆横向控制的精度。

附图说明

图1为交叉路口直行示意图；

图2为本方法中车辆硬件配置示意图；

图3为本方法中轨迹计算示意图；

图4为本方法中横向跟踪PID示意图；

图5为本方法中交叉路口直行的车辆横向控制结构中的控制模块的模块构成示意图。

具体实施方式

本申请包括一种交叉路口直行的车辆横向控制方法，其包括以下步骤。

S1：基于车载摄像头，实时监控当前车辆行驶的场景；

摄像头安装在当前车辆的前方，检测车辆前方环境信息，环境信息包括：车道线、车辆、行人、障碍物。

如图2所示，本方法需要在车辆上安装具备计算能力的摄像头，以及具备测速能力的设备。从成本上考虑，使用毫米波雷达即可达到本方法所需功能。通过基于毫米波雷达判断当前车辆是否存在前车，以及检测前车信息，前车信息包括：前方车辆与当前车辆的距离和前方车辆的速度。即，本方法不需要使用高精地图和激光雷达，在1V1R（一个摄像头和一个毫米波雷达）的传感器基础上，即可帮助L2级别车辆驾驶辅助***完成城市道路的交叉路口直行，不增加额外成本。

本方法中，其余的计算所需参数基于CAN总线，与智能车辆本身的控制***中的VCU车载控制器进行通信完成采集即可。最后把计算结果传输给ADU自动驾驶控制器和EPS电动助力转向***，完成车辆的横向控制。涉及到的智能车辆中的模块包括：

VCU车载控制器—负责整合和处理来自不同传感器的数据，并实施车辆的决策与控制、

ADU自动驾驶控制器，负责自动驾驶辅助功能的实现，是方法的部署控制器；

EPS电动助力转向***，负责车辆的转向操纵。

如图5所示，为了实现交叉路口直行的车辆横向控制方法，需要在车辆上构建控制模块，控制模块包括：传感器信息处理模块、轨迹计算模块、轨迹融合模块和横向轨迹跟踪PID(proportional-integral-derivative control，比例积分微分控制) 模块。

传感器信息模块处理中，包括四部分：摄像头信息处理模块、车辆信息处理模块、目标信息处理模块和场景判断模块。

摄像头信息处理过程接受的信号主要来自于摄像头采集处理的数据，主要操作是车道线质量判断，车道线数据低通滤波。

基于摄像头采集的数据包括：左车道线状态L_ava，左车道线置信度L_conf、左车道线横向距离L_C0、左车道线航向角L_C1、左车道线曲率L_C2、左车道线曲率变化率L_C3、左车道线可视距离L_dst、右车道线状态R_ava，右车道线置信度R_conf、右车道线横向距离R_C0、右车道线航向角R_C1、右车道线曲率R_C2、右车道线曲率变化率R_C3、左车道线可视距离R_dst、交通信号灯状态。

车辆信息处理过程接受的信号主要来自于车身控制器VCU， 主要操作是数据有效性校验和数据滤波；

基于车身控制器VCU采集的数据包括：车辆纵向速度V_log，车辆纵向加速度A_log，车辆横向加速度A_lat，车辆横摆角速度YawRate，方向盘角度R_str，车辆转向灯状态S_trn，刹车状态S_brk，油门状态S_acc，车辆航向角Heading。

目标信息处理过程接受的信号是ADU融合摄像头数据和毫米波雷达数据后的目标信息；主要操作是目标数据滤波，然后根据目标横向距离和纵向距离筛选出前车目标；当无有效前车目标时，前车目标有效性信号应置为0；并将前车目标数据以及前车有无的信息发送给轨迹计算模块。

接受的信号包括：目标横向距离OLat，目标纵向距离Olog，目标纵向速度OlogV，目标纵向加速度OlogA。

场景判断，接受的信号是处理后的摄像头信息，车辆信息和目标信息；主要操作是完成步骤S2中，对于车辆当前所处当前场景为是否为交叉路口直行场景，进行判断。

S2：确认当前场景为是否为交叉路口直行场景；

如果是，则执行步骤S3，进入交叉口车辆横向控制流程；

否则，循环执行步骤S1~S2。

本方法中，当同时满足以下场景条件时，则判断当前车辆处于交叉路口直行场景：

场景条件1：左右两侧车道线质量均可不用；

或者单侧车道线质量可用并且曲率大于设定阈值SC_curv、且曲率变化率大于设定阈值SC_crate；

场景条件2：车辆左右转向灯均为熄灭状态；

场景条件3：车辆横摆角速度小于设定阈值SC_yaw；

场景条件4：车辆航向角小于设置阈值SC_head；

场景条件5：方向盘角度小于设定阈值SC_strag；

场景条件6：交通灯状态为绿灯或者黄灯；

场景条件7：车道居中保持功能为打开状态；

场景条件8：自适应巡航功能为打开状态。

传感器信息模块一旦判定当前车辆进入了交叉路口直行场景，则将本方法激活，启动轨迹计算模块，进行后续计算，同时向中央网关发送提示信号，车辆发出提示声音，提醒驾驶员注意安全。

在车辆行驶过程中，传感器信息模块是实时启动并监测车辆所处当前场景，一旦当左右车道线质量均可用，并持续3s以上时，传感器信息模块判断车辆已经直行通过交叉路口，则将本方法标记为进入抑制状态，停止计算。或者，在本方法实施过程中，如果驾驶员接管方向盘操作，则同时也将本方法标记位进入抑制状态。具体实现时，通过一个方法状态标记位来标记本方法是处于激活状态还是抑制状态。

在轨迹计算模块中，进行的轨迹计算包括：车道中心线轨迹计算，自车运动轨迹计算，前车运动轨迹计算，对应设置三个子模块：车道中心线轨迹计算模块、自车运动轨迹计算模块和前车运动轨迹计算模块。

参考图3，三个子模块中，轨迹坐标系全部使用笛卡尔坐标系，原点O为轨迹计算开始时的车辆后轴中心，X轴与车辆前进方向一致，Y轴与车辆的右侧方向一致，轨迹计算方法均使用离散点拟合三次多项式曲线方程的方法，轨迹方程为：y = d +a×x+b×x²+c×x³；

其中；d代表轨迹起始点的横轴坐标，a代表轨迹点的斜率，b代表轨迹点曲率的二分之一，c代表轨迹点曲率变化率的六分之一，x代表轨迹点横轴坐标，y是轨迹点的纵轴坐标。

车道中心线轨迹计算模块中实现的具体的计算方法如下。

S3：采集车道线信息，计算车道中心线轨迹。

车道中心线轨迹的计算方法，具体包括以下步骤：

a1：采集当前车辆进入交叉路口直行场景的前一秒的车道线数据，采样周期为40ms；

a2：使用车辆纵向速度积分方法计算离散点的x轴坐标x_H：

；

其中， v_log为第i个离散点时刻的车辆纵向速度，x_Hi为第i个离散点的x轴坐标；t为离散点对应的时间；

a3：计算离散点的y轴坐标y_H：

当左右两侧车道线质量均可用的情况下：

y_Hi=0.5×(L_C0i+R_C0i)；

其中，L_C0i为第i个离散点对应的左车道线参考点y轴坐标；R_C0i为第i个离散点对应的右车道线参考点y轴坐标；

当左侧车道线可用，右侧车道线质量不可用的情况下：

y_Hi=L_C0i+H/2；

其中，H为左右两侧车道线质量均可用时锁存的自车车道宽度；

当右侧车道线可用，左侧车道线质量不可用的情况下：

y_Hi=R_C0i-H/2；

a4：使用离散点(x_H，y_H)拟合三次多项式曲线方程，得到车道中心线轨迹：

y_H=d_H+a_H×x_H+b_H×x² _H+c_H×x³ _H；

其中，y_H为拟合后车道中心线轨迹点的纵坐标，x_H为拟合后车道中心线轨迹横坐标， a_H为拟合后车道中心线轨迹点的斜率，b_H代表拟合后车道中心线轨迹点曲率的二分之一，c_H代表拟合后车道中心线轨迹点曲率变化率的六分之一，d_H为拟合后车道中心线轨迹起始点的横轴坐标。

使用方法激活前1秒的车道线数据完成对车道线中心轨迹的构建，车道中心线轨迹构建完毕后在本方法整个激活过程中保持不变，直至本方法跳转至抑制状态。确保即便出现车道线信息丢失的情况，也可以基于车道中心线轨迹完成后续计算。

自车运动轨迹计算模块在方法激活过程中实时进行自车运动轨迹计算，根据车辆运动信息预测自车未来0.4s内的运动轨迹，离散点坐标计算使用二自由度车辆稳态运动学模型推算，同时假设车辆在未来0.4s内保持横摆角速度不变。

S4：根据VCU的自车运动信息计算当前车辆的预测运动轨迹；

当前车辆的预测运动轨迹计算方法包括以下步骤：

b1：基于二自由度车辆稳态运动学模型推算当前车辆的运动离散点(x_V，y_V)；

x_Vi= ( L(1+Kv²)/(K_s*S))*cos(yaw*t)；

y_Vi= ( L(1+Kv²)/(K_s*S))*[1-sin(yaw*t)]；

其中，L为车辆轴距，v为当前车辆的纵向速度，K_s为转向传动比，S为方向盘角度，yaw为横摆角速度，t为离散点的时间；K为车辆侧偏系数；

K =[(a/k₂)-(b/k₁)] *m/L²；

其中，m为整车质量，a为车辆质心到前轮的距离，b为车辆质心到后轮的距离，k₁为前轮侧偏刚度，k₂为后轮侧偏刚度；

b2：基于当前车辆的运动离散点(x_V，y_V) 拟合三次多项式曲线方程，得到当前车辆的预测运动轨迹方程：

y_V=d_V+a_V×x_V+b_V×x_V ²+c_V×x_V ³，

其中，y_V为拟合后车辆预测运动轨迹点的纵轴坐标，x_V为拟合后车辆预测运动轨迹点的横轴坐标， a_V预测运动轨迹点的斜率，b_V代表预测运动轨迹点的曲率的二分之一，c_V代表预测运动轨迹点的曲率变化率的六分之一，d_V为预测运动轨迹起始点的横轴坐标。

本方法根据前车的横向距离、纵向距离得到前车的历史运动轨迹，当本方法处于激活状态时，且前车有效标志位为1时启动前车运动轨迹计算模块，否则关闭。

S5：判断当前车辆是否存在前车；

如果存在，则根据前车历史运动信息计算前车历史运动轨迹后，执行步骤S6；

否则，将前车历史运动轨迹赋值为空后，执行步骤S6。

步骤S5中，前车运动轨迹的计算发方法包括以下步骤：

c1：基于离散点选取计算瞬间之前1s的前车数据，采样周期为40ms，离散点（x_Ei，y_Ei）计算公式为：

x_Ei= OLog_i-OLog_i-1，

y_Ei= OLat_i；

其中，x_Ei和y_Ei表示第i个离散点的横纵坐标；OLog_i为第i个离散点对应的时刻前车距离自车的纵向距离，OLog_i-1为上个离散点时刻的前车距离自车的纵向距离；OLat_i为第i个离散点对应的时刻前车距离自车的横向距离；

c2：基于离散点（x_Ei，y_Ei）拟合出前车历史运动轨迹的3次多项式方程：

y_E=d_E+a_E×x_E+b_E×x² _E+c_E×x³ _E；

其中，y_E为前车历史运动轨迹点的纵轴坐标，x_E为前车历史运动轨迹点的横轴坐标，a_E前车历史运动轨迹点的斜率，b_E代表前车历史运动轨迹点曲率的二分之一，c_E代表前车历史运动轨迹点曲率变化率的六分之一，d_E为前车历史运动轨迹起始点的横轴坐标。

在轨迹计算模块中所有的轨迹计算都完毕后，将计算结果送入轨迹融合模块。轨迹融合模块使用扩展卡尔曼滤波对轨迹计算模块所计算出的车道中心线轨迹、自车运动轨迹、前车历史运动轨进行融合。

S6：判断前车历史运动轨迹是否为空；

如果是，则使用扩展卡尔曼滤波方法对车道中心线轨迹和自车的预测运动轨迹进行融合，得到最终的车辆行驶参考路径轨迹；

否则，使用扩展卡尔曼滤波方法对车道中心线轨迹、预测运动轨迹和前车历史运动轨迹进行融合，得到最终的车辆行驶参考路径轨迹。

扩展卡尔曼滤波主要包括以下步骤：

步骤一，确定滤波的状态向量x为:[x_S，y_S，a_S，b_S，c_S，d_S]，

其中，x_s为参考路径轨迹点的X轴坐标、y_s为参考路径轨迹点的Y轴坐标，a_S，b_S，c_S，d_S为融合轨迹方程的参数。

步骤二，确定状态更新矩阵F。

步骤三，状态协方差预测，其表达式为P_k=F_k-1P_k-1F^T _k-1+Q_k-1；

其中，Q_k-1为***噪声矩阵，P_k为k时刻的协方差矩阵，F_k-1表示k-1时刻的状态更新矩阵，P_k-1表示k-1时刻的协方差矩阵，F^T _k-1表示k-1时刻的状态更新矩阵的转置矩阵；

步骤四，计算卡尔曼增益，其表达式为：

K_k=P_kH^T _k(H_kP_kH^T _k+R_k)^-1；

其中，P为协方差矩阵，R为观测噪声矩阵，H为观测矩阵；k是表示k时刻，K_k当前k时刻的卡尔曼增益；

步骤五：计算观测值与预测值之间的误差y，表达式为：y=Z_k-H_kx_k；

其中，Z_k为当前时刻的观测值，是轨迹计算模块计算的三种轨迹通过线性插值后得到的简单融合轨迹；

具体应用中，通过线性插值矩阵对三种轨迹数据进行融合得到Z_k时，可以通过对线性插值矩阵中三种轨迹的权重系数进行分别设置，来调整三个轨迹在计算中的比重。比如，在无前车的情况下，则前车历史运动轨迹的权重系数设置为0，即前车数据不参与计算。在实际应用中，前车历史运动轨迹存在的情况下，如果预先判断前车的轨迹是在允许的误差范围内，则可以将前车历史运动轨迹的权重系数调高，将Z_k的值更多地倚重前车轨迹数据进行计算，进而提高计算结果的准确性。

步骤六：根据卡尔曼增益计算当前时刻的最优估计值，其表达式为:x_k=x_k-1+K_ky；

其中，x_k-1为上一时刻的预测值，K_k为当前时刻的卡尔曼增益，y当前时刻的误差；

步骤七：更新协方差矩阵，表达式为：P_k=(I-K_kH_k)P_k-1；

其中，I为单位矩阵， P_k-1为上一时刻的协方差矩阵。

扩展卡尔曼滤波不仅考虑了上一时刻的预测值，还考虑到了观测值和整个***的噪声，大大减少了因简单线性插值融合带来的误差，从而更接近于车辆的真实运动轨迹。

则：融合后的车辆行驶参考路径的轨迹方程为：

y_s=d_s+a_s×x_s+b_s×x² _s+c_s×x³ _s；

其中，轨迹计算基于笛卡尔车辆坐标系，原点O为轨迹计算开始时的车辆后轴中心，X轴与车辆前进方向一致，Y轴与车辆的右侧方向一致，y_s为参考路径轨迹点的Y轴坐标，x_s为参考路径轨迹点的X轴坐标，a_s、b_s、c_s和d_s为轨迹方程的系数。

轨迹融合模块将计算结果送入横向轨迹跟踪PID模块。横向轨迹跟踪控制PID模块基于单点驾驶员预瞄模型，参考图4，由前馈控制、反馈pid控制和方向盘角度输出模块组成。

根据参考轨迹的曲率进行前馈控制，前馈方向盘转角计算公式为：

Φ_f= K_f*i*(180/π)*arctan(L*b_F)；

其中，K _f 为前馈转角标定系数，由车速和综合偏差决定；i为转向***的传动比；L为车辆轴距，b_F为参考轨迹上参考点的曲率，参考点是参考轨迹上距离预瞄点最近的轨迹点。

反馈pid控制为PI控制器，输入为参考轨迹横向偏离距离d _S 和航向角a _S ，输出反馈pid控制方向盘转角Φ_pi：

；

其中，K_PY为横向偏离距离反馈控制比例项系数，d_Y为预瞄点与参考点之间的横向偏离距离；K_PH为航向角偏差反馈控制比例项系数，d_F为预瞄距离，由车速、参考点曲率和预瞄时间共同决定，a_Y为预瞄点与参考点之间的航向角偏差，K_i为反馈控制积分项系数。

方向盘角度输出模块基于前馈控制和反馈Pid控制的计算结果，横向轨迹跟踪控制PID最终输出的方向盘角度。

S7：使用基于预瞄的横向轨迹跟踪PID计算目标方向盘角度；

Φ_ST=Φ_f+Φ_pi+Φ_Z；

其中，Φ_ST为目标方向盘角度，Φ_f为前馈方向盘转角，Φ_pi为反馈pid控制方向盘转角，Φ_Z为预先标定的方向盘安装的零位偏差修正角度。关于零位偏差修正角Φ_Z，因为硬件原因，不同的方向盘安装到车辆上上之后，会有一定的偏差，为了确保驾驶安全，每个型号的车在出厂前，会根据使用的方向盘的具体特征在实验室进行标定。标定时，构建已给表格，根据不同的速度值，设置对应的Φ_Z，并将标定表格存储在***中。车辆在行驶中，会基于当前的车速调用这个标定表格中对应的Φ_Z值实现对方向盘角度的硬件纠偏。比如，本实施例中五座的轿车，当车速为20KM/H时，其对应的Φ_Z为1.0度。

S8：在方向盘角度输出模块中，还需要对目标方向盘角度进行干扰限制处理，得到处理后方向盘角度，将处理后方向盘角度输出到车辆VCU控制器，控制车辆横向运动；

干扰限制处理包括：限幅处理、滤波处理和变化率限制处理。

具体实现时，干扰限制处理具体包括：

由于是交叉路口直行，限幅处理为将方向盘角度限制在正负30度以内；

滤波处理使用低通滤波，防止出现干扰导致的突变；

同样根据交叉路口直行场景，防止方向角变化太快，变化率限制处理将方向盘的方向角限制在20度每秒以内。

S9：根据结束条件判断是否结束本次横向控制；

结束条件包括：

条件1：当左右车道线质量均可用，并持续3s以上；

条件2：驾驶员手动接管方向盘操作；

当上述两个条件中的任意一个时，则判断为结束本次横向控制；

如果判断本次横向控制结束，则将方法状态标记位F-LC设置为抑制状态，退出；

否则，保持方法状态标记位F-LC的激活状态，保持使用处理后方向盘角控制车辆横向运动。

现有方法中，如果采用现有方案中的“将车道线丢失前的车道中心线信息锁存，作为车辆直线行驶的参考轨迹”，因为现有的***中是实时采集车辆中心和车道线中心线的距离作为车道中心线信息进行保存的，所以如果一直采用丢失前的车道中心线信息，会导致无视车辆的实际运动方向，而横向控制PID的横向偏差和航向角偏差始终保持不变，输出方向盘角度会不断增大，进而导致车辆实际不断向车道一侧偏移。而采用现有技术方案中的另一个方案，保持车辆横向控制姿态不变，即方向盘角度不变，在实际应用中，车辆也会向一侧偏移；两种方案都有可能车辆跑偏的问题发生。

本方法中，采用扩展卡尔曼滤波方法对车道中心线轨迹、自车预测运动轨迹进行融合，可以有效地对自车与预测的车道中心线轨迹的横向偏差距离进行动态补偿，确保车辆可以沿着原有车道中心线轨迹的方向行进。

而当有前车存在时，当前车的轨迹在预设的安全范围时，本方法同时参照前车轨迹，采用扩展卡尔曼滤波方法对车道中心线轨迹、预测运动轨迹和前车历史运动轨迹进行融合，通过综合考虑多个数据来源的信息，能够得到更准确的车辆行驶参考路径轨迹，提高横向控制的精度和鲁棒性，提供更好的横向控制体验，并增强驾驶辅助***的安全性。具体应用时，会对前车轨迹设置专门的判断模块，判断前车轨迹是否处于允许偏差范围，超出允许偏差范围则认为前车不存在。允许偏差范围根据交叉路口的种类不同而进行不同的设置。比如，交叉路口前后车道数不发生变化和发生变化的情况，允许偏差范围会设置不同的数据。确保本方法适用于各种不同的路况。

使用本申请的技术方案后，降低了L2级别车辆驾驶辅助***在交叉路口直行场景下的人工接管率，提高了驾驶辅助***的便捷性。

Claims (10)

1.一种交叉路口直行的车辆横向控制方法，其特征在于，其包括以下步骤：

S1：基于车载摄像头，实时监控当前车辆行驶的场景；

所述摄像头安装在当前车辆的前方，检测车辆前方环境信息；所述环境信息包括：车道线、车辆、行人、障碍物；

S2：确认当前场景为是否为交叉路口直行场景；

如果是，则执行步骤S3，进入交叉口车辆横向控制流程；

否则，循环执行步骤S1~S2；

S3：采集车道线信息，计算车道中心线轨迹；

S4：根据VCU的自车运动信息计算当前车辆的预测运动轨迹；

S5：判断当前车辆是否存在前车；

如果存在，则根据前车历史运动信息计算前车历史运动轨迹后，执行步骤S6；

否则，将前车历史运动轨迹赋值为空后，执行步骤S6；

S6：判断所述前车历史运动轨迹是否为空；

如果是，则使用扩展卡尔曼滤波方法对所述车道中心线轨迹和所述预测运动轨迹进行融合，得到最终的车辆行驶参考路径轨迹；

否则，使用扩展卡尔曼滤波方法对所述车道中心线轨迹、所述预测运动轨迹和所述前车历史运动轨迹进行融合，得到最终的车辆行驶参考路径轨迹；

融合后的所述车辆行驶参考路径的轨迹方程为：

y_s=d_s+a_s×x_s+b_s×x² _s+c_s×x³ _s；

其中，轨迹计算基于笛卡尔车辆坐标系，原点O为轨迹计算开始时的车辆后轴中心，X轴与车辆前进方向一致，Y轴与车辆的右侧方向一致，y_s为参考路径轨迹点的Y轴坐标，x_s为参考路径轨迹点的X轴坐标，a_s、b_s、c_s和d_s为轨迹方程的系数；

S7：使用基于预瞄的横向轨迹跟踪PID计算目标方向盘角度；

Φ_ST=Φ_f+Φ_pi+Φ_Z；

其中，Φ_ST为目标方向盘角度，Φ_f为前馈方向盘转角，Φ_pi为反馈pid控制方向盘转角，Φ_Z为预先标定的方向盘安装的零位偏差修正角度；

Φ_f= K_f*i*(180/π)*arctan(L*b_F)；

其中，K _f 为前馈转角标定系数，由车速和综合偏差决定；i为转向***的传动比；L为车辆轴距，b_F为参考轨迹上参考点的曲率，参考点是参考轨迹上距离预瞄点最近的轨迹点；

；

其中，K_PY为横向偏离距离反馈控制比例项系数，d_Y为预瞄点与参考点之间的横向偏离距离；K_PH为航向角偏差反馈控制比例项系数，d_F为预瞄距离，由车速、参考点曲率和预瞄时间共同决定，a_Y为预瞄点与参考点之间的航向角偏差，K_i为反馈控制积分项系数；

S8：对所述目标方向盘角度进行干扰限制处理后，得到处理后方向盘角度，将所述处理后方向盘角度输出到车辆VCU控制器，控制车辆横向运动；

所述干扰限制处理包括：限幅处理、滤波处理和变化率限制处理。

2.根据权利要求1所述一种交叉路口直行的车辆横向控制方法，其特征在于：其还包括以下步骤：

S9：根据结束条件判断是否结束本次横向控制；

所述结束条件包括：

条件1：当左右车道线质量均可用，并持续3s以上；

条件2：驾驶员手动接管方向盘操作；

当上述两个条件中的任意一个时，则判断为结束本次横向控制；

如果判断本次横向控制结束，则退出；

否则，保持使用所述处理后方向盘角控制车辆横向运动。

3.根据权利要求1所述一种交叉路口直行的车辆横向控制方法，其特征在于：步骤S2中，当同时满足以下场景条件时，则判断当前车辆处于交叉路口直行场景：

场景条件1：左右两侧车道线质量均可不用；

或者单侧车道线质量可用并且曲率大于设定阈值SC_curv、且曲率变化率大于设定阈值SC_crate；

场景条件2：车辆左右转向灯均为熄灭状态；

场景条件3：车辆横摆角速度小于设定阈值SC_yaw；

场景条件4：车辆航向角小于设置阈值SC_head；

场景条件5：方向盘角度小于设定阈值SC_strag；

场景条件6：交通灯状态为绿灯或者黄灯；

场景条件7：车道居中保持功能为打开状态；

场景条件8：自适应巡航功能为打开状态。

4.根据权利要求1所述一种交叉路口直行的车辆横向控制方法，其特征在于：步骤S3中，所述车道中心线轨迹的计算方法，具体包括以下步骤：

a1：采集当前车辆进入交叉路口直行场景的前一秒的车道线数据，采样周期为40ms；

a2：使用车辆纵向速度积分方法计算离散点的x轴坐标x_H：

；

其中， v_log为第i个离散点时刻的车辆纵向速度，x_Hi为第i个离散点的x轴坐标；t为离散点对应的时间；

a3：计算离散点的y轴坐标y_H：

当左右两侧车道线质量均可用的情况下：

y_Hi=0.5×(L _C0i+R _C0i)；

其中，L_C0i为第i个离散点对应的左车道线参考点y轴坐标；R _C0i为第i个离散点对应的右车道线参考点y轴坐标；

当左侧车道线可用，右侧车道线质量不可用的情况下：

y_Hi=L _C0i+H/2；

其中，H为左右两侧车道线质量均可用时锁存的自车车道宽度；

当右侧车道线可用，左侧车道线质量不可用的情况下：

y_Hi=R _C0i -H/2；

a4：使用离散点(x_H，y_H)拟合三次多项式曲线方程，得到车道中心线轨迹：

y_H=d_H+a_H×x_H+b_H×x² _H+c_H×x³ _H；

其中，y_H为拟合后车道中心线轨迹点的纵坐标，x_H 为拟合后车道中心线轨迹横坐标，a_H为拟合后车道中心线轨迹点的斜率，b_H代表拟合后车道中心线轨迹点曲率的二分之一，c_H代表拟合后车道中心线轨迹点曲率变化率的六分之一，d_H为拟合后车道中心线轨迹起始点的横轴坐标。

5.根据权利要求1所述一种交叉路口直行的车辆横向控制方法，其特征在于：步骤S4中，当前车辆的所述预测运动轨迹的计算方法包括以下步骤：

b1：基于二自由度车辆稳态运动学模型推算当前车辆的运动离散点(x_V，y_V)；

x_Vi= ( L(1+Kv²)/(K_s*S))*cos(yaw*t)；

y_Vi = ( L(1+Kv²)/(K_s*S))*[1-sin(yaw*t)]；

其中，L为车辆轴距，v为当前车辆的纵向速度，K_s为转向传动比，S为方向盘角度，yaw为横摆角速度，t为离散点的时间；K为车辆侧偏系数；

K =[(a/k₂)-(b/k₁)] *m/L²；

其中，m为整车质量，a为车辆质心到前轮的距离，b为车辆质心到后轮的距离，k₁为前轮侧偏刚度，k₂为后轮侧偏刚度；

b2：基于当前车辆的运动离散点(x_V，y_V) 拟合三次多项式曲线方程，得到当前车辆的预测运动轨迹方程：

y_V=d_V+a_V×x_V+b_V×x_V ²+c_V×x_V ³，

其中，y_V为拟合后车辆预测运动轨迹点的纵轴坐标，x_V 为拟合后车辆预测运动轨迹点的横轴坐标， a_V预测运动轨迹点的斜率，b_V代表预测运动轨迹点的曲率的二分之一，c_V代表预测运动轨迹点的曲率变化率的六分之一，d_V为预测运动轨迹起始点的横轴坐标。

6.根据权利要求1所述一种交叉路口直行的车辆横向控制方法，其特征在于：步骤S5中，基于毫米波雷达判断当前车辆是否存在前车，以及检测前车信息；

所述前车信息包括：前方车辆与当前车辆的距离和前方车辆的速度。

7.根据权利要求1所述一种交叉路口直行的车辆横向控制方法，其特征在于：步骤S5中，所述前车运动轨迹的计算方法包括以下步骤：

c1：基于离散点选取计算瞬间之前1s的前车数据，采样周期为40ms，离散点（x_Ei，y_Ei）计算公式为：

x_Ei= OLog_i-OLog_i-1，

y_Ei= OLat_i；

其中，x_Ei和y_Ei表示第i个离散点的横纵坐标；OLog_i为第i个离散点对应的时刻前车距离自车的纵向距离，OLog_i-1为上个离散点时刻的前车距离自车的纵向距离；OLat_i为第i个离散点对应的时刻前车距离自车的横向距离；

c2：基于离散点（x_Ei，y_Ei）拟合出前车历史运动轨迹的3次多项式方程：

y_E=d_E+a_E×x_E+b_E×x² _E+c_E×x³ _E；

其中，y_E为前车历史运动轨迹点的纵轴坐标，x_E 为前车历史运动轨迹点的横轴坐标，a_E前车历史运动轨迹点的斜率，b_E代表前车历史运动轨迹点曲率的二分之一，c_E代表前车历史运动轨迹点曲率变化率的六分之一，d_E为前车历史运动轨迹起始点的横轴坐标。

8.根据权利要求1所述一种交叉路口直行的车辆横向控制方法，其特征在于：步骤S8中，所述干扰限制处理具体包括：

所述限幅处理为将方向盘角度限制在正负30度以内；

所述滤波处理使用低通滤波；

所述变化率限制处理将方向盘的方向角限制在20度每秒以内。

9.一种交叉路口直行的车辆横向控制结构，其包括：摄像头、毫米波雷达和控制模块，其特征在在于：所述控制模块包括：传感器信息处理模块、轨迹计算模块、轨迹融合模块和横向轨迹跟踪PID模块；所述摄像头和所述毫米微波雷达电连接所述控制模块，所述控制模块通信连接当前车辆的VCU车载控制器和ADU自动驾驶控制器；

所述传感器信息处理模块包括：摄像头信息处理模块、车辆信息处理模块、目标信息处理模块和场景判断模块；

所述摄像头信息处理模块接受所述摄像头采集处理的数据，主要操作是车道线质量判断，车道线数据低通滤波；所述车辆信息处理模块接受的车身控制器VCU的信号, 主要操作是数据有效性校验和数据滤波；所述目标信息处理模块接收ADU融合摄像头数据和毫米波雷达数据后的目标信息，主要操作是目标数据滤波，然后根据目标横向距离和纵向距离筛选出前车目标；当无有效前车目标时，前车目标有效性信号应置为0；所述场景判断模块接受处理后的摄像头信息、车辆信息和前车目标信息，主要操作是对于车辆当前所处当前场景为是否为交叉路口直行场景进行判断；

所述轨迹计算模块包括车道中心线轨迹计算模块、自车运动轨迹计算模块和前车运动轨迹计算模块，基于所述传感器信息处理模块采集到的数据，分别对车道中心线轨迹、自车运动轨迹计算和前车运动轨迹计算，并将结算结果送入所述轨迹融合模块；

所述轨迹融合模块中，轨迹融合模块使用扩展卡尔曼滤波对轨迹计算模块所计算出的轨迹数据进行融合，得到当前车辆对应的融合后的车辆行驶参考路径的轨迹方程；将得到的车辆行驶参考路径的轨迹方程送入到所述横向轨迹跟踪PID模块中；

所述横向轨迹跟踪PID模块包括：前馈控制、反馈pid和方向盘角度输出模块，所述前馈控制根据参考轨迹的曲率进行计算得到前馈方向盘转角；所述反馈pid控制为PI控制器，输入为参考轨迹横向偏离距离和航向角，输出反馈pid控制方向盘转角；

所述方向盘角度输出模块基于所述前馈方向盘转角、所述反馈pid控制方向盘转角和预先标定的方向盘安装的零位偏差修正角度计算得到目标方向盘角度；然后对所述目标方向盘角度进行干扰限制处理，得到处理后方向盘角度，将处理后方向盘角度输出到车辆VCU控制器，控制车辆横向运动。

10.根据权利要求9所述一种交叉路口直行的车辆横向控制结构，其特征在于：其还包括：方法状态标记位F-LC，所述方法状态标记位F-LC的状态包括：激活状态和抑制状态；

当所述方法状态标记位F-LC为激活状态时，则启动所述控制模块，执行车辆横向控制方法；

当所述方法状态标记位F-LC为抑制状态时，则停止所述控制模块，结束车辆横向控制方法。