CN112776886B - 智能电动车辆视觉车道居中辅助系统失效安全控制方法 - Google Patents

Abstract

本发明提出的一种智能电动车辆视觉车道居中辅助系统失效安全控制方法，通过自车搭载的视觉相机得到智能电动车辆行驶过程中视觉相机探测范围内的车道线信号，根据车道线方程得到自车的期望行驶轨迹；当车道线信号正常时，利用车道线信号计算当前时刻的预瞄横向偏差，以此计算当前时刻的自车期望方向盘转角信号，自车按照该转角信号进行操作；当车道线信号异常时，车道居中辅助系统将基于PDC算法推算自车与道路中心的相对位置关系，并求得自车需执行的转向命令，在一定时间内维持住自车在既定车道内行驶。本发明能够在车道线信号完全失效且驾驶员来不及接管的时间段，为驾驶员安全接管争取宝贵时间，从而达到安全行驶的目的。

Description

智能电动车辆视觉车道居中辅助系统失效安全控制方法

技术领域

本发明涉及智能电动车辆自动驾驶辅助技术领域，特别是涉及一种针对智能电动车辆视觉车道居中辅助系统(Visual System of Lane Centering Control.VS-LCC)，利用预瞄偏差推算(Preview Deviation Calculation.PDC)的失效安全控制方法。

背景技术

视觉相机在LCC(Lane Centering Control)控制过程中为车辆提供前方车道线信号，包括车道线方程、车道线质量、车道线线型等。但视觉相机作为车辆LCC过程中车道线信号的唯一来源，其本身易受到环境干扰，不可避免地会阶段性或持续性地发送错误信号，在驾驶员没有及时接管的情况下，车辆将沿偏离车道方向行驶，容易引发交通事故。如果引入相应的失效安全控制机制，及时检测出视觉信号的异常并制止车辆的非期望运动，将有力保障人们的生命安全。因此，VS-LCC的失效安全控制研究具有重要意义。

航位推算系统是一种常见的自主导航定位系统，不受外界干扰，信号无丢失，短时间内精度较高。航位推算系统在船舶、车辆上都有广泛的应用，其有不同的传感器配置方案，可以采用单轴陀螺仪加上单轴加速度计实现，也可以采用单轴陀螺仪和单里程计的方案，或者双里程差分的方案。航位推算的基本原理是从当前时刻已知的位置出发，根据当前车辆的行驶方向和速度来推算车辆下一时刻的位置。在计算的过程中，需要的输入信息是车辆的行驶方向和速度，可通过双里程计差分方案实现。

航位推算一般作为GPS的冗余定位系统应用于船舶车辆中。目前暂时还没有将航位推算应用于VS-LCC的案例。而在双侧车道线信号异常情况下，获得合理的车路相对位置关系至关重要。视觉相机感知到的车道线信号可通过相机内部参数和安装标定转化为绝对坐标系下的信号，车辆的绝对位置坐标也可通过航位推算求得，基于两个绝对位置信息就可求解相对位置信息。

因此可以考虑将航位推算和坐标转换应用到VS-LCC失效安全控制中来。

发明内容

为了克服现有技术的不足，本发明旨在针对智能电动车辆的视觉车道居中辅助系统，提供一种基于预瞄偏差推算的智能电动车辆VS-LCC失效安全控制方法，该方法在车道线信号完全丢失或不可用时，能够在一定时间内控制车辆不驶离既定车道，为驾驶员接管争取宝贵时间。

为了实现上述目的，本发明所采取的技术方案如下：

本发明提出的一种智能电动车辆视觉车道居中辅助系统失效安全控制方法，其特征在于，包括以下步骤：

1)当前控制周期开始时刻，通过自车搭载的视觉相机得到智能电动车辆行驶过程中视觉相机探测范围内的车道线信号，包括车道线方程、车道线质量、车道线线型和车道线线宽；根据车道线方程得到自车的期望行驶轨迹；当车道线信号正常时，执行步骤2)，当车道线信号异常时，则执行步骤3)；

2)车道居中辅助系统基于期望行驶轨迹控制自车保持在既定车道内行驶，直到当前控制周期结束返回步骤1)，进行下一控制周期；

3)车道居中辅助系统基于PDC算法推算自车与道路中心的相对位置关系，并求得自车需执行的转向命令，在一定时间内维持住自车在既定车道内行驶，直到当前控制周期结束返回步骤1)，进行下一控制周期。

进一步地，步骤2)具体包括以下步骤：

21)利用车道线信号计算当前时刻的预瞄横向偏差

根据车道线方程拟合出视觉相机坐标系下自车的期望运行轨迹，并计算出视觉相机坐标系下当前时刻t的预瞄横向偏差Δf(t)：

Δf(t)＝a₀(t)+a₁(t)x_pre(t)+a₂(t)x_pre(t)²+a₃(t)x_pre(t)³，x_pre(t)∈(x₁，x₂)

式中，x_pre(t)为当前时刻t视觉相机向前方路面的预瞄距离；a₀(t)为视觉相机坐标系下当前时刻t的自车初始横向坐标；a₁(t)为视觉相机坐标系下当前时刻t的自车轨迹航向；a₂(t)为视觉相机坐标系下当前时刻t的自车轨迹曲率；a₃(t)为视觉相机坐标系下当前时刻t的自车轨迹曲率变化率；x₁，x₂为视觉相机能够探测到前方路面的最小和最大距离；

22)按照下式计算当前时刻的自车期望方向盘转角信号δ_f(t)：

式中，δ_f(t-1)为前一时刻(t-1)的自车期望方向盘转角信号；D(t)为前一时刻(t-1)至当前时刻t自车走过轨迹形成扇形的弦长；v(t)为当前时刻t自车速度；t_p为预设的预瞄时间；K为反馈权重；β(t)为当前时刻t的自车质心侧偏角；θ(t)为当前时刻t的自车横摆角；

23)自车按照当前时刻的自车期望方向盘转角信号δ_f(t)进行转向操作，等待下一控制周期的到来，返回步骤1)。

进一步地，步骤3)具体包括以下步骤：

31)按照下式将失效前一时刻的视觉信号转换为大地绝对坐标系下的既定轨迹：

λ₀＝H^-1λ

式中：X_c(t-1)，Y_c(t-1)，Z_c(t-1)分别为视觉信号异常时前一时刻(t-1)相机坐标系中自车期望轨迹f_c(x，t-1)上各坐标；(X_g(t-1)，Y_g(t-1)，Z_g(t-1))分别是由自车期望轨迹f_c(x，t-1)转换得到的大地坐标系中自车既定轨迹f_w(x，t-1)上的各坐标；[τ₂ τ₃ τ₁]为相机内部旋转矩阵的逆矩阵H^-1的三个向量；λ₀为转换平移向量，由平移向量λ求出；

旋转矩阵H和平移向量λ的表达式为：

式中，κ为视觉相机相对车体的安装俯仰角；γ为视觉相机相对车体的安装侧倾角；α为视觉相机相对车体的安装方向角；l为视觉相机安装位置与自车后轴中心的距离；h为视觉相机安装位置与地面的垂直距离；d为视觉相机安装位置与自车中轴线的横向距离；[τ₁₁ τ₁₂ τ₁₃]、[τ₂₁ τ₂₂ τ₂₃]、[τ₃₁ τ₃₂ τ₃₃]分别为大地坐标系沿各坐标轴的一组基向量；

假设行驶路面平坦，即Z_g＝0，通过下式得到大地绝对坐标系下前一时刻(t-1)自车既定轨迹上的各点坐标X_g(t-1)，Y_g(t-1)：

式中，

分别为相机水平方向和垂直方向的焦距，q_r，q_c分别为成像坐标系原点在像素坐标系中的坐标值，(τ，c)为车道线图像上任一像素点在成像平面坐标系下的坐标。

根据下式拟合得到当前时刻t大地坐标系下的自车既定轨迹f_w(x，t)：

f_w(x，t)＝a_w0(t)+a_w1(t)x+a_w2(t)x²+a_w3(t)x³

式中，a_w0(t)为当前时刻t大地坐标系下自车横向坐标，a_w1(t)为当前时刻t大地坐标系下的自车既定轨迹航向，a_w2(t)为当前时刻t大地坐标系下的自车既定轨迹曲率，a_w3(t)为当前时刻t大地坐标系下的自车既定轨迹曲率的导数，x为大地坐标系下自车向前的预瞄距离；

32)利用PDC算法推算当前时刻的自车期望转向信号

首先推算当前时刻t自车在大地坐标系下的位置坐标(X_v(t)，Y_v(t))：

X_v(t)＝X_v(t-1)+v(t-1)Δt cos(θ(t-1)+ω(t-1)Δt)

Y_v(t)＝Y_v(t-1)+v(t-1)Δt sin(θ(t-1)+ω(t-1)Δt)

式中，(X_v(t-1)，Y_v(t-1))为车道线信号失效前一时刻(t-1)自车的大地坐标；Δt为单位采样时间；v(t-1)为车道线信号失效前一时刻(t-1)的自车车速，由车道线信号失效前一时刻(t-1)自车左后轮速v_l(t-1)和右后轮速v_r(t-1)求均值获得；θ(t-1)为车道线信号失效前一时刻(t-1)自车横摆角，ω(t-1)为车道线信号失效前一时刻(t-1)自车的横摆角速度；

然后根据大地坐标系下的既定轨迹f_w(x，t)和车辆坐标(X_v(t)，Y_v(t))，实时推算当前时刻t自车横向预瞄偏差Δf_PDC(t)：

Δf_PDC(t)＝Y_v(t)+x_pre(t)sinθ(t)-[a_w0(t)+a_w1(t)(X_v(t)+x_pre(t)cosθ(t))+a_w2(t)(X_v(t)+x_pre(t)cosθ(t))²+a_w3(t)(X_v(t)+x_pre(t)cosθ(t))³]

按照下式得到推算的当前时刻的自车期望转向信号δ_PDC(t)：

式中，D(t)为前一时刻(t-1)至当前时刻t自车走过轨迹形成扇形的弦长，δ_f(t-1)为前一时刻(t-1)自车的方向盘转角，β(t)为当前时刻t车辆的质心侧偏角；

33)自车按照当前时刻的自车期望转向信号δ_PDC(t)进行转向操作，等待下一控制周期的到来，返回步骤1)。

与现有技术相比，本发明显著特点体现在：本发明提出的PDC失效安全控制方法，能够在车道线信号完全失效且驾驶员来不及接管的时间段，仅利用双后轮速传感器信号和失效前车道线信号推算之后时刻车辆与道路中心的相对位置关系，并求解车辆需要执行的转向命令，为驾驶员安全接管争取宝贵时间，从而达到稳定控制车辆的目的。整个控制过程不需要额外感知传感器的冗余，大大降低成本。

本发明的其他特征和优点将在随后的说明书中阐述，并且部分的从说明书中变得显而易见，或者通过实施本发明而了解。

附图说明

附图仅用于出示具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为本发明实施例的一种智能电动车辆视觉车道居中辅助系统失效安全控制方法的流程框图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述，本领域的技术人员应该知道，以下实施例并不是对本发明技术方案作的唯一限定，凡是在本发明技术方案精神实质下所做的任何等同变换或改动，均应视为属于本发明的保护范围。

参见图1，本发明建立一种基于预瞄偏差推算(Preview DeviationCalculation.PDC)的智能电动车辆视觉车道居中辅助系统(Visual System of LaneCentering Control.VS-LCC)失效安全控制方法。包括如下步骤：

1)当前控制周期开始时刻，通过自车搭载的视觉相机得到智能电动车辆行驶过程中视觉相机探测范围内的车道线信号，包括车道线方程、车道线质量、车道线线型、车道线线宽等。根据质量可靠的车道线方程得到自车的期望行驶轨迹。当车道线信号正常时，执行步骤2)，当车道线信号异常时，则执行步骤3)。

2)车道线信号正常情况下，车道居中辅助系统基于期望行驶轨迹控制自车保持在既定车道内行驶，直到当前控制周期结束返回步骤1)，进行下一控制周期。

3)两侧车道线信号异常情况下，车道居中辅助系统将基于PDC算法推算自车与道路中心的相对位置关系，并求得自车需执行的转向命令，在一定时间内维持住自车在既定车道内行驶，直到当前控制周期结束返回步骤1)，进行下一控制周期。

进一步地，车道线信号正常时，步骤2)具体包括以下步骤：

21)利用车道线信号计算当前时刻预瞄横向偏差

视觉相机探测的是左右两侧的车道线方程，两侧车道线的中心线即为车辆的期望运行轨迹，因此可以根据车道线方程拟合出视觉相机坐标系下自车的期望运行轨迹，并计算出视觉相机坐标系下当前时刻t自车相对期望运行轨迹的预瞄横向偏差Δf(t)：

Δf(t)＝a₀(t)+a₁(t)x_pre(t)+a₂(t)x_pre(t)²+a₃(t)x_pre(t)³，xpre(t)∈(x₁，x₂)式中，x_pre(t))为当前时刻t视觉相机向前方路面的预瞄距离；a₀(t)为视觉相机坐标系下当前时刻t的自车初始横向坐标；a₁(t)为视觉相机坐标系下当前时刻t的自车轨迹航向；a₂(t)为视觉相机坐标系下当前时刻t的自车轨迹曲率；a₃(t)为视觉相机坐标系下当前时刻t的自车轨迹曲率变化率；x₁，x₂为视觉相机能够探测到前方路面的最小和最大距离，由视觉相机自身性能决定，与当前时刻t无关。

22)计算当前时刻的自车期望方向盘转角信号δ_f(t)，计算公式如下：

式中，δ_f(t-1)为前一时刻(t-1)的自车期望方向盘转角信号，由自车的方向盘转角信号传感器测量得到；Δf(t)为步骤21)计算得到的当前时刻t预瞄横向偏差；D(t)为前一时刻(t-1)至当前时刻t自车走过轨迹形成扇形的弦长；v(t)为当前时刻t自车速度；t_p为预设的预瞄时间，其值的确定由行驶工况和车辆响应延迟决定，需要通过标定获得；K为反馈权重，由车辆整车控制器的响应性能决定，可标定获得；β(t)为当前时刻t的自车质心侧偏角度，θ(t)为当前时刻t的自车横摆角；

23)自车按照当前时刻的自车期望方向盘转角信号δ_f(t)进行转向操作，等待下一控制周期的到来，返回步骤1)。

进一步地，车道线信号异常时，基于PDC算法的VS-LCC失效安全控制方法即步骤3)具体包括以下步骤：

31)将失效前一时刻的视觉信号转换为大地绝对坐标系下的既定轨迹，计算公式如下：

λ₀＝H^-1λ

式中：X_c(t-1)，Y_c(t-1)，Z_c(t-1)分别为视觉信号异常时前一时刻(t-1)相机坐标系中自车期望轨迹f_c(x，t-1)上各坐标；(X_g(t-1)，Y_g(t-1)，Z_g(t-1))分别是由自车期望轨迹f_c(x，t-1)转换得到的大地坐标系中自车既定轨迹f_w(x，t-1)上的各坐标；[τ₂ τ₃ τ₁]为相机内部旋转矩阵的逆矩阵H^-1的三个向量；λ₀为转换平移向量，由平移向量λ求出；

旋转矩阵H和平移向量λ的表达式为：

式中，κ为视觉相机相对车体的安装俯仰角；γ为视觉相机相对车体的安装侧倾角；α为视觉相机相对车体的安装方向角；l为视觉相机安装位置与自车后轴中心的距离；h为视觉相机安装位置与地面的垂直距离；d为视觉相机安装位置与自车中轴线的横向距离；[τ₁₁ τ₁₂ τ₁₃]、[τ₂₁ τ₂₂ τ₂₃]、[τ₃₁ τ₃₂ τ₃₃]分别为大地坐标系沿各坐标轴的一组基向量；

由于LCC的使用场景多为高速公路等路面良好的结构化道路，路面起伏高度很小，因此可以视Z_g＝0，并通过下式得到大地绝对坐标系下前一时刻(t-1)自车既定轨迹上的各点坐标X_g(t-1)，Y_g(t-1)：

式中，

分别为相机水平方向和垂直方向的焦距，q_r，q_c分别为成像坐标系原点在像素坐标系中的坐标值，(τ，c)为车道线图像上任一像素点在成像平面坐标系下的坐标。

由于视觉相机已经失效，X_g(t-1)，Y_g(t-1)是尚且还准确的坐标，这些坐标将保持不变直至信号恢复正常，由X_g(t-1)，Y_g(t-1)拟合得到的自车既定轨迹f_w(x，t-1)在之后时刻也不会发生变化，即当前时刻t大地坐标系下的自车既定轨迹f_w(x，t)与f_w(x，t-1)相等，拟合得到的f_w(x，t)表达式为：

f_w(x，t)＝a_w0(t)+a_w1(t)x+a_w2(t)x²+a_w3(t)x³

式中，a_w0(t)为当前时刻t大地坐标系下自车横向坐标，a_w1(t)为当前时刻t大地坐标系下的自车轨迹航向，a_w2(t)为当前时刻t大地坐标系下的自车轨迹曲率，a_w3(t)为当前时刻t大地坐标系下的自车轨迹曲率的导数，x为大地坐标系下自车向前的预瞄距离。

32)利用PDC算法推算当前时刻的自车期望转向信号

首先推算当前时刻t自车在大地坐标系下的位置坐标(X_v(t)，Y_v(t))：

X_v(t)＝X_v(t-1)+v(t-1)Δt cos(θ(t-1)+ω(t-1)Δt)

Y_v(t)＝Y_v(t-1)+v(t-1)Δt sin(θ(t-1)+ω(t-1)Δt)

式中，(X_v(t-1)，Y_v(t-1))为车道线信号失效前一时刻(t-1)自车的大地坐标；Δt为单位采样时间；v(t-1)为车道线信号失效前一时刻(t-1)的自车车速，由车道线信号失效前一时刻自车左后轮速v_l(t-1)和右后轮速v_r(t-1)求均值获得，左后轮速v_l(t-1)和右后轮速v_r(t-1)通过双后轮速传感器获得；θ(t-1)为车道线信号失效前一时刻(t-1)自车横摆角，ω(t-1)为车道线信号失效前一时刻(t-1)自车的横摆角速度。由于在坐标系转换时已经将大地坐标系二维化，即所有垂向坐标都为0，因此车辆自身位置在大地坐标系下的垂向坐标Z_v＝0。

然后根据大地坐标系下的既定轨迹f_w(x，t)和车辆坐标(X_v(t)，Y_v(t))，实时推算当前时刻t自车横向预瞄偏差Δf_PDC(t)：

Δf_PDC(t)＝Y_v(t)+x_pre(t)sinθ(t)-[a_w0(t)+a_w1(t)(X_v(t)+x_pre(t)cosθ(t))+a_w2(t)(X_v(t)+x_pre(t)cosθ(t))²+a_w3(t)(X_v(t)+x_pre(t)cosθ(t))³]

按照下式得到推算的当前时刻的自车期望转向信号δ_PDC(t)：

式中，D(t)为前一时刻(t-1)至当前时刻t自车走过轨迹形成扇形的弦长，δ_f(t-1)为前一时刻(t-1)自车的方向盘转角，β(t)为当前时刻t车辆的质心侧偏角；

33)自车按照当前时刻的自车期望转向信号δ_PDC(t)进行转向操作，等待下一控制周期的到来，返回步骤1)。

自车期望转向信号δ_PDC(t)在一定时间内维持自车在既定车道内行驶，为驾驶员接管争取更多时间。有效保证驾驶员安全。

Claims (3)

1.一种智能电动车辆视觉车道居中辅助系统失效安全控制方法，其特征在于，包括以下步骤：

1)当前控制周期开始时刻，通过自车搭载的视觉相机得到智能电动车辆行驶过程中视觉相机探测范围内的车道线信号，包括车道线方程、车道线质量、车道线线型和车道线线宽；根据车道线方程得到自车的期望行驶轨迹；当车道线信号正常时，执行步骤2)，当车道线信号异常时，则执行步骤3)；

2)车道居中辅助系统基于期望行驶轨迹控制自车保持在既定车道内行驶，直到当前控制周期结束返回步骤1)，进行下一控制周期；

3)车道居中辅助系统基于PDC算法并利用失效前一时刻的车道线信号推算自车与道路中心的相对位置关系，并求得自车需执行的转向命令，在一定时间内维持住自车在既定车道内行驶，直到当前控制周期结束返回步骤1)，进行下一控制周期。

2.根据权利要求1所述的智能电动车辆视觉车道居中辅助系统失效安全控制方法，其特征在于，步骤2)具体包括以下步骤：

21)利用车道线信号计算当前时刻的预瞄横向偏差

根据车道线方程拟合出视觉相机坐标系下自车的期望运行轨迹，并计算出视觉相机坐标系下当前时刻t的预瞄横向偏差Δf(t)：

Δf(t)＝a₀(t)+a₁(t)x_pre(t)+a₂(t)x_pre(t)²+a₃(t)x_pre(t)³，x_pre(t)∈(x₁，x₂)

式中，x_pre(t)为当前时刻t视觉相机向前方路面的预瞄距离；a₀(t)为视觉相机坐标系下当前时刻t的自车初始横向坐标；a₁(t)为视觉相机坐标系下当前时刻t的自车轨迹航向；a₂(t)为视觉相机坐标系下当前时刻t的自车轨迹曲率；a₃(t)为视觉相机坐标系下当前时刻t的自车轨迹曲率变化率；x₁，x₂为视觉相机能够探测到前方路面的最小和最大距离；

22)按照下式计算当前时刻的自车期望方向盘转角信号δ_f(t)：

式中，δ_f(t-1)为前一时刻(t-1)的自车期望方向盘转角信号；D(t)为前一时刻(t-1)至当前时刻t自车走过轨迹形成扇形的弦长；v(t)为当前时刻t自车速度；t_p为预设的预瞄时间；K为反馈权重；β(t)为当前时刻t的自车质心侧偏角；θ(t)为当前时刻t的自车横摆角；

23)自车按照当前时刻的自车期望方向盘转角信号δ_f(t)进行转向操作，等待下一控制周期的到来，返回步骤1)。

3.根据权利要求1或2所述的智能电动车辆视觉车道居中辅助系统失效安全控制方法，其特征在于，步骤3)具体包括以下步骤：

31)按照下式将失效前一时刻的视觉信号转换为大地绝对坐标系下的既定轨迹：

λ₀＝H^-1λ

式中：X_c(t-1)，Y_c(t-1)，Z_c(t-1)分别为视觉信号异常时前一时刻(t-1)相机坐标系中自车期望轨迹f_c(x，t-1)上各坐标；(X_g(t-1)，Y_g(t-1)，Z_g(t-1))分别是由自车期望轨迹f_c(x，t-1)转换得到的大地坐标系中自车既定轨迹f_w(x，t-1)上的各坐标；[τ₂ τ₃ τ₁]为相机内部旋转矩阵的逆矩阵H^-1的三个向量；λ₀为转换平移向量，由平移向量λ求出；

旋转矩阵H和平移向量λ的表达式为：

式中，κ为视觉相机相对车体的安装俯仰角；γ为视觉相机相对车体的安装侧倾角；α为视觉相机相对车体的安装方向角；l为视觉相机安装位置与自车后轴中心的距离；h为视觉相机安装位置与地面的垂直距离；d为视觉相机安装位置与自车中轴线的横向距离；[τ₁₁τ₁₂ τ₁₃]、[τ₂₁ τ₂₂ τ₂₃]、[τ₃₁ τ₃₂ τ₃₃]分别为沿大地坐标系各坐标轴的一组基向量；

设行驶路面平坦，即Z_g＝0，通过下式得到大地绝对坐标系下前一时刻(t-1)自车既定轨迹上的各点坐标X_g(t-1)，Y_g(t-1)：

式中，θ_r，θ_c分别为相机水平方向和垂直方向的焦距，q_r，q_c分别为成像坐标系原点在像素坐标系中的坐标值，(τ，c)为车道线图像上任一像素点在成像平面坐标系下的坐标。

根据下式拟合得到当前时刻t大地坐标系下的自车既定轨迹f_w(x，t)：

f_w(x，t)＝a_w0(t)+a_w1(t)x+a_w2(t)x²+a_w3(t)x³

式中，a_w0(t)为当前时刻t大地坐标系下自车横向坐标，a_w1(t)为当前时刻t大地坐标系下的自车既定轨迹航向，a_w2(t)为当前时刻t大地坐标系下的自车既定轨迹曲率，a_w3(t)为当前时刻t大地坐标系下的自车既定轨迹曲率的导数，x为大地坐标系下自车向前的预瞄距离；

32)利用PDC算法推算当前时刻的自车期望转向信号

首先推算当前时刻t自车在大地坐标系下的位置坐标(X_v(t)，Y_v(t))：

X_v(t)＝X_v(t-1)+v(t-1)Δtcos(θ(t-1)+ω(t-1)Δt)

Y_v(t)＝Y_v(t-1)+v(t-1)Δtsin(θ(t-1)+ω(t-1)Δt)

式中，(X_v(t-1)，Y_v(t-1))为车道线信号失效前一时刻(t-1)自车的大地坐标；Δt为单位采样时间；v(t-1)为车道线信号失效前一时刻(t-1)的自车车速，由车道线信号失效前一时刻(t-1)自车左后轮速v_l(t-1)和右后轮速v_r(t-1)求均值获得；θ(t-1)为车道线信号失效前一时刻(t-1)自车横摆角，ω(t-1)为车道线信号失效前一时刻(t-1)自车横摆角速度；

然后根据大地坐标系下的既定轨迹f_w(x，t)和车辆坐标(X_v(t)，Y_v(t))，实时推算当前时刻t自车横向预瞄偏差Δf_PDC(t)：

Δf_PDC(t)＝Y_v(t)+x_pre(t)sinθ(t)-[a_w0(t)+a_w1(t)(X_v(t)+x_pre(t)cosθ(t))+a_w2(t)(X_v(t)+x_pre(t)cosθ(t))²+a_w3(t)(X_v(t)+x_pre(t)cosθ(t))³]

按照下式得到推算的当前时刻的自车期望转向信号δ_PDC(t)：

式中，D(t)为前一时刻(t-1)至当前时刻t自车走过轨迹形成扇形的弦长，δ_f(t-1)为前一时刻(t-1)自车的方向盘转角，β(t)为当前时刻t车辆的质心侧偏角；

33)自车按照当前时刻的自车期望转向信号δ_PDC(t)进行转向操作，等待下一控制周期的到来，返回步骤1)。

Images