CN111896008A

CN111896008A - 一种改进的鲁棒无迹卡尔曼滤波组合导航方法

Info

Publication number: CN111896008A
Application number: CN202010843690.2A
Authority: CN
Inventors: 王伟; 龚佳承; 李欣; 薛冰; 黄平
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2020-08-20
Filing date: 2020-08-20
Publication date: 2020-11-06

Abstract

一种改进的鲁棒无迹卡尔曼滤波组合导航方法，涉及组合导航技术领域，针对现有技术中滤波精度低，进而导致定位精度差，且其对野值的处理能力较差的问题，本发明根据UWB和MEMS惯导定位原理，建立了UWB/MEMS紧组合数学模型，该模型比常规的UWB/MEMS松组合模型定位精度更高，对环境适应能力更强。本发明针对量测突变噪声和野值，在传统鲁棒算法的基础上引入测量误差判据，增强了对野值的处理能力。本发明针对传统鲁棒算法对部分观测受干扰调节能力差的问题，并结合UWB测距误差的实际情况，提出一种多重鲁棒自适应矩阵的计算方法，该算法针对各量测不同的误差情况，实时调节滤波增益矩阵，使滤波器具有更强的自适应性。

Description

一种改进的鲁棒无迹卡尔曼滤波组合导航方法

技术领域

本发明涉及组合导航技术领域，具体为一种改进的鲁棒无迹卡尔曼滤波组合导航方法。

背景技术

近几年来，随着科技的发展和人们生活的改变，基于位置服务(Location-basedservices，LBS)在工业和市场上都有着广泛的应用背景。现如今，全球导航卫星系统(Global Navigation Satellite System，GNSS)飞速发展，其在室外环境下可以为用户提供高精度的位置服务。然而，在室内环境下，GNSS信号极易受遮挡，使得用户无法通过GNSS实现正常的导航定位。同时，人类绝大部分的活动发生于室内环境下，高精度室内定位服务可以为用户提供路线导航、商品导购、资源引导等网络化服务，因此，高精度的室内定位技术具有广阔的应用前景。

作为LBS的核心内容之一，室内定位系统的设计以及成为该领域的热点。现如今，室内定位技术普遍采用无线定位，无线定位系统的缺陷是其定位效果易受到非视距(NLOS,non line of sight)等环境的影响。应用较广泛的室内定位技术主要有无线局域网(Wireless Local Area Networks，WLAN)、射频识别(Radio Frequence Identification，RFID)、超宽带(Ultra-wide Band，UWB)、蓝牙等，其中，UWB具有超大的带宽、良好的抗多径干扰能力等优点，更适合用于提供室内高精度定位服务。然而，UWB定位精度易受非视距环境的影响。因此，许多学者提出将两种或两种以上的传感器进行融合来弥补单一传感器导航系统的缺点。随着微电子机械系统(Micro Electro Mechanical System，MEMS)技术的兴起和发展，使得传统惯性导航设备在体积、成本等方面得到有效降低。基于UWB/MEMS的组合导航系统能够充分利用各自的优势，提高系统定位精度和稳定性，具有广阔的发展前景。

在室内组合导航领域，UWB/MEMS组合导航研究大致分为两类：一种是通过研究UWB/MEMS组合方式来提高室内定位精度和解决UWB失效时定位发散的问题。这些多传感器组合虽然都在一定程度上提高了组合定位精度，但引入其他传感器也使得其在工程上的实现变得更加困难。另一种是从滤波的角度来解决UWB的噪声和野值问题。虽然有学者从滤波的角度提出采用强跟踪Sage自适应滤波解决UWB定位噪声的问题，但强跟踪中的渐消因子若选取不当，会导致滤波精度降低，且其对野值的处理能力较差。

发明内容

本发明的目的是：针对现有技术中滤波精度低，进而导致定位精度差，且其对野值的处理能力较差的问题，提出一种改进的鲁棒无迹卡尔曼滤波组合导航方法。

本发明为了解决上述技术问题采取的技术方案是：

一种改进的鲁棒无迹卡尔曼滤波组合导航方法，包括以下几个步骤：

步骤一：收集UWB系统和MEMS系统输出的数据；

步骤二：选择系统的状态量和观测量，建立UWB/MEMS组合导航系统状态空间模型；

步骤三：对UWB/MEMS组合导航系统进行滤波初始化；

步骤四：在k时刻对UWB/MEMS组合导航系统进行时间更新和量测更新，得到系统状态一步预测的误差协方差和一步预测的互协方差；

步骤五：引入量测误差判据，若量测存在误差，则计算多重因子的鲁棒矩阵，并对状态一步预测的误差协方差进行校正，然后进入步骤六，若量测不存在误差，则直接进入步骤六；

步骤六：若量测存在误差，则利用校正后的状态一步预测误差协方差，计算k时刻系统的状态估计值和状态误差协方差，若量测不存在误差，则利用步骤四中的状态一步预测误差协方差，计算k时刻系统的状态估计值和状态误差协方差；

步骤七：根据步骤六中得到的状态估计值获取MEMS惯导的陀螺仪、加速度计的常值漂移，并将其反馈到组合系统中，输出姿态、速度和位置信息。

进一步的，所述UWB/MEMS组合导航系统状态空间模型的系统状态方程表示为：

X_k+1＝F_kX_k+w_k

其中，X_k为15维的惯导解算信息误差，表示为

其中，φ_k＝[φ_k,x φ_k,y φ_k,z]^T为k时刻惯导解算的姿态误差，ΔV_k＝[ΔV_k,x ΔV_k,y ΔV_k,z]^T为k时刻惯导解算的地理坐标系下的速度误差，ΔP_k＝[ΔL_k Δλ_k Δh_k]^T为k时刻惯导解算的经纬高误差，ε_k＝[ε_k,x ε_k,y ε_k,z]^T为k时刻陀螺常值漂移，

为k时刻加速度计零偏，w_k为k时刻组合系统过程噪声，w_k噪声协方差矩阵为Q_k，F_k为k时刻惯导系统状态转移矩阵。

进一步的，所述UWB/MEMS组合导航系统状态空间模型的系统量测方程表示为：

Z_k＝h(X'_k)+V_k

其中，Z_k为系统量测输入，h(X'_k)为系统非线性量测函数，V_k为组合系统量测噪声，V_k噪声协方差矩阵为R_k，h(X'_k)和V_k分别表示为：

V_k＝-2ρ_Ui,kε+ε²

其中，X'_k包含惯导在地心地固坐标系下的位置误差Δx_k、Δy_k，其中Δx_k、Δy_k与状态量X_k中惯导在地理坐标系下的位置误差ΔL_k、Δλ_k、Δh_k的关系表示为：

Δx_k＝-(R_n+h_k)cosλ_k sinL_kΔL_k-(R_n+h_k)cosL_k sinλ_kΔλ_k+cosL_k cosλ_kΔh_k

Δy_k＝-(R_n+h_k)sinλ_k sinL_kΔL_k+(R_n+h_k)cosL_k cosλ_kΔλ_k+cosL_ksinλ_kΔh_k

其中，e为地球离心率，R_n为地球子午圈半径。

进一步的，所述步骤五中量测误差判据具体表示为：

λ_k＞β时，量测存在误差；

λ_k＜β时，量测不存在误差；

其中，β为门限值，λ_k为检测函数，λ_k服从自由度为m的χ²分布，即λ_k～χ²(m)，λ_k具体表示为：

其中，v_k为系统k时刻的新息序列，

为新息序列v_k的理论协方差。

进一步的，所述步骤五中多重因子的鲁棒矩阵表示为：

其中，S_k为多重因子的鲁棒调节矩阵，s_k(i,i)表示第i个观测量的鲁棒调节因子，s_k(i,i)计算方法如下：

其中，

为新息理论协方差对角线的值，Y_k(i,i)为新息实际协方差对角线的值。

本发明的有益效果是：

(1)本发明根据UWB和MEMS惯导定位原理，建立了UWB/MEMS紧组合数学模型，该模型比常规的UWB/MEMS松组合模型定位精度更高，对环境适应能力更强。

(2)本发明针对量测突变噪声和野值，在传统鲁棒算法的基础上引入测量误差判据，增强了对野值的处理能力。

(3)本发明针对传统鲁棒算法对部分观测受干扰调节能力差的问题，并结合UWB测距误差的实际情况，提出一种多重鲁棒自适应矩阵的计算方法，该算法针对各量测不同的误差情况，实时调节滤波增益矩阵，使滤波器具有更强的自适应性。

附图说明

图1为本发明的原理图；

图2为本发明的方针运动轨迹图；

图3为UWB、MEMS、UWB/MEMS松组合和UWB/MEMS紧组合算法解算的载体轨迹对比图；

图4(a)为姿态角误差对比图1；

图4(b)为姿态角误差对比图2；

图4(c)为姿态角误差对比图3；

图5(a)为速度误差对比图1；

图5(b)为速度误差对比图2；

图6(a)为速度误差对比图1；

图6(b)为速度误差对比图2；

图7(a)为姿态角误差对比图1；

图7(b)为姿态角误差对比图2；

图7(c)为姿态角误差对比图3；

图8(a)为速度误差对比图1；

图8(b)为速度误差对比图2；

图9(a)为位置误差对比图1；

图9(b)为位置误差对比图2；

图10(a)为姿态角误差对比图1；

图10(b)为姿态角误差对比图2；

图10(c)为姿态角误差对比图3；

图11(a)为速度误差对比图1；

图11(b)为速度误差对比图2；

图12(a)为位置误差对比图1；

图12(b)为位置误差对比图2。

具体实施方式

为提高室内定位系统的精度和鲁棒性，本发明根据UWB和MEMS定位模型，推导出了一种UWB/MEMS紧组合算法，同时在传统的鲁棒算法的基础上引入测量误差判据，并针对传统鲁棒算法对部分观测受误差干扰调节能力差的问题，提出一种多重鲁棒自适应滤波算法，避免了对正确的量测量的调节，从而提高了组合系统的精度和鲁棒性。

具体实施方式一：参照图1具体说明本实施方式，本实施方式所述的一种改进的鲁棒无迹卡尔曼滤波组合导航方法，包括以下步骤：

(1)收集UWB系统和MEMS系统输出的数据；

(2)选择状态量和观测量，建立UWB/MEMS组合导航系统状态空间模型；

(3)UWB/MEMS组合系统滤波初始化；

(4)时间更新。由k-1时刻状态估计值及协方差计算k时刻系统状态预测值及协方差；

(5)Sigma点选择。根据状态预测值及其协方差选择一组加权Sigma点；

(6)量测更新。利用经过非线性量测方程变换后的Sigma点进行量测更新，得到一步状态预测的误差协方差和一步预测的互协方差；

(7)引入量测误差判据，若量测存在误差，则计算鲁棒矩阵并对一步状态预测的误差协方差进行校正；

(8)利用校正后的一步状态误差协方差，计算k时刻状态估计和状态误差协方差；

(9)将滤波输出的误差信息反馈补偿到UWB系统和MEMS系统，输出高精度姿态、速度、位置信息。

本发明一种改进的鲁棒无迹卡尔曼滤波组合导航方法还包括：

建立UWB/MEMS紧组合系统模型

(1)系统状态方程：

X_k+1＝F_kX_k+w_k

其中，X_k为15维的惯导解算信息误差，其可写成

为k时刻加速度计零偏。w_I,k为k时刻惯导系统过程噪声，且满足E[w_I,k]＝0，F_I,k为k时刻惯导系统状态转移矩阵。

(2)系统量测数学模型推导：

在地心地固坐标系中，假设k时刻载体的真实位置为(x_k,y_k)，惯导解算得到的载体的位置为(x_I,k,y_I,k)，第i个基站S_i的真实位置为

因此由MEMS惯导解算所得到的载体到基站S_i之间的伪距ρ_Ii,k为：

载体到基站S_i的真实距离ρ_i为

两式相减，得

由于惯导解算的位置(x_I,k,y_I,k)存在误差，其与真实值(x_k,y_k)之间的关系为：

x_k＝x_I,k-Δx_k

y_k＝y_I,k-Δy_k

其中，Δx_k和Δy_k为k时刻MEMS惯导解算的载体位置误差。

整理可得：

假设k时刻UWB测量的载体到基站S_i的距离为ρ_Ui,k，其与载体到基站S_i的真实距离ρ_i,k之间的关系可表示为：

ρ_Ui,k＝ρ_i,k+ε

其中，ε为UWB测量噪声。整理可得系统量测模型：

式中，

为量测输入，-2ρ_Ui,kε+ε²为系统观测噪声，其协方差矩阵为R_k。

(3)系统量测方程：

Z_k＝h(X'_k)+V_k

其中，Z_k为系统量测输入，h(X'_k)为系统非线性量测函数，V_k为系统量测噪声，其噪声协方差矩阵为R_k，h(X'_k)和V_k分别可以表示为：

V_k＝-2ρ_Ui,kε+ε²

其中，X'_k包含惯导在地心地固坐标系下的位置误差Δx_k、Δy_k，其中Δx_k、Δy_k与状态量X_k中惯导在地理坐标系下的位置误差ΔL_k、Δλ_k、Δh_k的关系可以表示为：

其中，e为地球离心率，R_n为地球子午圈半径。

2.导数无迹卡尔曼滤波(Derivative Unscented Kalman Filter，DUKF)算法

假设非线性离散时间系统为：

其中，x_k∈Rⁿ和z_k∈R^m分别为k时刻统的状态向量和量测向量，Φ_k/k-1为离散型状态转移矩阵，h(·)为描述量测模型的非线性函数，w_k和v_k为互不相关的零均值高斯白噪声，其方差为：

根据DUKF滤波算法，进行系统状态更新和量测更新得到载体的导航信息，具体步骤包括：

(1)给定状态估计值

及其协方差

(2)时间更新。由于系统状态方程为线性，状态预测值

及其协方差

的可表示为：

(3)Sigma点选择。根据状态预测值及其协方差选择一组加权Sigma点，选择的Sigma点为：

(4)量测更新。经过非线性量测方程h(·)变换后的Sigma点为：

γ_i,k/k-1＝h(ξ_i,k/k-1),i＝0,1,...,2n

计算量测预测值及其协方差：

计算状态预测值与量测预测值间的互协方差：

其中，ω_i为权值，其可表述为

(5)确定卡尔曼滤波增益矩阵：

(6)更新状态估计值

及其协方差

从DUKF的算法步骤可以看出，在时间更新过程中，它与线性KF具有相同的形式，可以简洁有效地计算状态预测值及其协方差，避免了标准UKF因UT变换导致的额外计算；在量测更新过程中，DUKF选择一组加权Sigma点捕获状态预测值及其方差的信息，然后根据所选择的Sigma点和经过非线性量测方程变换后的Sigma点的统计信息，更新状态估计值及其协方差，继承了标准UKF处理非线性滤波问题的优良特性。

3.量测噪声和野值判据

卡尔曼滤波框架中，量测误差对滤波结果的影响是通过新息引入的，因此可以从新息出发对量测误差进行在线检测，系统的新息序列可以表示为：

在无噪声和野值情况下，k时刻的新息v_k是零均值高斯白噪声，其协方差为：

当量测存在误差时，新息v_k均值不再为零，因此，通过新息序列的检测可以判断是否存在噪声和野值，对新息v_k作二元假设：

H₀:无误差E[v_k]＝0，

H₁:有误差E[v_k]＝μ_k，E[(v_k-μ_k)(v_k-μ_k)^T]＝A_k

通过上述对新息v_k的假设检验可得检测函数：

其中，λ_k是服从自由度为m的χ²分布，即λ_k～χ²(m)。当量测存在误差时，λ_k将会发生变化，如果取λ_k大于某一门限值β的概率为α，即p{λ_k＞β}＝α，则组合系统量测误差判断准则为：

λ_k＞β时，量测存在误差

λ_k＜β时，量测不存在误差

4.多重因子的鲁棒算法

传统的鲁棒算法是通过引入一个时变的鲁棒因子实时调整量测协方差

从而达到调节滤波增益的目的，使有误差的量测量被较小的用于状态量的估计中，实现系统对量测误差的鲁棒自适应性。调整后的量测协方差可以表示为：

其中，s_k是鲁棒因子。在量测存在误差时，新息的实际协方差大于理论的协方差，两者的关系可以表示为：

其中，Y_k是新息的实际协方差，ρ为遗忘因子，通常取值为0.95。传统的鲁棒因子的获取方法是计算新息序列实际协方差和理论协方差的比值，具体表示如下:

其中，tr(·)表示对矩阵求迹。从式中可以看出，传统的鲁棒因子虽然计算简单，但对各个观测量的调节能力相同。对于UWB系统，由于基站分布在四周，系统在非视距环境下，通常是某个基站与标签的通信受障碍物遮挡而产生测距误差，故组合系统各个观测量的误差大小不一样，因此，传统的标量鲁棒因子不适合对UWB/MEMS组合系统观测误差的调节。本发明根据UWB测距误差特性，建立多重鲁棒因子，对UWB测距的每个通道给予不同的调节能力，多重鲁棒因子的计算方法如下：

其中，S_k为鲁棒调节矩阵，s_k(i,i)表示第i个观测量的鲁棒调节因子，其计算方法如下：

本发明首先针对室内环境下UWB易受非视距环境(NLOS)影响而导致定位精度低的问题，提出UWB/MEMS紧组合策略；其次，在传统的鲁棒算法的基础上引入量测误差判据，并且针对传统鲁棒算法对部分观测受干扰调节能力差的问题，并结合UWB测距误差的实际情况，提出一种多重鲁棒自适应矩阵的计算方法，该算法针对各量测不同的误差情况，实时调节滤波增益矩阵，使滤波器具有更强的自适应性。所以本发明利用MATLAB仿真软件进行仿真实验，在各个基站通信均受误差干扰和部分基站通信受误差干扰的情况下，将本发明的MRUKF算法与现有的UKF算法和RUKF算法进行比较。

下面结合具体事例进行仿真分析：

传感器参数设置为：陀螺仪的常值漂移和随机噪声分别为10°/h和5°/h，加速度计的常值漂移和随机噪声分别为500ug和70ug，陀螺仪和加速度计的输出频率均为100Hz；UWB标签测距的随机漂移为0.3m，UWB系统的输出频率为10Hz。UWB基站含误差时，设置各基站位置存在1m的偏差。载体运动设置为：假设物体M在一个二维平面上做圆周运动，水平方向(x轴)上做匀加速直线运动，垂直方向(y轴)上也做匀加速直线运动，运动速度为1m/s，运动两周，总的运动时长为280s。4个基站布置在运动轨迹四周，图2位载体运动轨迹和基站位置分布。

图3为UWB、MEMS、UWB/MEMS松组合、UWB/MEMS紧组合算法解算的载体轨迹对比图，从图中可以看出，MEMS解算轨迹较为平滑稳定，但是随着时间推移惯导误差逐渐累加导致定位结果偏离参考轨迹。与MEMS惯导对比，UWB定位精度较高，但是UWB测距存在噪声扰动，导致UWB定位轨迹有较大的波动。相较于MEMS和UWB，UWB/MEMS组合系统继承了MEMS惯导和UWB的优点，因此解算结果与参考轨迹更为接近，稳定性更好。

图4(a)、图4(b)、图4(c)、图5(a)、图5(b)、图6(a)和图6(b)为各个基站通信均受误差干扰情况下本发明MRUKF算法与DUKF算法和传统RUKF算法滤波输出误差对比图，从误差图中可以看出，当标签和4个基站的通信都受噪声影响时，由于RUKF算法对量测增益进行调节，因此其滤波精度要高于传统的DUKF。虽然MRUKF是单独对各个量测进行调节，但由于各个量测所受噪声和野值大小相同，因此其解算的鲁棒矩阵和RUKF算出的鲁棒因子大小相同，故两个算法算法滤波精度相当。

表1为各个基站通信均受误差干扰情况下，本发明MRUKF算法与DUKF算法和RUKF算法滤波解算的姿态、速度、位置的均方根误差。从表中可以看到，当观测量均含误差时，RUKF和MRUKF对误差均具有很好的调节作用。

表1

图7(a)、图7(b)、图7(c)、图8(a)、图8(b)、图9(a)和图9(b)为部分基站通信受误差干扰情况下，当部分观测量含突变噪声时，本发明MRUKF算法与DUKF算法和传统RUKF算法滤波输出误差对比图。从图可以看出，本发明所提的MRUKF的滤波精度是明显高于RUKF的，其主要原因是RUKF算法对各个观测量的调节能力相同，当只有2个UWB测距受噪声影响时，通过RUKF算法求得的鲁棒因子，其会污染另外两个没有受噪声影响的观测量，迫使其原本合理的滤波增益发生改变，而本发明所提的MRUKF算法是根据其各个量测的误差情况，分别独立调整其滤波增益，因此，其滤波输出精度要明显高于RUKF算法。

表2为部分基站通信受误差干扰情况下，当部分观测量含突变噪声时，本发明MRUKF算法与DUKF算法和RUKF算法滤波解算的姿态、速度、位置的均方根误差。从表中也可以看出，本发明所提的MRUKF算法滤波精度明显高于RUKF算法和UKF算法。

表2

图10(a)、图10(b)、图10(c)、图11(a)、图11(b)、图12(a)和图12(b)为部分基站通信受误差干扰情况下，当部分观测量含野值时，本发明MRUKF算法与DUKF算法和传统RUKF算法滤波输出误差对比图，从上图可以看出，相较于UKF算法，RUKF算法对量测野值具有很好的调节能力，但由于其对各个量测的调节能力相同，会污染不受野值干扰的量测，因此，其滤波输出精度相对MRUKF较低，鲁棒性较差。

表3为部分基站通信受误差干扰情况下，当部分观测量含野值时，本发明MRUKF算法与DUKF算法和RUKF算法滤波解算的姿态、速度、位置的均方根误差。从表中也可以看出，本发明所提的MRUKF算法对部分观测量受误差干扰的调节能力明显高于RUKF算法和UKF算法。

表3

综上，本发明一种改进的鲁棒无迹卡尔曼滤波组合导航方法，定位精度高，鲁棒性强，对实际中部分UWB基站含误差具有很好的调节能力，工程应用价值高。

需要注意的是，具体实施方式仅仅是对本发明技术方案的解释和说明，不能以此限定权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的，仍应落入本发明的保护范围内。