CN108848462B - 适用于信号控制交叉口的实时车辆轨迹预测方法 - Google Patents

Abstract

本发明提供一种交通信息领域适用于信号控制交叉口的实时车辆轨迹预测方法，包括如下步骤：采集信号交叉口相位切换期间的轨迹数据，根据车辆停止、通过决策和跟车状态分别组建历史轨迹库；基于历史轨迹特征指标的统计数据预判车辆的停止、通过决策；基于历史轨迹，利用K‑NN模型和IDM跟车模型分别对单车和跟车进行预估计；利用卡尔曼滤波进行轨迹预测。本发明轨迹预测方法可对信号控制交叉口车辆进行多个步长的轨迹预测，可以提前识别车辆的危险驾驶行为，可用于实时的信号控制策略或车载预警系统，主动消除危险驾驶行为导致的潜在事故风险。

Description

适用于信号控制交叉口的实时车辆轨迹预测方法

技术领域

本发明属于交通信息领域，具体涉及一种适用于信号控制交叉口的实时车辆轨迹预测方法。

背景技术

对车辆行驶轨迹进行准确的预测，是为危险驾驶行为的提前识别做准备。若下一时刻的车辆运动能够提前被准确地预测出来，危险驾驶行为的识别效果会得到很大的提升，驾驶员也会及时获得提醒，以及时采取有效的碰撞预防措施。

目前，利用轨迹数据的研究并不多，原因就在于轨迹数据采集的难度大，成本高，以及多种车辆类型混行的多维度、动态行驶特性(时刻变化的速度和加速度)带来的技术复杂性。近年来在行为领域中的研究并不能用于车辆行驶轨迹预测，因为这些研究通常忽略了轨迹位置变化的时序性，且不是针对单条车辆轨迹。通常来说，基于模型的车辆追踪和预测算法优于那些没有以模型为基础的预测算法。但是，应用于预测系统的运动模型主要从基本的物理公式推导而来，并没有考虑驾驶员的控制因素。

轨迹预测算法在很多两难区保护算法中起着重要作用。在该类方法中，轨迹预测多基于定点线圈等固定检测设备数据，检测车辆通过某个或几个点的速度等信息，基于车辆速度不变的假设进行轨迹预测。这种方法计算简单，但与实际情况相差甚远。实际情况下，车辆在信号控制交叉口不仅会受到周围车辆的驾驶行为影响，还会受到信号灯的影响。

随着智能交通系统的发展，车辆轨迹采集技术也更为先进和准确，如视频检测技术、车路协同技术、车联网技术等，对车辆轨迹预测的研究也正稳步发展。部分研究考虑车辆的驾驶特性，即车辆在直行和转向或换道时的物理约束，基于车辆轨迹对车辆下一时刻的位置和速度进行预测。然而该类预测算法仅能对轨迹数据进行单步预测，且多适用于干道或高速公路，对于情况更为复杂的信号控制交叉口适用性不高。

发明内容

本发明所要解决的技术问题是针对现有技术的不足，提供一种适用于信号控制交叉口的实时车辆轨迹预测方法，该方法可以预判驾驶员在相位切换期间的决策行为，并进行多步长的轨迹预测。

本发明所要解决的技术问题是通过以下的技术方案来实现的。本发明是一种适用于信号控制交叉口的实时车辆轨迹预测方法，其特点是：该方法包括以下步骤：

1)提取轨迹数据，建立历史轨迹数据库。

针对信号切换期间交叉口车辆影响轨迹特征的三个主要因素：驾驶性能、决策行为和跟驰过程中的前导车影响，将车辆轨迹依据车辆的类型(大车和小车)、跟驰状态(自由行驶和跟车)、决策行为(通过和停止)分为8个类型，包括停止的单车小车、通过的单车小车、停止的单车大车、通过的单车大车、停止的跟车小车、通过的跟车小车、停止的跟车大车、通过的跟车大车，如图2所示，并分别建立历史轨迹数据库。

2)驾驶员决策行为预判。

针对有绿闪的信号控制交叉口，相位切换时期包括绿闪和黄灯。对于较长的相位切换时期，驾驶员的决策过程是较为复杂的。因此，考虑驾驶员在相位切换期间决策经过预决策和决策两个阶段。

2.1)对于单车的决策过程(决策流程图如图3所示)，预决策行为的确定根据历史车辆轨迹不同绿闪启亮时刻TTS下车辆的通过比例确定。TTS计算公式如下：

其中，TTS为预计到达停车线的时间；s为绿闪启亮时刻距离停车线距离；v为绿闪启亮时刻车辆速度。

如图4所示。依据历史的统计数据，目标车辆在绿闪启亮时刻会首先进行预决策，其决策通过的概率由图4中的曲线确定。预决策通过概率P_fg与绿闪启亮时刻TTS_fg拟合曲线关系式如下：

预决策采用随机数模仿真方式，生成[0，1]内随机数δ，若δ＜P_fg，则预决策为通过，否则为停止。

目标车辆的决策过程在黄灯启亮时确定。通过历史轨迹绿闪启亮时刻TTS_fg和黄灯启亮时刻TTS_y分布散点图可以得到通过车辆与停止车辆存在明显的界限，如图5-6所示。

决策过程判断依据如下式：

小车：

大车：

根据以上预决策和决策两个过程，可以预判车辆在信号切换期间的决策行为。

2.2)对于跟车的决策过程，其决策在单车决策的基础上还需要考虑前车的决策结果，跟车决策流程图如图7所示。前车的驾驶行为作为强约束，即前车若决策停止，后车必定停止；前车决策通过，后车决策可以为通过或停止，此时后车决策过程与单车过程一致。

3)轨迹预估计。

对车辆的决策行为进行预判后，分别基于历史轨迹库对车辆进行轨迹预估计。

3.1)针对自由行驶车辆(单车)，采用KNN方法进行轨迹预估计。以K＝4，且预判决策为停止的车辆为例，KNN法轨迹点筛选如图8所示。为了减小计算的复杂度，满足预测的实时性要求，采用单变量搜索方法。首先对历史轨迹点进行初步筛选，即选取t_c±Δt范围内的轨迹点，即K＝{K|t_K∈[t_c-Δt，t_c+Δt]}＝{K₍₁₎，K₍₂₎，……，K_(N)}，其中K_(·)为距离差值的次序统计量，距离差值ΔD计算如下：

其中，Δs’，Δt’分别为目标点与初步估计点位置差和时间差的离差标准化数值。标准化可以去除量纲限制，离差标准化公式如下：

再选取与初步估计点距离差值ΔD最小的K个轨迹点，{K₍₁₎，K₍₂₎，K₍₃₎，K₍₄₎，……，K_(K)}，为最终选取的K邻近点。其中，初步估计点为考虑车辆速度不变情况下预计在该时刻的位置。

用选取的K个邻近点进行t_c时刻速度和位置的预估计，公式如下：

其中，v_i为被选取的第i辆车的行驶速度；w_i为权重，计算如下：

其中，K为选取的历史轨迹点数；N为选取范围内所有的轨迹数。

重复以上KNN估计过程，即可对轨迹进行多步长估计。

3.2)针对跟驰车辆(跟车)，采用智能驾驶人模型(IDM)进行轨迹估计。IDM由2项公式构成，包含自由状态下的加速趋势和考虑与前导车碰撞的减速趋势，其具体方程为：

式中：s^*(v_n(t)，Δv_n(t))为期望最小间距；s₀为停车间距；v₀为期望速度；T_s为安全车头时距；a_m为最大加速度；b_n为期望减速度；δ、s₁为相关参数，参考文献中的取值，δ＝4，s₁＝0。

考虑跟驰车辆的驾驶行为不仅受到信号灯影响，同时受到前车的影响。前车和后车的决策组合由：1、前车通过，后车通过；2、前车通过，后车停止；3、前车停止，后车停止。考虑以上三种决策组合，在1、3组合情况下，采用IDM模型进行轨迹预估计，在2组合情况下，可将后车认为是单车情况，即采用KNN模型进行轨迹预估计。

4)轨迹预测。

4.1)基于(3)得到的车辆的速度和位置的与估计值，可以将车辆状态表示为：

Z_k是时刻k观察的车辆状态。

本研究的时间步长为50(0.12s/步，共6s)，结合下一个时刻的预估的车辆状态，可以得到车辆的状态转移矩阵：

A_k是时刻k的2×2的状态转移矩阵。其中，

和

分别为时刻k和时刻k+1的估计位置，

和

分别为时刻k和时刻k+1的估计速度。

因此，卡尔曼滤波的系统状态公式可表示如下：

X_k+1＝A_kX_k+B_kU_k+W_k (13)

观测公式如下：

Z_k＝H_kX_k+V_k (14)

系统状态向量定义如下：

其中，s_k和v_k是时刻k预测的车辆位置和速度；B_k是2×1的控制权重矩阵，此处为零矩阵；U_k是系统的控制输入，此处为加速度；H_k是时刻k的2×2的观察矩阵；W_k和V_k分别为系统噪声和观察噪声。

4.2)时刻k的预测状态可以由公式(17)～(21)获得，其中包括预测和更新两个阶段。式(17)和(18)为预测阶段，上一时刻k-1的预测状态X_k-1和P_k-1被用于初步预测下一时刻k的轨迹状态。

初步预测轨迹状态

如下：

初步预测协方差如下：

4.3)式(19)～(21)为更新阶段，第三步中的预估计轨迹状态将用于优化预测阶段初步预测的轨迹状态，最终获得更为准确的预测轨迹。

卡尔曼效益矩阵：

优化轨迹状态：

更新协方差：

其中，

和

为时刻k初步预测的位置和速度；I是2×2的特征矩阵；

和P_k分别是时刻k的初步预测和更新协方差矩阵；

和

分别是时刻k的估计值和观察值的噪声方差，计算如下：

重复式(17)～式(21)过程直到车辆通过停车线或停止，即得到车辆的实时预测轨迹。采用卡尔曼滤波进行轨迹预测的过程示意图如图9所示。

本发明方法是基于相位切换期间信号控制交叉口的历史轨迹建立驾驶员决策预判模型，并分车辆类型和行驶状态建立多步长轨迹预测模型。

该方法通过采集历史轨迹数据，建立历史轨迹数据库；采集目标车辆的绿闪启亮时刻的位置和速度信息，计算相应的绿闪启亮时刻TTS(预计到达停车线时间)，并用数模仿真的行驶根据统计概率随机模型该车辆的预决策行为；在预决策条件下，实施第三、四步中的轨迹预测算法，预测车辆至黄灯启亮时刻的轨迹；计算车辆的黄灯启亮时刻的TTS，并依据第二步过程预判车辆的决策，再次进行轨迹预测直至车辆通过停车线或红灯启亮。

与现有技术相比，本发明方法具有以下优点：

一、本发方法揭示了驾驶员在交叉口相位切换期间的决策规律，即在轨迹预测过程中考虑了驾驶员因素的影响。

二、输入仅为当前时刻及当前时刻以前的轨迹真实值，并可以进行多步长的高精轨迹预测，且预测步长与模型轨迹的输入步长一致，灵活性和移植性强。

三、轨迹预测结果的精度和准确度足以再现车辆的危险驾驶行为，可进一步运用于危险驾驶行为识别，基于车辆轨迹的交叉口安全评价方法、预防危险驾驶行为的主动安全控制策略等等。

四、该方法可以预判驾驶员在相位切换期间的决策行为，并进行多步长的轨迹预测。旨在基于信号控制交叉口相位切换期间的车辆历史轨迹数据，对不同的车辆类型(重型车辆、小汽车等)和行驶状态(自由行驶、跟驰行驶)进行区分，分别进行车辆轨迹预测。

附图说明

图1为本发明提出的轨迹预测方法示意图；

图2为历史轨迹数据库建立过程；

图3为单车决策行为预判流程图；

图4为绿闪时刻TTS与车辆通过比例图；

图5-6为绿闪时刻和黄灯时刻TTS散点图及决策依据；

图7为跟车决策行为预判流程图；

图8为K邻近模型示意图；

图9为卡尔曼滤波示意图；

图10为实例历史轨迹数据库；

图11为KNN模型标定结果示意图；

图12-15为单车和跟车轨迹预测示例图。

具体实施方式

下面结合附图和具体实施方式对本发明作详细说明。

实施例1：一种适用于信号控制交叉口的实时车辆轨迹预测方法，具体流程图如图1所示。大致可以分为四个主要步骤：

1)提取轨迹数据，建立历史轨迹数据库。

针对信号切换期间交叉口车辆影响轨迹特征的三个主要因素：驾驶性能、决策行为和跟驰过程中的前导车影响，将车辆轨迹依据车辆的类型(大车和小车)、跟驰状态(自由行驶和跟车)、决策行为(通过和停止)分为8个类型，包括停止的单车小车、通过的单车小车、停止的单车大车、通过的单车大车、停止的跟车小车、通过的跟车小车、停止的跟车大车、通过的跟车大车，如图2所示，并分别建立历史轨迹数据库。

2)驾驶员决策行为预判。

针对有绿闪的信号控制交叉口，相位切换时期包括绿闪和黄灯。对于较长的相位切换时期，驾驶员的决策过程是较为复杂的。因此，考虑驾驶员在相位切换期间决策经过预决策和决策两个阶段。

2.1)对于单车的决策过程(决策流程图如图3所示)，预决策行为的确定根据历史车辆轨迹不同绿闪启亮时刻TTS下车辆的通过比例确定。TTS计算公式如下：

其中，TTS为预计到达停车线的时间；s为绿闪启亮时刻距离停车线距离；v为绿闪启亮时刻车辆速度。

如图4所示。依据历史的统计数据，目标车辆在绿闪启亮时刻会首先进行预决策，其决策通过的概率由图4中的曲线确定。预决策通过概率P_fg与绿闪启亮时刻TTS_fg拟合曲线关系式如下：

预决策采用随机数模仿真方式，生成[0，1]内随机数δ，若δ＜P_fg，则预决策为通过，否则为停止。

目标车辆的决策过程在黄灯启亮时确定。通过历史轨迹绿闪启亮时刻TTS_fg和黄灯启亮时刻TTS_y分布散点图可以得到通过车辆与停止车辆存在明显的界限，如图5-6所示。

决策过程判断依据如下式：

小车：

大车：

根据以上预决策和决策两个过程，可以预判车辆在信号切换期间的决策行为。

2.2)对于跟车的决策过程，其决策在单车决策的基础上还需要考虑前车的决策结果，跟车决策流程图如图7所示。前车的驾驶行为作为强约束，即前车若决策停止，后车必定停止；前车决策通过，后车决策可以为通过或停止，此时后车决策过程与单车过程一致。

3)轨迹预估计。

对车辆的决策行为进行预判后，分别基于历史轨迹库对车辆进行轨迹预估计。

3.1)针对自由行驶车辆(单车)，采用KNN方法进行轨迹预估计。以K＝4，且预判决策为停止的车辆为例，KNN法轨迹点筛选如图8所示。为了减小计算的复杂度，满足预测的实时性要求，采用单变量搜索方法。首先对历史轨迹点进行初步筛选，即选取t_c±Δt范围内的轨迹点，即K＝{K|t_K∈[t_c-Δt，t_c+Δt]}＝{K₍₁₎，K₍₂₎，……，K(_N)}，其中K_(·)为距离差值的次序统计量，距离差值ΔD计算如下：

其中，Δs’，Δt’分别为目标点与初步估计点位置差和时间差的离差标准化数值。标准化可以去除量纲限制，离差标准化公式如下：

再选取与初步估计点距离差值ΔD最小的K个轨迹点，{K₍₁₎，K₍₂₎，K₍₃₎，K₍₄₎，……，K_(K)}，为最终选取的K邻近点。其中，初步估计点为考虑车辆速度不变情况下预计在该时刻的位置。

用选取的K个邻近点进行t_c时刻速度和位置的预估计，公式如下：

其中，v_i为被选取的第i辆车的行驶速度；w_i为权重，计算如下：

其中，K为选取的历史轨迹点数；N为选取范围内所有的轨迹数。

重复以上KNN估计过程，即可对轨迹进行多步长估计。

3.2)针对跟驰车辆(跟车)，采用智能驾驶人模型(IDM)进行轨迹估计。IDM由2项公式构成，包含自由状态下的加速趋势和考虑与前导车碰撞的减速趋势，其具体方程为：

式中：s(v_n(t)，Δv_n(t))为期望最小间距；s₀为停车间距；v₀为期望速度；T_s为安全车头时距；a_m为最大加速度；b_n为期望减速度；δ、s₁为相关参数，参考文献中的取值，δ＝4，s₁＝0。

考虑跟驰车辆的驾驶行为不仅受到信号灯影响，同时受到前车的影响。前车和后车的决策组合由：1、前车通过，后车通过；2、前车通过，后车停止；3、前车停止，后车停止。考虑以上三种决策组合，在1、3组合情况下，采用IDM模型进行轨迹预估计，在2组合情况下，可将后车认为是单车情况，即采用KNN模型进行轨迹预估计。

4)轨迹预测。

4.1)基于(3)得到的车辆的速度和位置的与估计值，可以将车辆状态表示为：

Z_k是时刻k观察的车辆状态。

本研究的时间步长为50(0.12s/步，共6s)，结合下一个时刻的预估的车辆状态，可以得到车辆的状态转移矩阵：

A_k是时刻k的2×2的状态转移矩阵。其中，

和

分别为时刻k和时刻k+1的估计位置，

和

分别为时刻k和时刻k+1的估计速度。

因此，卡尔曼滤波的系统状态公式可表示如下：

X_k+1＝A_kX_k+B_kU_k+W_k (13)

观测公式如下：

Z_k＝H_kX_k+V_k (14)

系统状态向量定义如下：

其中，s_k和v_k是时刻k预测的车辆位置和速度；B_k是2×1的控制权重矩阵，此处为零矩阵；U_k是系统的控制输入，此处为加速度；H_k是时刻k的2×2的观察矩阵；W_k和V_k分别为系统噪声和观察噪声。

4.2)时刻k的预测状态可以由公式(17)～(21)获得，其中包括预测和更新两个阶段。式(17)和(18)为预测阶段，上一时刻k-1的预测状态X_k-1和P_k-1被用于初步预测下一时刻k的轨迹状态。

初步预测轨迹状态

如下：

初步预测协方差如下：

4.3)式(19)～(21)为更新阶段，第三步中的预估计轨迹状态将用于优化预测阶段初步预测的轨迹状态，最终获得更为准确的预测轨迹。

卡尔曼效益矩阵：

优化轨迹状态：

更新协方差：

其中，

和

为时刻k初步预测的位置和速度；I是2×2的特征矩阵；

和P_k分别是时刻k的初步预测和更新协方差矩阵；

和

分别是时刻k的估计值和观察值的噪声方差，计算如下：

重复式(17)～式(21)过程直到车辆通过停车线或停止，即得到车辆的实时预测轨迹。采用卡尔曼滤波进行轨迹预测的过程示意图如图9所示。

实施例2，基于实施例1，对轨迹预测方法进行验证。

选取上海市曹安公路沿线的三个交叉口：曹安公路-嘉松北路、曹安公路-翔江公路和曹安公路-曹丰路作为郊区公路信号控制交叉口的代表；选取四平路-大连路作为城市道路信号控制交叉口的代表，利用录像的方式采集2013年6月至10月间的16个天气晴朗的工作日平峰时段(9:00～16:00)原始的视频数据。利用轨迹提取软件提取绿闪前2s至通过停车线或停止在停车线前时段的单车轨迹和跟车轨迹，包括大车和小车。轨迹的精度为0.12s，位置误差不超过0.05m。对于单车和跟车两类驾驶行为定义轨迹提取条件如下：

●单车轨迹提取条件：a)车辆在相位切换期间通过停车线或减速停车；b)车辆无前车或与前车距离较远(大于100m)；c)车辆无换道行为。

●跟车轨迹提取条件：a)车辆与前车距离小于等于100m；b)跟车行为发生在相位切换期间，且前后车均在此期间通过停车线或减速停车；c)跟车期间前后车均无换道行为。

基于以上的轨迹提取条件本实施例共采集832条单车轨迹(617辆小车和215辆大车) 和449辆跟车轨迹(419辆小车和30辆大车)。其中，3/4用于历史轨迹库的建立，即按照上述第一步建立历史轨迹库如图10；其余1/4则用于本专利方法验证。

由于KNN算法中，不同的K值会影响邻近数据的权重，如式(4)所示。因此，此处采用均方误差(Root Mean Square Error，RMSE)作为模型标定结果的指标，RMSE计算如下式：

各K值下各指标均方误差如图11所示。因此，最佳的K值取4。

IDM模型标定采用遗传算法，标定大车和小车的跟车结果分别如下：

表1 IDM标定结果

用预测轨迹和真实轨迹之间的RMSE作为验证指标进行模型的精度验证。验证结果如下表所示：

表2模型的预测精度

*停止小车1，前车停止；停止小车2，前车通过.

图12-15为单车和跟车部分轨迹预测示例图。可见，本算法可以很好的拟合车辆的运动趋势，在KNN或IDM法估计值较为准确的情况下，预测值自然也更为准确。而在KNN或IDM估计值偏差较大的情况下，卡尔曼滤波能够修正偏离过大的轨迹值，降低预测值的偏差。实施例中单车轨迹的估计精度达到94.98％，跟车轨迹的估计精度达到97.67％。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (1)

1.一种适用于信号控制交叉口的实时车辆轨迹预测方法，其特征在于：该方法包括以下步骤：

(1)提取轨迹数据，建立历史轨迹数据库；

针对信号切换期间交叉口车辆影响轨迹特征的三个主要因素：驾驶性能、决策行为和跟驰过程中的前导车影响，将车辆轨迹依据车辆的类型、跟驰状态、决策行为分为8个类型，分别是：停止的单车小车、通过的单车小车、停止的单车大车、通过的单车大车、停止的跟车小车、通过的跟车小车、停止的跟车大车、通过的跟车大车，并分别建立历史轨迹数据库；

(2)驾驶员决策行为预判；

针对有绿闪的信号控制交叉口，相位切换时期包括绿闪和黄灯；将驾驶员在相位切换期间决策经过分为预决策和决策两个阶段；

(3)轨迹预估计；

对车辆的决策行为进行预判后，分别基于历史轨迹库对车辆进行轨迹预估计；

(4)轨迹预测；

步骤(2)驾驶员决策行为预判方法如下：

2.1对于单车的决策过程，预决策行为的确定根据历史车辆轨迹不同绿闪启亮时刻TTS下车辆的通过比例确定；t时刻TTS计算公式如下：

其中，TTS_t为t时刻预计到达停车线的时间；s_t为t时刻距离停车线距离；v_t为t时刻车辆速度；

依据历史的统计数据，目标车辆在绿闪启亮时刻会首先进行预决策，预决策通过概率P_fg与绿闪启亮时刻TTS_fg拟合曲线关系式如下：

预决策采用随机数模仿真方式，生成[0,1]内随机数δ，若δ<P_fg，则预决策为通过，否则为停止；

目标车辆的决策过程在黄灯启亮时确定；通过历史轨迹绿闪启亮时刻TTS_fg和黄灯启亮时刻TTS_y分布散点图可以得到通过车辆与停止车辆存在明显的界限；

决策过程判断依据如下式：

小车：

大车：

根据以上预决策和决策两个过程，预判车辆在信号切换期间的决策行为；

2.2对于跟车的决策过程，其决策在单车决策的基础上还需要考虑前车的决策结果；前车的驾驶行为作为强约束，即前车若决策停止，后车必定停止；前车决策通过，后车决策可以为通过或停止，此时后车决策过程与单车过程一致；

步骤(3)轨迹预估计方法如下：

3.1针对自由行驶车辆即单车，采用KNN方法进行轨迹预估计；采用单变量搜索方法；首先对历史轨迹点进行初步筛选，即选取t_c±Δt范围内的轨迹点，即K＝{K|t_K∈[t_c-Δt,t_c+Δt]}＝{K₍₁₎,K₍₂₎,……,K_(N)}，其中K_(·)为距离差值的次序统计量，距离差值ΔD计算如下：

其中，Δs’，Δt’分别为目标点与初步估计点位置差和时间差的离差标准化数值；标准化去除量纲限制，离差标准化公式如下：

再选取与初步估计点距离差值ΔD最小的K个轨迹点，{K₍₁₎，K₍₂₎，K₍₃₎，K₍₄₎，······，K_(K)}，为最终选取的K邻近点；其中，初步估计点为考虑车辆速度不变情况下预计在该时刻的位置；用选取的K个邻近点进行t_c时刻速度和位置的预估计，公式如下：

其中，v_i为被选取的第i辆车的行驶速度；w_i为权重，计算如下：

其中，K为选取的历史轨迹点数；N为选取范围内所有的轨迹数；

重复以上KNN估计过程，即可对轨迹进行多步长估计；

3.2针对跟驰车辆，采用智能驾驶人模型IDM进行轨迹估计；IDM由2项公式构成，包含自由状态下的加速趋势和考虑与前导车碰撞的减速趋势，其具体方程为：

式中：s^*(v_n(t)，Δv_n(t))为期望最小间距；s₀为停车间距；v₀为期望速度；T_s为安全车头时距；a_m为最大加速度；b_n为期望减速度；δ、s₁为相关参数，参考文献中的取值，δ＝4，s₁＝0；

前车和后车的决策组合由：1、前车通过，后车通过；2、前车通过，后车停止；3、前车停止，后车停止；考虑以上三种决策组合，在1、3组合情况下，采用IDM模型进行轨迹预估计，在2组合情况下，可将后车认为是单车情况，即采用KNN模型进行轨迹预估计；

步骤(4)轨迹预测的具体步骤如下：

4.1基于步骤(3)得到的车辆的速度和位置的预估计值，将车辆状态表示为：

Z_k是时刻k观察的车辆状态；

车辆的状态转移矩阵：

A_k是时刻k的2×2的状态转移矩阵；其中，

和

分别为时刻k和时刻k+1的估计位置，

和

分别为时刻k和时刻k+1的估计速度；

因此，卡尔曼滤波的系统状态公式可表示如下：

X_k+1＝A_kX_k+B_kU_k+W_k (13)

观测公式如下：

Z_k＝H_kX_k+V_k (14)

系统状态向量定义如下：

其中，s_k和v_k是时刻k预测的车辆位置和速度；B_k是2×1的控制权重矩阵，此处为零矩阵；U_k是系统的控制输入，此处为加速度；H_k是时刻k的2×2的观察矩阵；W_k和V_k分别为系统噪声和观察噪声；

4.2时刻k的预测状态由公式(17)～(21)获得，其中包括预测和更新两个阶段；式(17)和(18)为预测阶段，上一时刻k-1的预测状态X_k-1和P_k-1被用于初步预测下一时刻k的轨迹状态；

初步预测轨迹状态

如下：

初步预测协方差如下：

4.3式(19)～(21)为更新阶段，第三步中的预估计轨迹状态将用于优化预测阶段初步预测的轨迹状态，最终获得更为准确的预测轨迹；

卡尔曼效益矩阵：

优化轨迹状态：

更新协方差：

其中，

和

为时刻k初步预测的位置和速度；I是2×2的特征矩阵；

和P_k分别是时刻k的初步预测和更新协方差矩阵；

和

分别是时刻k的估计值和观察值的噪声方差，计算如下：

重复式(17)～式(21)过程直到车辆通过停车线或停止，即得到车辆的实时预测轨迹。

Images