1.一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法,其特征在于,所述方法包括如下步骤:
1)在智能网联交通系统中,通过DSRC技术自动识别行驶的车辆并获取相关数据,实现车载系统与路边单元的信息交互,其中,信息交互的步骤如下:步骤1.1:当行驶车辆进入定向天线所覆盖的范围内时,车载系统会与路边单元通过DSRC技术实现双向通信,使得双方能同时发送自身存储单元上的信息,其中,车载系统发送的信息包括车辆的当前速度、当前位置和时间戳,路边单元发送的信息包括其他车道上车辆的预测位置、位于哪个方向上、几车道和加速度;
步骤1.2:路边单元将获取到的车辆信息发送到边缘云服务器进行一系列的运算操作;
2)边缘云服务器根据路边单元与车载系统之间的方位差,进行方位角计算并做相应的量化处理,针对位置信息对车辆行驶方向进行量化,其中,量化过程为:步骤2.1:将位置信息转换成以视角存在的数字信息,其中,车辆的实际承载角 相对于路边单元的定义为:在此,各参数定义如下:
在k时刻路边单元与车载系统之间的方位角;
θk:将k时刻的车辆位置转化为方位角的反三角函数;
σk:在时刻k由信号反射引起的轴承误差噪声;
(xk,yk):在时刻k车辆的当前位置;
步骤2.2:以十字路口中心作为坐标原点,对方向角 进行量化处理,确定车辆所在的方向,其中,量化公式如下:在此,各参数定义如下:
bk:在时刻k十字路口的量化方向;
q(·):量化函数;
i:十字路口的方向标识;
步骤2.3:为了实现方向信息具体化,对该方向的车道进行量化,将实际轴承角度 重命名为 将其记录为:在此,各参数定义如下:
在时刻k路边单元与车载系统之间的方位角;
θk':k时刻,将车辆与路边单元的相对位置转化为方位角的反三角函数;
(xi,yi):方向i上路边单元的固定位置;
步骤2.4:针对上述量化公式,对车辆所在的方向进行第二次量化,确定车辆所在的车道;
在此,各参数定义如下:
qt:在t时刻车道的量化方向;
ji:位于方向i上的第j个车道;
n:车道总数;
3)假设仅使用最近的p+1个车辆速度进行加速度估计,第p个加速度计算方式为:在此,各参数定义如下:
Δτ:采样时间间隔;
Δv:后一时刻与前一时刻的速度之差;
vk-p:在t-p时刻小车的速度;
τk-p:在t-p时刻小车的时间戳;
ak-p:第p个加速度值;
此后,根据p个加速度值,利用自回归滑动平均法进行车辆加速度预测,其中,预测公式如下:在此,各参数定义如下:
ak:在k时刻小车的加速度;
p:自回归阶数,即加速度总数;
q:移动平均阶数,即滑动总数;
β:不为零的待定系数;
不为零的待定系数;
ξk:在k时刻独立的误差项;
4)假设驾驶员在不变道的情况下,针对采集的相关信息,利用自适应扩展卡尔曼滤波算法对行驶车辆进行水平位置预测,其中,水平位置的计算公式为:利用状态空间模型将上述公式转化为状态方程和观测方程,其中,方程如下所示:xk=f(xk-1)+Wk-1; (1)zk=h(xk-1)+Vk-1; (2)在此,各参数定义如下:
xk:车辆在k时刻的水平位置值;
f(·):水平位置的真实轨迹函数,是非线性函数;
h(·):水平位置的观测轨迹函数,是非线性函数;
Wk-1:在k-1时刻的系统误差,服从高斯分布N(0,Qk-1),其中,Qk-1=cov(Wk-1),Qk-1是为在k-1时刻的过程噪声方差;
Vk-1:在k-1时刻的观测误差,服从高斯分布N(0,Rk-1),其中,Rk-1=cov(Vk-1),Rk-1是为在k-1时刻的观测噪声方差;
zk:在k时刻系统的状态观测值;
此后,根据状态空间模型利用自适应扩展卡尔曼滤波算法对车辆水平位置进行预测,其中,车辆位置更新的步骤如下:步骤4.1:先对(1)式和(2)式中的非线性函数进行线性化处理,降低计算量和模型复杂度,即将非线性函数f(xk-1)和h(xk-1)在滤波预测值 处做一阶泰勒展开,如下:在此,各参数定义如下:
Φk|k-1:k-1时刻到k时刻的系统状态转移矩阵,另外这里的k|k-1为k-1时刻到k时刻;
Hk-1:k-1时刻的观测转移矩阵;
Δt1:次数高于一次的高阶无穷小量;
Δt2:次数高于一次的高阶无穷小量;
此后,忽略高阶无穷小量,并将(3)式和(5)式分别代入(1)式和(2)式得:步骤4.2:计算先验估计值,假设当前状态在k-1时刻,由k-1时刻的预测值 以及真实轨迹函数f(xk-1)去预测k时刻小车水平位置的先验估计值xk|k-1,其中,k-1|k-1指的是基于k-1时刻计算k-1时刻下的某个值,当k|k-1不是状态转移矩阵的下标时所描述的意思为基于k-1时刻计算k时刻下的某个值;
步骤4.3:计算先验误差协方差,由k-1时刻的后验误差协方差矩阵Pk-1|k-1去预测当前时刻的先验误差协方差矩阵Pk|k-1,其中, μk为自适应遗忘因子;
步骤4.4:根据当前时刻先验误差协方差矩阵Pk|k-1计算卡尔曼增益Kk,其中,Rk-1为呈高斯分布的观测噪声;
步骤4.5:根据k时刻的先验估计值xk|k-1和观测值zk,得到k时刻的后验估计值xk|k,其中,xk|k=xk|k-1+Kk(zk-h(xk|k-1));
步骤4.6:更新后验误差协方差矩阵Pk|k,为计算k+1时刻的先验误差协方差矩阵做准备,其中,Pk|k=(I-KkHk)Pk|k-1,I为单位矩阵;
步骤4.7:更新k为k=k+1并重新回到步骤4.2开始新一轮的计算;
5)最后,云服务器将处理好的信息广播给路边单元,以便于下一次与车载系统的信息交互。
2.如权利要求1所述的一种基于量化自适应卡尔曼滤波的车辆速度预测方法,其特征在于:所述步骤4.3中,自适应遗忘因子的计算公式为:Gk=d(Mk-Hk-1Qk-1Hk-1-Rk-1);
ek=zk-h(xk|k-1);
在此,各参数定义如下:
max{·}:比较后取最大值;
α:修正系数,可以强制提高滤波器的跟踪性能;
Gk,ξk:中间推导变量,无实际物理意义;
ek:新息,即在k时刻真实观测值与估计输出值之间的差值;
U:最大可容忍误差;
d:权重因子,当新息超过最大可容忍误差时,会通过降低权重因子来减低自适应因子的值,最终使得误差下降;
Mk:新息在k时刻下的协方差矩阵。
3.如权利要求1或2所述的一种基于量化自适应卡尔曼滤波的车辆速度预测方法,其特征在于:所述步骤1)中,在智能网联交通系统中,路边单元安装在十字路口的红绿灯上且旁边附带有云服务器和定向天线,其中,定线天线的发射角度为60°,使路边单元能更好的与车辆中的车载系统进行信息交互。
4.如权利要求1或2所述的一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法,其特征在于,所述步骤1.2中,考虑到边缘云服务器的存储容量有限,所以将服务器中的数据每隔一周进行清零。