CN109724600A - 一种用于智能驾驶车辆的局部轨迹容错方法 - Google Patents

Abstract

一种用于智能驾驶车辆的局部轨迹容错方法，其包括：步骤一、初始化跟随模式，构建智能驾驶车辆的车辆坐标系；步骤二、基于车辆坐标系，根据获取到的GPS数据进行GPS数据的有效性判别，得到GPS数据有效性判别结果；步骤三、根据GPS数据和获取到的车道线数据确定车道线识别状态，并确定真实容错偏差；步骤四、根据GPS数据有效性判别结果、车道线识别状态和真实容错偏差，更新智能驾驶车辆的跟随模式，根据更新后的跟随模式确定智能驾驶车辆的规划轨迹。本方法能够基于各数据状态进行容错，并对相应数据进行实时动态的更新。该方法易于实际应用，其不仅能够提高数据处理的鲁棒性，还能够有效简化系统的复杂度。

Description

一种用于智能驾驶车辆的局部轨迹容错方法

技术领域

本发明涉及智能驾驶技术领域，具体地说，涉及一种用于智能驾驶车辆的局部轨迹容错方法。

背景技术

智能驾驶车辆是指能通过车载传感器系统感知道路环境来自动规划行车路线并控制车辆到达预定目标的智能汽车。智能驾驶技术中的路径轨迹规划是智能驾驶领域的关键技术之一。在半结构化环境下的智能驾驶过程中，车辆容易遇到GPS/IMU由于受到干扰而导致信号不稳定、车道线不清晰或不连续或错误识别车道线等复杂突发情况。局部轨迹规划为非特定场景下的动态规划，要求准确完成车道跟随，且需对异常突发情况快速响应。

目前，现有的智能驾驶车辆局部路径轨迹规划方法主要包括基于启发式搜索、智能仿生、行为规划以及再励学习等路径规划方法，这些方法在全局路径规划方面取得了良好的效果，但并未考虑如半结构化道路下车道不连续和车道错误识别或GPS/IMU定位失真等因数据不稳定对规划系统带来的难适应性。

发明内容

为解决上述问题，本发明提供了一种用于智能驾驶车辆的局部轨迹容错方法，所述方法包括：

步骤一、初始化跟随模式，构建智能驾驶车辆的车辆坐标系；

步骤二、基于所述车辆坐标系，根据获取到的GPS数据进行GPS数据的有效性判别，得到GPS数据有效性判别结果；

步骤三、根据所述GPS数据和获取到的车道线数据确定车道线识别状态，并确定真实容错偏差；

步骤四、根据所述GPS数据有效性判别结果、车道线识别状态和真实容错偏差，更新所述智能驾驶车辆的跟随模式，根据更新后的跟随模式确定所述智能驾驶车辆的规划轨迹。

根据本发明的一个实施例，在所述步骤一中，通过初始化跟随模式，将所述智能驾驶车辆的跟随模式设置为GPS跟随模式。

根据本发明的一个实施例，在所述车辆坐标系中，所述智能驾驶车辆的车辆正前方朝向为X轴，车辆正左方为Y轴，车辆正上方为Z轴。

根据本发明的一个实施例，所述步骤二包括：

基于所述车辆坐标系，根据当前时刻的GPS数据以及前一时刻的GPS数据确定GPS位置误差；

根据所述GPS位置误差进行GPS数据的有效性判别，确定所述GPS数据有效性判别结果。

根据本发明的一个实施例，在所述步骤二中，根据如下表达式计算所述GPS位置误差：

error_gps＝P_gps(x＝0|t)-P_gps(x＝0|t-1)

其中，error_gps表示GPS位置误差，P_gps(x＝0|t)表示x＝0位置处的t时刻的GPS期望路径，P_gps(x＝0|t-1)表示x＝0位置处的t-1时刻的GPS期望路径。

根据本发明的一个实施例，判断所述GPS位置误差是否小于或等于预设运动误差，其中，如果所述GPS位置误差小于或等于预设运动误差，则判定所述GPS数据有效，否则判定所述GPS数据无效。

根据本发明的一个实施例，在所述步骤三中，获取车道线实时追踪帧数，分别将所述车道线实时追踪帧数中的左车道线实时追踪帧数和右车道线实时追踪帧数与预设追踪帧数阈值进行比较，根据比较结果确定所述车道线识别状态。

根据本发明的一个实施例，如果所述左车道线实时追踪帧数小于或等于预设追踪帧数阈值，则判定所述左车道线识别无效，否则判定所述左车道线识别有效；

如果所述右车道线实时追踪帧数小于或等于预设追踪帧数阈值，则判定所述右车道线识别无效，否则判定所述右车道线识别有效。

根据本发明的一个实施例，在所述步骤三中，

根据所述车道线数据分别确定左车道线中心数据和右车道线中心数据；

根据所述左车道线中心数据和右车道线中心数据确定车道识别宽度；

根据所述车道识别宽度和车道真实宽度确定车道线识别状态。

根据本发明的一个实施例，如果所述车道识别宽度与车道真实宽度之间差值的绝对值小于或等于预设宽度差值阈值，则判定双车道线识别有效，否则判定双车道线识别无效。

根据本发明的一个实施例，在所述步骤三中，

根据所述车道线数据分别确定左车道线中心数据和右车道线中心数据；

根据所述GPS数据确定车道中心数据；

根据所述左车道线中心数据和车道中心数据确定第一距离误差，根据所述右车道线中心数据和车道中心数据确定第二距离误差；

根据所述第一距离误差、第二距离误差和预设中心偏差阈值，分别确定所述左车道线和右车道线的识别有效性。

根据本发明的一个实施例，分别根据如下表达式计算所述左车道线中心数据和右车道线中心数据：

Y_centerLeft＝(P_laneLeft(x₁)+P_laneLeft(x₂)+P_laneLeft(x₃))/3

Y_centerRight＝(P_laneRight(x₁)+P_laneRight(x₂)+P_laneRight(x₃))/3

根据如下表达式确定所述车道中心数据：

Y_centergps＝(P_gps(x₁)+P_gps(x₂)+P_gps(x₃))/3

分别根据如下表达式计算所述第一距离误差和第二距离误差：

error_laneLeft＝abs(Y_centerLeft-W_real/2)-Y_centerGps

error_laneRight＝abs(Y_centerRight-W_real/2)-Y_centerGps

其中，P_laneLeft(x₁)、P_laneLeft(x₂)和P_laneLeft(x₃)分别表示横坐标为x₁、x₂和x₃的位置处的左车道线轨迹，P_laneRight(x₁)、P_laneRight(x₂)和P_laneRight(x₃)分别表示横坐标为x₁、x₂和x₃的位置处的右车道线轨迹，Y_centerLeft和Y_centerRight分别表示左车道线中心数据和右车道线中心数据，Y_centergps表示GPS车道中心数据，P_gps(x₁)、P_gps(x₂)和P_gps(x₃)分别表示横坐标为x₁、x₂和x₃的位置处的GPS车道线数据，error_laneLeft和error_laneRight分别表示第一距离误差和第二距离误差，W_real表示轨迹真实宽度。

根据本发明的一个实施例，如果所述第一距离误差小于或等于预设中心偏差阈值，则确定左车道线识别有效，否则确定左车道线识别无效。

根据本发明的一个实施例，在所述步骤三中，还根据所述车道线识别状态确定单车道持续时长T_{MissSingleLane}、丢失左车道时长T_MissLeftLane和丢失右车道时长T_{MissRightLane}。

根据本发明的一个实施例，在所述步骤三中，

根据所述左车道线中心数据、右车道线中心数据和车道中心数据确定检测车道中心与GPS车道中心之间的偏差，得到车道中心偏差；

根据所述车道中心确定当前容错偏差；

根据所述当前容错偏差，计算不同跟随模式下的真实容错偏差。

根据本发明的一个实施例，根据如下表达式计算所述当前容错偏差：

error_GpsLane＝(Y_centerLeft+Y_centerRight)/2-Y_centerGps

error_{offsetCurrent}＝λerror_GpsLane+error_calibration

其中，error_{offsetCurrent}表示当前容错偏差，λ表示偏差比例系数，error_GpsLane表示车道中心偏差，error_calibration表示标准偏差，Y_centerLeft和Y_centerRight分别表示左车道线中心数据和右车道线中心数据，Y_centerGps分别表示GPS车道中心数据。

根据本发明的一个实施例，根据如下表达式计算不同跟随模式下的真实容错偏差：

其中，error_offset(t)和error_offset(t-1)分别表示t时刻和t-1时刻的真实容错偏差，error_{offsetCurrent}(t)表示t时刻的当前容错偏差，M_follow表示跟随模式，M_GPS表示GPS跟随模式，M_lane表示车道线跟随模式，M_gradient表示渐变跟随模式。

根据本发明的一个实施例，在所述步骤四中，如果所述GPS数据有效并且所述智能驾驶车辆在前一时刻的跟随模式为GPS跟随模式，则根据所述车道线识别状态确定所述智能驾驶车辆在当前时刻的跟随模式，其中，

如果所述车道线识别状态为双车道线识别有效，则将所述当前时刻的跟随模式更新为车道线跟随模式；否则将所述当前时刻的跟随模式保持为GPS跟随模式，并且将真实容错偏差置为零。

根据本发明的一个实施例，在所述步骤四中，如果所述GPS数据有效并且所述智能驾驶车辆在前一时刻的跟随模式为车道线跟随模式，根据所述车道线识别状态确定所述智能驾驶车辆在当前时刻的跟随模式，其中，

如果所述车道线识别状态为双车道线识别无效，则将所述当前时刻的跟随模式更新为渐变跟随模式；如果所述所述车道线识别状态为单车道线识别有效，则进一步判断单车道丢失时长是否大于预设车道保留时长，其中，如果大于，则将所述当前时刻的跟随模式更新为渐变跟随模式，否则将所述当前时刻的跟随模式保持为车道线跟随模式，并且将真实容错偏差置为零。

根据本发明的一个实施例，在所述步骤四中，如果所述GPS数据有效并且所述智能驾驶车辆在前一时刻的跟随模式为渐变跟随模式，判断所述真实容错偏差是否为零，其中，

如果所述真实容错偏差为零，则将所述当前时刻的跟随模式更新为GPS跟随模式；如果所述真实容错偏差不为零且双车道识别有效，则将所述当前时刻的跟随模式更新为车道线跟随模式；否则将所述当前时刻的跟随模式保持为渐变跟随模式。

根据本发明的一个实施例，在所述步骤四中，如果所述GPS数据无效但所述左车道线识别有效或右车道线识别有效，则将所述当前时刻的跟随模式保持为车道线跟随模式，并且将真实容错偏差置为零。

根据本发明的一个实施例，在所述步骤四中，如果所述GPS数据无效并且双车道线识别无效，则停止局部轨迹规划并生成告警信息。

根据本发明的一个实施例，在所述步骤四中，

根据所述当前时刻的跟随模式确定局部路径；

根据所述局部路径生成所述智能驾驶车辆的规划轨迹。

根据本发明的一个实施例，根据如下表达式确定所述局部路径：

P＝P_followMode+error_offset

其中，P表示局部路径，P_followMode表示当前跟随模式下的期望路径，error_offset表示真实容错偏差。

根据本发明的一个实施例，根据如下表达式确定所述规划轨迹：

P_trajectory(X)＝A₃x³+A₂x²+A₁x+A₀

其中，(X_f,Y_f,θ_f)表示轨迹终点坐标，y_offset表示终点偏移量，A₀、A₁、A₂和A₃均表示轨迹系数。

本发明所提供的用于智能驾驶车辆的局部轨迹容错方法能够在数据有效性判别过程中国对诸如GPS数据以及车道线数据进行甄别，其有效提高了系统对数据异常的甄别能力，同时还为后续得到的最终结果的准确性和可靠性奠定了基础。

同时，本方法能够基于各数据状态进行容错，并对相应数据进行实时动态的更新。该方法易于实际应用，其不仅能够提高数据处理的鲁棒性，还能够有效简化系统的复杂度。

此外，本方法针对GPS数据以及车道线数据等多传感数据对跟随模式进行实时状态转移，其简化了各个跟随模式之间的关系，实现了多跟随模式之间的连续、平滑控制，提高了智能驾驶车辆的舒适性和稳定性。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要的附图做简单的介绍：

图1是根据本发明一个实施例的用于智能驾驶车辆的局部轨迹容错方法的实现流程示意图；

图2是根据本发明一个实施例的确定GPS数据有效性判别结果的实现流程示意图；

图3是根据本发明一个实施例的确定车道线识别有效性的实现流程示意图；

图4是根据本发明另一个实施例的确定车道线识别有效性的实现流程示意图；

图5是根据本发明一个实施例的确定真实容错偏差的实现流程示意图。

具体实施方式

以下将结合附图及实施例来详细说明本发明的实施方式，借此对本发明如何应用技术手段来解决技术问题，并达成技术效果的实现过程能充分理解并据以实施。需要说明的是，只要不构成冲突，本发明中的各个实施例以及各实施例中的各个特征可以相互结合，所形成的技术方案均在本发明的保护范围之内。

同时，在以下说明中，出于解释的目的而阐述了许多具体细节，以提供对本发明实施例的彻底理解。然而，对本领域的技术人员来说显而易见的是，本发明可以不用这里的具体细节或者所描述的特定方式来实施。

另外，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

在半结构化环境下，对智能驾驶局部轨迹规划的连续性、错误数据的高容错性提出了较高的要求。半结构化环境下的导航过程容易存在突发异常，包括：车道不连续、车道错误识别或GPS/IMU定位失真等。该情况下的局部路径轨迹规划需要具备对异常问题的高容错性和稳定性。为此，需研究一种智能驾驶局部轨迹容错规划方法，使得智能驾驶车辆具有高容错性和高稳定性，提升智能车辆路径轨迹规划的实时性和鲁棒性。

本发明提供了一种新的用于智能驾驶车辆的局部轨迹容错方法，该方法能够基于车道线数据和GPS数据来克服数据信息异常所导致的不稳定性，从而实现智能驾驶车道跟随。

图1示出了本实施例所提供的用于智能驾驶车辆的局部轨迹容错方法的实现流程示意图。

如图1所示，本实施例所提供的方法首先在步骤S101中初始化跟随模式，并建立智能驾驶车辆的车辆坐标系。本实施例中，通过对跟随模式进行初始化，该方法可以将智能驾驶车辆的跟随模式谁都能够为GPS跟随模式，即使得M_follow＝M_gps。其中，M_follow表示智能驾驶车辆的跟随模式，M_gps表示GPS跟随模式。

本实施例中，智能驾驶车辆的GPS/IMU接收器优选地安装在车辆的车头位置，这样能够有效地减少车辆转向误差以及GPS反馈误差。而该方法在步骤S101中所构建的车辆坐标系也就可以以车头中心位置为坐标原点，以车辆正前方朝向为X轴，以车辆正左方为Y轴，以车辆正上方为Z轴。

当然，在本发明的其它实施例中，该方法在步骤S101中所构建得到的车辆坐标系还可以为其它合理形式，本发明不限于此。

如图1所示，本实施例中，在构建得到车辆坐标系后，该方法会在步骤S102中基于上述车辆坐标系，来根据获取到的GPS数据进行GPS数据有效性判别，从而得到GPS数据有效性判别结果。

图2示出了本实施例所提供的方法确定GPS数据有效性判别结果的实现流程示意图。

如图2所示，本实施例中，该方法会在步骤S201中基于上述车辆坐标系，根据当前时刻的GPS数据以及前一时刻的GPS数据确定GPS位置误差。具体地，本实施例中，该方法会在步骤S201中根据所获取到的当前时刻的GPS数据和前一时刻的GPS数据分别确定当前时刻的GPS期望路径P_gps(t)以及前一时刻的GPS期望路径P_gps(t-1)。

需要指出的是，本实施例中，GPS期望路径P_gps优选地表征来自高精度地图全局规划提供的基于GPS数据的车道中心路径，车道线期望路径P_lane优选地表征源于智能驾驶车辆摄像头所检测到的图像提供的两条车道线。

本实施例中，期望路径P(包括GPS期望路径P_gps和车道线期望路径P_lane)均可以采用如下表达式进行表示：

P(x)＝A₃x³+A₂x²+A₁x+A₀ (1)

其中，x表示车辆坐标系下X轴位置坐标，A₀、A₁、A₂和A₃均表示轨迹系数。

该方法在步骤S101中会分别确定x＝0位置处的当前时刻(即t时刻)的GPS期望路径P_gps(x＝0|t)以及前一时刻(即t-1时刻)的GPS期望路径P_gps(x＝0|t-1)，随后根据P_gps(x＝0|t)与P_gps(x＝0|t-1)的差值确定出GPS位置误差error_gps。

具体地，本实施例中，该方法优选地根据如下表达式计算GPS位置误差error_gps：

error_gps＝P_gps(x＝0|t)-P_gps(x＝0|t-1) (2)

在得到GPS位置误差error_gps后，该方法会在步骤S202中根据GPS位置误差error_gps进行GPS数据的有效性判别，从而得到GPS有效性判别结果。

具体地，本实施例中，该方法在步骤S202中优选地判断GPS位置误差error_gps是否小于或等于预设运动误差error_move。其中，如果GPS位置误差error_gps小于或等于预设运动误差error_move，则判定GPS数据有效；否则判定GPS数据无效。即存在：

其中，valid_gps表示GPS数据有效性。valid_gps＝true表示GPS数据有效，valid_gps＝false表示GPS数据无效。

error_move表示当前时刻t与前一时刻t-1下GPS数据在局部坐标系下因智能驾驶车辆移动而产生的运动误差。本实施例中，运动误差error_move可以采用如下表达式计算得到：

error_move＝a₁v+b₁ (4)

其中，a₁和b₁表示计算系数，v表示智能驾驶车辆的行驶速度。

当然，在本发明的其它实施例中，该方法在步骤S202中还可以采用其它合理方式来进行GPS数据的有效性判别。

再次如图1所示，该方法在步骤S103中根据所获取到的GPS数据以及车道线数据来确定车道线识别状态，并在步骤S104中根据车道线识别状态确定真实容错偏差。

具体地，本实施例中，该方法优选地根据车道线的实时追踪帧数(包括左车道线实时追踪帧数F_trackLeft和右车道线实时追踪帧数F_trackRight)来确定车道线识别状态(包括左车道线识别状态valid_left和右车道线识别状态valid_right)。具体地，本实施例中，该方法会将车道线的实时追踪帧数与预设追踪帧数阈值进行比较，并根据比较结果来分别确定左车道线识别状态和右车道线识别状态。

其中，当左车道线实时追踪帧数F_trackLeft小于或等于预设追踪帧数阈值F_threshold(即F_trackLeft≤F_threshold)时，该方法也就可以判定此时左车道线识别无效，即valid_left＝false；而当左车道线实时追踪帧数F_trackLeft大于预设追踪帧数阈值F_threshold(即F_trackLeft＞F_threshold)时，该方法也就可以判定此时左车道线识别有效，即valid_left＝true。

类似地，当右车道线实时追踪帧数F_trackRight小于或等于预设追踪帧数阈值F_threshold(即F_trackRight≤F_threshold)时，该方法也就可以判定此时右车道线识别无效，即valid_right＝flase；而当右车道线实时追踪帧数F_trackRight大于预设追踪帧数阈值F_threshold(即F_trackRight＞F_threshold)时，该方法也就可以判定此时右车道线识别有效，即valid_right＝true。

当然，在本发明的其它实施例中，该方法还可以采用其它合理方式来确定车道线识别状态，本发明不限于此。例如，在本发明的一个实施例中，该方法还可以采用如图3所示的方式来确定车道线识别状态。

具体地，如图3所示，在该实施例中，该方法首先会在步骤S301中根据车道线数据分别确定左车道线中心数据Y_centerLeft和右车道线中心数据Y_centerRight。其中，该方法优选地根据如下表达式来计算上述左车道线中心数据Y_centerLeft和右车道线中心数据Y_centerRight：

Y_centerLeft＝(P_laneLeft(x₁)+P_laneLeft(x₂)+P_laneLeft(x₃))/3 (5)

Y_centerRight＝(P_laneRight(x₁)+P_laneRight(x₂)+P_laneRight(x₃))/3 (6)

其中，P_laneLeft(x₁)、P_laneLeft(x₂)和P_laneLeft(x₃)分别表示横坐标为x₁、x₂和x₃的三个不同位置处的左车道线轨迹，P_laneRight(x₁)、P_laneRight(x₂)和P_laneRight(x₃)分别表示横坐标为x₁、x₂和x₃的三个不同位置处的右车道线轨迹。

随后，该方法会在步骤S302中根据上述左车道线中心数据Y_centerLeft和右车道线中心数据Y_centerRight确定车道识别宽度W。具体地，本实施例中，该方法优选地根据如下表达式来确定车道识别宽度W：

W＝Y_centerLeft-Y_centerRight (7)

在步骤S303中，该方法能够根据上述车道识别宽度W和真实车道宽度W_real确定出车道线的有效性。具体地，本实施例中，该方法在步骤303中会计算车道识别宽度W和真实车道宽度W_real的差值，从而得到车道线宽度误差error_laneWidth，即存在：

error_laneWidth＝W-W_real (8)

随后，该方法会判断车道线宽度误差error_laneWidth是否大于预设宽度误差阈值error_{widthThreshold}。其中，如果车道线宽度误差error_laneWidth大于预设宽度误差阈值error_{widthThreshold}，那么该方法也就可以判断出双车道线识别无效，即valid_left＝false且valid_right＝flase；而如果如果车道线宽度误差error_laneWidth小于或等于预设宽度误差阈值error_{widthThreshold}，那么该方法也就可以判断出双车道线识别有效，即valid_left＝true且valid_right＝true。

在本发明的另一个实施例中，该方法还可以采用如图4所示的方式来确定车道线的识别有效性。具体地，如图4所示，在该实施例中，该方法首先会在步骤S401中根据车道线数据分别确定左车道线中心数据Y_centerLeft和右车道线中心数据Y_centerRight。需要指出的是，该实施例中步骤S401的实现原理以及实现过程与上述步骤S301的具体实现原理以及实现过程相同，故在此不再对步骤S401的具体内容进行赘述。

该方法还会在步骤S402中根据接收到的GPS数据来确定GPS车道车道中心数据Y_centerGps。随后，该方法会在步骤S403中根据左车道中心数据Y_centerLeft和GPS车道中心数据Y_centerGps确定第一距离误差error_laneLeft，并在步骤S404中根据右车道线中心数据和GPS车道中心数据Y_centerGps确定第二距离误差error_laneRight。

具体地，在该实施例中，该方法优选地采用如下表达式来计算上述第一距离误差Y_centerGps和第二距离误差error_laneRight：

error_laneLeft＝abs(Y_centerLeft-W_real/2)-Y_centerGps (9)

error_laneRight＝abs(Y_centerRight-W_real/2)-Y_centerGps (10)

其中，存在：

Y_centergps＝(P_gps(x₁)+P_gps(x₂)+P_gps(x₃))/3 (11)

P_gps(x₁)、P_gps(x₂)和P_gps(x₃)分别表示横坐标为x₁、x₂和x₃的位置处的GPS车道线数据。

随后，该方法会在步骤S405中根据上述第一距离误差error_laneLeft、第二距离误差error_laneRight以及预设中心偏差阈值error_{widthThreshold}，分别确定左车道线的识别有效性和右车道线的识别有效性。

其中，如果第一距离误差error_laneLeft小于或等于预设中心偏差阈值error_{widthThreshold}，那么该方法也就可以确定左车道线识别有效，即valid_left＝true；而如果如果第一距离误差error_laneLeft大于预设中心偏差阈值error_{widthThreshold}，那么该方法也就可以确定左车道线识别无效，即valid_left＝false。

类似地，如果第二距离误差error_laneRight大于预设中心偏差阈值error_{widthThreshold}，那么该方法也就可以确定右车道线识别有效，即valid_right＝true；而如果如果第二距离误差error_laneRight大于预设中心偏差阈值error_{widthThreshold}，那么该方法也就可以确定右车道线识别无效即valid_right＝flase。

当然，在本发明的其它实施例中，根据实际需要，该方法还可以采用多种方法结合的方式来确定车道线识别的有效性，本发明不限于此。

再次如图1所示，本实施例中，在确定出车道线识别的有效性结果后，该方法便可以根据车道线识别的有效性结果来确定车道线识别状态Status_lane。具体地，如果车道线识别的有效性结果为双车道线识别无效，即valid_left＝false且valid_right＝flase，那么该方法则可以确定车道线识别状态Status_lane为无车道线识别状态，即Status_lane＝NoneLaneDetected；如果车道线识别的有效性结果为单车道线识别有效，即valid_left＝false且valid_right＝true，或valid_left＝true且valid_right＝flase，那么该方法则可以确定车道线识别状态Status_lane为单车道线识别状态，即Status_lane＝SingleLaneDetected；而如果车道线识别的有效性结果为双车道线识别有效，即valid_left＝true且valid_right＝true，那么该方法则可以确定车道线识别状态Status_lane为双车道线识别状态，即Status_lane＝DoubleLaneDetected。

在步骤S103中，该方法还可以根据车道识别状态来确定出单车道持续时长T_{MissSingleLane}、丢失左车道时长T_MissLeftLane和丢失右车道时长T_{MissRightLane}。具体地，本实施例中，该方法优选地根据如下表1所示的方式来确定出单车道持续时长T_{MissSingleLane}、丢失左车道时长T_MissLeftLane和丢失右车道时长T_{MissRightLane}。

表1

valid<sub>left</sub>	valid<sub>right</sub>	Status<sub>lane</sub>	T<sub>MissSingleLane</sub>	T<sub>MissLeftLane</sub>	T<sub>MissRightLane</sub>
						true	true	DoubleLaneDetected	0	0	0
false	true	SingleLaneDetected	+1	+1	0
						true	false	SingleLaneDetected	+1	0	+1
false	false	NoneLaneDetected	INF	INF	INF

从表1中可以看出，本实施例中，当车道线识别状态为双车道线识别有效时，该方法会将上述三种时长均置为0；当车道线识别状态为单车道线识别有效时，该方法会将对应无效车道的丢失时长增加1，将对应有效车道的丢失时长置为0；而当车道线识别状态为双车道线识别无效时，该方法会将上述三种时长均置为INF。

再次如图1所示，本实施例中，该方法会在步骤S104中确定真实容错偏差error_offset。其中，图5示出了本实施例中该方法确定真实容错偏差error_offset的具体流程示意图。

如图5所示，在确定真实容错偏差error_offset的过程中，该方法会在步骤S501中根据左车道线中心数据、右车道线中心数据和车道中心数据确定检测车道中心与GPS车道中心之间的偏差，得到车道中心偏差，随后再在步骤S502中根据车道中心偏差来确定当前容错偏差。

具体地，本实施例中，该方法在步骤S501中优选地采用如下表达式来确定车道中心偏差error_GpsLane：

error_GpsLane＝(Y_centerLeft+Y_centerRight)/2-Y_centerGps (11)

其中，Y_centerLeft和Y_centerRight分别表示左车道线中心数据和右车道线中心数据，Y_centerGps分别表示GPS车道中心数据。

该方法在步骤S502中优选地根据如下表达式来确定当前容错偏差error_{offsetCurrent}：

error_{offsetCurrent}＝λerror_GpsLane+error_calibration (12)

其中，error_calibration表示标准偏差，λ表示偏差比例系数。

在得到当前容错偏差error_{offsetCurrent}后，如图5所示，该方法便可以在步骤S503中根据当前容错偏差error_{offsetCurrent}来确定出不同跟随模式下的真实容错偏差error_offset。

具体地，本实施例中，该方法在步骤S503中优选地根据如下表达式来确定不同跟随模式下的真实容错偏差error_offset：

其中，error_offset(t)和error_offset(t-1)分别表示t时刻和t-1时刻的真实容错偏差，error_{offsetCurrent}(t)表示t时刻的当前容错偏差，M_follow表示跟随模式，M_GPS表示GPS跟随模式，M_lane表示车道线跟随模式，M_gradient表示渐变跟随模式。

本实施例中，真实容错偏差error_offset的更新周期优选地与车道线以及规划轨迹的更新周期相同，其优选地为50ms。

当然，在本发明的其它实施例中，该方法还可以采用其它合理方式来确定真实容错偏差error_offset，本发明不限于此。

再次如图1所示，本实施例中，该方法会在步骤S105中根据步骤S 102中所确定出的GPS有效性判别结果、步骤S103所确定出的车道线识别状态以及步骤S104中所确定出的真实容错偏差来更新智能驾驶车辆的跟随模式，并在步骤S106中根据更新后的智能驾驶车辆的跟随模式来确定智能驾驶车辆的规划轨迹。

具体地，本实施例中，如果GPS数据有效并且智能驾驶车辆在前一时刻的跟随模式为GPS跟随模式，该方法则根据车道线识别状态确定智能驾驶车辆在当前时刻的跟随模式。

其中，如果车道线识别状态为双车道线识别有效，该方法则将当前时刻的跟随模式更新为车道线跟随模式；否则将当前时刻的跟随模式保持为GPS跟随模式，并且将真实容错偏差置为零。

即存在：

而如果GPS数据有效并且智能驾驶车辆在前一时刻的跟随模式为车道线跟随模式，该方法会根据车道线识别状态确定智能驾驶车辆在当前时刻的跟随模式。

其中，如果车道线识别状态为双车道线识别无效(即case1)，该方法则将当前时刻的跟随模式更新为渐变跟随模式；如果所述车道线识别状态为单车道线识别有效，该方法则进一步判断单车道丢失时长是否大于预设车道保留时长。其中，如果大于(即case2)，该方法则将当前时刻的跟随模式更新为渐变跟随模式，否则将当前时刻的跟随模式保持为车道线跟随模式，并且将真实容错偏差置为零。

即存在：

如果GPS数据有效并且智能驾驶车辆在前一时刻的跟随模式为渐变跟随模式，该方法则会判断所述真实容错偏差是否为零。其中，如果真实容错偏差为零，该方法则会将当前时刻的跟随模式更新为GPS跟随模式；如果真实容错偏差不为零且双车道识别有效(即case3)，该方法则会将当前时刻的跟随模式更新为车道线跟随模式；否则将当前时刻的跟随模式保持为渐变跟随模式。

即存在：

如果GPS数据无效但左车道线识别有效或右车道线识别有效，该方法则会将当前时刻的跟随模式保持为车道线跟随模式，并且将真实容错偏差置为零。

而如果GPS数据无效并且双车道线识别无效，该方法则会停止局部轨迹规划并生成告警信息。

如图1所示，在确定出完成智能驾驶车辆的跟随模式的更新后，该方法会在步骤S106中根据更新后的跟随模式来确定智能智能驾驶车辆的规划轨迹。

具体地，本实施例中，该方法在步骤S106中首先根据当前时刻的跟随模式(即更新后的跟随模式)确定局部路径，随后再根据局部路径生成智能驾驶车辆的规划轨迹。

本实施例中，该方法优选地采用如下表达式来确定局部路径P：

P＝Y_followMode+error_offset (17)

其中，value_decrease表示一个更新周期(例如50ms)真实容错偏差error_offset的应减小值。

在得到局部路径P后，该方法可以根据局部路径P确定基于三次多项式的最优运动轨迹P_trajectory。具体地，本实施例中，该方法根据局部路径P来确定出轨迹终点坐标(X_f,Y_f,θ_f)。其中，轨迹终点坐标(X_f,Y_f,θ_f)可以采用如下表达式确定得到：

在得到上述轨迹终点坐标(X_f,Y_f,θ_f)后，该方法也就可以根据轨迹终点坐标(X_f,Y_f,θ_f)确定出基于三次多项式的最优运动轨迹P_trajectory。

具体地，本实施例中，该方法优选地采用如下表达式来确定最优运动轨迹P_trajectory：

P_trajectory(X)＝A₃x³+A₂x²+A₁x+A₀ (22)

其中，y_offset表示终点偏移量，即轨迹原点到轨迹终点切线方向的距离。(X_f,Y_f,θ_f)表示轨迹终点坐标，A₀、A₁、A₂和A₃均表示轨迹系数，x表示车辆坐标系下X轴位置坐标，v表示智能驾驶车辆的行驶速度，a和b表示计算系数，P表示期望路径(即局部路径)。

本实施例中，该方法在得到最优运行轨迹后，会将该最优运行轨迹传输至跟踪控制系统，从而由跟踪控制系统来控制智能驾驶车辆的运动轨迹。如果该方法没有接收到全局系统所传输来的结束容错规划信号，那么该方法则会继续当前循环；而如果该方法接收到了全局系统所传输来的结束容错规划信号，那么该方法则会结束容错规划过程。

从上述描述中可以看出，本实施例所提供的用于智能驾驶车辆的局部轨迹容错方法能够在数据有效性判别过程中国对诸如GPS数据以及车道线数据进行甄别，其有效提高了系统对数据异常的甄别能力，同时还为后续得到的最终结果的准确性和可靠性奠定了基础。

同时，本方法能够基于各数据状态进行容错，并对相应数据进行实时动态的更新。该方法易于实际应用，其不仅能够提高数据处理的鲁棒性，还能够有效简化系统的复杂度。

此外，本方法针对GPS数据以及车道线数据等多传感数据对跟随模式进行实时状态转移，其简化了各个跟随模式之间的关系，实现了多跟随模式之间的连续、平滑控制，提高了智能驾驶车辆的舒适性和稳定性。

应该理解的是，本发明所公开的实施例不限于这里所公开的特定结构或处理步骤，而应当延伸到相关领域的普通技术人员所理解的这些特征的等同替代。还应当理解的是，在此使用的术语仅用于描述特定实施例的目的，而并不意味着限制。

说明书中提到的“一个实施例”或“实施例”意指结合实施例描述的特定特征、结构或特性包括在本发明的至少一个实施例中。因此，说明书通篇各个地方出现的短语“一个实施例”或“实施例”并不一定均指同一个实施例。

虽然上述示例用于说明本发明在一个或多个应用中的原理，但对于本领域的技术人员来说，在不背离本发明的原理和思想的情况下，明显可以在形式上、用法及实施的细节上作各种修改而不用付出创造性劳动。因此，本发明由所附的权利要求书来限定。

Claims (25)

1.一种用于智能驾驶车辆的局部轨迹容错方法，其特征在于，所述方法包括：

步骤一、初始化跟随模式，构建智能驾驶车辆的车辆坐标系；

步骤二、基于所述车辆坐标系，根据获取到的GPS数据进行GPS数据的有效性判别，得到GPS数据有效性判别结果；

步骤三、根据所述GPS数据和获取到的车道线数据确定车道线识别状态，并确定真实容错偏差；

步骤四、根据所述GPS数据有效性判别结果、车道线识别状态和真实容错偏差，更新所述智能驾驶车辆的跟随模式，根据更新后的跟随模式确定所述智能驾驶车辆的规划轨迹。

2.如权利要求1所述的方法，其特征在于，在所述步骤一中，通过初始化跟随模式，将所述智能驾驶车辆的跟随模式设置为GPS跟随模式。

3.如权利要求1或2所述的方法，其特征在于，在所述车辆坐标系中，所述智能驾驶车辆的车辆正前方朝向为X轴，车辆正左方为Y轴，车辆正上方为Z轴。

4.如权利要求1～3中任一项所述的方法，其特征在于，所述步骤二包括：

基于所述车辆坐标系，根据当前时刻的GPS数据以及前一时刻的GPS数据确定GPS位置误差；

根据所述GPS位置误差进行GPS数据的有效性判别，确定所述GPS数据有效性判别结果。

5.如权利要求4所述的方法，其特征在于，在所述步骤二中，根据如下表达式计算所述GPS位置误差：

error_gps＝P_gps(x＝0|t)-P_gps(x＝0|t-1)

其中，error_gps表示GPS位置误差，P_gps(x＝0|t)表示x＝0位置处的t时刻的GPS期望路径，P_gps(x＝0|t-1)表示x＝0位置处的t-1时刻的GPS期望路径。

6.如权利要求4或5所述的方法，其特征在于，判断所述GPS位置误差是否小于或等于预设运动误差，其中，如果所述GPS位置误差小于或等于预设运动误差，则判定所述GPS数据有效，否则判定所述GPS数据无效。

7.如权利要求1～6中任一项所述的方法，其特征在于，在所述步骤三中，获取车道线实时追踪帧数，分别将所述车道线实时追踪帧数中的左车道线实时追踪帧数和右车道线实时追踪帧数与预设追踪帧数阈值进行比较，根据比较结果确定所述车道线识别状态。

8.如权利要求7所述的方法，其特征在于，如果所述左车道线实时追踪帧数小于或等于预设追踪帧数阈值，则判定所述左车道线识别无效，否则判定所述左车道线识别有效；

如果所述右车道线实时追踪帧数小于或等于预设追踪帧数阈值，则判定所述右车道线识别无效，否则判定所述右车道线识别有效。

9.如权利要求1～8中任一项所述的方法，其特征在于，在所述步骤三中，

根据所述车道线数据分别确定左车道线中心数据和右车道线中心数据；

根据所述左车道线中心数据和右车道线中心数据确定车道识别宽度；

根据所述车道识别宽度和车道真实宽度确定车道线识别状态。

10.如权利要求9所述的方法，其特征在于，如果所述车道识别宽度与车道真实宽度之间差值的绝对值小于或等于预设宽度差值阈值，则判定双车道线识别有效，否则判定双车道线识别无效。

11.如权利要求1～10中任一项所述的方法，其特征在于，在所述步骤三中，

根据所述车道线数据分别确定左车道线中心数据和右车道线中心数据；

根据所述GPS数据确定车道中心数据；

根据所述左车道线中心数据和车道中心数据确定第一距离误差，根据所述右车道线中心数据和车道中心数据确定第二距离误差；

根据所述第一距离误差、第二距离误差和预设中心偏差阈值，分别确定所述左车道线和右车道线的识别有效性。

12.如权利要求11所述的方法，其特征在于，

分别根据如下表达式计算所述左车道线中心数据和右车道线中心数据：

Y_centerLeft＝(P_laneLeft(x₁)+P_laneLeft(x₂)+P_laneLeft(x₃))/3

Y_centerRight＝(P_laneRight(x₁)+P_laneRight(x₂)+P_laneRight(x₃))/3

根据如下表达式确定所述车道中心数据：

Y_centergps＝(P_gps(x₁)+P_gps(x₂)+P_gps(x₃))/3

分别根据如下表达式计算所述第一距离误差和第二距离误差：

error_laneLeft＝abs(Y_centerLeft-W_real/2)-Y_centerGps

error_laneRight＝abs(Y_centerRight-W_real/2)-Y_centerGps

其中，P_laneLeft(x₁)、P_laneLeft(x₂)和P_laneLeft(x₃)分别表示横坐标为x₁、x₂和x₃的位置处的左车道线轨迹，P_laneRight(x₁)、P_laneRight(x₂)和P_laneRight(x₃)分别表示横坐标为x₁、x₂和x₃的位置处的右车道线轨迹，Y_centerLeft和Y_centerRight分别表示左车道线中心数据和右车道线中心数据，Y_centergps表示GPS车道中心数据，P_gps(x₁)、P_gps(x₂)和P_gps(x₃)分别表示横坐标为x₁、x₂和x₃的位置处的GPS车道线数据，error_laneLeft和error_laneRight分别表示第一距离误差和第二距离误差，W_real表示轨迹真实宽度。

13.如权利要求11或12所述的方法，其特征在于，如果所述第一距离误差小于或等于预设中心偏差阈值，则确定左车道线识别有效，否则确定左车道线识别无效。

14.如权利要求11～13中任一项所述的方法，其特征在于，在所述步骤三中，还根据所述车道线识别状态确定单车道持续时长T_{MissSingleLane}、丢失左车道时长T_MissLeftLane和丢失右车道时长T_{MissRightLane}。

15.如权利要求11～14中任一项所述的方法，其特征在于，在所述步骤三中，

根据所述左车道线中心数据、右车道线中心数据和车道中心数据确定检测车道中心与GPS车道中心之间的偏差，得到车道中心偏差；

根据所述车道中心确定当前容错偏差；

根据所述当前容错偏差，计算不同跟随模式下的真实容错偏差。

16.如权利要求15所述的方法，其特征在于，根据如下表达式计算所述当前容错偏差：

error_GpsLane＝(Y_centerLeft+Y_centerRight)/2-Y_centerGps

error_{offsetCurrent}＝λerror_GpsLane+error_calibration

其中，error_{offsetCurrent}表示当前容错偏差，λ表示偏差比例系数，error_GpsLane表示车道中心偏差，error_calibration表示标准偏差，Y_centerLeft和Y_centerRight分别表示左车道线中心数据和右车道线中心数据，Y_centerGps分别表示GPS车道中心数据。

17.如权利要求16所述的方法，其特征在于，根据如下表达式计算不同跟随模式下的真实容错偏差：

其中，error_offset(t)和error_offset(t-1)分别表示t时刻和t-1时刻的真实容错偏差，error_{offsetCurrent}(t)表示t时刻的当前容错偏差，M_follow表示跟随模式，M_GPS表示GPS跟随模式，M_lane表示车道线跟随模式，M_gradient表示渐变跟随模式。

18.如权利要求1～17中任一项所述的方法，其特征在于，在所述步骤四中，如果所述GPS数据有效并且所述智能驾驶车辆在前一时刻的跟随模式为GPS跟随模式，则根据所述车道线识别状态确定所述智能驾驶车辆在当前时刻的跟随模式，其中，

如果所述车道线识别状态为双车道线识别有效，则将所述当前时刻的跟随模式更新为车道线跟随模式；否则将所述当前时刻的跟随模式保持为GPS跟随模式，并且将真实容错偏差置为零。

19.如权利要求1～18中任一项所述的方法，其特征在于，在所述步骤四中，如果所述GPS数据有效并且所述智能驾驶车辆在前一时刻的跟随模式为车道线跟随模式，根据所述车道线识别状态确定所述智能驾驶车辆在当前时刻的跟随模式，其中，

如果所述车道线识别状态为双车道线识别无效，则将所述当前时刻的跟随模式更新为渐变跟随模式；如果所述所述车道线识别状态为单车道线识别有效，则进一步判断单车道丢失时长是否大于预设车道保留时长，其中，如果大于，则将所述当前时刻的跟随模式更新为渐变跟随模式，否则将所述当前时刻的跟随模式保持为车道线跟随模式，并且将真实容错偏差置为零。

20.如权利要求1～19中任一项所述的方法，其特征在于，在所述步骤四中，如果所述GPS数据有效并且所述智能驾驶车辆在前一时刻的跟随模式为渐变跟随模式，判断所述真实容错偏差是否为零，其中，

如果所述真实容错偏差为零，则将所述当前时刻的跟随模式更新为GPS跟随模式；如果所述真实容错偏差不为零且双车道识别有效，则将所述当前时刻的跟随模式更新为车道线跟随模式；否则将所述当前时刻的跟随模式保持为渐变跟随模式。

21.如权利要求1～20中任一项所述的方法，其特征在于，在所述步骤四中，如果所述GPS数据无效但所述左车道线识别有效或右车道线识别有效，则将所述当前时刻的跟随模式保持为车道线跟随模式，并且将真实容错偏差置为零。

22.如权利要求1～21中任一项所述的方法，其特征在于，在所述步骤四中，如果所述GPS数据无效并且双车道线识别无效，则停止局部轨迹规划并生成告警信息。

23.如权利要求1～22中任一项所述的方法，其特征在于，在所述步骤四中，

根据所述当前时刻的跟随模式确定局部路径；

根据所述局部路径生成所述智能驾驶车辆的规划轨迹。

24.如权利要求23所述的方法，其特征在于，根据如下表达式确定所述局部路径：

P＝P_followMode+error_offset

其中，P表示局部路径，P_followMode表示当前跟随模式下的期望路径，error_offset(t)和error_offset(t-1)分别表示t时刻和t-1时刻的真实容错偏差，M_follow表示跟随模式，M_GPS表示GPS跟随模式，M_lane表示车道线跟随模式，M_gradient表示渐变跟随模式，value_decrease表示一个更新周期真实容错偏差的应减小值，P_gps和P_lane分别表示GPS期望路径和车道线期望路径。

25.如权利要求23或24所述的方法，其特征在于，根据如下表达式确定所述规划轨迹：

P_trajectory(X)＝A₃x³+A₂x²+A₁x+A₀

其中，(X_f,Y_f,θ_f)表示轨迹终点坐标，y_offset表示终点偏移量，A₀、A₁、A₂和A₃均表示轨迹系数，x表示车辆坐标系下X轴位置坐标，v表示智能驾驶车辆的行驶速度，a和b表示计算系数。