一种基于双层车道意图推断的目标车辆轨迹预测方法

1.一种基于双层车道意图推断的目标车辆轨迹预测方法，其特征在于，包括如下步骤：步骤S1、基于扩展卡尔曼滤波，获取目标车辆的短期预测轨迹；基于路径相似度评分模型分析所述短期预测轨迹，获取目标车辆进入交叉路口时选择的道路预测值；步骤S2、基于目标车辆的横向偏移，构建基于交互式多模型的车道选择意图预测模型，所述车道选择意图预测模型基于所述道路预测值，获取车道预测值；步骤S3、基于所述车道预测值，采用五次多项式曲线方法生成目标车辆的预测轨迹。 2.根据权利要求1所述的基于双层车道意图推断的目标车辆轨迹预测方法，其特征在于，步骤S1具体包括：步骤S1-1、搜索目标车辆可以到达的候选车道集合；步骤S1-2、预测目标车辆的短时局部状态量，基于扩展卡尔曼滤波预测目标车辆的短期预测轨迹；步骤S1-3、建立路径相似度评分模型，评估所述目标车辆的短期预测轨迹与所述候选车道集合中的各车道的相似度；步骤S1-4、基于所述相似度确定目标车辆最大概率选择的道路，作为所述道路预测值。 3.根据权利要求2所述的基于双层车道意图推断的目标车辆轨迹预测方法，其特征在于，步骤S1-1包括：根据目标车辆实时状态信息，由深度优先搜索算法从高清地图中搜索得到目标车辆的候选车道集合，候选车道集合中包含多个候选车道，表示交叉路口处目标车辆可以驶向其他道路中的车道的所有可能选择。 4.根据权利要求3所述的基于双层车道意图推断的目标车辆轨迹预测方法，其特征在于，步骤S1-2包括：步骤S1-2-1、预测目标车辆的短时局部状态量，采用以下方式：其中， 为未来时间窗口中k+m时刻的纵向速度和偏航角速度， 为未来时间窗口中k+1时刻的纵向速度和偏航角速度，m为短期未来时间窗口宽度，n为短期历史时间窗口宽度，a、b为参数索引，w a 为权重系数，v k-a+1 、γ k-a+1 为短期历史时间窗口中k-a+1时刻的纵向速度和偏航角速度；步骤S1-2-2、基于扩展卡尔曼滤波预测短时轨迹，采用以下方式：(1)建立过程模型：x k+1 ＝f(x k )+q k＝[f 1 ，f 2 ,f 3 ,f 4 ，f 5 ]+q k其中，f(x k )为车辆运动学的状态转移方程，计算方式为：f 1 ：x k+1 ＝x k +(v k cosθ k )Tf 2 ：y k+1 ＝y k +(v k sinθ k )Tf 3 :θ k+1 ＝θ k +γ k Tf 4 :v k+1 ＝v kf 5 :γ k+1 ＝γ k其中，x k ＝(x k ，y k ，θ k ，v k ，γ k )是k时刻目标车辆的状态向量，(x k ，y k )为目标车辆的位置坐标，θ k 为偏航角，v k 为纵向速度，γ k 为偏航角速度，q k ～N(0，Q k )是过程噪声，符合均值为零且功率谱密度为过程噪声矩阵Q k 的正态分布，Q k ＝diag{0，0，q dθ ，q dv ，q dγ }，q dθ 表示偏航角过程噪声强度，q dv 表示纵向速度过程噪声强度，q dγ 表示偏航角速度过程噪声强度，T为采样周期；(2)建立测量模型：其中，z k 为k时刻目标车辆的状态向量测测量值，H是测量矩阵，r k ～N(0，R k )是测量噪声，符合均值为零且噪声协方差矩阵为R k 的正态分布，R k ＝diag{r v ，r γ }，r v 表示纵向速度测量噪声强度，r γ 表示偏航角速度测量噪声强度；(3)进行扩展卡尔曼滤波器的预测，其计算过程为：其中， 是k时刻的先验估计状态， 是k-1时刻的后验估计状态，f(·)为状态转移方程， 是k时刻的先验协方差矩阵， 是k-1时刻的后验协方差矩阵， 是k-1时刻的雅可比矩阵，Q k-1 是k-1时刻的过程噪声矩阵；(4)进行扩展卡尔曼滤波器的更新，其计算过程为：其中，K k 是k时刻的卡尔曼增益；重复进行扩展卡尔曼滤波器的更新，直到完成未来时间窗口中的所有轨迹，最终获取短期预测轨迹 5.根据权利要求4所述的基于双层车道意图推断的目标车辆轨迹预测方法，其特征在于，步骤S1-3具体包括：通过以下方式计算目标车辆的短期预测轨迹与所述候选车道集合中的各车道的相似性分数：其中， 是第i条车道与短期预测轨迹 之间的相似性评分，k 1 、k 2 是位置相似性和偏航角相似性的加权系数， 和 分别是位置相似性和偏航角相似性；位置相似性 的计算方式为：其中， 为欧式距离均值，α为短期预测轨迹点索引，x α ，y α 为短期预测轨迹点坐标，x′ α ，y′ α 为短期预测轨迹点最近点位置坐标，m为短期未来时间窗口宽度，L为映射阈值参数；偏航角相似性 的计算方式为：其中， 为偏航角平均差异，α为短期预测轨迹点索引，θ α 为短期预测轨迹点偏航角，θ′ a 为短期预测轨迹点对应的车道中的航向。 6.根据权利要求5所述的基于双层车道意图推断的目标车辆轨迹预测方法，其特征在于，步骤S1-4包括：使用softmax函数对候选车道集 的相似性分数 进行归一化，获取目标车辆选择不同车道的概率 其中，n3为所有候选车道的总数；通过对同一条道路的车道组中各车道的概率进行求和，得到车辆选择各个道路的概率，将最大概率的道路作为所述道路预测值。 7.根据权利要求1-6中任一项所述的基于双层车道意图推断的目标车辆轨迹预测方法，其特征在于，步骤S2具体包括：步骤S2-1、构建基于交互式多模型的车道选择意图预测模型，基于目标车辆横向偏移量及横向偏移速度，获取所述车道预测值内部概率最高的车道作为目标车道；步骤S2-2、计算预瞄距离，基于所述预瞄距离获取所述目标车道的中心线的锚点，作为所述车道预测值。 8.根据权利要求7所述的基于双层车道意图推断的目标车辆轨迹预测方法，其特征在于，步骤S2-1具体包括：(1)构建状态转移概率矩阵：其中，下标i和j表示车道编号，n′为车道总数，p ij 表示k-1时刻的i车道转换到k时刻的j车道的概率，计算方式为；其中， 是由概率密度函数 计算得出的先验过渡概率， 是i车道转换到j车道的概率密度函数的累积值，ρ ij 和 是从i车道转换到j车道的目标车辆横向偏移速度的均值和协方差；(2)交互式多模型的交互部分，进行如下更新计算：其中， 和 分别是从k-1时刻得出的后验车道模型概率、车道横向偏移和j车道概率模型的协方差， 为混合概率， 为先验车道模型概率， 为j车道的横向偏移的混合状态， 为j车道的横向偏移的混合协方差， 为i车道的横向偏移的混合状态；(3)交互式多模型的滤波部分，对于每个车道模型 都进行标准的线性卡尔曼滤波，计算过程为：其中，KF prediction (·)和KF update (·)分别是标准卡尔曼滤波器的预测和更新步骤，A是转移矩阵，Q是过程噪声矩阵， 为i车道的横向偏移的混合协方差， 和 分别为k时刻的先验和后验车道横向偏移， 为已有车道横向偏移均值， 和 分别为车道i的先验和后验协方差；(4)交互式多模型的车道模型概率更新，进行如下计算：其中， 是第i个车道模型的滤波器测量的似然度， 为k时刻第i个车道模型的滤波器的测量残差， 为k时刻第i个车道模型的滤波器的协方差，l k 为k时刻目标车辆偏离车道中心线的横向偏移，R为噪声矩阵；(5)在所述道路预测值的内部的车道中选择具有最高的后验车道模型概率的车道作为目标车道。 9.根据权利要求8所述的基于双层车道意图推断的目标车辆轨迹预测方法，其特征在于，步骤S2-2具体包括：计算预瞄距离dis：dis＝v k T predict其中T predict 是预瞄时间，以预瞄距离为基准查询所述目标车道上的中心线锚点，获取锚点的状态P T (x T ，y T ，θ T )作为所述车道预测值，其中x T ，y T ，θ T 分别为锚点的横纵坐标和偏航角。 10.根据权利要求9所述的基于双层车道意图推断的目标车辆轨迹预测方法，其特征在于，步骤S3具体包括：建立如下五次多项式轨迹曲线插值公式：x(t)＝a 5 t 5 +a 4 t 4 +a 3 t 3 +a 2 t 2 +a 1 t+a 0y(t)＝b 5 t 5 +b 4 t 4 +b 3 t 3 +b 2 t 2 +b 1 t+b 0其中t为时间，a 0 ，...，a 5 、b 0 ，...，b 5 为多项式轨迹参数，x(t)、y(t)为轨迹曲线坐标；五次多项式轨迹曲线起点与终点约束条件为：x(0)＝x k y(0)＝y kx′(0)＝v k cosθ k y′(0)＝v k sinθ kx″(0)＝a k cosθ k y″(0)＝a k sinθ kx(T predict )＝x T y(T predict )＝y Tx′(T predict )＝v T cosθ T y′(T predict )＝v T cosθ Tx″(T predict )＝0 y″(T predict )＝0其中，x k ，y k ，θ k ，v k ，a k 分别为目标车辆k时刻的位置坐标、偏航角、纵向速度和纵向加速度，x T ，y T ，θ T ，v T ，a T 分别为所述车道预测值的位置坐标、偏航角、纵向速度和纵向加速度，且v T ＝v k ，a T ＝a k ，x′(·)、x″(·)分别为轨迹曲线横坐标的一阶导数、二阶导数，y′(·)、y″(·)分别为轨迹曲线纵坐标的一阶导数、二阶导数；基于所述起点与终点约束条件，求解出五次多项式轨迹参数，将五次多项式轨迹曲线的插值结果作为目标车辆的预测轨迹。