CN110572139A - 用于车辆状态估计的融合滤波实现方法以及装置、存储介质、车辆 - Google Patents

用于车辆状态估计的融合滤波实现方法以及装置、存储介质、车辆 Download PDF

Info

Publication number
CN110572139A
CN110572139A CN201910762909.3A CN201910762909A CN110572139A CN 110572139 A CN110572139 A CN 110572139A CN 201910762909 A CN201910762909 A CN 201910762909A CN 110572139 A CN110572139 A CN 110572139A
Authority
CN
China
Prior art keywords
vehicle
extended
state estimation
filter
ufir
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
CN201910762909.3A
Other languages
English (en)
Other versions
CN110572139B (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.)
Shanghai Zhijia Technology Co.,Ltd.
Original Assignee
Shanghai Zhi Jia Automobile Technology Co Ltd
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 Shanghai Zhi Jia Automobile Technology Co Ltd filed Critical Shanghai Zhi Jia Automobile Technology Co Ltd
Priority to CN201910762909.3A priority Critical patent/CN110572139B/zh
Publication of CN110572139A publication Critical patent/CN110572139A/zh
Application granted granted Critical
Publication of CN110572139B publication Critical patent/CN110572139B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H17/00—Networks using digital techniques
    • H03H17/02—Frequency selective networks
    • H03H17/0248—Filters characterised by a particular frequency response or filtering method
    • H03H17/0255—Filters based on statistics
    • H03H17/0257—KALMAN filters

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Mathematical Physics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Algebra (AREA)
  • Probability & Statistics with Applications (AREA)
  • Computer Hardware Design (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Traffic Control Systems (AREA)
  • Navigation (AREA)

Abstract

本申请公开了一种用于车辆状态估计的融合滤波实现方法以及装置、存储介质、车辆。该方法包括建立非线性车辆运动模型；根据所述非线性车辆运动学模型计算扩展卡尔曼滤波器和扩展UFIR滤波器的计算参数；根据噪声信号的统计特性，对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器进行融合滤波，并在n≥N以后的每个采样周期进行初始化、预测、更新和估计融合的操作阶段。本申请解决了在车辆的状态估计中无法获得较准确且鲁棒性较强的结果的技术问题。

Description

用于车辆状态估计的融合滤波实现方法以及装置、存储介质、 车辆
技术领域
本申请涉及估计算法和信号处理领域领域，具体而言，涉及一种用于车辆状态估计的融合滤波实现方法以及装置、存储介质、车辆。
背景技术
准确的车辆状态估计数据可为网联车辆尤其是自动驾驶车辆提供更为精确的环境感知信息和进行更为精确的路径规划所需信息。
通常由摄像头图像数据估计的车辆状态可以通过车路通信网络传到行驶车辆并用于事故的预测和预警，提高驾驶的安全性。
发明人发现，由摄像头或其他传感器测得的车辆状态(位置)不可避免地要受到噪声的干扰，并且车辆模型的建模误差也带来了过程噪声，从而在车辆的状态估计中无法获得较准确且鲁棒性较强的结果。
针对相关技术中在车辆的状态估计中无法获得较准确且鲁棒性较强的结果的问题，目前尚未提出有效的解决方案。
发明内容
本申请的主要目的在于提供一种用于车辆状态估计的融合滤波实现方法以及装置、存储介质、车辆，以解决在车辆的状态估计中无法获得较准确且鲁棒性较强的结果的问题。
为了实现上述目的，根据本申请的一个方面，提供了一种用于车辆状态估计的融合滤波实现方法。
根据本申请的用于车辆状态估计的融合滤波实现方法包括：建立非线性车辆运动模型；根据所述非线性车辆运动学模型计算扩展卡尔曼滤波器和扩展UFIR滤波器的计算参数；根据噪声信号的统计特性，对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器进行融合滤波，并在n≥N以后的每个采样周期进行如下操作阶段：在初始化阶段设定所述扩展卡尔曼滤波器的状态估计及协方差矩阵；在预测阶段进行状态估计的预测，包括对所述扩展卡尔曼滤波器的预测阶段的预设算法、所述扩展UFIR滤波器的预测阶段的预设算法；在更新阶段根据测量量对车辆状态估计进行更新，包括对所述扩展卡尔曼滤波器的车辆状态估计更新结果和对所述扩展UFIR滤波器的车辆状态估计更新结果；在估计融合阶段对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器得到的状态估计结果进行融合以以更新车辆的状态估计。
进一步地，所述状态估计用于对车辆速度的估计结果或车辆轨迹的估计结果。
进一步地，所述噪声信号的统计特性至少包括：车辆行驶过程中的系统过程噪声的统计特性或者测量噪声的统计特性。
进一步地，方法还包括：从摄像头的监控录像中提取出目标时间信息和所述目标时间信息中带有噪声的车辆位置信息。
进一步地，根据所述非线性车辆运动学模型计算扩展卡尔曼滤波器和扩展UFIR滤波器的计算参数包括：
将所述扩展卡尔曼滤波器和所述扩展UFIR滤波器作为滤波单元，并根据噪声信号的统计特性的变换情况，使用扩展卡尔曼滤波器的结果或扩展UFIR滤波器的结果，确定滤波单元的参数矩阵，并在所述滤波单元中进行预测和更新。
进一步地，建立非线性车辆运动模型包括：
定义车辆模型的状态向量，其中所述状态向量至少包括：车辆的横向位置、车辆的纵向位置、车辆沿行驶方向的速率、车辆行驶方向与横向坐标轴的夹角、车辆的横摆角速度；
在假设车辆的速度大小和横摆角速度恒定时，得到车辆运动模型，其中所述车辆运动模型中确定对于过程噪声的协方差矩阵的计算参数、确定对于对于测量噪声的协方差矩阵的计算参数、确定对于车辆运动模型的初始值中期望和方差的计算参数。
为了实现上述目的，根据本申请的另一个方面，提供了车辆状态估计方法，包括：获取图像并识别车辆位置信息；根据融合滤波单元和所述车辆位置信息对车辆进行状态估计；根据噪声信号统计特性信息不准确程度，调整所述融合滤波单元中扩展UFIR滤波器和扩展卡尔曼滤波器的融合结果占比；其中所述噪声信号至少包括：车辆行驶过程中的系统过程噪声或者设备测量噪声。
为了实现上述目的，根据本申请的又一方面，提供了一种用于车辆状态估计的融合滤波实现装置。
根据本申请的用于车辆状态估计的融合滤波实现装置包括：建立模块，用于建立非线性车辆运动模型；确定模块，用于根据所述非线性车辆运动学模型计算扩展卡尔曼滤波器和扩展UFIR滤波器的计算参数；融合模块，用于根据噪声信号的统计特性，对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器进行融合滤波，并在n≥N以后的每个采样周期进行如下操作阶段：在初始化阶段设定所述扩展卡尔曼滤波器的状态估计及协方差矩阵；在预测阶段进行状态估计的预测，包括对所述扩展卡尔曼滤波器的预测阶段的预设算法、所述扩展UFIR滤波器的预测阶段的预设算法；在更新阶段根据测量量对车辆状态估计进行更新，包括对所述扩展卡尔曼滤波器的车辆状态估计更新结果和对所述扩展UFIR滤波器的车辆状态估计更新结果；在估计融合阶段对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器得到的状态估计结果进行融合以以更新车辆的状态估计。
为了实现上述目的，根据本申请的再一方面，提供了计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现所述的用于车辆状态估计的融合滤波实现方法的步骤。
为了实现上述目的，根据本申请的另一方面，提供了一种自动驾驶车辆，包括：用于车辆状态估计的融合滤波实现装置。
在本申请实施例中，采用建立非线性车辆运动模型的方式，通过根据所述非线性车辆运动学模型计算扩展卡尔曼滤波器和扩展UFIR滤波器的计算参数，达到了根据噪声信号的统计特性，对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器进行融合滤波，并在n≥N以后的每个采样周期进行相应操作阶段的目的，从而实现了基于融合扩展卡尔曼或扩展UFIR滤波器改进的车辆状态估计的技术效果，进而解决了在车辆的状态估计中无法获得较准确且鲁棒性较强的结果的技术问题。
附图说明
构成本申请的一部分的附图用来提供对本申请的进一步理解，使得本申请的其它特征、目的和优点变得更明显。本申请的示意性实施例附图及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1是根据本申请实施例的用于车辆状态估计的融合滤波实现方法流程示意图；
图2是根据本申请实施例的车辆状态估计方法流程示意图；
图3是根据本申请实施例的用于车辆状态估计的融合滤波实现装置结构示意图；
图4是根据本申请实施例的用于车辆状态估计的融合滤波实现方法实现原理流程示意图；
图5是根据本申请实施例的融合扩展卡尔曼/UFIR滤波算法的流程示意图；
图6是根据本申请实施例的融合扩展卡尔曼/UFIR滤波算法实现过程流程示意图。
具体实施方式
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。
需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
在本申请中，术语“上”、“下”、“左”、“右”、“前”、“后”、“顶”、“底”、“内”、“外”、“中”、“竖直”、“水平”、“横向”、“纵向”等指示的方位或位置关系为基于附图所示的方位或位置关系。这些术语主要是为了更好地描述本申请及其实施例，并非用于限定所指示的装置、元件或组成部分必须具有特定方位，或以特定方位进行构造和操作。
并且，上述部分术语除了可以用于表示方位或位置关系以外，还可能用于表示其他含义，例如术语“上”在某些情况下也可能用于表示某种依附关系或连接关系。对于本领域普通技术人员而言，可以根据具体情况理解这些术语在本申请中的具体含义。
此外，术语“安装”、“设置”、“设有”、“连接”、“相连”、“套接”应做广义理解。例如，可以是固定连接，可拆卸连接，或整体式构造；可以是机械连接，或电连接；可以是直接相连，或者是通过中间媒介间接相连，又或者是两个装置、元件或组成部分之间内部的连通。对于本领域普通技术人员而言，可以根据具体情况理解上述术语在本申请中的具体含义。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
如图1所示，该方法包括如下的步骤S102至步骤S114：
步骤S102，建立非线性车辆运动模型；
步骤S104，根据所述非线性车辆运动学模型计算扩展卡尔曼滤波器和扩展UFIR滤波器的计算参数；
步骤S106，根据噪声信号的统计特性，对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器进行融合滤波，并在n≥N以后的每个采样周期进行如下操作阶段：
步骤S1061，在初始化阶段设定所述扩展卡尔曼滤波器的状态估计及协方差矩阵；
步骤S1062，在预测阶段进行状态估计的预测，包括对所述扩展卡尔曼滤波器的预测阶段的预设算法、所述扩展UFIR滤波器的预测阶段的预设算法；
步骤S1063，在更新阶段根据测量量对车辆状态估计进行更新，包括对所述扩展卡尔曼滤波器的车辆状态估计更新结果和对所述扩展UFIR滤波器的车辆状态估计更新结果；
步骤S1064，在估计融合阶段对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器得到的状态估计结果进行融合以以更新车辆的状态估计。
首先建立非线性车辆运动模型，然后根据车辆的运动模型，确定扩展卡尔曼和扩展UFIR滤波器算法及相关参数，最后根据非线性车辆模型、扩展卡尔曼滤波器和扩展UFIR滤波器的参数来确定融合扩展卡尔曼/UFIR滤波器的参数和具体算法。
根据本申请实施例，作为本实施例中的优选，所述状态估计用于对车辆速度的估计结果或车辆轨迹的估计结果。
根据本申请实施例，作为本实施例中的优选，所述噪声信号的统计特性至少包括：车辆行驶过程中的系统过程噪声的统计特性或者测量噪声的统计特性。
具体地，在车辆行驶过程中，为了较为精确的估计车辆状态并进行轨迹追踪，需要较为精确的车辆模型，因而需要考虑采用非线性模型来描述车辆的运动。车辆模型的过程噪声和由传感器产生的测量噪声的统计特性在实际应用中会不断发生变化，从而造成传统的扩展卡尔曼滤波算法会产生较大的估计误差，即鲁棒性较差。这是因为扩展卡尔曼滤波需要较为准确的噪声统计特性信息。在实际应用中，测量噪声统计特性一般可由传感器厂家得到，而过程噪声的统计特性难以在行驶过程中保持不变。因而需要在基于更精确的非线性模型的基础上对扩展卡尔曼滤波算法在车辆状态估计中的应用进行改进。扩展UFIR滤波算法可以对噪声统计特性有很好的鲁棒性，然而相对于扩展卡尔曼滤波而言，不能利用已知的噪声的统计特性来提高估计的准确性。本申请实施例中提出的一种基于融合扩展卡尔曼和扩展UFIR滤波算法改进的车辆状态估计和追踪算法可以在应用中结合扩展卡尔曼滤波和扩展UFIR滤波的优点，达到较好的估计效果。
融合扩展卡尔曼/UFIR滤波器的算法在每一个采样周期的估计分为四个阶段，分别为初始化、预测、更新和估计融合。在此步骤中，考虑在第n步的融合扩展滤波器算法。由于扩展UFIR滤波器从n>N开始定义，在n<N时车辆状态估计只采用扩展卡尔曼滤波器得到的结果。
从以上的描述中，可以看出，本申请实现了如下技术效果：
基于融合扩展卡尔曼/UFIR滤波算法在噪声即包含过程噪声和测量噪声在内的环境中由传感器位置测量信息进行车辆状态估计和轨迹追踪，结合了扩展卡尔曼滤波算法的准确性和扩展UFIR滤波算法的鲁棒性优点，使基于单种扩展滤波算法的估计和追踪性能得到了改进。
优选地，通过结合现有的模型，针对车辆的追踪提出了改进的非线性模型来描述车辆的运动，使模型对车辆运动的描述更加准确，有利于提高状态估计的准确性。
优选地，既考虑了准确的噪声信号统计特性信息对车辆状态估计精度的有利作用，又考虑了噪声信号统计特性变化时估计算法的鲁棒性，因而更适用于实际应用环境中的车辆状态估计和追踪。
根据本申请实施例，作为本实施例中的优选，方法还包括：从摄像头的监控录像中提取出目标时间信息和所述目标时间信息中带有噪声的车辆位置信息。
根据本申请实施例，作为本实施例中的优选，根据所述非线性车辆运动学模型计算扩展卡尔曼滤波器和扩展UFIR滤波器的计算参数包括：
将所述扩展卡尔曼滤波器和所述扩展UFIR滤波器作为滤波单元，并根据噪声信号的统计特性的变换情况，使用扩展卡尔曼滤波器的结果或扩展UFIR滤波器的结果，确定滤波单元的参数矩阵，并在所述滤波单元中进行预测和更新。
具体地，本申请的实施例中设计的车辆状态估计算法，考虑了更能准确描述车辆运动的非线性模型。相关估计算法的目的是在噪声环境中更精确地估计出车辆行驶过程中的状态(包含位置、速度、偏航角及偏航角速度)并进行轨迹追踪。所采用的基于融合扩展卡尔曼滤波/UFIR滤波器改进的算法可以有效的处理非线性模型的估计问题并且能更好的抑制噪声特性变化和非线性带来的对估计的不利影响。应用本发明中的算法，首先需要根据车辆运动模型得到扩展卡尔曼滤波器和扩展UFIR滤波器的算法作为后续步骤融合卡尔曼/UFIR滤波器算法的基础。
根据本申请实施例，作为本实施例中的优选，建立非线性车辆运动模型包括：
定义车辆模型的状态向量，其中所述状态向量至少包括：车辆的横向位置、车辆的纵向位置、车辆沿行驶方向的速率、车辆行驶方向与横向坐标轴的夹角、车辆的横摆角速度；
在假设车辆的速度大小和横摆角速度恒定时，得到车辆运动模型，其中所述车辆运动模型中确定对于过程噪声的协方差矩阵的计算参数、确定对于对于测量噪声的协方差矩阵的计算参数、确定对于车辆运动模型的初始值中期望和方差的计算参数。
具体地，为了更精确的描述车辆的行驶过程，本发明采用非线性车辆运动模型。首先车辆模型的状态向量定义为，
x[k]＝[x[k] y[k] v[k] θ[k] ω[k]]T
其中，x[k]和y[k]是车辆的横向和纵向位置，v[k]是车辆沿行驶方向的速率，θ[k]是车辆行驶方向与横向坐标轴的夹角(逆时针为正)，ω[k]是车辆的横摆角速度。假设车辆的速度大小和横摆角速度恒定，由此可得到车辆运动模型为，
z[k]＝Cx[k]+v[k],
(1)
其中， 中代表车辆的线加速度和加速度(此处假设为过程噪声)，Δt是离散系统的采样时间， 是车辆位置测量的测量噪声向量。
对于过程噪声，其协方差矩阵表示为，
Q＝E{w[k]wT[k]}
其中，
对于测量噪声，其协方差矩阵表示为，
R＝E{v[k]vT[k]}
对于系统的初始值，假设期望和方差为，
P0＝E{x[0]xT[0]}
由于本发明的算法考虑了噪声统计特性的不准确性，因而此处的协方差矩阵不需要特别精确。
如图2所示，在本申请的另一实施例中，还提供了一种车辆状态估计方法，包括：
步骤S202，获取图像并识别车辆位置信息；
步骤S202，根据融合滤波单元和所述车辆位置信息对车辆进行状态估计；
步骤S202，根据噪声信号统计特性信息不准确程度，调整所述融合滤波单元中扩展UFIR滤波器和扩展卡尔曼滤波器的融合结果占比；
其中所述噪声信号至少包括：车辆行驶过程中的系统过程噪声或者设备测量噪声。
具体地，从传感器输出信号中提取出时间信息和相应时间的带有噪声的车辆位置信息。此步骤以摄像头为例进行说明。由摄像头得到的图像数据，需要按系统的采样周期选取相应的各帧图像。下一步，在选出的各帧图像中应用图像处理算法识别出车辆位置测量量。
需要注意的是，现有很多成熟的图像处理算法可以对图像中车辆的位置进行较准确的识别。在本申请中并不进行具体限定，本领域技术人员可以根据情况进行选择。
在本申请的实施例中由设计的融合扩展卡尔曼/UFIR滤波器和识别出的车辆位置测量数据进行车辆状态估计和追踪。基于融合滤波算法和车辆位置测量量，应用融合扩展卡尔曼/UFIR滤波算法实现车辆状态估计和追踪。
需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
根据本申请实施例，还提供了一种用于实施上述方法的用于车辆状态估计的融合滤波实现装置，如图3所示，该装置包括：建立模块10，用于建立非线性车辆运动模型；确定模块20，用于根据所述非线性车辆运动学模型计算扩展卡尔曼滤波器和扩展UFIR滤波器的计算参数；融合模块30，用于根据噪声信号的统计特性，对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器进行融合滤波，并在n≥N以后的每个采样周期进行如下操作阶段：在初始化阶段设定所述扩展卡尔曼滤波器的状态估计及协方差矩阵；在预测阶段进行状态估计的预测，包括对所述扩展卡尔曼滤波器的预测阶段的预设算法、所述扩展UFIR滤波器的预测阶段的预设算法；在更新阶段根据测量量对车辆状态估计进行更新，包括对所述扩展卡尔曼滤波器的车辆状态估计更新结果和对所述扩展UFIR滤波器的车辆状态估计更新结果；在估计融合阶段对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器得到的状态估计结果进行融合以以更新车辆的状态估计。
首先建立非线性车辆运动模型，然后根据车辆的运动模型，确定扩展卡尔曼和扩展UFIR滤波器算法及相关参数，最后根据非线性车辆模型、扩展卡尔曼滤波器和扩展UFIR滤波器的参数来确定融合扩展卡尔曼/UFIR滤波器的参数和具体算法。
根据本申请实施例，作为本实施例中的优选，所述状态估计用于对车辆速度的估计结果或车辆轨迹的估计结果。
根据本申请实施例，作为本实施例中的优选，所述噪声信号的统计特性至少包括：车辆行驶过程中的系统过程噪声的统计特性或者测量噪声的统计特性。
具体地，在车辆行驶过程中，为了较为精确的估计车辆状态并进行轨迹追踪，需要较为精确的车辆模型，因而需要考虑采用非线性模型来描述车辆的运动。车辆模型的过程噪声和由传感器产生的测量噪声的统计特性在实际应用中会不断发生变化，从而造成传统的扩展卡尔曼滤波算法会产生较大的估计误差，即鲁棒性较差。这是因为扩展卡尔曼滤波需要较为准确的噪声统计特性信息。在实际应用中，测量噪声统计特性一般可由传感器厂家得到，而过程噪声的统计特性难以在行驶过程中保持不变。因而需要在基于更精确的非线性模型的基础上对扩展卡尔曼滤波算法在车辆状态估计中的应用进行改进。扩展UFIR滤波算法可以对噪声统计特性有很好的鲁棒性，然而相对于扩展卡尔曼滤波而言，不能利用已知的噪声的统计特性来提高估计的准确性。本申请实施例中提出的一种基于融合扩展卡尔曼和扩展UFIR滤波算法改进的车辆状态估计和追踪算法可以在应用中结合扩展卡尔曼滤波和扩展UFIR滤波的优点，达到较好的估计效果。
融合扩展卡尔曼/UFIR滤波器的算法在每一个采样周期的估计分为四个阶段，分别为初始化、预测、更新和估计融合。在此步骤中，考虑在第n步的融合扩展滤波器算法。由于扩展UFIR滤波器从n>N开始定义，在n<N时车辆状态估计只采用扩展卡尔曼滤波器得到的结果。
在本申请的另一实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现所述的用于车辆状态估计的融合滤波实现方法的步骤。
在本申请的又一实施例中，一种自动驾驶车辆包括：所述的用于车辆状态估计的融合滤波实现装置。
请参考图4至图6，对本申请的实现原理进行详细说明。
由摄像头的图像数据或其他传感器如激光雷达等，测得的车辆状态(位置)不可避免地要受到噪声的干扰，并且车辆模型的建模误差也带来了过程噪声。在实际应用中噪声尤其是过程噪声的特性会随时间和环境而变化，因而很难得到较准确的噪声统计特性。因此需要考虑如何在噪声统计特性不确定或变化的情况下进行较为精确的车辆状态估计。另一方面，更为精确的车辆运动学模型有助于得到更为精确的状态估计结果。因此在常用的线性模型基础上，用更精确的非线性模型来描述被跟踪车辆的运动有利于提高估计结果准确性。现有的适用于非线性模型的随机估计算法有扩展卡尔曼滤波和扩展UFIR滤波算法。这两种算法都有各自的优点和限制。扩展卡尔曼滤波在噪声统计特性(协方差)比较准确的情况下有较好的估计结果。当噪声统计特性难以获得或不断变化的情况下，卡尔曼滤波算法有较大的估计误差，鲁棒性较差。扩展UFIR滤波可以在噪声统计特性未知的情况下进行系统状态的估计。但如果有较准确的噪声统计特性，扩展UFIR滤波的估计效果不如扩展卡尔曼滤波。在车辆状态估计的情形中，噪声信号的统计特性会出现不可预知的变化。在此情形下，可以考虑将扩展卡尔曼滤波和扩展UFIR滤波算法相结合，在噪声信号的统计特性信息较准确时，以扩展卡尔曼滤波的结果占主导地位，当噪声信号统计特性信息不准确时，使扩展UFIR滤波的结果占主导地位。
具体步骤如下：
步骤一、建立非线性车辆运动模型；
步骤二、根据车辆的运动模型，确定扩展卡尔曼和扩展UFIR滤波器算法及相关参数；
步骤三、根据非线性车辆模型、扩展卡尔曼滤波器和扩展UFIR滤波器来确定融合扩展卡尔曼/UFIR滤波器的参数和具体算法。
步骤三、从传感器输出信号中提取出时间信息和相应时间的带有噪声的车辆位置信息；
步骤四、对提取的位置测量应用融合扩展卡尔曼/UFIR滤波算法进行状态估计，得到每个时间点的状态估计并追踪车辆的轨迹。
具体地，在所述步骤一中，建立非线性车辆运动模型；
为了更精确的描述车辆的行驶过程，本发明采用非线性车辆运动模型。首先车辆模型的状态向量定义为，
x[k]＝[x[k] y[k] v[k] θ[k] ω[k]]T
其中，x[k]和y[k]是车辆的横向和纵向位置，v[k]是车辆沿行驶方向的速率，θ[k]是车辆行驶方向与横向坐标轴的夹角(逆时针为正)，ω[k]是车辆的横摆角速度。假设车辆的速度大小和横摆角速度恒定，由此可得到车辆运动模型为，
z[k]＝Cx[k]+v[k],
(1)
其中， 中代表车辆的线加速度和加速度(此处假设为过程噪声)，Δt是离散系统的采样时间， 是车辆位置测量的测量噪声向量。
对于过程噪声，其协方差矩阵表示为，
Q＝E{w[k]wT[k]}
其中，对于测量噪声，其协方差矩阵表示为，
R＝E{v[k]vT[k]}
对于系统的初始值，假设期望和方差为，
P0＝E{x[0]xT[0]}
由于本发明的算法考虑了噪声统计特性的不准确性，因而此处的协方差矩阵不需要特别精确。
具体地，在所述步骤步骤二中根据车辆的运动模型，确定扩展卡尔曼和扩展UFIR滤波器算法及相关参数；
本发明设计的车辆状态估计算法，考虑了更能准确描述车辆运动的非线性模型。相关估计算法的目的是在噪声环境中更精确地估计出车辆行驶过程中的状态(包含位置、速度、偏航角及偏航角速度)并进行轨迹追踪。所采用的基于融合扩展卡尔曼滤波/UFIR滤波器改进的算法可以有效的处理非线性模型的估计问题并且能更好的抑制噪声特性变化和非线性带来的对估计的不利影响。应用本发明中的算法，首先需要根据车辆运动模型得到扩展卡尔曼滤波器和扩展UFIR滤波器的算法作为后续步骤融合卡尔曼/UFIR滤波器算法的基础。对于系统(1)，扩展卡尔曼滤波器的预测和更新过程为，
其中为扩展卡尔曼滤波器在n步时的预测估计，为扩展卡尔曼滤波器在n步时的更新估计，∑n|n-1为n步时误差协方差矩阵的预测估计，∑n|n为n步时的更新估计误差协方差矩阵，扩展卡尔曼滤波的初始条件为∑0|-1＝P0。
假设本申请实施例中所取的扩展UFIR滤波器的估计区间长度为N，在第n步估计时扩展UFIR滤波器的迭代形式为，
其中，m＝n-N+1,i从s+1开始至n结束，s定义为s＝m+4。扩展UFIR得到的估计结果即为在迭代至n时的状态估计结果。扩展UFIR滤波器迭代形式的始状态计算如下，
其中，Zs,m＝[zT[s] … zT[m]]T，
具体地，在所述步骤三中、根据非线性车辆模型、扩展卡尔曼滤波器和扩展UFIR滤波器的参数来确定融合扩展卡尔曼/UFIR滤波器的参数和具体算法；融合扩展卡尔曼/UFIR滤波器的算法在每一个采样周期的估计分为四个阶段，如图2所示，分别为初始化、预测、更新和估计融合。在此步骤中，考虑在第n步的融合扩展滤波器算法。由于扩展UFIR滤波器从n>N开始定义，在n<N时车辆状态估计只采用扩展卡尔曼滤波器得到的结果。在融合扩展卡尔曼/UFIR滤波算法中，马尔科夫概率转移矩阵设定为结合附图6，n≥N时的融合扩展卡尔曼/UFIR滤波算法如下：
(1)初始化阶段：
在此阶段，扩展卡尔曼滤波器的状态估计及协方差矩阵设定为
为融合向量，在n＝N时的初始值为∑n-1|n-1，分别来自于对应的扩展卡尔曼滤波和扩展UFIR滤波部分。所需要设定的初始值为
(1)预测阶段：
此阶段由系统的模型来进行状态估计的预测。对于扩展卡尔曼滤波相关部分，预测阶段的算法为
对于扩展UFIR滤波相关的部分，预测阶段算法为
其中 由(9)中的扩展UFIR迭代算法得到。融合向量中的元素在预测阶段更新如下
(2)更新阶段：
此阶段由新得到的测量量对融合算法的状态估计进行更新。对于扩展卡尔曼滤波部分，车辆状态的估计更新如下
∑n|n＝(I-KnC)∑n|n-1
Kn＝∑n-1|n-1CTSn -1
Sn＝C∑n-1|n-1CT+R
对于扩展UFIR滤波部分，车辆状态估计更新如下
根据以上更新，融合向量中的元素更新为
其中，
(2)融合阶段：
此阶段基于扩展卡尔曼滤波和扩展UFIR滤波得到的结果进行状态估计融合以进行估计算法的迭代和估计的更新。进行估计融合的表达式如下
显然，本领域的技术人员应该明白，上述的本申请的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本申请不限制于任何特定的硬件和软件结合。
以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (10)

1.一种用于车辆状态估计的融合滤波实现方法，其特征在于，包括：
建立非线性车辆运动模型；
根据所述非线性车辆运动学模型计算扩展卡尔曼滤波器和扩展UFIR滤波器的计算参数；
根据噪声信号的统计特性，对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器进行融合滤波，并在n≥N以后的每个采样周期进行如下操作阶段：
在初始化阶段设定所述扩展卡尔曼滤波器的状态估计及协方差矩阵；
在预测阶段进行状态估计的预测，包括对所述扩展卡尔曼滤波器的预测阶段的预设算法、所述扩展UFIR滤波器的预测阶段的预设算法；
在更新阶段根据测量量对车辆状态估计进行更新，包括对所述扩展卡尔曼滤波器的车辆状态估计更新结果和对所述扩展UFIR滤波器的车辆状态估计更新结果；
在估计融合阶段对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器得到的状态估计结果进行融合以以更新车辆的状态估计。
2.根据权利要求1所述的用于车辆状态估计的融合滤波实现方法，其特征在于，所述状态估计用于对车辆速度的估计结果或车辆轨迹的估计结果。
3.根据权利要求1所述的用于车辆状态估计的融合滤波实现方法，其特征在于，所述噪声信号的统计特性至少包括：车辆行驶过程中的系统过程噪声的统计特性或者测量噪声的统计特性。
4.根据权利要求1所述的用于车辆状态估计的融合滤波实现方法，其特征在于，还包括：从摄像头的监控录像中提取出目标时间信息和所述目标时间信息中带有噪声的车辆位置信息。
5.根据权利要求1所述的用于车辆状态估计的融合滤波实现方法，其特征在于，根据所述非线性车辆运动学模型计算扩展卡尔曼滤波器和扩展UFIR滤波器的计算参数包括：
将所述扩展卡尔曼滤波器和所述扩展UFIR滤波器作为滤波单元，并根据噪声信号的统计特性的变换情况，使用扩展卡尔曼滤波器的结果或扩展UFIR滤波器的结果，确定滤波单元的参数矩阵，并在所述滤波单元中进行预测和更新。
6.根据权利要求1所述的用于车辆状态估计的融合滤波实现方法，其特征在于，建立非线性车辆运动模型包括：
定义车辆模型的状态向量，其中所述状态向量至少包括：车辆的横向位置、车辆的纵向位置、车辆沿行驶方向的速率、车辆行驶方向与横向坐标轴的夹角、车辆的横摆角速度；
在假设车辆的速度大小和横摆角速度恒定时，得到车辆运动模型，其中所述车辆运动模型中确定对于过程噪声的协方差矩阵的计算参数、确定对于对于测量噪声的协方差矩阵的计算参数、确定对于车辆运动模型的初始值中期望和方差的计算参数。
7.一种车辆状态估计方法，其特征在于，包括：
获取图像并识别车辆位置信息；
根据融合滤波单元和所述车辆位置信息对车辆进行状态估计；
根据噪声信号统计特性信息不准确程度，调整所述融合滤波单元中扩展UFIR滤波器和扩展卡尔曼滤波器的融合结果占比；
其中所述噪声信号至少包括：车辆行驶过程中的系统过程噪声或者设备测量噪声。
8.一种用于车辆状态估计的融合滤波实现装置，其特征在于，包括：
建立模块，用于建立非线性车辆运动模型；
确定模块，用于根据所述非线性车辆运动学模型计算扩展卡尔曼滤波器和扩展UFIR滤波器的计算参数；
融合模块，用于根据噪声信号的统计特性，对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器进行融合滤波，并在n≥N以后的每个采样周期进行如下操作阶段：
在初始化阶段设定所述扩展卡尔曼滤波器的状态估计及协方差矩阵；
在预测阶段进行状态估计的预测，包括对所述扩展卡尔曼滤波器的预测阶段的预设算法、所述扩展UFIR滤波器的预测阶段的预设算法；
在更新阶段根据测量量对车辆状态估计进行更新，包括对所述扩展卡尔曼滤波器的车辆状态估计更新结果和对所述扩展UFIR滤波器的车辆状态估计更新结果；
在估计融合阶段对所述扩展卡尔曼滤波器和所述扩展UFIR滤波器得到的状态估计结果进行融合以以更新车辆的状态估计。
9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现权利要求1至6任一项所述的用于车辆状态估计的融合滤波实现方法的步骤。
10.一种自动驾驶车辆，其特征在于，包括：如权利要求8所述的用于车辆状态估计的融合滤波实现装置。
CN201910762909.3A 2019-08-16 2019-08-16 用于车辆状态估计的融合滤波实现方法以及装置、存储介质、车辆 Active CN110572139B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910762909.3A CN110572139B (zh) 2019-08-16 2019-08-16 用于车辆状态估计的融合滤波实现方法以及装置、存储介质、车辆

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910762909.3A CN110572139B (zh) 2019-08-16 2019-08-16 用于车辆状态估计的融合滤波实现方法以及装置、存储介质、车辆

Publications (2)

Publication Number Publication Date
CN110572139A true CN110572139A (zh) 2019-12-13
CN110572139B CN110572139B (zh) 2022-11-29

Family

ID=68775766

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910762909.3A Active CN110572139B (zh) 2019-08-16 2019-08-16 用于车辆状态估计的融合滤波实现方法以及装置、存储介质、车辆

Country Status (1)

Country Link
CN (1) CN110572139B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111623779A (zh) * 2020-05-20 2020-09-04 哈尔滨工程大学 一种适用于噪声特性未知的时变系统自适应级联滤波方法
CN112183196A (zh) * 2020-08-20 2021-01-05 北京航空航天大学 一种基于kf/ufir自适应融合滤波器的交通路口车辆状态估计方法
CN112464564A (zh) * 2020-11-27 2021-03-09 北京罗克维尔斯科技有限公司 车辆动力学参数的确定方法和装置
CN113566821A (zh) * 2021-06-28 2021-10-29 江南造船(集团)有限责任公司 基于交互式滤波的无人机航向估计方法、系统及电子设备

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7181323B1 (en) * 2004-10-25 2007-02-20 Lockheed Martin Corporation Computerized method for generating low-bias estimates of position of a vehicle from sensor data
EP2562681A1 (de) * 2011-08-25 2013-02-27 Delphi Technologies, Inc. Objektverfolgungsverfahren für ein Kamerabasiertes Fahrerassistenzsystem
CN105974329A (zh) * 2016-07-22 2016-09-28 深圳市沃特玛电池有限公司 一种估算电池组soh的方法
CN107765242A (zh) * 2017-09-16 2018-03-06 太原理工大学 基于状态增广迭代扩展卡尔曼滤波的系统状态估计方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7181323B1 (en) * 2004-10-25 2007-02-20 Lockheed Martin Corporation Computerized method for generating low-bias estimates of position of a vehicle from sensor data
EP2562681A1 (de) * 2011-08-25 2013-02-27 Delphi Technologies, Inc. Objektverfolgungsverfahren für ein Kamerabasiertes Fahrerassistenzsystem
CN105974329A (zh) * 2016-07-22 2016-09-28 深圳市沃特玛电池有限公司 一种估算电池组soh的方法
CN107765242A (zh) * 2017-09-16 2018-03-06 太原理工大学 基于状态增广迭代扩展卡尔曼滤波的系统状态估计方法

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111623779A (zh) * 2020-05-20 2020-09-04 哈尔滨工程大学 一种适用于噪声特性未知的时变系统自适应级联滤波方法
CN112183196A (zh) * 2020-08-20 2021-01-05 北京航空航天大学 一种基于kf/ufir自适应融合滤波器的交通路口车辆状态估计方法
CN112183196B (zh) * 2020-08-20 2021-08-27 北京航空航天大学 基于自适应融合滤波器的交通路口车辆状态估计方法
CN112464564A (zh) * 2020-11-27 2021-03-09 北京罗克维尔斯科技有限公司 车辆动力学参数的确定方法和装置
CN113566821A (zh) * 2021-06-28 2021-10-29 江南造船(集团)有限责任公司 基于交互式滤波的无人机航向估计方法、系统及电子设备

Also Published As

Publication number Publication date
CN110572139B (zh) 2022-11-29

Similar Documents

Publication Publication Date Title
CN110572139B (zh) 用于车辆状态估计的融合滤波实现方法以及装置、存储介质、车辆
US10852139B2 (en) Positioning method, positioning device, and robot
CN109459019A (zh) 一种基于级联自适应鲁棒联邦滤波的车载导航计算方法
CN111666891B (zh) 用于估计障碍物运动状态的方法和装置
CN110422175B (zh) 车辆状态估计方法以及装置、电子设备、存储介质、车辆
CN110490273A (zh) 噪声方差不精确建模的多传感器系统融合滤波算法
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN110675435A (zh) 基于卡尔曼滤波和χ2检测平滑处理的车辆轨迹跟踪方法
CN110926460A (zh) 一种基于IMU的uwb定位异常值处理方法
CN115066631A (zh) 用于跟踪对象的扩张状态的系统和方法
CN109631894A (zh) 一种基于滑动窗口的单目视觉惯性紧耦合方法
CN115066632A (zh) 利用模型几何学习跟踪移动对象的扩张状态的系统和方法
CN113960622A (zh) 融合激光雷达及imu传感器信息的实时定位方法及装置
CN108592907A (zh) 一种基于双向滤波平滑技术的准实时步进式行人导航方法
CN108871365A (zh) 一种航向约束下的状态估计方法及系统
US11080562B1 (en) Key point recognition with uncertainty measurement
CN110637209A (zh) 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质
CN104777465B (zh) 基于b样条函数任意扩展目标形状及状态估计方法
CN108051001B (zh) 一种机器人移动控制方法、系统及惯性传感控制装置
CN113511194A (zh) 一种纵向避撞预警方法及相关装置
CN112067007A (zh) 地图生成方法、计算机存储介质及电子设备
CN110007298B (zh) 一种目标超前预测跟踪方法
CN116729412A (zh) 车辆定位中运动学模型自适应方法、装置、设备及介质
CN115979288A (zh) 一种航向角确定方法、电子设备和存储介质
CN108230370B (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
TR01 Transfer of patent right

Effective date of registration: 20240430

Address after: 201203, Unit 402, Building 2, No. 2966 Jinke Road, Pudong New Area (Shanghai) Pilot Free Trade Zone, Shanghai

Patentee after: Shanghai Zhijia Technology Co.,Ltd.

Country or region after: China

Address before: Room 303-304, 570 shengxia Road, China (Shanghai) pilot Free Trade Zone, Pudong New Area, Shanghai, 201203

Patentee before: SHANGHAI MAXIEYE AUTOMOBILE TECHNOLOGY CO.,LTD.

Country or region before: China

TR01 Transfer of patent right