CN115762182A - 一种基于运动学约束的车辆智能轨迹预测方法 - Google Patents

Abstract

本发明公开了一种基于运动学约束的车辆智能轨迹预测方法，包括步骤：构建长短期记忆网络模型，根据车辆过去t₁内的横向误差、航向误差、横摆角速度，预测未来t₂内车辆的横摆角速度；基于当前实时速度与实时加速度，构建衰减加速度模型，在t₂内加速度衰减到0，迭代求出未来t₂内纵向速度，作为未来t₂内车辆的纵向速度测量值；建立车辆运动学模型，将长短期记忆网络预测出的横摆角速度和衰减加速度模型输出的纵向速度作为中间变量，预测车辆的未来轨迹。该方法考虑预测算法的泛化性及容错性，在综合考虑车辆的可执行性的情形下，采用结合长短期记忆网络与车辆运动学模型的车辆轨迹预测方法，使预测出的轨迹在满足车辆可执行的条件下，更加准确。

Description

一种基于运动学约束的车辆智能轨迹预测方法

技术领域

本发明涉及自动驾驶车辆技术领域，尤其涉及自动驾驶车辆轨迹预测领域。具体讲，涉及一种基于运动学约束的车辆智能轨迹预测方法。

背景技术

现有的关于车辆轨迹预测的研究可以大致分为两类。分别是基于浅层学习的方法和基于深度学习的轨迹预测算法。基于浅层学习的方法主要是利用基于动力学(运动学)特征与贝叶斯滤波器相结合的方法，该模型能够预测出符合车辆实际的预测轨迹，但是该模型以恒定速度为假设，不符合实际情况。基于深度学习的方法主要利用大规模的数据集学习出合理的映射关系，其比较典型的深度学习网络有RNN、长短期记忆网络(LSTM)等。深度学习的方法不需要对模型进行复杂的建模，但是该方法会预测出一些车辆无法执行的轨迹。

基于上述两种方法的局限性，本发明提出了一种结合长短期记忆网络与车辆运动学模型的车辆轨迹预测方法，该方法首先利用长短期记忆网络对大量的行车数据进行训练，得到一个泛化性较高的网络模型，利用该模型预测车辆的横摆角速度。其次，构建衰减加速度模型预测未来的纵向车速，由于车辆在行车过程中是一个变加速运动，因此采用衰减加速度模型能够更符合车辆实际行驶过程。最后，利用预测出的横摆角速度与纵向速度，结合从车辆动力学模型生成符合车辆运动学的轨迹。

发明内容

为克服现有技术的不足，本发明旨在提出一种能够应用于车辆实际行驶过程中的轨迹预测算法。具体而言，考虑预测算法的泛化性及容错性，在综合考虑车辆的可执行性的情形下，采用结合长短期记忆网络与车辆运动学模型的车辆轨迹预测方法，利用长短期记忆网络的能够使预测的车辆横摆角速度更具有泛化性，同时利用车辆运动学模型对规划轨迹进行约束，从而使预测出的轨迹在满足车辆可执行的条件下，更加准确。

本发明采用的技术方案是：一种基于运动学约束的车辆智能轨迹预测方法，包括以下步骤，

第一步，构建长短期记忆网络模型，根据车辆过去t₁内的横向误差、航向误差、横摆角速度，预测未来t₂内车辆的横摆角速度；

第二步，基于当前实时速度与实时加速度，构建衰减加速度模型，在t₂内加速度衰减到0，迭代求出未来t₂内纵向速度，作为未来t₂内车辆的纵向速度测量值；

第三步，建立车辆运动学模型，将长短期记忆网络预测出的横摆角速度和衰减加速度模型输出的纵向速度作为中间变量，预测车辆的未来轨迹。

进一步，所述第一步，所述长短期记忆网络模型的网络架构包括输入层、LSTM网络层、全链接层以及回归层；其中输入层为一层，输入维度为特征数目；LSTM网络层设置有250层；全链接层设置为1层，输出维度为1，表示输出特征数目。

进一步，所述第三步包括以下子步骤：

(1)建立车辆的状态矢量：

其中，x_k，y_k，θ_k，v_k，γ_k，a_k，

分别为全局坐标x，全局坐标y，车辆航向，纵向速度，横摆角速度，纵向加速度，横摆角加速度；

(2)建立过程模型：基于车辆的运动学方程，建立(1)中状态矢量下一时刻与当前时刻关系的过程模型，

x_k+1＝f(x_k)+q_k＝[f₁，f₂，f₃，f₄，f₅，f₆，f₇]+q_k

其中，f(x_k)是状态矩阵；q_k是过程噪声，q_k～N(0，Q_k)，

非线性状态转移模型：

f₁：

f₂：

f₃：

f₄:v_k+1＝v_k+a_kT

f₅：

f₆:a_k+1＝a_k

f₇：

其中，采样周期T＝0.05s；

(3)获取模拟测量值：将第一步与第二步中预测出的速度与横摆角速度作为模拟测量值，用于后续的EKF计算，

y_k＝[v_k，γ_k]^T

其中γ_k、v_k分别为第一步中利用长短期记忆网络神经网络获得的横摆角速度和第二步中利用衰减加速度模型获得的纵向速度；

(4)利用非线性卡尔曼滤波对(1)中的车辆状态矢量x_k进行预测、更新：

预测当前时刻先验状态矢量及协方差矩阵：

其中，

是当前时刻先验估计状态，x_k-1是上一时刻后验估计状态，f(·)是状态转移方程，

是当前时刻的先验协方差矩阵，F是状态转移矩阵，P_k-1是上一时刻的后验协方差矩阵，Q_k-1为上一时刻的过程噪声；

更新当前时刻状态矢量及协方差矩阵：

A.卡尔曼增益更新：

其中，K_k是当前时刻卡尔曼增益，

为观测矩阵，R_k是当前时刻观测噪声协方差矩阵，R_k＝diag{0.01，0.001}；

B.状态矢量更新：

C.协方差矩阵更新：

其中，P_k是当前时刻的后验协方差矩阵，I是单位矩阵。

本发明与现有技术相比所具有的有益效果：

(1)将长短期记忆网络作为车辆状态观测器，基于车辆历史轨迹预测未来横摆角速度。由于基于神经网络可以充分逼近任意复杂的非线性关系的特性，可以在无法建立准确的车辆预测模型的情况下准确预测出车辆未来的横摆角速度。

(2)建立衰减加速度模型，将预测车辆在未来5s内为匀速运动转为变加速运动，能够更加符合真实的纵向车速变化；

(3)建立车辆运动学模型，将第一步预测出的横摆角速度以及第二步经过衰减加速度模型输出的速度作为输入，通过对长短期记忆网络加入运动学约束来预测未来的车辆轨迹，解决了长短期记忆网络预测结果不符合车辆运动学轨迹的问题，提高了预测算法的适应性及鲁棒性；

附图说明

图1是结合长短期记忆网络与车辆运动学模型的车辆轨迹预测方法流程图。

图2是长短期记忆网络预测横摆角速度流程。

具体实施方式

本发明采用的技术方案是，利用长短期记忆网络与衰减加速度模型预测中间变量横摆角速度和纵向速度，将中间变量输入到车辆运动学模型中进行车辆轨迹的预测。一种基于运动学约束的车辆智能轨迹预测方法，如图1所示，具体步骤如下：

第一步，车辆横向参数预测——长短期记忆网络预测车辆横摆角速度：为了能够得到准确的车辆的横摆角速度，采用长短期记忆网络模型进行预测；长短期记忆网络通过大量的数据学习，再结合车辆的历史轨迹就能够预测出比较准确的横摆角速度。

建立长短期记忆网络模型，设置网络架构为输入层，LSTM网络层，全链接层以及回归层。其中输入层为一层，输入维度为特征数目；LSTM网络层设置有250层；全链接层设置为1层，输出维度为1，表示输出特征数目。

模型的输入为：横向误差、航向误差、横摆角速度，输出为：横摆角速度。即根据车辆过去3s内的横向误差、航向误差、横摆角速度，预测未来5s内的车辆横摆角速度。其中，横向误差为参考路径关键点相对于车辆实际位置的横向距离，取左正右负。航向误差为参考路径关键点相对于车辆实际位置的航向角偏差。长短期记忆网络预测横摆角速度流程如图2所示。

第二步，车辆纵向参数预测——衰减加速度模型预测车辆纵向车速：车辆在运动过程中车速并不是恒定不变的，而未来的加速度难以预测，因此采用衰减加速度模型，模拟车辆的变加速度运动。

车辆在运动过程中车速并不是恒定不变的，而未来的加速度难以预测，因此采用衰减加速度模型，模拟车辆的变加速度运动。输入参数为当前速度与当前加速度，输出参数为未来5s内的速度。

具体过程为：基于当前实时速度与实时加速度，构建衰减加速度模型，在5s内加速度衰减到0，迭代求出未来5s内纵向速度，作为未来有限时域(5s)内的纵向速度测量值。

第三步，车辆轨迹预测——结合车辆运动学模型预测车辆未来轨迹：单纯的基于数据模型的方法预测出的轨迹有时候不符合车辆的运动学特性。因此建立车辆运动学模型，将长短期记忆网络预测出的结果和衰减加速度模型输出的结果作为中间变量，在保证预测轨迹的可行性的同时，也能够防止网络预测失败导致的轨迹预测结果偏大。

基于EKF(扩展卡尔曼滤波)对预测时域内的车辆轨迹进行预测，由此将轨迹预测问题转化为状态估计问题。

(1)建立车辆的状态矢量：

其中，x_k，y_k，θ_k，v_k，γ_k，a_k，

分别为全局坐标x，全局坐标y，车辆航向，纵向速度，横摆角速度，纵向加速度，横摆角加速度。

(2)建立过程模型：基于车辆的运动学方程，建立(1)中状态矢量下一时刻与当前时刻关系的过程模型，

x_k+1＝f(x_k)+q_k＝[f₁，f₂，f₃，f₄，f₅，f₆，f₇]+q_k

其中，f(x_k)是状态矩阵，采样周期T＝0.05s；q_k是过程噪声，q_k～N(0，Q_k)，

非线性状态转移模型：

f₁：

f₂：

f₃：

f₄：v_k+1＝v_k+a_kT

f₅：

f₆：a_k+1＝a_k

f₇：

其中，采样周期T＝0.05s。

(3)获取模拟测量值：将第一步与第二步中预测出的速度与横摆角速度作为模拟测量值，用于后续的EKF计算，

y_k＝[v_k，γ_k]^T

其中γ_k、v_k分别为第一步中利用长短期记忆网络神经网络获得的横摆角速度和第二步中利用衰减加速度模型获得的纵向速度。

(4)利用非线性卡尔曼滤波对(1)中的车辆状态矢量x_k进行预测、更新：

预测当前时刻先验状态矢量及协方差矩阵：

其中，

是当前时刻先验估计状态，x_k-1是上一时刻后验估计状态，f(·)是状态转移方程，

是当前时刻的先验协方差矩阵，F是状态转移矩阵，P_k-1是上一时刻的后验协方差矩阵，Q_k-1为上一时刻的过程噪声。

更新当前时刻状态矢量及协方差矩阵：

A.卡尔曼增益更新：

其中，K_k是当前时刻卡尔曼增益，

为观测矩阵，R_k是当前时刻观测噪声协方差矩阵，R_k＝diag{0.01，0.001}。

B.状态矢量更新：

C.协方差矩阵更新：

其中，P_k是当前时刻的后验协方差矩阵，I是单位矩阵。

以上所述仅为本发明的具体实施方式，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于运动学约束的车辆智能轨迹预测方法，其特征在于，包括以下步骤：

第一步，构建长短期记忆网络模型，根据车辆过去t₁内的横向误差、航向误差、横摆角速度，预测未来t₂内车辆的横摆角速度；

第二步，基于当前实时速度与实时加速度，构建衰减加速度模型，在t₂内加速度衰减到0，迭代求出未来t₂内纵向速度，作为未来t₂内车辆的纵向速度测量值；

第三步，建立车辆运动学模型，将长短期记忆网络预测出的横摆角速度和衰减加速度模型输出的纵向速度作为中间变量，预测车辆的未来轨迹。

2.根据权利要求1所述的一种基于运动学约束的车辆智能轨迹预测方法，其特征在于，所述第一步，所述长短期记忆网络模型的网络架构包括输入层、LSTM网络层、全链接层以及回归层；其中输入层为一层，输入维度为特征数目；LSTM网络层设置有250层；全链接层设置为1层，输出维度为1，表示输出特征数目。

3.根据权利要求1或2所述的一种基于运动学约束的车辆智能轨迹预测方法，其特征在于，所述第三步包括以下子步骤：

(1)建立车辆的状态矢量：

其中，x_k，y_k，θ_k，v_k，γ_k，a_k，

分别为全局坐标x，全局坐标y，车辆航向，纵向速度，横摆角速度，纵向加速度，横摆角加速度；

(2)建立过程模型：基于车辆的运动学方程，建立(1)中状态矢量下一时刻与当前时刻关系的过程模型，

x_k+1＝f(x_k)+q_k＝[f₁，f₂，f₃，f₄，f₅，f₆，f₇]+q_k

其中，f(x_k)是状态矩阵；q_k是过程噪声，q_k～N(0，Q_k)，

非线性状态转移模型：

f₄：v_k+1＝v_k+a_kT

f₆：a_k+1＝a_k

其中，采样周期T＝0.05s；

(3)获取模拟测量值：将第一步与第二步中预测出的速度与横摆角速度作为模拟测量值，用于后续的EKF计算，

y_k＝[v_k，γ_k]^T

其中γ_k、v_k分别为第一步中利用长短期记忆网络神经网络获得的横摆角速度和第二步中利用衰减加速度模型获得的纵向速度；

(4)利用非线性卡尔曼滤波对(1)中的车辆状态矢量x_k进行预测、更新：

预测当前时刻先验状态矢量及协方差矩阵：

其中，

是当前时刻先验估计状态，x_k-1是上一时刻后验估计状态，f(·)是状态转移方程，

是当前时刻的先验协方差矩阵，F是状态转移矩阵，P_k-1是上一时刻的后验协方差矩阵，Q_k-1为上一时刻的过程噪声；

更新当前时刻状态矢量及协方差矩阵：

A.卡尔曼增益更新：

其中，K_k是当前时刻卡尔曼增益，

为观测矩阵，R_k是当前时刻观测噪声协方差矩阵，R_k＝diag{0.01，0.001}；

B.状态矢量更新：

C.协方差矩阵更新：

其中，P_k是当前时刻的后验协方差矩阵，I是单位矩阵。

Images