CN112859126A - 一种基于ufir滤波器的gnss定位漂移处理方法 - Google Patents

Abstract

本发明提供了一种基于UFIR滤波器的GNSS定位漂移处理方法，包括以下步骤：首先，建立车辆行驶的运动学模型；其次确定UFIR滤波器无偏估计下的最优滑动窗口长度；然后根据车辆运动学模型与滑动窗口长度确定相关计算UFIR的参数矩阵，以及累积相应长度的历史定位观测信息；最后，通过环境信息确定GNSS定位数据的置信区间，若当前定位信息不满足置信区间，则基于UFIR滤波器进行状态估计，得到更为精确的车辆位置信息。本发明所采用的UFIR滤波算法不需要相关噪声统计信息，适用于多种环境下，满足车辆行驶工控复杂的需求，具有较强的鲁棒性。

Description

一种基于UFIR滤波器的GNSS定位漂移处理方法

技术领域

本发明涉及信号处理与控制算法技术领域，具体为一种基于UFIR滤波器的GNSS定位漂移处理方法。

背景技术

自动驾驶感知功能的目标之一是实现车辆定位，当前主要应用的定位方式为全球卫星导航系统(Global Navigation Satellite System,GNSS)。虽然GNSS能够实现持续、稳定、高精确度的定位，但其效果受外界环境影响较大，例如在隧道或者室内、在高楼环绕阻挡信号、卫星颗数接收较少与电磁干扰信号传输等条件下，GNSS无法正常有效工作，精度与准确度会出现一定的偏差，定位结果往往出现不同程度的漂移现象。不精确的定位信息会导致自动驾驶系统错误的路径规划与车辆行驶状态控制，很可能引发严重的交通事故。如何通过对GNSS定位信息进行一定的滤波处理的方式，消除或减小定位偏差，提高定位精度与准确度，提升自动驾驶功能安全具有重要意义。

常用的经典卡尔曼滤波算法可以有效解决上述问题，但是算法的执行需要得到车辆运动模型的过程噪声以及GNSS定位信息的测量噪声，然而在不同的车辆、行驶状况、外界环境、定位影响条件下，过程噪声与测量噪声会产生很大的差异，需要根据具体条件调整对应参数，否则该算法无法取得很好的滤波结果。

发明内容

本发明的目的为解决GNSS定位信息在外界环境较差的情况下所出现的漂移现象，以及现有常用的卡尔曼滤波算法普适性较弱情况，提出了一种基于UFIR滤波器的GNSS定位漂移处理方法，该方法检测GNSS定位出现漂移现象时，采用累积历史状态观测值对当前位置信息进行估测，将更为精确的位置信息估测值提供给车辆系统，保障车辆自动驾驶功能的正常运行。

本发明所解决的技术问题采用以下技术方案来实现：一种基于UFIR滤波器的GNSS定位漂移处理方法，具体包括以下步骤：

步骤一、建立车辆行驶过程中的运动学模型，确定相关矩阵中具体参数值；

步骤二、确定UFIR滤波器无偏估计下的最优滑动窗口长度，保证估测结果的可靠性；

步骤三、根据车辆运动学模型参数，确定UFIR滤波器迭代形式下的各矩阵参数，读取并存储最优滑动窗口长度相对应的GNSS历史定位信息数据；

步骤四、设定GNSS定位信息置信范围，通过阈值对当前GNSS定位信息进行判断，当获取的定位信息超过阈值时，判断定位出现漂移现象，启动UFIR滤波器，利用累积的历史定位信息数据对当前位置信息进行估测，得到此时较为精确的定位信息。

优选的，所述步骤一中的运动学模型为：

其中，X_k∈R⁴为车辆运动的真实状态；A∈R^4×4为状态转移矩阵；W_k∈R²为过程噪声；B∈R^4×2为过程噪声的系数矩阵；Z_k∈R²为GNSS观测得到的车辆定位信息；C∈R^2×4为观测转移矩阵；v_k∈R²为测量噪声；

所述车辆运动的真实状态X_k、过程噪声Z_k和GNSS观测得到的车辆定位信息W_k对应的矩阵分别为：

其中，x(k)和y(k)分别表示k时刻时，车辆在GNSS系统所定义测量坐标系中的x轴与y轴方向上的坐标值；v_x(k)和v_y(k)分别表示k时刻时，车辆在GNSS系统所定义测量坐标系中的x轴与y轴方向上的速度值；x_z(k)和y_z(k)分别表示GNSS所测得的车辆轴x与y轴方向上的坐标值；假设车辆的加速度为随机变量，即a_x(k)与a_y(k)分别表示车辆在x轴与y轴方向上的加速度值；

所述状态转移矩阵A、过程噪声的系数矩阵B、观测转移矩阵C具体为：

其中，T为GNSS定位采样周期；

同时，过程噪声与测量噪声假设其均满足均值为零的条件：

优选的，所述步骤二中的UFIR滤波器无偏估计下的最优滑动窗口长度为：

其中V_k的表达形式如下：

其中Z_k为k时刻的观测值，

为k时刻的先验状态变量，C为系统的观测转移矩阵。

优选的，所述步骤三中的确定UFIR滤波器迭代形式下的各矩阵参数具体内容包括：

确定迭代参数矩阵H_m,k，其表达形式如下：

式中，F_m,k表示从m时刻到k时刻的总累积状态转移矩阵；C_m,k从m时刻到k时刻的总累积观测转移矩阵；h_m,k表示从m时刻到k时刻的迭代系数矩阵；H_m,k表示从m时刻到k时刻的迭代参数矩阵；

具体地，其中

为m+1时刻到k时刻单累积状态转移矩阵，其广泛定义为：

所述步骤三中的存储对应最优滑动窗口长度的历史定位信息数据的存储表示形式如下：

Z_m,k＝[Z_m ^TZ_m+1 ^T...Z_k ^T]^T

式中Z_m,k为从m时刻到k时刻的总累积观测矩阵，Z_m为m时刻所观测得到的定义信息数据。

优选的，所述步骤四中的设定GNSS定位信息置信范围，通过阈值对当前GNSS定位信息进行判断的具体方法为：

针对车辆行驶环境中对GNSS定位信息精确度影响的主要因素，构造了如下所示的自适应调整的置信区间确定公式：

σ＝f₁M+f₂Q+f₃P+R

式中，M为接收卫星颗数所对应的参数值；Q为电磁干扰强度所对应的参数值；P为定位信号强弱评判值；R为调整系数，由车辆的行驶状态确定；f₁、f₂与f₃分别为对应参数的重要因子；

通过当前时刻的位置信息与上一时刻的位置信息进行绝对距离作差，将得到的计算值与置信区间比对：

J＝F(Z_k,Z_k-1)-σ

若J＞0，即距离差值小于置信区间值，则判定获得的当前位置信息准确；若J＜0，即距离差值大于置信区间值，则认定获取的定位信息受到外界干扰，出现了漂移现象，启动UFIR滤波器，利用历史积累定位数据估测当前位置信息，得到较为准确的数据，保证车辆自动驾驶功能的正常运行。

与已公开技术相比，本发明存在以下优点：(1)本发明将UFIR滤波器应用于GNSS定位信息的过滤处理中，解决因环境原因而出现定位信息飘移的现象，在过程噪声与测量噪声方差矩阵难以确定的情况下，相较于其他滤波算法具有更好的性能。

(2)本发明所采用的UFIR滤波器对数据的处理过程不需要相关噪声与干扰信号等统计信息，可应用于车辆行驶的多种不同环境，表现出较强的鲁棒性与普适性。

(3)本发明提供了一种GNSS定位信息置信区间确定方法，根据车辆当前位置所接收卫星颗数、天气空气情况、高楼遮挡率等条件自适应调整当前的置信区间，该方法更加匹配车辆行驶环境的复杂度，有利于得到更为精确的定位信息。

附图说明

图1为本发明的流程图；

图2为本发明UFIR滤波算法原理示意图；

图3为本发明UFIR迭代形式下状态估测流程图。

具体实施方式

为了使本发明的技术手段、创作特征、工作流程、使用方法达成目的与功效易于明白了解，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例

如图1所示一种基于UFIR滤波器的GNSS定位漂移处理方法，具体包括以下内容：

步骤一、建立车辆的运动学模型如下：

其中，X_k∈R⁴为车辆运动的真实状态；A∈R^4×4为状态转移矩阵；W_k∈R²为过程噪声；B∈R^4×2为过程噪声的系数矩阵；Z_k∈R²为GNSS观测得到的车辆定位信息；C∈R^2×4为观测转移矩阵；v_k∈R²为测量噪声，具体矩阵定义如下：

其中，x(k)和y(k)分别表示k时刻时，车辆在GNSS系统所定义测量坐标系中的x轴与y轴方向上的坐标值；v_x(k)和v_y(k)分别表示k时刻时，车辆在GNSS系统所定义测量坐标系中的x轴与y轴方向上的速度值；x_z(k)和y_z(k)分别表示GNSS所测得的车辆轴x与y轴方向上的坐标值；假设车辆的加速度为随机变量，即a_x(k)与a_y(k)分别表示车辆在x轴与y轴方向上的加速度值。

假设GNSS定位采样周期为T，模型中的相关参数矩阵定位如下：

同时，过程噪声与测量噪声假设其均满足均值为零的条件：

步骤二、确定UFIR滤波器无偏估计下的最优滑动窗口长度

UFIR滤波器的计算处理虽然不需要过程噪声以及测量噪声，但其需要计算无偏估计下的最优滑动窗口长度N_opt，即计算处理历史数据的个数，具体原理如图2所示。N_opt可以通过观测值Z_k的相关计算获得，即最小化残差协方差矩阵的导数，即：

其中V_k的表达形式如下：

其中Z_k为k时刻的观测值，

为k时刻的先验状态变量，C为系统的观测转移矩阵。

步骤三、确定UFIR滤波器迭代形式下的各矩阵参数，读取并存储最优滑动窗口长度相对应的GNSS历史定位信息数据。

确定迭代参数矩阵H_m,k，其表达形式如下：

式中，F_m,k表示从m时刻到k时刻的总累积状态转移矩阵；C_m,k从m时刻到k时刻的总累积观测转移矩阵；h_m,k表示从m时刻到k时刻的迭代系数矩阵；H_m,k表示从m时刻到k时刻的迭代参数矩阵。

具体地，其中

为m+1时刻到k时刻单累积状态转移矩阵，其广泛定义为：

带有一个下标的参数表示在下标时刻的参数值，例如C_m与A_r分别表示m时刻的观测转移矩阵C，r时刻的系统状态转移矩阵A，在本发明中构建的是线性时不变系统，故任意时刻观测转移矩阵与系统状态转移矩阵均相同。

存储对应最优滑动窗口长度的历史定位信息数据，存储表示形式如下：

Z_m,k＝[Z_m ^TZ_m+1 ^T...Z_k ^T]^T

式中Z_m,k为从m时刻到k时刻的总累积观测矩阵，Z_m为m时刻所观测得到的定义信息数据。

步骤四、设定GNSS定位信息置信范围，启动UFIR滤波器对当前位置信息进行估测：

针对车辆行驶环境中对GNSS定位信息精确度影响的主要因素，构造了如下所示的自适应调整的置信区间确定公式：

σ＝f₁M+f₂Q+f₃P+R

式中，M为接收卫星颗数所对应的参数值；Q为电磁干扰强度所对应的参数值；P为定位信号强弱评判值；R为调整系数，由车辆的行驶状态确定；f₁、f₂与f₃分别为对应参数的重要因子。

通过当前时刻的位置信息与上一时刻的位置信息进行绝对距离作差，将得到的计算值与置信区间比对。

J＝F(Z_k,Z_k-1)-σ

若J＞0，即距离差值小于置信区间值，则判定获得的当前位置信息准确；若J＜0，即距离差值大于置信区间值，则认定获取的定位信息受到外界干扰，出现了漂移现象，启动UFIR滤波器，利用历史积累定位数据估测当前位置信息，得到较为准确的数据，保证车辆自动驾驶功能的正常运行。

如图3所示，UFIR滤波器迭代形式具体计算流程与公式如下：

首先通过UFIR按批处理形式得到迭代计算的初始值，假设当前时刻为k，最优滑动窗口值为N，按批处理的长度为K，设起始点为m＝k-N+1，结束点为s＝k-N+K，所以迭代形式的初始值可以通过下式计算得到：

式中G_s为广义噪声功率增益矩阵，H_m,s表示从m时刻到s时刻的迭代系数矩阵，X_s为s时刻的后验状态估计值，Z_m,s为从m时刻到s时刻的总累积观测矩阵。

得到迭代形式的初始值之后，r＝s+1为初始点时刻，通过UFIR迭代公式逐步计算得到k时刻的系统状态估测值，公式如下：

式中X_r-1＝X_s，G_r-1＝G_s，K_r为系统的偏差纠正增益矩阵。当r＝k时得到当前车辆位置信息的后验估计值X_k。该结果可以代替GNSS定位信息提供给车辆系统进行相关的自动驾驶功能的处理与应用。

以上显示和描述了本发明的基本原理、主要特征及本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明的要求保护范围由所附的权利要求书及其等效物界定。

Claims (5)

1.一种基于UFIR滤波器的GNSS定位漂移处理方法，其特征在于：具体包括以下步骤，

步骤一、建立车辆行驶过程中的运动学模型，确定相关矩阵中具体参数值；

步骤二、确定UFIR滤波器无偏估计下的最优滑动窗口长度，保证估测结果的可靠性；

步骤三、根据车辆运动学模型参数，确定UFIR滤波器迭代形式下的各矩阵参数，读取并存储最优滑动窗口长度相对应的GNSS历史定位信息数据；

步骤四、设定GNSS定位信息置信范围，通过阈值对当前GNSS定位信息进行判断，当获取的定位信息超过阈值时，判断定位出现漂移现象，启动UFIR滤波器，利用累积的历史定位信息数据对当前位置信息进行估测，得到此时较为精确的定位信息。

2.根据权利要求1所述的一种基于UFIR滤波器的GNSS定位漂移处理方法，其特征在于：所述步骤一中的运动学模型为：

其中，X_k∈R⁴为车辆运动的真实状态；A∈R^4×4为状态转移矩阵；W_k∈R²为过程噪声；B∈R^{4 ×2}为过程噪声的系数矩阵；Z_k∈R²为GNSS观测得到的车辆定位信息；C∈R^2×4为观测转移矩阵；v_k∈R²为测量噪声；

所述车辆运动的真实状态X_k、过程噪声Z_k和GNSS观测得到的车辆定位信息W_k对应的矩阵分别为：

其中，x(k)和y(k)分别表示k时刻时，车辆在GNSS系统所定义测量坐标系中的x轴与y轴方向上的坐标值；v_x(k)和v_y(k)分别表示k时刻时，车辆在GNSS系统所定义测量坐标系中的x轴与y轴方向上的速度值；x_z(k)和y_z(k)分别表示GNSS所测得的车辆轴x与y轴方向上的坐标值；假设车辆的加速度为随机变量，即a_x(k)与a_y(k)分别表示车辆在x轴与y轴方向上的加速度值；

所述状态转移矩阵A、过程噪声的系数矩阵B、观测转移矩阵C具体为：

其中，T为GNSS定位采样周期；

同时，过程噪声与测量噪声假设其均满足均值为零的条件：

3.根据权利要求2所述的一种基于UFIR滤波器的GNSS定位漂移处理方法，其特征在于：所述步骤二中的UFIR滤波器无偏估计下的最优滑动窗口长度为：

其中V_k的表达形式如下：

其中Z_k为k时刻的观测值，

为k时刻的先验状态变量，C为系统的观测转移矩阵。

4.根据权利要求3所述的一种基于UFIR滤波器的GNSS定位漂移处理方法，其特征在于：所述步骤三中的确定UFIR滤波器迭代形式下的各矩阵参数具体内容包括：

确定迭代参数矩阵H_m,k，其表达形式如下：

式中，F_m,k表示从m时刻到k时刻的总累积状态转移矩阵；C_m,k从m时刻到k时刻的总累积观测转移矩阵；h_m,k表示从m时刻到k时刻的迭代系数矩阵；H_m,k表示从m时刻到k时刻的迭代参数矩阵；

具体地，其中

为m+1时刻到k时刻单累积状态转移矩阵，其广泛定义为：

所述步骤三中的存储对应最优滑动窗口长度的历史定位信息数据的存储表示形式如下：

Z_m,k＝[Z_m ^TZ_m+1 ^T...Z_k ^T]^T

式中Z_m,k为从m时刻到k时刻的总累积观测矩阵，Z_m为m时刻所观测得到的定义信息数据。

5.根据权利要求4所述的一种基于UFIR滤波器的GNSS定位漂移处理方法，其特征在于：所述步骤四中的设定GNSS定位信息置信范围，通过阈值对当前GNSS定位信息进行判断的具体方法为：

针对车辆行驶环境中对GNSS定位信息精确度影响的主要因素，构造了如下所示的自适应调整的置信区间确定公式：

σ＝f₁M+f₂Q+f₃P+R

式中，M为接收卫星颗数所对应的参数值；Q为电磁干扰强度所对应的参数值；P为定位信号强弱评判值；R为调整系数，由车辆的行驶状态确定；f₁、f₂与f₃分别为对应参数的重要因子；

通过当前时刻的位置信息与上一时刻的位置信息进行绝对距离作差，将得到的计算值与置信区间比对：

J＝F(Z_k,Z_k-1)-σ

若J＞0，即距离差值小于置信区间值，则判定获得的当前位置信息准确；若J＜0，即距离差值大于置信区间值，则认定获取的定位信息受到外界干扰，出现了漂移现象，启动UFIR滤波器，利用历史积累定位数据估测当前位置信息，得到较为准确的数据，保证车辆自动驾驶功能的正常运行。

Images