[发明专利]一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法

权利要求书

1.一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法，其特征在于，所述方法包括如下步骤：

1)在智能网联交通系统中，通过DSRC技术自动识别行驶的车辆并获取相关数据，实现车载系统与路边单元的信息交互，其中，信息交互的步骤如下：

步骤1.1：当行驶车辆进入定向天线所覆盖的范围内时，车载系统会与路边单元通过DSRC技术实现双向通信，使得双方能同时发送自身存储单元上的信息，其中，车载系统发送的信息包括车辆的当前速度、当前位置和时间戳，路边单元发送的信息包括其他车道上车辆的预测位置、位于哪个方向上、几车道和加速度；

步骤1.2：路边单元将获取到的车辆信息发送到边缘云服务器进行一系列的运算操作；

2)边缘云服务器根据路边单元与车载系统之间的方位差，进行方位角计算并做相应的量化处理，针对位置信息对车辆行驶方向进行量化，其中，量化过程为：

步骤2.1：将位置信息转换成以视角存在的数字信息，其中，车辆的k时刻路边单元与车载系统之间的方位角相对于路边单元的定义为：

在此，各参数定义如下：

在k时刻路边单元与车载系统之间的方位角；

θ_k：将k时刻的车辆位置转化为方位角的反三角函数；

σ_k：在时刻k由信号反射引起的轴承误差噪声；

(x_k,y_k)：在时刻k车辆的当前位置；

步骤2.2：以十字路口中心作为坐标原点，对k时刻路边单元与车载系统之间的方位角进行量化处理，确定车辆所在的方向，其中，量化公式如下：

1≤i≤4；

在此，各参数定义如下：

b_k：在时刻k十字路口的量化方向；

q(·)：量化函数；

i：十字路口的方向标识；

步骤2.3：为了实现方向信息具体化，对该方向的车道进行量化，将k时刻路边单元与车载系统之间的方位角重命名为将其记录为：

在此，各参数定义如下：

在时刻k路边单元与车载系统之间的方位角；

θ_k'：k时刻，将车辆与路边单元的相对位置转化为方位角的反三角函数；

(x_i,y_i)：方向i上路边单元的固定位置；

步骤2.4：针对上述量化公式，对车辆所在的方向进行第二次量化，确定车辆所在的车道；

1≤j≤n≤6，1≤i≤4；

在此，各参数定义如下：

q_t：在t时刻车道的量化方向；

j_i：位于方向i上的第j个车道；

n：车道总数；

3)假设仅使用最近的p+1个车辆速度进行加速度估计，第p个加速度计算方式为：

在此，各参数定义如下：

Δτ：采样时间间隔；

Δv：后一时刻与前一时刻的速度之差；

v_k-p：在t-p时刻小车的速度；

τ_k-p：在t-p时刻小车的时间戳；

a_k-p：第p个加速度值；

此后，根据p个加速度值，利用自回归滑动平均法进行车辆加速度预测，其中，预测公式如下：

在此，各参数定义如下：

a_k：在k时刻小车的加速度；

p：自回归阶数，即加速度总数；

q：移动平均阶数，即滑动总数；

β：不为零的待定系数；

不为零的待定系数；

ξ_k：在k时刻独立的误差项；

4)假设驾驶员在不变道的情况下，针对采集的相关信息，利用自适应扩展卡尔曼滤波算法对行驶车辆进行水平位置预测，其中，水平位置的计算公式为：

利用状态空间模型将上述公式转化为状态方程和观测方程，其中，方程如下所示：

x_k＝f(x_k-1)+W_k-1； (1)

z_k＝h(x_k-1)+V_k-1； (2)

在此，各参数定义如下：

x_k：车辆在k时刻的水平位置值；

f(·)：水平位置的真实轨迹函数，是非线性函数；

h(·)：水平位置的观测轨迹函数，是非线性函数；

W_k-1：在k-1时刻的系统误差，服从高斯分布N(0,Q_k-1)，其中，

Q_k-1＝cov(W_k-1)，Q_k-1是为在k-1时刻的过程噪声方差；

V_k-1：在k-1时刻的观测误差，服从高斯分布N(0,R_k-1)，其中，

R_k-1＝cov(V_k-1)，R_k-1是为在k-1时刻的观测噪声方差；

z_k：在k时刻系统的状态观测值；

此后，根据状态空间模型利用自适应扩展卡尔曼滤波算法对车辆水平位置进行预测，其中，车辆位置更新的步骤如下：

步骤4.1：先对(1)式和(2)式中的非线性函数进行线性化处理，降低计算量和模型复杂度，即将非线性函数f(x_k-1)和h(x_k-1)在滤波预测值处做一阶泰勒展开，如下：

在此，各参数定义如下：

Φ_k|k-1：k-1时刻到k时刻的系统状态转移矩阵，另外这里的k|k-1为k-1时刻到k时刻；

H_k-1：k-1时刻的观测转移矩阵；

Δt₁：次数高于一次的高阶无穷小量；

Δt₂：次数高于一次的高阶无穷小量；

此后，忽略高阶无穷小量，并将(3)式和(5)式分别代入(1)式和(2)式得：

步骤4.2：计算先验估计值，假设当前状态在k-1时刻，由k-1时刻的预测值以及真实轨迹函数f(x_k-1)去预测k时刻小车水平位置的先验估计值x_k|k-1，其中，k-1|k-1指的是基于k-1时刻计算k-1时刻下的某个值，当k|k-1不是状态转移矩阵的下标时所描述的意思为基于k-1时刻计算k时刻下的某个值；

步骤4.3：计算先验误差协方差，由k-1时刻的后验误差协方差矩阵P_k-1|k-1去预测当前时刻的先验误差协方差矩阵P_k|k-1，其中，μ_k为自适应遗忘因子；

步骤4.4：根据当前时刻先验误差协方差矩阵P_k|k-1计算卡尔曼增益K_k，其中，R_k-1为呈高斯分布的观测噪声；

步骤4.5：根据k时刻的先验估计值x_k|k-1和观测值z_k，得到k时刻的后验估计值x_k|k，其中，x_k|k＝x_k|k-1+K_k(z_k-h(x_k|k-1))；

步骤4.6：更新后验误差协方差矩阵P_k|k，为计算k+1时刻的先验误差协方差矩阵做准备，其中，P_k|k＝(I-K_kH_k)P_k|k-1，I为单位矩阵；

步骤4.7：更新k为k＝k+1并重新回到步骤4.2开始新一轮的计算；

5)最后，云服务器将处理好的信息广播给路边单元，以便于下一次与车载系统的信息交互。