CN109275121A - 一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法 - Google Patents
一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法 Download PDFInfo
- Publication number
- CN109275121A CN109275121A CN201810946889.0A CN201810946889A CN109275121A CN 109275121 A CN109275121 A CN 109275121A CN 201810946889 A CN201810946889 A CN 201810946889A CN 109275121 A CN109275121 A CN 109275121A
- Authority
- CN
- China
- Prior art keywords
- moment
- vehicle
- follows
- roadside unit
- information
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/30—Services specially adapted for particular environments, situations or purposes
- H04W4/40—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P]
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/01—Detecting movement of traffic to be counted or controlled
- G08G1/0104—Measuring and analyzing of parameters relative to traffic conditions
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/025—Services making use of location information using location based information parameters
- H04W4/027—Services making use of location information using location based information parameters using movement velocity, acceleration information
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/029—Location-based management or tracking services
Landscapes
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Chemical & Material Sciences (AREA)
- Analytical Chemistry (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Traffic Control Systems (AREA)
Abstract
一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法,包括以下步骤:首先,在智能网联交通系统中,通过DSRC技术自动识别行驶的车辆并获取相关数据,实现车载系统与路边单元的信息交互;接着,针对采集的相关信息,首先通过量化公式对路边单元与车载系统的方位差进行量化,其次通过自回归滑动平均法对加速度进行预测,最后利用自适应扩展卡尔曼滤波进行轨迹预测;最后,将处理好的信息广播给路边单元,以便于下一次与车载系统的信息交互。本发明提供了一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法。
Description
技术领域
本发明属于交通领域,尤其是涉及智能网联交通系统下的一种基 于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法。
背景技术
随着我国经济的快速发展,人民生活水平的日益提高,私家车开 始进入各家各户,很好的方便了家人的出行。但车辆的普及和大众化 也使得城市交通环境日益恶化,出现了车流不均衡、交通拥挤、车尾 碰撞、侧方碰撞等交通现象。伴随着基础设施薄弱和交通网络拥堵, 道路交通事故的数量日益增加,高交通事故发生率正在向全社会拉响 警报,因此道路交通安全受到极大的关注。近年来,虽然我国对道路 基础设施以及交通网络进行了很大的改善,使得交通事故数量和伤亡 人数有所减少,但事故总数和发生率仍然很高。
与传统的道路交通系统相比,智能网联交通系统更加趋向于由 “人”、“路”、“车”以及公路交通设施等进行信息交互的动态系 统。根据各国大量的统计研究后发现,驾驶人的失误是导致交通事故 的主要因素。因此,在当前道路基础设施已经不能再完善的情况下, 获取道路其他车道车辆的状态信息并加工处理广播给当前车辆的工作 刻不容缓,这使得驾驶员能更好的采取相应的补救措施,减小驾驶员 因失误导致的交通事故。
发明内容
为了克服现有车辆之间信息传输方式的安全性较低、车辆事故发 生概率较高的不足,本发明提供了一种基于自适应扩展卡尔曼滤波的 车辆轨迹跟踪方法。
本发明解决其技术问题所采用的技术方案是:
一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法,所述预测 方法包括如下步骤:
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)最后,云服务器将处理好的信息(车辆的预测速度、位于哪个 方向上、几车道和加速度)广播给路边单元,以便于下一次与车载系 统的信息交互。
进一步,所述步骤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时刻下的协方差矩阵。
再进一步,所述步骤1)中,在智能网联交通系统中,路边单元 安装在十字路口的红绿灯上且旁边附带有云服务器和定向天线,其中, 定线天线的发射角度为60°,使路边单元能更好的与车辆中的车载系 统进行信息交互。
更进一步,所述步骤1.2中,考虑到边缘云服务器的存储容量有 限,所以将服务器中的数据每隔一周进行清零。
本发明的技术构思为:首先,在智能网联交通系统中,通过DSRC 技术自动识别行驶的车辆并获取相关数据,实现车载系统与路边单元 的信息交互。接着,针对采集的相关信息,利用量化公式对路边单元 与车载系统的方位差进行量化;利用自回归滑动平均法对加速度进行 预测;利用自适应扩展卡尔曼滤波进行水平位置预测。最后,将处理 好的信息广播给路边单元,以便于下一次与车载系统的信息交互。
本发明的有益效果主要表现在:1、通过对路边单元与车载系统的 方位差的量化,可以清楚的了解当前车辆位于哪个方向的几车道。2、 结合自回归滑动平均法和自适应扩展卡尔曼滤波法实现加速度以及位 置的预测,并将结果传送给驾驶员,以便驾驶员可以根据车辆的相关 信息以及自身经验来做出合适的判断和行为,有效降低了交通事故的 发生率。
附图说明
图1是移动互联交通系统信息交互的示意图。
具体实施方式
下面结合附图对本发明作进一步详细描述。
参照图1,一种基于量化自适应扩展卡尔曼滤波的车辆轨迹跟踪 方法,本发明基于一种在DSRC技术通讯下的信息交互模型(如图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)最后,云服务器将处理好的信息(车辆的预测速度、位于哪个 方向上、几车道和加速度)广播给路边单元,以便于下一次与车载系 统的信息交互。
进一步,所述步骤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时刻下的协方差矩阵。
再进一步,所述步骤1)中,在智能网联交通系统中,路边单元 安装在十字路口的红绿灯上且旁边附带有云服务器和定向天线,其中, 定线天线的发射角度为60°,使路边单元能更好的与车辆中的车载系 统进行信息交互。
Claims (4)
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中,考虑到边缘云服务器的存储容量有限,所以将服务器中的数据每隔一周进行清零。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810946889.0A CN109275121B (zh) | 2018-08-20 | 2018-08-20 | 一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810946889.0A CN109275121B (zh) | 2018-08-20 | 2018-08-20 | 一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109275121A true CN109275121A (zh) | 2019-01-25 |
CN109275121B CN109275121B (zh) | 2021-08-03 |
Family
ID=65153924
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810946889.0A Active CN109275121B (zh) | 2018-08-20 | 2018-08-20 | 一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109275121B (zh) |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110264721A (zh) * | 2019-07-01 | 2019-09-20 | 北京理工大学 | 一种城市交叉口周边车辆轨迹预测方法 |
CN110675435A (zh) * | 2019-10-08 | 2020-01-10 | 嘉兴智驾科技有限公司 | 基于卡尔曼滤波和χ2检测平滑处理的车辆轨迹跟踪方法 |
CN111340855A (zh) * | 2020-03-06 | 2020-06-26 | 电子科技大学 | 一种基于轨迹预测的道路移动目标检测方法 |
CN111415531A (zh) * | 2019-06-17 | 2020-07-14 | 北京航空航天大学 | 一种基于车辆行驶轨迹预测的高速公路合流区交通冲突判别方法 |
CN113115230A (zh) * | 2021-03-26 | 2021-07-13 | 北京工业大学 | 一种基于信息物理系统的车辆广播通信控制方法 |
CN113203422A (zh) * | 2021-04-14 | 2021-08-03 | 武汉理工大学 | 一种基于尺寸测量装置的货车状态惯性参数联合估计方法 |
CN113504558A (zh) * | 2021-07-14 | 2021-10-15 | 北京理工大学 | 一种考虑道路几何约束的地面无人车辆定位方法 |
CN114822028A (zh) * | 2022-04-25 | 2022-07-29 | 北京宏瓴科技发展有限公司 | 一种车辆行驶轨迹的矫正方法、装置和计算机设备 |
CN117455960A (zh) * | 2023-12-22 | 2024-01-26 | 中国人民解放军海军工程大学 | 时变观测噪声条件下机载光电系统对地无源定位滤波算法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104635231A (zh) * | 2015-02-12 | 2015-05-20 | 西安电子科技大学 | 基于自适应卡尔曼滤波的机车雷达测速方法 |
CN104880189A (zh) * | 2015-05-12 | 2015-09-02 | 西安克拉克通信科技有限公司 | 一种动中通天线低成本跟踪抗干扰方法 |
US20170299729A1 (en) * | 2016-04-19 | 2017-10-19 | Clarion Co., Ltd. | Position estimation system and estimation method |
CN107909815A (zh) * | 2017-12-07 | 2018-04-13 | 浙江工业大学 | 一种基于自适应卡尔曼滤波的车辆速度预测方法 |
-
2018
- 2018-08-20 CN CN201810946889.0A patent/CN109275121B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104635231A (zh) * | 2015-02-12 | 2015-05-20 | 西安电子科技大学 | 基于自适应卡尔曼滤波的机车雷达测速方法 |
CN104880189A (zh) * | 2015-05-12 | 2015-09-02 | 西安克拉克通信科技有限公司 | 一种动中通天线低成本跟踪抗干扰方法 |
US20170299729A1 (en) * | 2016-04-19 | 2017-10-19 | Clarion Co., Ltd. | Position estimation system and estimation method |
CN107909815A (zh) * | 2017-12-07 | 2018-04-13 | 浙江工业大学 | 一种基于自适应卡尔曼滤波的车辆速度预测方法 |
Non-Patent Citations (2)
Title |
---|
MARCO LIPPI, MATTEO BERTINI, AND PAOLO FRASCONI: "Short-Term Traffic Flow Forecasting An Experimental Comparison of Time-Series Analysis and Supervised Learning", 《IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS》 * |
季露等: "基于模型融合和特征关联的视频目标跟踪算法", 《计算机技术与发展》 * |
Cited By (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111415531B (zh) * | 2019-06-17 | 2021-08-24 | 北京航空航天大学 | 基于行驶轨迹预测的高速公路合流区交通冲突判别方法 |
CN111415531A (zh) * | 2019-06-17 | 2020-07-14 | 北京航空航天大学 | 一种基于车辆行驶轨迹预测的高速公路合流区交通冲突判别方法 |
CN110264721A (zh) * | 2019-07-01 | 2019-09-20 | 北京理工大学 | 一种城市交叉口周边车辆轨迹预测方法 |
CN110675435A (zh) * | 2019-10-08 | 2020-01-10 | 嘉兴智驾科技有限公司 | 基于卡尔曼滤波和χ2检测平滑处理的车辆轨迹跟踪方法 |
CN110675435B (zh) * | 2019-10-08 | 2022-08-02 | 上海智驾汽车科技有限公司 | 基于卡尔曼滤波和χ2检测平滑处理的车辆轨迹跟踪方法 |
CN111340855A (zh) * | 2020-03-06 | 2020-06-26 | 电子科技大学 | 一种基于轨迹预测的道路移动目标检测方法 |
CN113115230B (zh) * | 2021-03-26 | 2023-08-18 | 北京工业大学 | 一种基于信息物理系统的车辆广播通信控制方法 |
CN113115230A (zh) * | 2021-03-26 | 2021-07-13 | 北京工业大学 | 一种基于信息物理系统的车辆广播通信控制方法 |
CN113203422A (zh) * | 2021-04-14 | 2021-08-03 | 武汉理工大学 | 一种基于尺寸测量装置的货车状态惯性参数联合估计方法 |
CN113504558A (zh) * | 2021-07-14 | 2021-10-15 | 北京理工大学 | 一种考虑道路几何约束的地面无人车辆定位方法 |
CN113504558B (zh) * | 2021-07-14 | 2024-02-27 | 北京理工大学 | 一种考虑道路几何约束的地面无人车辆定位方法 |
CN114822028A (zh) * | 2022-04-25 | 2022-07-29 | 北京宏瓴科技发展有限公司 | 一种车辆行驶轨迹的矫正方法、装置和计算机设备 |
CN114822028B (zh) * | 2022-04-25 | 2023-10-10 | 北京宏瓴科技发展有限公司 | 一种车辆行驶轨迹的矫正方法、装置和计算机设备 |
CN117455960A (zh) * | 2023-12-22 | 2024-01-26 | 中国人民解放军海军工程大学 | 时变观测噪声条件下机载光电系统对地无源定位滤波算法 |
CN117455960B (zh) * | 2023-12-22 | 2024-03-08 | 中国人民解放军海军工程大学 | 时变观测噪声条件下机载光电系统对地无源定位滤波方法 |
Also Published As
Publication number | Publication date |
---|---|
CN109275121B (zh) | 2021-08-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109275121A (zh) | 一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法 | |
CN109190811A (zh) | 一种基于自适应扩展卡尔曼滤波的车辆速度跟踪方法 | |
US10654476B2 (en) | Autonomous vehicle operational management control | |
CN109147390B (zh) | 一种基于量化自适应卡尔曼滤波的车辆轨迹跟踪方法 | |
BR112019016268A2 (pt) | gerenciamento operacional de veículo autônomo que inclui operar uma instância de modelo de processo de decisão de markov parcialmente observável | |
CN102855760B (zh) | 基于浮动车数据的在线排队长度检测方法 | |
CN109285376A (zh) | 一种基于深度学习的公交车客流统计分析系统 | |
CN108053645A (zh) | 一种基于轨迹数据的信号交叉口周期流量估计方法 | |
CN109272745B (zh) | 一种基于深度神经网络的车辆轨迹预测方法 | |
BR112019016274A2 (pt) | gerenciamento operacional de veículo autônomo | |
Kerper et al. | Learning traffic light phase schedules from velocity profiles in the cloud | |
CN104634352A (zh) | 一种基于浮动车移动轨迹与电子地图融合的道路匹配方法 | |
CN103310651A (zh) | 一种基于实时路况信息的公交到站预测方法 | |
CN109118787B (zh) | 一种基于深度神经网络的车辆速度预测方法 | |
CN103927872A (zh) | 一种基于浮动车数据预测多时段行程时间分布的方法 | |
CN103258427A (zh) | 基于信息物理网络的城市快速路交通实时监控系统与方法 | |
CN105046956A (zh) | 一种基于转向系数的交通流模拟及预测方法 | |
CN109118786A (zh) | 一种基于量化自适应卡尔曼滤波的车辆速度预测方法 | |
CN103440422A (zh) | 一种基于带时间窗的到站时间预测的公交晚点恢复方法 | |
CN102708698A (zh) | 基于车联网的车辆最优路径导航方法 | |
CN110400461B (zh) | 一种路网变更检测方法 | |
CN100466010C (zh) | 异类交通信息实时融合方法 | |
CN106971546B (zh) | 基于公交车gps数据的路段公交车渗透率估计方法 | |
CN110446160B (zh) | 一种基于多路径信道状态信息的车辆位置估计的深度学习方法 | |
CN105096590B (zh) | 交通信息生成方法和交通信息生成设备 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |