基于概率统计的车辆运动轨迹预测方法

张金旺，章永进，徐友春

(1.军事交通学院 研究生管理大队，天津 300161； 2.军事交通学院 军用车辆系，天津 300161)

基于概率统计的车辆运动轨迹预测方法

张金旺1，章永进2，徐友春2

(1.军事交通学院 研究生管理大队，天津 300161； 2.军事交通学院 军用车辆系，天津 300161)

针对自主驾驶车辆预测障碍物车辆运行轨迹的问题，以高速公路上的车辆为研究对象，对其运动进行简化建模，采集大量数据作为训练样本对模型进行训练，并分别采用统计距离和马尔科夫链对其横向运动和纵向运动进行预测。仿真实验表明，该模型能够有效预测障碍物车辆3 s时间内的轨迹，可为自主驾驶车辆碰撞预警和轨迹规划提供可靠依据。

马尔科夫预测；轨迹预测；自主驾驶车辆；统计距离；高速公路

自动驾驶领域的安全问题是人们一直关注的重要问题之一。对于驾驶环境中的静态障碍物，自主驾驶车辆可安全地沿规划层规划的轨迹行驶，但对于动态障碍物，仅靠实时感知来进行规划存在较高的安全隐患。基于这样的考虑，自主驾驶车辆在运行时应该对其周围的运动目标进行有效预测，获取未来时刻的运动轨迹，这样可以大大提高自主驾驶车辆行驶的安全性。

现有的轨迹预测方法根据建模的抽象程度可以分为3类：基于物理学模型的轨迹预测方法、基于运动模式的轨迹预测方法和基于交互感知的轨迹预测方法[1]。Zhang R等[2]运用卡尔曼滤波(Kalman filter)和基于恒定车速的物理学模型对障碍物车辆进行轨迹预测。Schreier M等[3]将障碍物车辆运动模式通过贝叶斯网络进行分类，然后运用Monte-Carlo方法进行仿真，计算出碰撞概率。Lefèvre S等[4]引入了一个名为“预期机动”的中间变量，对道路交叉口车辆的联合运动进行建模，运用状态因子来解释车辆间相互影响，并将车辆之间的因果依赖性表达为环境的函数。陈思静等[5]利用车辆轨迹的历史数据构建状态转移矩阵，提出了基于马尔科夫链的车辆轨迹预测方法。

自主驾驶车辆在高速公路环境运行时，对障碍物车辆轨迹预测的实时性要求比较高，为平衡对障碍物车辆预测的准确性与实时性要求，采用基于运动模式的轨迹预测方法对障碍物车辆轨迹进行预测，为自主驾驶车辆碰撞预警和轨迹规划提供依据。

1 总体思路和假设条件

自主驾驶车辆在高速公路上行驶时，障碍物车辆轨迹预测将为其轨迹规划提供重要的参考，是自动驾驶中的关键问题。基于物理学模型的轨迹预测方法在特定运行状态下可以获取较好的预测精度，但在真实驾驶环境中，车辆运动存在随机性，在相同初始条件下，两次实验到达位置可能并不相同，这样的不确定主要来自于感知层误差、驾驶状态、地面摩擦系数等因素，这些误差很难通过建立准确的模型来解决。为了提高预测精度，获取障碍物更全面的运动状态信息，本文采用概率思想解决本问题，运用统计距离和马尔科夫链分别对障碍物车辆将要行驶的车道和纵向位置进行预测，进而表示出障碍物在不同时刻运动状态的概率分布。

为了方便描述，做几点假设：

(1)在高速公路上行驶时，道路曲率较小，可假设障碍物车辆在直道上行驶；

(2)将自主驾驶车辆和障碍物车辆简化为质点，其运动分解为相互独立的横向运动和纵向运动，并建立坐标系，以主车GPS位置作为坐标原点、前进方向为x轴正方向、车体右侧为y轴正方向，建立平面直角坐标系；

(3)障碍物车辆运动可分解为自主驾驶车辆坐标系下的纵向运动和横向运动，障碍物车辆在高速公路直线行驶时，一直与自主驾驶车辆同向行驶，行驶航向基本保持一致；

(4)假设障碍物车辆按照交通法规正常行驶，没有应急状况。在高速公路行驶时，很少出现应急状态。应激状态在统计上属于小概率事件，在本文中不考虑。

2 运动模型

在高速公路上行驶的车辆运动分解为沿车体前进方向的纵向运动和与其方向垂直的横向运动。在车辆换道过程中，换道长度约100 m，与道路宽度相差两个量级，航向差别很小，在假设中纵向失速可以忽略。在高速公路上行驶时，车辆之间会保持相应的安全距离，很少出现距离很近的换道行为。因此，将横向运动作为定性分析障碍物车辆运动模式的依据，并在运动模式预测的基础上将纵向位置作为是否发生碰撞的依据。

2.1 横向运动模型

在高速公路环境中定义两种运动模式：直道行驶和换道行驶。自主驾驶车辆正常行驶时，需要对障碍物车辆将要处于的车道进行预测。首先，获取本车GPS信息，完成地图定位；然后进行坐标系转化，定义自主驾驶车辆的位置向量G(e)为

G(e)=[0，0]T

(1)

自主驾驶车辆感知层获取的目标车辆位置向量G(t)为

G(t)=[x，y]T

(2)

式中x、y分别为目标车辆在车体坐标系下的纵坐标和横坐标。

在高精度地图中可以获取目标车辆所在车道中心线的GPS点，转化到车体坐标系后，其相应表达式为

y(x)=c2x2+c1x+c0

(3)

式中c0、c1和c2为方程参数。

障碍物车辆运动模式可由其距离所在车道两侧边界长度来判断，定义Y(p)=[dl，dr]。dl和dr分别代表障碍物车辆当前位置距离车道左边界和右边界的距离。车道中心线上的点则定义为Y(l)=[dl，dr]，此时dl、dr都等于车道宽度的一半，这样就可以得出统计距离的平方：

D2=(Y(l)-Y(p))T·(P(l)-P(p))-1·

(Y(l)-Y(p))

(4)

式中：P(l)和P(p)分别为Y(l)和Y(p)协方差矩阵；D2为卡方分布。

为了避免由于感知误差所引起的误判，选取最近的N组数据，赋给相应的权重ωi，进行加权计算，并且越新的数据权值越大：

(5)

在实际应用中设定阈值η，当Dk小于或者等于η时，表示车辆将保持当前车道行驶，记为0；当Dk大于η时，表明障碍物车辆将换道到临近车道，然后对dl进行判断，当dl>dr时，车辆将换道至右侧车辆，记为1；当dl

(6)

(7)

式中：Yl为位置区间((l-1)Δy，lΔy)；Δy=Ylane/L为区间长度。

2.2 纵向运动模型

a(t+τ1)=λ1(v*-v(t))+ξ1

(8)

式中：τ1为驾驶员的反应时间；v*和v(t)分别为车辆的期望行驶速度和当前行驶速度；λ1为敏感系数；ξ1为随机变量。

车辆的期望行驶速度依赖于交通规则、道路条件和车辆条件。对于行驶在公路环境下的车辆，为了简化，将交通规则和道路条件视为不变量，假设v*仅依赖于车辆类型，同时忽略驾驶员的反应时间，那么在自由行驶模式下，任意时刻车辆的加速度a是关于当前速度v的函数，记为a=ξ1(v)，考虑到驾驶员操作的不确定性，所以ξ1(v)是一个随机函数。

设障碍物车辆的纵向位置和速度分别为x和v，定义(x,v)表示车辆的运动状态，其纵向运动满足如下方程:

(9)

式中a为系统输入，是关于v的随机函数。

(10)

式中Xi为位置区间(xi-1，xi)，xi=x0+iΔx，Δx为区间长度，x0为x的最小可能值。

综上所述，障碍物的横向轨迹和纵向轨迹分别用不同离散时刻对其横向位置和纵向位置的离散概率分布描述。参考概率碰撞检测[7]中建模障碍物轨迹的方法，以t=0为起始时刻，设轨迹预测的时间长度为T，综合考虑侧向和纵向位置分布，将障碍物的概率轨迹定义为离散时刻障碍物在二维道路平面上位置的概率分布:

(11)

以车体为坐标系的模型示意图如图1所示，图中自主驾驶车辆两侧的障碍物车辆前方二维平面被离散化为大小相同的矩形区域，每个区域的灰度值表示一定时间后该区域的概率，颜色越深表示概率越高。

图1 车辆轨迹预测

3 障碍物车辆轨迹预测

在实际情况中影响障碍物车辆的横向运动状态和纵向运动状态的关键变量很多，建立较为准确的模型难度较大，因此从统计学角度统计不同运动状态下关键变量经验分布的方法，并运用其分布对障碍物车辆运动状态进行预测。

横向运动的预测方法在2.1节已基本描述清楚，纵向运动较为复杂，重点介绍运动模式预测结束后的纵向运动的预测。

在纵向位置预测上，将时间轴离散化为等间隔的区间，在每个时间区间内假设障碍物的加速度为常量，从而可以迭代计算各离散时刻障碍物的运动状态。根据式(8)，障碍物加速度a仅依赖于当前时刻的运动状态，统计出不同速度段的加速度a的分布(如图2所示)。高速公路上行驶速度较慢时，车辆基本处于加速状态；随着速度的提高，车辆速度趋于平稳，加速度大都集中在0附近，这也符合日常行车习惯。

图2 加速度在不同速度阶段概率分布

马尔科夫链是一种特殊的离散时间随机过程，由于具有“无记忆性”，即下一时刻系统状态的概率分布仅依赖于当前时刻状态，与之前的状态无关，已被广泛应用于车辆运动建模[7-8]。基于此，定义障碍物的纵向运动状态为系统状态，采用马尔科夫链描述障碍物的纵向运动过程，由障碍物加速度的随机性引导障碍物车辆向不同的运动状态转移。

对障碍物的运动状态变量做离散化处理，将纵向位置离散化为I个区间X1，X2，…，XI的基础上，进一步将速度离散化为等长度的J个区间V1，V2，…，VJ。定义系统状态由纵向位置状态和速度状态构成，每个离散状态Sm(m∈{1，2，…，M}，M=I·J)对应于一个位置区间xi(i∈{1，2，…，I})和一个速度区间Vj(j∈{1，2，…，J})：

Sm=(xi，Vj)，m=(i-1)J+j

(12)

式中Vj为速度区间(vj-1,vj),vj=v0+jΔv，Δv为区间长度，v0为障碍物速度的最小可能值。

根据障碍物在t=rΔt时刻的位置和速度，将此时障碍物处于状态的概率表示为

(13)

(14)

进一步，定义转移概率矩阵Φlon=[φnm]M×M，其中φnm为第n行第m列的元素，则障碍物纵向运动的状态预测方程可表示为

Plon(rΔt)=Φlon·Plon((r-1)Δt)

(15)

(16)

假设在t=(r-1)Δt时刻障碍物处于Sm状态，位置xstart和速度vend分别计算为

(17)

障碍物以加速度ak运动时间T后的位置xend和速度vend为

(18)

设xend∈Xi，vend∈Vj，则对n= 1，2，…，M，转移概率矩阵Φlon中的元素φnm为

(19)

式中：

(20)

当进行纵向轨迹预测时，设在t=0时刻障碍物的位置为xinit、速度为vinit。首先，根据xinit和vinit初始化纵向运动状态概率向量Plon(0)；然后，设r=1，迭代执行如下过程：根据Plon((r-1)Δt)计算Plon(T)，直至得到Plon(2T)、Plon(3T)。

针对Plon((r-1)Δt)每一个可能的运动状态，根据运动状态确定加速度条件分布，根据式(17)—(19)，计算转移概率矩阵，根据计算障碍物在t=r时刻的纵向位置的离散分布：

根据以上轨迹预测算法，障碍物加速度的条件概率分布决定了纵向运动的转移概率矩阵，进而决定该方向上的概率轨迹。因此，基于线下历史轨迹数据，通过离线训练得到不同运动状态下的加速度条件分布，将其保存为计算参数。当进行在线轨迹预测时，根据运动状态选择相应的加速度条件分布，迭代计算各个离散时刻其运动状态的概率分布。

4 实验验证

为检验本文设计的基于概率统计的轨迹预测方法的性能，设计了基于卡尔曼滤波[9]的预测算法和基于概率统计的轨迹预测方法。其中，卡尔曼滤波是一种应用普遍的回归预测方法。实验运行在主频为2.6GHz的Intel core i5-4200U计算机上，内存为4GHz，操作系统为Windows 10，实验平台为Matlab R2012b。实验数据为军事交通学院智能车实验平台JJUV6采集的包含车辆运行时的经度、纬度、航向、速度和加速度等信息的实验数据。实验数据总量为京津高速时长为10 h记录数据。运动模型训练结束后，再进行10次数据采集，随机抽取3次数据进行仿真(如图3所示)。

图3 预测障碍车辆所达区域

仿真结果表明，在完成的37次换道过程中，阈值设定在2.5时，检测准确率达100%，阈值设定在1.8时，因驾驶员习惯和技术问题，车辆不可能完全保证在车道中央，换道准确率下降到83.7%。阈值过大时换道预测延迟过大，虽然本文对换道过程中的横向运动不进行精确预测，只进行定性预测，但当阈值过大时换道延时会较高，预测结果将会对车辆规划产生影响。

在纵向预测时，车道内横向按正态分布进行预测，将剔除次点后的预测概率点转化到高精度地图中，任取100个位置点进行预测，分别预测1 s、2 s、3 s的位置点，预测结果见表1。

表1 预测结果统计

仿真实验表明，本文所采用的马尔科夫预测方法能够有效预测障碍物车辆所达位置，并且能够较为全面地反映障碍物车辆的可达位置的概率信息。在与卡尔曼滤波算法对比(如图4所示)中表明，马尔科夫预测具有较小的预测偏差。分析两种算法的原理可知，在相同的行驶速度和时间下，卡尔曼滤波算法对于加速度的分布认识不够，不能有效地对特定速度的加速度进行有效选取、更改，而马尔科夫预测算法能够根据经验数据的加速度进行有效预测，从而在预测障碍物车辆时有较小偏差。

图4 纵向预测偏差对比

自主驾驶车辆在高速公路上行驶时，程序运行时效性是很重要的指标，将两种算法的运行时效性进行对比，卡尔曼滤波算法在预测障碍物车辆3 s后运动状态的时间为0.054 s，基于概率统计的轨迹预测方法的相应运行时间见表2。

表2 基于概率统计的轨迹预测时效 s

当设置迭代时间间隔为0.1 s，也就是预测障碍物车辆3 s内的概率轨迹的迭代次数为30次时，预测运行时间为0.094 s，完全能满足自主驾驶车辆规划时间低于200 ms的要求。随着迭代间隔的增大，程序运行时间有所降低，但在迭代间隔达到0.3 s后，程序运行时间与卡尔曼滤波算法基本相同。迭代间隔的增长也就意味着在预测过程中假设障碍物车辆恒速运行的时间增长，对速度变化的敏感度性降低，进而影响到对障碍物车辆的预测精度。

5 结 语

本文将障碍物车辆运动分解为横向运动和纵向运动，并根据高速公路环境特点，采用统计距离来预测横向运动的换道行为，运用马尔科夫链预测纵向运动上的可达位置。通过训练模型在仿真实验中的实验数据表明，此方法可实现高速公路上障碍物车辆3 s时间内的预测，并可为自主驾驶车辆在进行轨迹规划时提供可靠信息参考。但还存在所采用的训练数据样本不够丰富、转移概率矩阵过大、横向预测不够等问题，是下一步需要继续改进和优化的方向。

[2] ZHANG R, CAO L, BAO S, et al. A method for connected vehicle trajectory prediction and collision warning algorithm based on V2V communication[J]. International Journal of Crashworthiness, 2017, 22(1): 15-25.

[3] SCHREIER M, WILLERT V, ADAMY J. Bayesian, maneuver-based, long-term trajectory prediction and criticality assessment for driver assistance systems[C]//Intelligent Transportation Systems (ITSC), 2014 IEEE 17th International Conference on. IEEE, 2014: 334-341.

[5] 陈思静,张可.VANETs中的车辆移动规律性及轨迹预测研究[J].计算机工程与应用,2016,52(18):139-143.

[6] 李力.现代交通流理论与应用:卷1[M].北京:清华大学出版社, 2011:71-72.

[7] ALTHOFF M, STURSBERG O, BUSS M. Model-based prob-abilistic collision detection in autonomous driving[J]. IEEE Transactions on Intelligent Transportation Systems, 2009,10(2):299-310.

[8] KRUMM J A. Markov model for driver turn prediction[C]//SAE 2008 World Congress. Detroit, MI，2008:1-25.

[9] DENG H B, ZHANG L, WU Y, et al. Research on track estimation based on Kalman filtering algorithm[J]. Transducer and Microsystem Technologies, 2012, 31(5):4-7.

(编辑： 张峰)

Prediction Method of Vehicle Trajectory Based on Probability Statistics

ZHANG Jinwang1, ZHANG Yongjin2, XU Youchun2

(1.Postgraduate Training Brigade, Military Transportation University, Tianjin 300161, China; 2.Military Vehicle Department, Military Transportation University, Tianjin 300161, China)

Considering the problem of autonomous vehicles predicting obstacle vehicle trajectory, the paper firstly takes vehicles on expressway as study object, and simplifies the movement and models on it. Then, it trains the model by collecting large amounts of data, and predicts its lateral and longitudinal movement with statistical distance and Markov chain respectively. The simulation experiment shows that this model can predict the trajectory of obstacle vehicle within 3s, which can provide reliable basis for collision warning and trajectory planning for autonomous vehicles.

Markov prediction; trajectory prediction; autonomous vehicles; statistical distance; expressway

2017-03-14；

2017-05-08. 基金项目：国家自然科学基金重大项目(91220301). 作者简介： 张金旺(1988—)，男，硕士研究生； 徐友春(1972—)，男，博士，教授，博士研究生导师.

10.16807/j.cnki.12-1372/e.2017.08.010

TP273

A

1674-2192(2017)08- 0041- 06

● 车辆工程 Vehicle Engineering