CN111060944B - 一种基于current-ellipse模型的车辆动态跟踪方法 - Google Patents

一种基于current-ellipse模型的车辆动态跟踪方法 Download PDF

Info

Publication number
CN111060944B
CN111060944B CN201911367763.9A CN201911367763A CN111060944B CN 111060944 B CN111060944 B CN 111060944B CN 201911367763 A CN201911367763 A CN 201911367763A CN 111060944 B CN111060944 B CN 111060944B
Authority
CN
China
Prior art keywords
vehicle
acceleration
current
model
max
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.)
Active
Application number
CN201911367763.9A
Other languages
English (en)
Other versions
CN111060944A (zh
Inventor
刘建圻
王欧宇
曾碧
张慧
何威
辛苗
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangdong University of Technology
Original Assignee
Guangdong University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN201911367763.9A priority Critical patent/CN111060944B/zh
Publication of CN111060944A publication Critical patent/CN111060944A/zh
Application granted granted Critical
Publication of CN111060944B publication Critical patent/CN111060944B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type

Abstract

本发明公开了一种基于current‑ellipse模型的车辆动态跟踪方法，包括以下步骤：S1、在GNSS信号受限区域部署RSUs车辆无线定位系统，获取目标跟踪车辆的运动观测数据；S2、以current模型为基础，改进得到目标跟踪车辆的current‑ellipse运动数学模型；S3、在步骤S2改进得到的current‑ellipse运动数学模型的基础上，对来自RSUs的目标跟踪车辆观测数据执行自适应卡尔曼滤波算法，从而得到精确的目标跟踪车辆的运动数据。本发明引入RSUs辅助定位，并基于一种新的车辆运动数学模型current‑ellipse和采用自适应卡尔曼滤波的融合算法，实现动态车辆精确跟踪。

Description

一种基于current-ellipse模型的车辆动态跟踪方法
技术领域
本发明涉及车辆动态跟踪技术领域，尤其涉及到一种基于current-ellipse模型的车辆动态跟踪方法。
背景技术
车辆的跟踪或定位是构建车联网，实现智能交通，促进社会经济发展的重要环节，长期以来，受到了研究人员的广泛关注。
目前，如GPS、北斗和伽利略等全球导航卫星系统(Global Navigation SatelliteSystem，GNSS)可以提供精确到米的车辆定位应用。然而，现实中会常常遇到当车辆行驶在较为封闭的区域时，GNSS信号的接收和发送受到了阻碍，导致无法满足车辆跟踪或定位的精度需求。
虽然采用基于激光雷达(如采用Velodyne-64和Velodyne-128)的定位系统可以取得厘米级精度的定位效果。然而激光雷达设备比较昂贵，同时激光雷达传感器的使用也受到了如下雨、下雪和大雾等天气条件的限制；基于视觉相机的定位方法成本较低，而定位的精度取决于获取图像的分辨率大小(分辨率越高，定位精度越高)，该方法极易受到光照条件的影响。
另外，在车辆的跟踪系统中，确定车辆运动的数学模型也是极其重要的，一个基于合理的车辆运动数学模型的定位算法能比没有明确模型的定位算法更高效。
根据不同的行驶情况，车辆的运动模型主要分为以下等级：强运动等级(如在高速公路中高速行驶的车辆)和弱运动等级(如使用巡航控制系统匀速行驶的车辆)。目前，最简单的两种车辆运动数学模型是恒速度模型(Constant-Velocity,CV)和恒加速度模型(Constant-Acceleration，CA)。CV模型假定目标车辆保持一个速度行驶，车辆的加速度比较小，以至于可以忽略；CA模型假定目标车辆拥有恒定的加速度。然而在在实际中，两种数学模型过于简单，都不适用于复杂的车辆行驶。
R.A.Singer在1970年提出了Singer模型，在该模型中，假定目标车辆的加速度是一阶零均值的马尔科夫过程，该模型可以通过调整参数实现自适应，但模型本质上依然是先验模型，因为没有利用目标车辆的在线运动信息。实际强运动情况中，目标车辆的加速度平均值也明显是不为零的；Zhou等人提出了“current”模型，该模型本质上是具有自适应非零加速度平均值的Singer模型，采用的是改进的瑞利分布(Rayleigh distribution)。与Singer模型相比，“current”模型更适合描述车辆加速度的统计分布，但该模型依然需要改进，以适应强、弱和均匀的不同车辆运动等级。
还有的是，在车辆跟踪过程中，根据目标车辆的运动数学模型采用何种信息融合算法也是极其重要的环节。当前，业界常用的是T2TF(tract-to-track fusion)算法，该算法可以充分反映传感器的可靠性。此外，Chang Joo Lee等人提出了改进的T2TF算法；MAS.等人利用加权线性最小二乘算法实现车辆的静态定位，取得了米级的精度；Rabiee R.等人使用粒子滤波来估计车辆位置，粒子滤波适用于非线性和非高斯的系统，然而在线性高斯系统中，上述几种算法均存在各自的缺点。
发明内容
本发明的目的在于克服现有技术的不足，提供一种跟踪精度高的基于current-ellipse模型的车辆动态跟踪方法。
为实现上述目的，本发明所提供的技术方案为：
一种基于current-ellipse模型的车辆动态跟踪方法，包括以下步骤：
S1、在GNSS信号受限区域部署RSUs(Roadside Units)车辆无线定位系统，获取目标跟踪车辆的运动观测数据；
S2、以current模型为基础，改进得到目标跟踪车辆的current-ellipse运动数学模型；
S3、在步骤S2改进得到的current-ellipse运动数学模型的基础上，对来自RSUs的目标跟踪车辆观测数据执行自适应卡尔曼滤波算法，从而得到精确的目标跟踪车辆的运动数据。
进一步地，所述步骤S2的具体过程如下：
current模型使用Rayleigh分布来描述车辆行驶加速度的分布，车辆的正向加速度的概率密度函数为：
Figure BDA0002338886430000031
上式中，a为车辆加速度，amax为车辆的正向加速度上限；μ>0，为常数；相应的，根据概率密度函数表达式，计算出加速度的均值和方差：
Figure BDA0002338886430000032
Figure BDA0002338886430000033
对于车辆的反向加速度，有：
Figure BDA0002338886430000034
Figure BDA0002338886430000035
根据以下公式，确定正向加速度均值E[a]的取值范围：
Figure BDA0002338886430000036
Figure BDA0002338886430000037
Figure BDA0002338886430000038
化简可得：
Figure BDA0002338886430000041
因此，正向加速度的均值范围如下：
Figure BDA0002338886430000042
对于反向加速度有：
Figure BDA0002338886430000043
Figure BDA0002338886430000044
根据current模型，计算得出车辆加速度的协方差：
Figure BDA0002338886430000045
由于当加速度均值处在[(4-π)a-max/4,(4-π)amax/4]范围时，描述加速度分布的概率密度函数失效，同时加速度的协方差
Figure BDA0002338886430000049
也变得不准确；因此，对current模型进行改进，得到目标跟踪车辆的current-ellipse运动数学模型；具体改进如下：
[(4-π)a-max/4,(4-π)amax/4]区间内，用椭圆曲线的一半来代替current模型的协方差曲线，该椭圆的顶点为：
Figure BDA0002338886430000046
和
Figure BDA0002338886430000047
该椭圆的长轴长度A为(4-π)amax/4，短轴长度B为(4-π)amax/6，即椭圆方程如下：
Figure BDA0002338886430000048
在current-ellipse运动数学模型中，车辆加速度的协方差计算公式如下：
Figure BDA0002338886430000051
其中，
Figure BDA0002338886430000052
表示当前加速度的均值，
Figure BDA0002338886430000053
进一步地，所述步骤S3的具体过程如下：
S3-1、输入来自RSUs的目标跟踪车辆观测数据：观测数据数N、加速度时间常数τ的倒数α、加速度上限αmax、加速度下限α-max；
S3-2、初始化采样周期T、状态转移矩阵Φ(k/k-1)和Φ*(T)、观测矩阵H(k)、观测噪声的协方差矩阵R(k)、误差的协方差矩阵P(k-1)，零初始化预测结果predicted_result[N]；k＝1；
S3-3、预测过程：
1)先验状态估计：X(k/k-1)＝Φ*(T)X(k-1)；
2)基于current-ellipse模型计算协方差
Figure BDA0002338886430000054
3)计算过程噪声的协方差矩阵Q(k)；
4)先验误差的协方差：P(k/k-1)＝Φ(k/k-1)P(k-1)ΦT(k/k-1)+Q(k)；
S3-4、更新过程：
5)优化卡尔曼增益：K(k)＝P(k/k-1)HT(k)[H(k)P(k/k-1)HT(k)+R(k)]-1；
6)更新后的估计协方差：P(k)＝[1-K(k)H(k)]P(k/k-1)；
7)更新后的状态估计：X(k)＝X(k/k-1)+K(k)[Z(k)-H(k)X(k/k-1)]；
8)预测结果predicted_result[k]＝X(k)；
S3-5、判断k是否小于等于观测数据数N，若是，则k＝k+1，并返回步骤S3-3；若不是，则直接输出预测结果。
与现有技术相比，本方案原理及优点如下：
1.在封闭区域内引入RSUs车辆无线定位系统，辅助目标车辆定位，使得在封闭区域获取比较精确的目标跟踪车辆的运动观测数据。
2.基于RSUs的车辆跟踪观测数据比较精确，但原始数据依然存在噪声，需要考虑目标跟踪车辆的运动数学模型并完成滤波处理，得到跟踪车辆的最优状态估计。当前经典的current数学模型可以在一定范围内，较好地反映目标车辆的运动状态。然而，当车辆的加速度均值处在[(4-π)a-max/4,(4-π)amax/4]范围时，current模型描述加速度分布的概率密度函数失效了；同时，基于模型计算的加速度协方差
Figure BDA0002338886430000061
也变得不准确，从而影响后续滤波算法的使用。本方案对此以current模型为基础，改进得到current-ellipse模型，在[(4-π)a-max/4,(4-π)amax/4]区间内，用椭圆曲线的一半来代替current模型的协方差曲线，从下面的实验分析结果可以看出，采用本方案的current-ellipse模型，取得的车辆跟踪效果更佳。
3.采用基于线性系统的自适应卡尔曼滤波算法，对原始观测数据进行处理，具有方法简单，结果精确，鲁棒性好等优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的服务作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一种基于current-ellipse模型的车辆动态跟踪方法的原理流程图；
图2为经典current模型和本发明所使用的current-ellipse模型的加速度协方差对比图；
图3为本发明一种基于current-ellipse模型的车辆动态跟踪方法中自适应卡尔曼滤波的原理流程图；
图4为车辆A实验结果的RSME数据对比图；
图5为车辆B实验结果的RSME数据对比图；
图6为车辆C实验结果的RSME数据对比图；
图7为实验地点概况图；
图8为对应图7中A,B,C地点的GNSS信号强度示意图；
图9为GNSS跟踪结果示意图(颜色较浅的为实际的行走线路)；
图10为采用本发明后的跟踪结果示意图(颜色较深的为实际的行走线路)。
具体实施方式
下面结合具体实施例对本发明作进一步说明：
如图1所示，一种基于current-ellipse模型的车辆动态跟踪方法，包括以下步骤：
S1、在GNSS信号受限区域部署RSUs车辆无线定位系统，获取目标跟踪车辆的运动观测数据；
S2、以current模型为基础，改进得到目标跟踪车辆的current-ellipse运动数学模型；
本步骤的具体过程如下：
current模型使用Rayleigh分布来描述车辆行驶加速度的分布，车辆的正向加速度的概率密度函数为：
Figure BDA0002338886430000071
上式中，a为车辆加速度，amax为车辆的正向加速度上限；μ>0，为常数；相应的，根据概率密度函数表达式，计算出加速度的均值和方差：
Figure BDA0002338886430000072
Figure BDA0002338886430000073
对于车辆的反向加速度，有：
Figure BDA0002338886430000074
Figure BDA0002338886430000081
根据以下公式，确定正向加速度均值E[a]的取值范围：
Figure BDA0002338886430000082
Figure BDA0002338886430000083
Figure BDA0002338886430000084
化简可得：
Figure BDA0002338886430000085
因此，正向加速度的均值范围如下：
Figure BDA0002338886430000086
同理，对于反向加速度有：
Figure BDA0002338886430000087
Figure BDA0002338886430000088
综上，可以知道在current模型中，[(4-π)a-max/4,(4-π)amax/4]不是加速度均值E[a]的取值范围；故在实际情况中，当目标车辆的加速度均值处在该区间内时，该模型所使用的原概率密度函数就不再适用于精确描述车辆加速度的分布；
此外，根据current模型，还可以计算出车辆加速度的协方差：
Figure BDA0002338886430000089
上式中，
Figure BDA00023388864300000810
为当前加速度的均值；当加速度均值处在[(4-π)a-max/4,(4-π)amax/4]范围时，描述加速度分布的概率密度函数失效了，同时加速度的协方差
Figure BDA00023388864300000811
也变得不准确，从而影响了自适应卡尔曼滤波算法所需的加速度协方差矩阵的计算；
对此，本实施例提出的改进current-ellipse模型的具体内容如下：
如图2所示，是current模型和本实施例提出的current-ellipse模型的加速度协方差图；
在[(4-π)a-max/4,(4-π)amax/4]区间内，本实施例采用椭圆曲线的一半来代替原来current模型的协方差曲线；
该椭圆的顶点为：
Figure BDA0002338886430000091
和
Figure BDA0002338886430000092
该椭圆的长轴长度A为(4-π)amax/4，短轴长度B为(4-π)amax/6，因此，椭圆方程如下：
Figure BDA0002338886430000093
综合上述内容，在本实施例提出的current-ellipse数学模型下，车辆加速度的协方差计算公式如下：
Figure BDA0002338886430000094
其中，
Figure BDA0002338886430000095
表示当前加速度的均值，
Figure BDA0002338886430000096
S3、在步骤S2改进得到的current-ellipse运动数学模型的基础上，对来自RSUs的目标跟踪车辆观测数据执行自适应卡尔曼滤波算法，得到精确的目标跟踪车辆的运动数据；
本步骤中应用到：
(1)状态方程
基于current-ellipse运动数学模型，车辆加速度满足以下公式：
Figure BDA0002338886430000101
其中，
Figure BDA0002338886430000102
Figure BDA0002338886430000103
为加速度均值，设定为常数；
Figure BDA0002338886430000104
为零均值加速度过程；w(t)为零均值高斯白噪声，τ为车辆加速度机动时间常数，令α＝1/τ；
由于车辆的运动是二维的，取正东方向和正北方向的坐标轴，构造车辆的运动状态方程如下：
Figure BDA0002338886430000105
上式中，xe，ve，ae分别表示车辆正东方向的位置，速度和加速度；xn，vn，an分别表示车辆正北方向的位置，速度和加速度；
Figure BDA0002338886430000106
和
Figure BDA0002338886430000107
分别代表两个方向的零均值高斯白噪声，它们各自的协方差分别为
Figure BDA0002338886430000108
和
Figure BDA0002338886430000109
参数αe＝1/τe，αn＝1/τn，其中
Figure BDA00023388864300001010
和
Figure BDA00023388864300001011
分别代表两个方向的机动时间常数；
将采样周期设为T，对车辆的运动状态方程离散化，得到以下离散的状态方程：
Figure BDA00023388864300001012
其中，X(k)＝[xe(k) ve(k) ae(k) xn(k) vn(k) an(k)]T，
Figure BDA00023388864300001013
系统的状态转移矩阵为：
Φ(k/k-1)＝diag[Φe(k/k-1),Φn(k/k-1)]；
Figure BDA00023388864300001014
Figure BDA00023388864300001015
控制输入矩阵U(k)＝diag(Ue(k),Un(k))，Ue(k)和Un(k)形式相同，可得：
Figure BDA0002338886430000111
随后，得到过程噪声的协方差矩阵：
Q(k)＝diag(Qe(k),Qn(k))；
Figure BDA0002338886430000112
Figure BDA0002338886430000113
Figure BDA0002338886430000114
Figure BDA0002338886430000115
Figure BDA0002338886430000116
Figure BDA0002338886430000117
Figure BDA0002338886430000118
此处，Qe(k)和Qn(k)形式相同，只是下标不同；
(2)观测方程
利用RSUs车辆无线定位系统，可以得到目标车辆的跟踪观测值Z(k)＝[eobs(k)nobs(k)]T，其中eobs和nobs分别代表了两个方向上的分量；
观测值和系统的真实状态有以下关系：
Z(k)＝H(k)X(k)+V(k)；
其中，观测矩阵
Figure BDA0002338886430000119
能够将状态空间映射到观测空间；观测噪声V(k)＝[Ve(k) Vn(k)]T为零均值的高斯白噪声，观测噪声的协方差矩阵为：
Figure BDA00023388864300001110
如图3所示，本步骤中执行自适应卡尔曼滤波算法的具体过程如下：
S3-1、输入来自RSUs的目标跟踪车辆观测数据：观测数据数N、加速度时间常数τ的倒数α、加速度上限αmax、加速度下限α-max；
S3-2、初始化采样周期T、状态转移矩阵Φ(k/k-1)和Φ*(T)、观测矩阵H(k)、观测噪声的协方差矩阵R(k)、误差的协方差矩阵P(k-1)，零初始化预测结果predicted_result[N]；k＝1；
S3-3、预测过程：
1)先验状态估计：X(k/k-1)＝Φ*(T)X(k-1)；
2)基于current-ellipse模型计算协方差
Figure BDA0002338886430000121
3)计算过程噪声的协方差矩阵Q(k)；
4)先验误差的协方差：P(k/k-1)＝Φ(k/k-1)P(k-1)ΦT(k/k-1)+Q(k)；
S3-4、更新过程：
5)优化卡尔曼增益：K(k)＝P(k/k-1)HT(k)[H(k)P(k/k-1)HT(k)+R(k)]-1；
6)更新后的估计协方差：P(k)＝[1-K(k)H(k)]P(k/k-1)；
7)更新后的状态估计：X(k)＝X(k/k-1)+K(k)[Z(k)-H(k)X(k/k-1)]；
8)预测结果predicted_result[k]＝X(k)；
S3-5、判断k是否小于等于观测数据数N，若是，则k＝k+1，并返回步骤S3-3；若不是，则直接输出预测结果。
本实施例中，current-ellipse模型下的车辆加速度均值可以先赋值为一个先验加速度预测值，例如
Figure BDA0002338886430000122
和
Figure BDA0002338886430000123
这样，目标跟踪车辆在例如正东方向上有以下计算过程：
xe(k/k-1)＝xe(k-1)+Tve(k-1)+T2ae(k-1)/2
ve(k/k-1)＝ve(k-1)+Tae(k-1)
ae(k/k-1)＝ae(k-1)；
因此，公式
Figure BDA0002338886430000124
可以简化为：X(k/k-1)＝Φ*(T)X(k-1)；
其中，
Figure BDA0002338886430000131
Figure BDA0002338886430000132
为了证明本实施例的有效性，下面进行实验分析：
实验一，比较基于不同数学模型的车辆跟踪算法精度
本次实验在同一直线路段的实验场地进行，使用3辆参数不同的实验车辆，利用RSUs车辆无线定位系统获取目标跟踪车辆的运动数据，并比较基于current数学模型和本实施例的current-ellipse运动数学模型的算法的实验精度(实验真值由激光雷达传感器获取)。3辆汽车的参数如下：
表1加速度参数
车辆 100公里加速时间 最大加速度
A 9.8seconds 2.83m/s2
B 3.5seconds 7.93m/s2
C 4.5seconds 6.17m/s2
实验结果如图4-图6所示，根据三种条件下实验结果的均方根误差(root meansquare error,RMSE)分析可知，基于本实施例的current-ellipse模型的算法具有很高精度，与current模型相比，取得了更好地跟踪效果。
实验二，本实施例方法的应用实例
本次实验的实验地点为广东工业大学(广州大学城校区)的部分区域，如图7所示；该区域对应的GNSS信号强度如图8所示。在该区域，本次实验对比了单纯使用GNSS跟踪方法，单纯使用RSUs静态定位和本实施例采用的动态跟踪方法。三种方法得到的车辆跟踪轨迹结果分别如图9-图10所示，从这些图分析，可以明显看出本实施例方法取得的效果最好，具有误差小，低漂移，跟踪精确的优点。
以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (2)

1.一种基于current-ellipse模型的车辆动态跟踪方法，其特征在于，包括以下步骤：
S1、在GNSS信号受限区域部署RSUs车辆无线定位系统，获取目标跟踪车辆的运动观测数据；
S2、以current模型为基础，改进得到目标跟踪车辆的current-ellipse运动数学模型；
S3、在步骤S2改进得到的current-ellipse运动数学模型的基础上，对来自RSUs的目标跟踪车辆观测数据执行自适应卡尔曼滤波算法，从而得到精确的目标跟踪车辆的运动数据；
所述步骤S2的具体过程如下：
current模型使用Rayleigh分布来描述车辆行驶加速度的分布，车辆的正向加速度的概率密度函数为：
Figure FDA0003018061890000011
上式中，a为车辆加速度，amax为车辆的正向加速度上限；μ>0，为常数；相应的，根据概率密度函数表达式，计算出加速度的均值和方差：
Figure FDA0003018061890000012
Figure FDA0003018061890000013
对于车辆的反向加速度，有：
Figure FDA0003018061890000014
Figure FDA0003018061890000015
a-max为车辆的正向加速度下限；
根据以下公式，确定正向加速度均值E[a]的取值范围：
Figure FDA0003018061890000021
Figure FDA0003018061890000022
Figure FDA0003018061890000023
化简可得：
Figure FDA0003018061890000024
因此，正向加速度的均值范围如下：
Figure FDA0003018061890000025
对于反向加速度有：
Figure FDA0003018061890000026
Figure FDA0003018061890000027
根据current模型，计算得出车辆加速度的协方差：
Figure FDA0003018061890000028
由于当加速度均值处在[(4-π)a-max/4,(4-π)amax/4]范围时，描述加速度分布的概率密度函数失效，同时加速度的协方差
Figure FDA00030180618900000211
也变得不准确；因此，对current模型进行改进，得到目标跟踪车辆的current-ellipse运动数学模型；具体改进如下：
[(4-π)a-max/4,(4-π)amax/4]区间内，用椭圆曲线的一半来代替current模型的协方差曲线，该椭圆的顶点为：
Figure FDA0003018061890000029
和
Figure FDA00030180618900000210
该椭圆的长轴长度A为(4-π)amax/4，短轴长度B为(4-π)amax/6，即椭圆方程如下：
Figure FDA0003018061890000031
在current-ellipse运动数学模型下，车辆加速度的协方差计算公式如下：
Figure FDA0003018061890000032
其中，
Figure FDA0003018061890000033
表示当前加速度的均值，
Figure FDA0003018061890000034
2.根据权利要求1所述的一种基于current-ellipse模型的车辆动态跟踪方法，其特征在于，所述步骤S3的具体过程如下：
S3-1、输入来自RSUs的目标跟踪车辆观测数据：观测数据数N、加速度时间常数τ的倒数α、正向加速度上限a-max、正向加速度下限a-max；
S3-2、初始化采样周期T、状态转移矩阵Φ(k/k-1)和Φ*(T)、观测矩阵H(k)、观测噪声的协方差矩阵R(k)、误差的协方差矩阵P(k-1)，零初始化预测结果predicted_result[N]；k＝1；
S3-3、预测过程：
1)先验状态估计：X(k/k-1)＝Φ*(T)X(k-1)；
2)基于current-ellipse模型计算协方差
Figure FDA0003018061890000035
3)计算过程噪声的协方差矩阵Q(k)；
4)先验误差的协方差：P(k/k-1)＝Φ(k/k-1)P(k-1)ΦT(k/k-1)+Q(k)；
S3-4、更新过程：
5)优化卡尔曼增益：K(k)＝P(k/k-1)HT(k)[H(k)P(k/k-1)HT(k)+R(k)]-1；
6)更新后的估计协方差：P(k)＝[1-K(k)H(k)]P(k/k-1)；
7)更新后的状态估计：X(k)＝X(k/k-1)+K(k)[Z(k)-H(k)X(k/k-1)]；
8)预测结果predicted_result[k]＝X(k)；
S3-5、判断k是否小于等于观测数据数N，若是，则k＝k+1，并返回步骤S3-3；若不是，则直接输出预测结果。
CN201911367763.9A 2019-12-26 2019-12-26 一种基于current-ellipse模型的车辆动态跟踪方法 Active CN111060944B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911367763.9A CN111060944B (zh) 2019-12-26 2019-12-26 一种基于current-ellipse模型的车辆动态跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911367763.9A CN111060944B (zh) 2019-12-26 2019-12-26 一种基于current-ellipse模型的车辆动态跟踪方法

Publications (2)

Publication Number Publication Date
CN111060944A CN111060944A (zh) 2020-04-24
CN111060944B true CN111060944B (zh) 2021-08-17

Family

ID=70303923

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911367763.9A Active CN111060944B (zh) 2019-12-26 2019-12-26 一种基于current-ellipse模型的车辆动态跟踪方法

Country Status (1)

Country Link
CN (1) CN111060944B (zh)

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103853908B (zh) * 2012-12-04 2017-11-14 中国科学院沈阳自动化研究所 一种自适应交互式多模型的机动目标跟踪方法
CN104574540A (zh) * 2014-12-24 2015-04-29 北京速通科技有限公司 电子不停车收费系统及方法
CN105338497B (zh) * 2015-09-03 2018-07-13 广东机电职业技术学院 一种基于协议跨层优化的车辆定位方法
CN105866812B (zh) * 2016-03-24 2018-11-09 广东机电职业技术学院 一种车辆组合定位算法
CN108897026A (zh) * 2018-08-28 2018-11-27 北京讯腾智慧科技股份有限公司 卫星导航信号受遮挡情况下的合作车辆定位方法及装置

Also Published As

Publication number Publication date
CN111060944A (zh) 2020-04-24

Similar Documents

Publication Publication Date Title
CN111307162B (zh) 用于自动驾驶场景的多传感器融合定位方法
CN110906923B (zh) 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆
CN106840179B (zh) 一种基于多传感器信息融合的智能车定位方法
US20200216076A1 (en) Method for determining the location of an ego-vehicle
Jo et al. GPS-bias correction for precise localization of autonomous vehicles
Tao et al. Lane marking aided vehicle localization
DE102010005293B4 (de) System und Verfahren zur Spurpfadschätzung unter Verwendung einer Sensorvereinigung
JP5162849B2 (ja) 不動点位置記録装置
CN104635233B (zh) 基于车载毫米波雷达的前方物体运动状态估计及分类方法
Barjenbruch et al. Joint spatial-and Doppler-based ego-motion estimation for automotive radars
DE102011120497A1 (de) Systeme und Verfahren zur präzisen Fahrzeugpositionsbestimmung innerhalb einer Fahrspur
EP3907720B1 (en) Own position estimating device, automatic driving system comprising same, and own generated map sharing device
CN112378410B (zh) 车辆行驶盲区校准方法、装置、设备及存储介质
CN111324848A (zh) 移动激光雷达测量系统车载轨迹数据优化方法
CN114999228B (zh) 一种自动驾驶车在恶劣天气防碰撞方法
CN112147651B (zh) 一种异步多车协同目标状态鲁棒估计方法
CN108345021A (zh) 一种多普勒雷达辅助gps/ins车辆测速方法
CN114323050A (zh) 车辆定位方法、装置和电子设备
CN116045972A (zh) 一种基于车辆姿态角的道路坡度估计方法
JP2023164553A (ja) 位置推定装置、推定装置、制御方法、プログラム及び記憶媒体
CN111060944B (zh) 一种基于current-ellipse模型的车辆动态跟踪方法
Alrifaee et al. Satellite-and map-based long range cooperative adaptive cruise control system for road vehicles
Mamchenko et al. Algorithm for sensor data merging using analytical module for priority sensor selection
CN115046547A (zh) 一种复杂城市环境下的自动驾驶汽车定位系统及方法
Niknejad et al. Multi-sensor data fusion for autonomous vehicle navigation and localization through precise map

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