CN113971752A - 一种抗观测数据干扰的多车协同状态估计方法 - Google Patents

一种抗观测数据干扰的多车协同状态估计方法 Download PDF

Info

Publication number
CN113971752A
CN113971752A CN202111100895.2A CN202111100895A CN113971752A CN 113971752 A CN113971752 A CN 113971752A CN 202111100895 A CN202111100895 A CN 202111100895A CN 113971752 A CN113971752 A CN 113971752A
Authority
CN
China
Prior art keywords
vehicle
state
observation
cooperative
noise
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
Application number
CN202111100895.2A
Other languages
English (en)
Other versions
CN113971752B (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.)
Jiangsu University
Original Assignee
Jiangsu University
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 Jiangsu University filed Critical Jiangsu University
Priority to CN202111100895.2A priority Critical patent/CN113971752B/zh
Publication of CN113971752A publication Critical patent/CN113971752A/zh
Application granted granted Critical
Publication of CN113971752B publication Critical patent/CN113971752B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • General Physics & Mathematics (AREA)
  • General Engineering & Computer Science (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Physics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Geometry (AREA)
  • Operations Research (AREA)
  • Probability & Statistics with Applications (AREA)
  • Computer Hardware Design (AREA)
  • Algebra (AREA)
  • Artificial Intelligence (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种抗观测数据干扰的多车协同状态估计方法，主车、协同车分别对目标车进行感知，获得含有异常噪声的目标车状态观测数据。主车作为信息融合中心，对收集到的观测数据进行预处理，得到组合观测数据。基于学生t分布模拟重尾、非高斯的观测噪声可有效抑制异常噪声的影响；初始化状态变量、观测噪声协方差的先验超参数和调节参数；利用变分推断，计算第t次迭代后目标车状态的变分后验分布和观测噪声的变分后验分布；判断目标车状态是否收敛，直至收敛后输出当前时刻k估计的目标车状态；跟踪结束后，输出本次跟踪位置和速度的均方根误差图。本发明的多车协同状态估计方法，通过融入更多观测信息，能有效提高目标车状态估计的精度。

Description

一种抗观测数据干扰的多车协同状态估计方法
技术领域
本发明属于智能车辆技术领域，涉及车联网的多车协同目标车跟踪方法，尤其是一种抗观测数据干扰的多车协同目标车跟踪方法。
背景技术
周边车辆运动状态的准确估计是汽车辅助驾驶系统研究领域的关键技术之一，其关键在于如何有效地从观测数据中过滤噪声以提取目标车的真实运动状态。随着传感器技术与通信技术的发展，多传感器被引进到辅助驾驶系统中进行环境感知。多源信息融合技术不仅能够解决不同传感器所获得的信息融合问题，还能改善状态估计性能。目前，主要利用GPS/导航、摄像头和雷达等多种传感设备来估计道路车辆的运动状态。
非线性最优滤波被广泛应用于多源信息融合，可以由递推贝叶斯方法统一描述。核心思想是基于获得的观测值计算非线性系统状态向量的后验概率密度函数。对于线性系统来说，最优贝叶斯滤波的闭合解由卡尔曼滤波方程描述；对于非线性系统，由于需要处理复杂的积分问题，想要获得精准的最优滤波闭合解非常困难。
随着车联网及通信技术的快速发展，基于车-车(V2V)或车-基础设施(V2I)通信的网联式协同感知引起广泛关注。我国主推的C-V2X成为全球范围内车联网行业的事实标准，其低延迟、高带宽等优点能够满足多车协同感知的可靠性要求。
实际道路交通环境下，单车跟踪易受感知信息不完整、目标车遮挡以及环境噪声的影响，导致目标车状态估计的误差较大。
发明内容
为了解决传统目标跟踪只依赖单个车载传感器观测数据难以准确估计车辆状态的问题，本发明提出了一种抗观测数据干扰的多车协同状态估计方法。利用学生t分布对观测噪声建模，以缓解异常数据对目标状态估计的影响，并结合多车对同一目标车的观测数据，基于变分贝叶斯推断的方法，估计观测噪声参数和超参数，同时结合扩展卡尔曼滤波估计出目标车状态。
为了实现上述技术问题，本发明所采用的技术方案如下：
一种抗观测数据干扰的多车协同状态估计方法，包括如下步骤：
步骤1：主车、协同车按照各自频率分别对目标车进行感知，继而分别获得含有噪声的目标车状态观测数据；同时，协同车将协同车的自车状态以及协同车所感知的目标车状态观测数据发送给主车；
步骤2：主车作为信息融合中心，将主车收集到的目标车状态观测数据以及协同车发送的目标车状态观测数据进行时间同步和坐标转换处理，得到经过预处理后的组合观测数据；为抑制异常噪声对观测数据的影响，基于学生t分布模拟重尾、非高斯的观测噪声；
步骤3：在k＝0时刻，初始化状态变量
Figure BDA0003270690160000021
表示x0服从均值m0和协方差P0的高斯分布；初始化观测噪声协方差(u0Λ0)-1的先验超参数
Figure BDA0003270690160000022
其中，u0，Λ0分别为初始自举变量值、初始精确度值，
Figure BDA0003270690160000023
均为初始先验超参数；
步骤4：利用扩展卡尔曼滤波器，预测k时刻的目标车状态分布
Figure BDA0003270690160000024
表示xk|k-1服从均值
Figure BDA0003270690160000025
协方差
Figure BDA0003270690160000026
的高斯分布；预测k时刻观测噪声协方差(uk，Λk)-1的先验超参数
Figure BDA0003270690160000027
和
Figure BDA0003270690160000028
步骤5：变分内循环：利用变分贝叶斯推断理论，计算第t次迭代后的目标后验状态，经过变分推断的均值和协方差为
Figure BDA0003270690160000029
和
Figure BDA00032706901600000210
计算第t次迭代后的观测噪声参数的后验分布，经过变分推断得到精确度、自举变量和自由度分别为
Figure BDA00032706901600000211
和
Figure BDA00032706901600000212
并计算期望<(ukΛk)-1>t；
步骤6：判断目标车状态是否收敛，如果收敛则进行下一步骤，否则，令迭代次数t←t+1,返回步骤5继续迭代，直至收敛；收敛的判断标准为：如果两次迭代状态的相对变化量ε小于阈值G，则终止迭代；
步骤7：输出每个时刻估计的目标车状态
Figure BDA00032706901600000213
步骤8：选择是否继续跟踪，是则返回步骤1继续跟踪，否则结束跟踪；跟踪结束后，输出本次跟踪位置和速度的均方根误差图。
进一步，所述步骤2中基于学生t分布模拟重尾、非高斯的观测噪声的方法为：
S1、分别构造主车对协同车和目标车的观测方程
Figure BDA00032706901600000214
和协同车对目标车的观测方程
Figure BDA00032706901600000215
S2、通过组合主车对协同车和目标车的观测结果和协同车对目标车的观测结果得到增广观测向量
Figure BDA00032706901600000216
进而获得增广观测噪声
Figure BDA00032706901600000217
和增广观测噪声
Figure BDA00032706901600000218
H、Jhk分别是主车传感器的观测矩阵和协同车传感器的雅可比观测矩阵，
Figure BDA00032706901600000219
分别是主车传感器的观测噪声和协同车传感器的观测噪声；
S3、以学生t分布模拟观测噪声vk，概率密度函数表示为：
Figure BDA0003270690160000031
其中，S(·)表示学生t分布，d为状态变量维度，v为d维随机向量，且服从学生t分布，μ为均值，Λ为精确度，λ为自由度；
将学生t分布看作多个高斯分布的组合，表示为：
Figure BDA0003270690160000032
其中，
Figure BDA0003270690160000033
表示高斯分布，Gamma(·)表示伽马分布，u为自举变量；
最终协同跟踪的增广观测模型可以表示为：
Figure BDA0003270690160000034
协同跟踪的增广观测噪声可以表示为：vk～S(vk；μk，Λk，λk)；其中，μk，Λk，λk分别为k时刻的均值，k时刻的精确度和k时刻的自由度；
进一步，步骤4中计算均值
Figure BDA0003270690160000035
和协方差
Figure BDA0003270690160000036
的方法为：
设置多车协同状态方程xk＝Fxk-1+wk和具体相关参数，F是状态转移矩阵，wk是过程噪声，xk是系统状态向量；
基于多车协同状态方程得预测状态和协方差，表示为：
Figure BDA0003270690160000037
其中，
Figure BDA0003270690160000038
mk-1分别为状态一步预测和上一时刻更新后的目标状态，
Figure BDA0003270690160000039
Pk-1分别为协方差矩阵一步预测和上一时刻更新后的协方差矩阵；Q是过程噪声协方差。
进一步，步骤4中计算先验超参数
Figure BDA00032706901600000310
和
Figure BDA00032706901600000311
的方法为：引入调节因子ρ∈(0，1)反映噪声统计特性的波动特性，设置观测噪声协方差分布超参数的先验分布为：
Figure BDA00032706901600000312
其中，
Figure BDA00032706901600000313
和
Figure BDA00032706901600000314
均为上一时刻更新后的先验参数。
进一步，所述步骤5包括如下步骤：
步骤5.1更新k时刻第t次变分迭代的状态和协方差：
Figure BDA00032706901600000315
Figure BDA00032706901600000316
其中，雅可比观测矩阵表示为：
Figure BDA0003270690160000041
Figure BDA0003270690160000042
θ为协同车相对于主车坐标系的航向角；
步骤5.2更新k时刻第t次变分循环内超参数：
Figure BDA0003270690160000043
Figure BDA0003270690160000044
Figure BDA0003270690160000045
其中，
Figure BDA0003270690160000046
分别是更新后的先验分布超参数。
进一步，协同车通过V2V通信技术将自车状态以及协同车所感知的目标车状态观测数据发送给主车。
进一步，所述步骤2中对主车收集到的目标车状态观测数据以及协同车发送的目标车状态观测数据进行时间同步和坐标转换处理的方法为：
时间同步：将主车以及协同车所采集的目标车状态观测数据的采集时间点分别与融合中心调用的时间点对齐；利用调用时间与传感器信息接收时间的时间差进行目标状态线性插值；分别获得主车对目标车的观测位置
Figure BDA0003270690160000047
主车对协同车的观测位置
Figure BDA0003270690160000048
和协同车对目标车的观测位置
Figure BDA0003270690160000049
进一步，坐标转换：基于协同车相对于主车坐标系的航向角θk，协同车对目标车的观测位置涉及到的坐标转换，表示为：
Figure BDA0003270690160000051
其中，
Figure BDA0003270690160000052
分别表示在主车坐标系下，目标车和协同车的真实位置。
进一步，两次迭代状态的相对变化量ε表示为：
Figure BDA0003270690160000053
其中，
Figure BDA0003270690160000054
mk-1分别为状态一步预测和上一时刻更新后的目标状态，||*||2表示向量的L2范数。
进一步，两次迭代状态的相对变化量的阈值取G＝5×10-6。
本发明的有益效果：
本发明所提出一种抗观测噪声干扰的多车协同目标车状态估计方法，利用主车和协同车同时对目标车进行观测，协同车利用V2V通信技术将自车状态以及对目标车的观测数据发送给主车，主车作为信息融合中心将接收的数据与局部感知数据进行融合，以提高目标车运动状态的估计性能，具有重要的应用前景。本方法考虑到真实驾驶场景的非线性、非高斯和高度不确定性，为克服单个车载传感器感知信息不准确，造成的目标车状态估计误差较大的问题，本发明提出多车协同状态估计方法，通过融入更多观测信息，可以有效提高目标车状态估计的精度。
附图说明
图1为本发明的流程图；
图2为本发明模型的层次贝叶斯结构示意图；
图3中，3a为本发明场景图和位置、3b速度的均方根误差图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，并不用于限定本发明。
如图1所示本申请公开了一种抗观测数据干扰的多车协同状态估计方法，包括如下步骤：
步骤1：主车、协同车按照各自频率分别对目标车进行感知，继而分别获得含有噪声的目标车状态观测数据；同时，协同车通过V2V通信技术将协同车的自车状态以及协同车所感知的目标车状态观测数据发送给主车。
步骤2：主车作为信息融合中心，将主车收集到的目标车状态观测数据以及协同车发送的目标车状态观测数据进行时间同步和坐标转换处理，得到经过预处理后的组合观测数据。为抑制异常噪声对观测数据的影响，基于学生t分布模拟重尾、非高斯的观测噪声。
具体实施步骤如下：
步骤2.1：时间同步：由于主车和协同车车载传感器采集信息频率不一致，导致时间不同步，且V2V通信可能会存在延迟，所以需要对多源观测数据实现时间同步。
将多传感器的目标车状态观测数据采集时间点与融合中心调用的时间点对齐。利用调用时间与传感器信息接收时间的时间差进行目标状态线性插值。线性插值是针对一维数据的插值方法，根据一维数据序列中需要插值的点的左右临近两个数据来进行数值估计。
以主车车载传感器观测数据为例，平台调用时刻k，其左右临近时刻的数据为
Figure BDA0003270690160000061
分别表示车载传感器设备在时刻k0、k1采集到的在x，y方向上的目标车观测位置。
插值公式如下：
Figure BDA0003270690160000062
其中，
Figure BDA0003270690160000063
表示经过插值处理后，在平台调用时刻k，主车对目标车的观测位置。
协同车通过V2V向主车传递的观测数据的处理同上，即可获得在平台调用时刻k，主车对协同车的观测位置
Figure BDA0003270690160000064
和协同车对目标车的观测位置
Figure BDA0003270690160000065
步骤2.2：坐标转换：经过时间配准后，由于
Figure BDA0003270690160000066
是基于协同车自车坐标系下，对目标车的观测，与融合中心的坐标系不一致，因此协同车对目标车的观测位置
Figure BDA0003270690160000067
涉及到坐标转换，表述如下：
Figure BDA0003270690160000068
其中，θk表示协同车相对于主车坐标系的航向角，
Figure BDA0003270690160000069
分别表示在主车坐标系下，目标车和协同车的真实位置。
步骤2.3：基于上述思想，建立对应的非线性、动态的离散系统。该系统包含两个方程：状态方程和观测方程。
多车协同状态方程：
xk＝Fxk-1+wk (3)
其中，
Figure BDA0003270690160000071
为状态转移矩阵，
Figure BDA0003270690160000072
为系统噪声，服从均值为零、协方差为Q的高斯分布，
Figure BDA0003270690160000073
为系统状态向量。
本实施例中，n＝10，
Figure BDA0003270690160000074
Figure BDA0003270690160000075
为协同车在主车坐标系下的运动状态，分别为位置和速度在x，y方向上的分量；
Figure BDA0003270690160000076
为目标车在主车坐标系下的运动状态，分别为位置和速度在x，y方向上的分量；
Figure BDA0003270690160000077
表示协同车相对于主车坐标系的航向角和角速度，两者线性关系为θk＝θk-1+ωkT，T为传感器的采样时间间隔。
主车对协同车和目标车的观测方程：
Figure BDA0003270690160000078
其中，
Figure BDA0003270690160000079
为主车传感器在时间k对协同车和目标车的观测值，H为观测矩阵，
Figure BDA00032706901600000710
为主车传感器的观测噪声。
协同车对目标车的观测方程：
Figure BDA00032706901600000711
其中，
Figure BDA00032706901600000712
为协同车传感器在时间k对目标车的观测值，
Figure BDA00032706901600000713
为观测函数，
Figure BDA00032706901600000714
为协同车传感器的观测噪声。由于
Figure BDA00032706901600000715
是非线性观测系统，使用一阶泰勒公式展开式对非线性观测函数
Figure BDA00032706901600000716
进行线性化处理，求偏导得到雅可比观测矩阵Jhk。
通过组合上述观测结果(即
Figure BDA00032706901600000717
和
Figure BDA00032706901600000718
)，可得增广观测向量
Figure BDA00032706901600000719
同理，可得增广观测噪声
Figure BDA00032706901600000720
增广观测噪声
Figure BDA00032706901600000721
如图2以学生t分布模拟观测噪声vk，使用变分贝叶斯框架和扩展卡尔曼滤波(EKF)算法解决状态估计问题。相较于高斯分布，学生t分布具有重尾、非高斯的特性，对异常值的抗干扰能力较强，其概率密度函数为：
Figure BDA00032706901600000722
其中，S(·)表示学生t分布，d为状态变量维度，v为d维随机向量，且服从学生t分布，μ为均值，Λ为精确度，λ为自由度。学生t分布尾的厚度由λ决定。λ→∞时，收敛到高斯分布。由于学生t分布的最大似然估计很难得到闭式解，通常将学生t分布看作多个高斯分布的组合：
Figure BDA0003270690160000081
其中，
Figure BDA0003270690160000082
表示高斯分布，Gamma(·)表示伽马分布，u为自举变量。
最终协同跟踪的增广观测模型可以表示为：
Figure BDA0003270690160000083
协同跟踪的增广观测噪声可以表示为：vk～S(vk；μk，Λk，λk) (9)
其中，μk，Λk，λk分别为k时刻的均值，精确度和自由度
本实施例中，
Figure BDA0003270690160000084
其中,
Figure BDA0003270690160000085
和
Figure BDA0003270690160000086
分别表示协同车在主车坐标系下的位置、主车对目标车的观测位置和协同车对目标车的观测位置。
使用学生t分布来模拟重尾、非高斯的测量噪声分布，则模型中未知参数的似然函数可表示为：
Figure BDA0003270690160000087
根据公式(7)，引入自举变量u，公式(10)可以拆分为：
Figure BDA0003270690160000088
其中，学生t分布中精确度、自由度两参数分别服从不同超参数的伽马先验分布：
Figure BDA0003270690160000089
其中d表示传感器的观测维度，
Figure BDA00032706901600000810
为模型超参数。
步骤3：在k＝0时刻，初始化状态变量
Figure BDA00032706901600000811
表示x0服从均值m0，协方差P0的高斯分布；初始化观测噪声协方差(u0Λ0)-1的先验超参数
Figure BDA00032706901600000812
本实施例中，m0＝[10 10 1 1 15 20 1 1 0 0]T，P0＝diag([2 2 5 5 2 2 5 50.01 0.01])；d＝6，超参数分别为
Figure BDA00032706901600000813
步骤4：时间外循环：利用扩展卡尔曼滤波器，预测k(k≥1)时刻的目标车状态分布
Figure BDA00032706901600000814
表示xk|k-1服从均值
Figure BDA00032706901600000815
协方差
Figure BDA00032706901600000816
的高斯分布；预测k时刻观测噪声协方差(ukΛk)-1的先验超参数
Figure BDA00032706901600000817
和
Figure BDA00032706901600000818
实验仿真分析，设置多目标场景系统状态方程和相关参数。具体步骤如下：
状态方程xk＝Fxk-1+wk：
Figure BDA0003270690160000091
其中，在本实施例中，T＝0.1s。过程噪声
Figure BDA0003270690160000092
其中
Figure BDA0003270690160000093
U1＝10-4×I8×8，U2＝10-4×I2×2，所以
Figure BDA0003270690160000094
Q2＝L2×U2×L2 T，
Figure BDA0003270690160000095
符号表示克罗内克积。
观测方程
Figure BDA0003270690160000096
Figure BDA0003270690160000097
雅可比观测矩阵Jhk：
Figure BDA0003270690160000098
其中，
Figure BDA0003270690160000099
根据公式(3)，可得预测状态和协方差。
Figure BDA0003270690160000101
其中，
Figure BDA0003270690160000102
mk-1分别为状态一步预测和上一时刻更新后的目标状态，
Figure BDA0003270690160000103
Pk-1分别为协方差矩阵一步预测和上一时刻更新后的协方差矩阵。
考虑未知观测噪声参数时变性，引入调节因子ρ∈(0，1)反映噪声统计特性的波动特性。ρ越趋近于0表示噪声的不稳定性越明显，在本实例中ρ＝0.9。设置观测噪声协方差分布超参数的先验分布为：
Figure BDA0003270690160000104
步骤5：变分内循环：利用变分贝叶斯推断理论，计算第t次迭代后的目标后验状态，经过变分推断的均值和协方差为
Figure BDA0003270690160000105
和
Figure BDA0003270690160000106
计算第t次迭代后的观测噪声参数的后验分布，经过变分推断得到精确度、自举变量和自由度为
Figure BDA0003270690160000107
和
Figure BDA0003270690160000108
并计算期望<(ukΛk)-1>t。
变分贝叶斯推断理论具体步骤如下：
贝叶斯框架下，由观测数据推理隐变量的后验分布公式为：
Figure BDA0003270690160000109
其中，未知参数为Φk＝{xk，uk，Λk，λk}。因为超参数Δk是Λk，λk的先验，于是后验分布可被改写为：
Figure BDA00032706901600001010
采用变分推断思想，利用变分分布q(Φk)近似复杂的后验p(Φk|zk，Δk)。为简单计算，将边缘似然函数先对数化：
Figure BDA00032706901600001011
等式两边以分布q(Φk)求取期望得到下式：
Figure BDA00032706901600001012
其中，KL散度是一种量化两种概率分布q(Φk)和p(Φk|zk，Δk)之间差异的方式。因为KL散度非负，当KL散度达到最小值0时，q(Φk)等于真实后验分布p(Φk|zk，Δk)，自由能量F(q(Φk))达到最大值。
假设q(Φk)＝q(xk，uk，Λk，λk)＝q(xk)q(uk)q(Λk)q(λk) (23)
其中，噪声参数分布先验为：
Figure BDA0003270690160000111
每一个状态参数彼此独立。对F(q(Φk))进行求导，并且令导数为零，得到
Figure BDA0003270690160000112
通解为：
Figure BDA0003270690160000113
其中，
Figure BDA0003270690160000114
符号表示联合概率密度关于除了
Figure BDA0003270690160000115
之外的所有参数期望。
模型的联合概率分布为：
Figure BDA0003270690160000116
依据扩展卡尔曼滤波，更新k时刻第t次变分迭代的状态和协方差：
Figure BDA0003270690160000117
Figure BDA0003270690160000118
根据公式(25)，参数Λk的后验分布为：
Figure BDA0003270690160000119
结合公式(24)，更新k时刻第t次变分循环内超参数：
Figure BDA00032706901600001110
所以，
Figure BDA00032706901600001111
根据公式(25)，参数uk的后验分布为：
Figure BDA00032706901600001112
结合公式(24)，更新k时刻第t次变分循环内超参数：
Figure BDA00032706901600001113
所以，
Figure BDA00032706901600001114
根据公式(25)，参数λk的后验分布为：
Figure BDA0003270690160000121
结合公式(24)，更新k时刻第t次变分循环内超参数：
Figure BDA0003270690160000122
所以，
Figure BDA0003270690160000123
步骤6：判断目标车状态是否收敛，如果收敛则进行下一步骤，否则，令t←t+1,返回步骤5继续迭代，直至收敛。收敛的判断标准为：如果两次迭代状态的相对变化量ε小于阈值G，则终止迭代。
Figure BDA0003270690160000124
其中，||*||2表示向量的L2范数，本实施例中，G＝5×10-6。
步骤7：输出每个时刻估计的目标车状态
Figure BDA0003270690160000125
步骤8：选择是否继续跟踪，是则返回步骤1继续跟踪，否则结束跟踪。跟踪结束后，输出本次跟踪位置和速度的均方根误差图，如图3a和3b所示。
以上实施例仅用于说明本发明的设计思想和特点，其目的在于使本领域内的技术人员能够了解本发明的内容并据以实施，本发明的保护范围不限于上述实施例。所以，凡依据本发明所揭示的原理、设计思路所作的等同变化或修饰，均在本发明的保护范围之内。

Claims (10)

1.一种抗观测数据干扰的多车协同状态估计方法，其特征在于，包括如下步骤：
步骤1：主车、协同车按照各自频率分别对目标车进行感知，继而分别获得含有噪声的目标车状态观测数据；同时，协同车将协同车的自车状态以及协同车所感知的目标车状态观测数据发送给主车；
步骤2：主车作为信息融合中心，将主车收集到的目标车状态观测数据以及协同车发送的目标车状态观测数据进行时间同步和坐标转换处理，得到经过预处理后的组合观测数据；为抑制异常噪声对观测数据的影响，基于学生t分布模拟重尾、非高斯的观测噪声；
步骤3：在k＝0时刻，初始化状态变量
Figure FDA0003270690150000011
表示x0服从均值m0和协方差P0的高斯分布；初始化观测噪声协方差(u0Λ0)-1的先验超参数
Figure FDA0003270690150000012
其中，u0，Λ0分别为初始自举变量值、初始精确度值，
Figure FDA0003270690150000013
均为初始先验超参数；
步骤4：利用扩展卡尔曼滤波器，预测k时刻的目标车状态分布
Figure FDA0003270690150000014
表示xk|k-1服从均值
Figure FDA0003270690150000015
协方差
Figure FDA0003270690150000016
的高斯分布；预测k时刻观测噪声协方差(ukΛk)-1的先验超参数
Figure FDA0003270690150000017
和
Figure FDA0003270690150000018
k≥1；
步骤5：变分内循环：利用变分贝叶斯推断理论，计算第t次迭代后的目标后验状态，经过变分推断的均值和协方差为
Figure FDA0003270690150000019
和
Figure FDA00032706901500000110
计算第t次迭代后的观测噪声参数的后验分布，经过变分推断得到精确度、自举变量和自由度分别为
Figure FDA00032706901500000111
和
Figure FDA00032706901500000112
并计算期望<(ukΛk)-1>t；
步骤6：判断目标车状态是否收敛，如果收敛则进行下一步骤，否则，令迭代次数t←t+1，返回步骤5继续迭代，直至收敛；收敛的判断标准为：如果两次迭代状态的相对变化量ε小于阈值G，则终止迭代；
步骤7：输出每个时刻估计的目标车状态
Figure FDA00032706901500000113
步骤8：选择是否继续跟踪，是则返回步骤1继续跟踪，否则结束跟踪；跟踪结束后，输出本次跟踪位置和速度的均方根误差图。
2.根据权利要求1所述的一种抗观测数据干扰的多车协同状态估计方法，其特征在于，所述步骤2中基于学生t分布模拟重尾、非高斯的观测噪声的方法为：
S1、分别构造主车对协同车和目标车的观测方程
Figure FDA00032706901500000114
和协同车对目标车的观测方程
Figure FDA00032706901500000115
S2、通过组合主车对协同车和目标车的观测结果和协同车对目标车的观测结果得到增广观测向量
Figure FDA0003270690150000021
进而获得增广观测噪声
Figure FDA0003270690150000022
和增广观测噪声
Figure FDA0003270690150000023
H、Jhk分别是主车传感器的观测矩阵和协同车传感器的雅可比观测矩阵，
Figure FDA0003270690150000024
分别是主车传感器的观测噪声和协同车传感器的观测噪声；
S3、以学生t分布模拟观测噪声vk，概率密度函数表示为：
Figure FDA0003270690150000025
其中，S(·)表示学生t分布，d为状态变量维度，v为d维随机向量，且服从学生t分布，μ为均值，Λ为精确度，λ为自由度；
将学生t分布看作多个高斯分布的组合，表示为：
Figure FDA0003270690150000026
其中，
Figure FDA0003270690150000027
表示高斯分布，Gamma(·)表示伽马分布，u为自举变量；
最终协同跟踪的增广观测模型可以表示为：
Figure FDA0003270690150000028
协同跟踪的增广观测噪声可以表示为：vk～S(vk；μk，Λk，λk)；其中，μk，Λk，λk分别为k时刻的均值，k时刻的精确度和k时刻的自由度。
3.根据权利要求1所述的一种抗观测数据干扰的多车协同状态估计方法，其特征在于，步骤4中计算均值
Figure FDA0003270690150000029
和协方差
Figure FDA00032706901500000210
的方法为：
设置多车协同状态方程xk＝Fxk-1+wk和具体相关参数，F是状态转移矩阵，wk是过程噪声，xk是系统状态向量；
基于多车协同状态方程得预测状态和协方差，表示为：
Figure FDA00032706901500000211
其中，
Figure FDA00032706901500000212
分别为状态一步预测和上一时刻更新后的目标状态，
Figure FDA00032706901500000213
分别为协方差矩阵一步预测和上一时刻更新后的协方差矩阵；Q是过程噪声协方差。
4.根据权利要求1所述的一种抗观测数据干扰的多车协同状态估计方法，其特征在于，步骤4中计算先验超参数
Figure FDA00032706901500000214
和
Figure FDA00032706901500000215
的方法为：引入调节因子ρ∈(0，1)反映噪声统计特性的波动特性，设置观测噪声协方差分布超参数的先验分布为：
Figure FDA00032706901500000216
其中，
Figure FDA0003270690150000031
和
Figure FDA0003270690150000032
均为上一时刻更新后的先验参数。
5.根据权利要求4所述的一种抗观测数据干扰的多车协同状态估计方法，其特征在于，所述步骤5包括如下步骤：
步骤5.1更新k时刻第t次变分迭代的状态和协方差：
Figure FDA0003270690150000033
Figure FDA0003270690150000034
其中，雅可比观测矩阵表示为：
Figure FDA0003270690150000035
Figure FDA0003270690150000036
θ为协同车相对于主车坐标系的航向角；
步骤5.2更新k时刻第t次变分循环内超参数：
Figure FDA0003270690150000037
Figure FDA0003270690150000038
Figure FDA0003270690150000039
其中，
Figure FDA00032706901500000310
分别是更新后的先验分布超参数。
6.根据权利要求4所述的一种抗观测数据干扰的多车协同状态估计方法，其特征在于，协同车通过V2V通信技术将自车状态以及协同车所感知的目标车状态观测数据发送给主车。
7.根据权利要求4所述的一种抗观测数据干扰的多车协同状态估计方法，其特征在于，所述步骤2中对主车收集到的目标车状态观测数据以及协同车发送的目标车状态观测数据进行时间同步和坐标转换处理的方法为：
时间同步：将主车以及协同车所采集的目标车状态观测数据的采集时间点分别与融合中心调用的时间点对齐；利用调用时间与传感器信息接收时间的时间差进行目标状态线性插值；分别获得主车对目标车的观测位置
Figure FDA0003270690150000041
主车对协同车的观测位置
Figure FDA0003270690150000042
和协同车对目标车的观测位置
Figure FDA0003270690150000043
8.根据权利要求7所述的一种抗观测数据干扰的多车协同状态估计方法，其特征在于，坐标转换：基于协同车相对于主车坐标系的航向角θk，协同车对目标车的观测位置涉及到的坐标转换，表示为：
Figure FDA0003270690150000044
其中，
Figure FDA0003270690150000045
分别表示在主车坐标系下，目标车和协同车的真实位置。
9.根据权利要求1所述的一种抗观测数据干扰的多车协同状态估计方法，其特征在于，两次迭代状态的相对变化量ε表示为：
Figure FDA0003270690150000046
其中，
Figure FDA0003270690150000047
分别为状态一步预测和上一时刻更新后的目标状态，||*||2表示向量的L2范数。
10.根据权利要求1所述的一种抗观测数据干扰的多车协同状态估计方法，其特征在于，两次迭代状态的相对变化量的阈值取G＝5×10-6。
CN202111100895.2A 2021-09-18 2021-09-18 一种抗观测数据干扰的多车协同状态估计方法 Active CN113971752B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111100895.2A CN113971752B (zh) 2021-09-18 2021-09-18 一种抗观测数据干扰的多车协同状态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111100895.2A CN113971752B (zh) 2021-09-18 2021-09-18 一种抗观测数据干扰的多车协同状态估计方法

Publications (2)

Publication Number Publication Date
CN113971752A true CN113971752A (zh) 2022-01-25
CN113971752B CN113971752B (zh) 2024-03-19

Family

ID=79586670

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111100895.2A Active CN113971752B (zh) 2021-09-18 2021-09-18 一种抗观测数据干扰的多车协同状态估计方法

Country Status (1)

Country Link
CN (1) CN113971752B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108763167A (zh) * 2018-05-07 2018-11-06 西北工业大学 一种变分贝叶斯的自适应滤波方法
CN109508445A (zh) * 2019-01-14 2019-03-22 哈尔滨工程大学 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法
CN111178385A (zh) * 2019-12-02 2020-05-19 江苏大学 一种鲁棒在线多传感器融合的目标跟踪方法
CN111985093A (zh) * 2020-08-03 2020-11-24 哈尔滨工程大学 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108763167A (zh) * 2018-05-07 2018-11-06 西北工业大学 一种变分贝叶斯的自适应滤波方法
CN109508445A (zh) * 2019-01-14 2019-03-22 哈尔滨工程大学 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法
CN111178385A (zh) * 2019-12-02 2020-05-19 江苏大学 一种鲁棒在线多传感器融合的目标跟踪方法
CN111985093A (zh) * 2020-08-03 2020-11-24 哈尔滨工程大学 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法

Also Published As

Publication number Publication date
CN113971752B (zh) 2024-03-19

Similar Documents

Publication Publication Date Title
CN105737832A (zh) 基于全局最优数据融合的分布式slam方法
CN109241674B (zh) 一种智能网联车辆编队的多时延稳定性分析方法
CN105467382A (zh) 基于svm的多传感器的目标跟踪数据融合算法及其系统
CN111862165B (zh) 一种基于深度强化学习更新卡尔曼滤波器的目标追踪方法
CN107123265B (zh) 一种基于并行计算的高速公路交通状态估计方法
CN111291471B (zh) 一种基于l1正则无迹变换的约束多模型滤波方法
Zhang et al. An enhanced adaptive unscented kalman filter for vehicle state estimation
CN109782269B (zh) 一种分布式多平台协同有源目标跟踪方法
CN108871365B (zh) 一种航向约束下的状态估计方法及系统
Malleswaran et al. IMM-UKF-TFS model-based approach for intelligent navigation
Fényes et al. Side-slip angle estimation of autonomous road vehicles based on big data analysis
CN113950018A (zh) 一种异步多传感器网络系统及全局椭球状态估计方法
CN105373805A (zh) 一种基于最大熵准则的多传感器机动目标跟踪方法
Ji et al. Data-driven distributed information-weighted consensus filtering in discrete-time sensor networks with switching topologies
CN113971752A (zh) 一种抗观测数据干扰的多车协同状态估计方法
He et al. Multi-sensor fusion tracking algorithm by square root cubature kalman filter for intelligent vehicle
Chen et al. Research on adaptive Monte Carlo location method based on fusion posture estimation
CN105095153A (zh) 一种广义数据融合方法
CN115114985A (zh) 一种基于集合理论的传感器系统分布式融合方法
Ren et al. New terminal sliding mode consensus algorithm for disturbed second-order multi-agent systems
Tang et al. A game-theory based UKF algorithm for multi-robots cooperative localization
Zhao Covariance intersection fusion estimator for nonlinear system with correlated noise
CN107590509A (zh) 基于最大期望近似的切尔诺夫融合方法
CN112836354B (zh) 一种目标跟踪定位方法、系统、装置及可读存储介质
Yu et al. An improved Gaussian filter with asynchronously correlated noises

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