一种基于自适应加权混合卡尔曼滤波的多源联合定位方法

摘要

本发明涉及一种基于自适应加权混合卡尔曼滤波的多源联合定位方法。该方法通过GNSS/RFID/IMU进行组合定位。该方法能在GNSS信号失锁时，通过搜索权重数据库，对IMU和RFID的定位信息进行加权自适应卡尔曼滤波，输出更加精准的定位。本发明所述基于自适应加权混合卡尔曼滤波的多源联合定位方法，利用扩展卡尔曼滤波算法提高GNSS的定位精度；利用多传感器联合定位的算法改善了盲区的定位精度，克服了高层建筑遮挡带来的影响；通过建立权值数据库，降低了权重计算的难度，提高了运算速率。

说明书

技术领域

本发明涉及一种基于自适应加权混合卡尔曼滤波的多源联合定位方法，属于多源联合定位的技术领域。

背景技术

多源信息融合的方法已被广泛应用在航天航空工程、航海安全、无人机导航与定位等领域。单个传感器在空间上仅仅能覆盖环境中的某个特定区域，而且若单独使用一个传感器，整个系统可能会因为单个传感器故障，造成量测数据的丢失，从而导致整个系统瘫痪或崩溃，影响定位精度，所以多传感器联合定位在提高定位精度方面发挥着越来越重要的作用。

现有的主要定位技术有以下几种：(一)全球导航卫星系统(Global Navigation Satellite System，GNSS),泛指所有的卫星导航系统包括美国的全球定位系统(GPS)、俄罗斯的格洛纳斯卫星系统(GLONASS)、欧盟的伽利略系统(Galileo)、中国的北斗系统(COMPASS)，它能够实时地为军民用户提供三维位置、速度等信息，但是存在着定位盲区信号缺失的问题。(二)无线射频识别(RFID)是利用现有的无线网络系统来跟踪和定位特定场合的RF标识。接收机通过对接收到的信号进行处理，从而定位该设备。RFID虽然有价格便宜、短距离精度高等优点，但是随着定位距离的增加，定位精度会受到很大的影响。(三)惯性导航单元(IMU)定位利用加速度计和陀螺仪来定位，不需要辅助信号基站，具有完全自主、不受外界环境的影响等优点，但是IMU存在着定位误差会随着时间推移累加的缺点。

国内外许多学者对多源联合定位技术有深入的研究。中国专利CN1908587公开了一种GPS和DR车载组合定位系统及定位方法，该方法将GPS和DR的定位信息分别送到微控制器内的基于遗传优化算法的模糊联合卡尔曼滤波器，经过融合处理后输出定位信息。中国专利CN202631740U中研究了RFID和GPS的车辆定位装置，该装置通过读取电子标签中的准确信息，计算GPS误差，实现车辆的准确定位。中国专利CN101319902中研究了低成本的IMU和GPS的组合定位定向装置。该装置将GPS测得的信息和IMU解算得到的信息通过信息融合处理，得到载体最优的导航信息。但是上述定位装置和方法仍然无法解决或者避免各定位技术本身的缺点和不足。

卡尔曼滤波算法是导航定位系统中常用的算法。卡尔曼滤波是由计算机实现的实时递推算法，它根据系统状态方程和观测方程估计出所有需要处理的信号。普通的卡尔曼滤波是在系统线性的前提下讨论的，而在实际中，系统总是存在不同程度的非线性，运用扩展卡尔曼滤波(EKF)可以有效解决非线性滤波的问题，实现更加精准的定位。关于扩展卡尔曼算法的推导等具体内容，参考付梦印、邓志红等人编写的《Kalman滤波理论及其在导航系统中的应用》。

发明内容

针对现有技术的不足，本发明提供一种基于自适应加权混合卡尔曼滤波的多源联合定位方法。

发明概述：

用GNSS/RFID/IMU进行组合定位。该方法能在GNSS信号失锁时，通过搜索权重数据库，对IMU和RFID的定位信息进行加权自适应卡尔曼滤波，输出更加精准的定位。

定位算法，主要包括两个方面：1)GNSS信号接收良好时，分别对GNSS、RFID和IMU的测量信息进行扩展卡尔曼滤波，同时建立RFID和IMU的权重数据库；2)在GNSS信号缺失时，首先把RFID和IMU的测量信息放入扩展卡尔曼滤波器，得到滤波信息，搜索权重数据库，将滤波后的信息与数据库预存的GNSS滤波校正信息按照最小误差准则进行配对，确定最优的RFID和IMU的权值，最后对RFID和IMU的数据进行加权信息融合，输出最优的定位信息。

术语说明：

阅读器：阅读器是RFID定位系统的组成部分，起到读取标签的ID号或数据区内容的作用。

参考点：参考点是GNSS定位系统的组成部分，通过测量参考点和移动物体的距离，起到提高定位精度的作用。

标签：标签是RFID定位系统的组成部分，起到标识识别、信息采集的作用。

接收机：起到接收、跟踪、变换和测量信号的作用，为目标定位提供原始数据。

本发明的技术方案为：

一种基于自适应加权混合卡尔曼滤波的多源联合定位方法，包括以下步骤：

1)接收机分别接收GNSS、RFID、IMU的信号，判断GNSS信号是否受到干扰；

2)如果GNSS信号未受到干扰，以定位目标的坐标为状态向量，以为观测向量建立GNSS系统运动模型和GNSS观测模型，通过扩展卡尔曼滤波器计算得到GNSS状态最优估计其中r₁、...、r_n分别是GNSS测定的定位目标的坐标到n个基站的距离；x_k,y_k分别为WGS-84坐标下，k时刻，高斯投影平面的东向坐标和北向坐标；n≥2；

3)以定位目标的坐标为状态向量，以为RFID观测向量，建立RFID系统运动模型和RFID观测模型，通过扩展卡尔曼滤波器计算得到RFID状态最优估计其中，分别是通过RFID测量的定位目标携带的阅读器到m个基站的距离；m≥3；

4)以定位目标的坐标为状态向量，以为IMU观测向量，建立IMU系统运动模型和IMU观测模型，通过扩展卡尔曼滤波器计算得到IMU状态最优估计其中，w、v_y、v_x分别为通过IMU测量的定位目标的航向角、定位目标x方向的速度、定位目标y方向的速度；

5)根据确定RFID和IMU的权重系数并将各个时刻的GNSS状态最优估计、RFID和IMU的权重系数存入数据库；其中是GNSS信号未受到干扰时，k时刻的全局最优状态；

6)当GNSS信号受到干扰时，通过RFID和IMU组合导航对定位目标定位；将RFID和IMU的定位信息分别放入扩展卡尔曼滤波器，得到RFID的最优估计和IMU的最优估计

7)分别在数据库中搜索RFID的权重系数和IMU的权重系数；

8)对RFID的最优估计和IMU的最优估计根据误差最小原则进行加权信息融合，得到i时刻的全局状态最优估计

优选的，系统运动模型的公式：

X_k＝f(X_k-1,k-1)+Γ(X_k-1,k-1)W_k-1

其中，d_k是k-1时刻到k刻定位目标的位移，w_k-1为k-1时刻IMU测量的定位目标的航向角；Γ(X_k-1,k-1)是噪声输入函数，{W_k-1}是零均值系统白噪声序列，E[W_kW_j^T]＝Q_kδ_kj，Q_k是系统噪声序列的方差；

优选的，GNSS观测模型的公式：

$Z_k^G=h(X_k^G,k)+V_k^G$

其中，(x₁,y₁)、...、(x_n,y_n)分别是参与定位的n个基站的坐标；是零均值观测白噪声序列，其中，是观测噪声序列的方差；

和是不相关的白噪声序列；是GNSS的零均值系统白噪声；

优选的，扩展卡尔曼滤波的步骤如下：

A、将系统运动模型和观测模型线性化；

B、根据线性化的系统运动模型和线性化的观测模型，结合系统噪声协方差Q_k和观测噪声协方差R_k，给定k-1时刻的状态最优估计和协方差矩阵，根据扩展卡尔曼滤波方程计算得到k时刻的状态最优估计和协方差矩阵P_k。状态最优估计和协方差矩阵使用的是递归的算法，所述“给定”即假设k-1时刻状态最优估计和协方差矩阵是已知的，通过递归算法可确定下一时刻k时刻的状态最优估计和协方差矩阵。

进一步优选的，步骤2)中通过扩展卡尔曼滤波器计算得到GNSS状态最优估计的过程如下：

GNSS系统运动模型线性化：将系统状态方程按照泰勒公式围绕滤波值展开：

$X_k^G\approx f({\hat{X}}_{k-1}^G,k-1)+\frac{\partial f}{\partial {\hat{X}}_{k-1}^G}(X_{k-1}-{\hat{X}}_{k-1}^G)+\Gamma ({\hat{X}}_{k-1}^G,k-1)W_{k-1}^G$

略去高阶项得：其中：

GNSS观测模型线性化：其中

GNSS状态最优估计和估计的协方差矩阵P_k^G的求解过程如下：

状态一步预测：

一步预测协方差矩阵

滤波增益

GNSS状态最优估计

协方差矩阵

进一步优选的，步骤3)中RFID状态最优估计和估计的协方差矩阵的计算过程如下：

RFID系统运动模型：

RFID观测模型：

其中，分别是m个标签的坐标；

k-1时刻RFID状态最优估计RFID估计的协方差矩阵为通过扩展卡尔曼滤波器计算得到k时刻RFID状态最优估计估计的协方差矩阵是

进一步优选的，步骤4)中IMU状态最优估计和估计的协方差矩阵的计算过程如下：

IMU系统运动模型：

IMU状态观测模型：

其中，ΔT是k时刻与k-1时刻的时间差；

k-1时刻IMU状态最优估计IMU估计的协方差矩阵为通过扩展卡尔曼滤波器计算得到k时刻的IMU状态最优估计估计的协方差矩阵

优选的，步骤5)中确定RFID的权重系数和IMU的权重系数的方法如下：

k时刻RFID的权重系数：

k时刻IMU的权重系数：

优选的，步骤7)中在数据库中分别搜索RFID的权重系数和IMU的权重系数的具体方法如下：

其中，i＝1,2,…k,…搜索数据库中的确定使最小的则其对应的为RFID的权重系数和IMU的权重系数。

优选的，步骤10)中确定全局状态最优估计的具体方法如下：

首先建立目标误差函数：

使目标误差函数最小的为全局状态最优估计：

解方程得：

${\hat{X}}_o=\frac{w_i^R{\left(P_i^R\right)}^{-1}{\hat{X}}_i^R+w_i^I{\left(P_i^I\right)}^{-1}{\hat{X}}_i^I}{w_i^R{\left(P_i^R\right)}^{-1}+w_i^I{\left(P_i^I\right)}^{-1}}$

$P_o={(w_i^R{\left(P_i^R\right)}^{-1}+w_i^I{\left(P_i^I\right)}^{-1})}^{-1}$

融合后的全局状态最优估计和误差最优估计P_o分别为：

${\hat{x}}_o=\frac{w_i^R{\left(P_i^R\right)}^{-1}{\hat{x}}_i^R+w_i^I{\left(P_i^I\right)}^{-1}{\hat{x}}_i^I}{w_i^R{\left(P_i^R\right)}^{-1}+w_i^I{\left(P_i^I\right)}^{-1}}$

${\hat{y}}_o=\frac{w_i^R{\left(P_i^R\right)}^{-1}{\hat{y}}_i^R+w_i^I{\left(P_i^I\right)}^{-1}{\hat{y}}_i^I}{w_i^R{\left(P_i^R\right)}^{-1}+w_i^I{\left(P_i^I\right)}^{-1}}$

$P_o={(w_i^R{\left(P_i^R\right)}^{-1}+w_i^I{\left(P_i^I\right)}^{-1})}^{-1}$

全局状态最优估计是i时刻在GNSS没有信号时，最终的经过校正后输出的定位信息。

优选的，IMU中的航向角根据陀螺仪测得。

本发明的有益效果为：

1.本发明所述基于自适应加权混合卡尔曼滤波的多源联合定位方法，利用扩展卡尔曼滤波算法提高GNSS的定位精度；利用多传感器联合定位的算法改善了盲区的定位精度，克服了高层建筑遮挡带来的影响；通过建立权值数据库，降低了权重计算的难度，提高了运算速率；

2.本发明所述基于自适应加权混合卡尔曼滤波的多源联合定位方法，用GNSS/RFID/IMU进行组合定位，在GNSS信号失锁时，通过搜索权重数据库，对IMU和RFID的定位信息进行加权自适应卡尔曼滤波，输出更加精准的定位。

附图说明

图1为本发明所述方法的流程示意图；

图2为GNSS信号未受到干扰时建立数据库的流程示意图；

图3为GNSS信号受到干扰时时，通过RFID和IMU组合导航对定位目标定位的流程示意图。

具体实施方式

下面结合实施例和说明书附图对本发明做进一步说明，但不限于此。

实施例1

如图1-3所示。

一种基于自适应加权混合卡尔曼滤波的多源联合定位方法，包括以下步骤：

1)接收机分别接收GNSS、RFID、IMU的信号，判断GNSS信号是否受到干扰；

2)如果GNSS信号未受到干扰，以定位目标的坐标为状态向量，以为观测向量建立GNSS系统运动模型和GNSS观测模型，通过扩展卡尔曼滤波器计算得到GNSS状态最优估计其中r₁、r₂、r₃分别是GNSS测定的定位目标的坐标到三个基站的距离；x_k,y_k分别为WGS-84坐标下，k时刻，高斯投影平面的东向坐标和北向坐标；

3)以定位目标的坐标为状态向量，以为RFID观测向量，建立RFID系统运动模型和RFID观测模型，通过扩展卡尔曼滤波器计算得到RFID状态最优估计其中，分别是通过RFID测量的定位目标携带的阅读器到四个基站的距离；

4)以定位目标的坐标为状态向量，以为IMU观测向量，建立IMU系统运动模型和IMU观测模型，通过扩展卡尔曼滤波器计算得到IMU状态最优估计其中，w、v_y、v_x分别为通过IMU测量的定位目标的航向角、定位目标x方向的速度、定位目标y方向的速度；

5)根据确定RFID和IMU的权重系数并将各个时刻的GNSS状态最优估计、RFID和IMU的权重系数存入数据库；其中是GNSS信号未受到干扰时，k时刻的全局最优状态；

6)当GNSS信号受到干扰时，通过RFID和IMU组合导航对定位目标定位；将RFID和IMU的定位信息分别放入扩展卡尔曼滤波器，得到RFID的最优估计和IMU的最优估计

7)分别在数据库中搜索RFID的权重系数和IMU的权重系数；

8)对RFID的最优估计和IMU的最优估计根据误差最小原则进行加权信息融合，得到i时刻的全局状态最优估计

实施例2

如实施例1所述的基于自适应加权混合卡尔曼滤波的多源联合定位方法，其区别在于，GNSS系统运动模型的公式：

$X_k^G=f(X_{k-1}^G,k-1)+{\Gamma }^G(X_{k-1}^G,k-1)W_{k-1}^G$

其中，d_k是k-1时刻到k刻定位目标的位移，w_k-1为k-1时刻IMU测量的定位目标的航向角；是噪声输入函数，是零均值系统白噪声序列，是系统噪声序列的方差矩阵。

实施例3

如实施例1所述的基于自适应加权混合卡尔曼滤波的多源联合定位方法，其区别在于，GNSS观测模型的公式：

$Z_k^G=h(X_k^G,k)+V_k^G$

其中，(x₁,y₁)、(x₂,y₂)、(x₃,y₃)分别是参与定位的3个基站的坐标；r₁、r₂、r₃分别是GNSS测定的定位目标的坐标到三个基站的距离；是零均值观测白噪声序列，其中，是观测噪声序列的方差。

实施例4

如实施例1所述的基于自适应加权混合卡尔曼滤波的多源联合定位方法，其区别在于，扩展卡尔曼滤波的步骤如下：

A、将系统运动模型和观测模型线性化；

B、根据线性化的系统运动模型和线性化的观测模型，结合系统噪声协方差Q_k和观测噪声协方差R_k，给定k-1时刻的状态最优估计和协方差矩阵，根据扩展卡尔曼滤波方程计算得到k时刻的状态最优估计和协方差矩阵P_k。状态最优估计和协方差矩阵使用的是递归的算法，所述“给定”即假设k-1时刻状态最优估计和协方差矩阵是已知的，通过递归算法可确定下一时刻k时刻的状态最优估计和协方差矩阵。

实施例5

如实施例4所述的基于自适应加权混合卡尔曼滤波的多源联合定位方法，其区别在于，步骤2)中通过扩展卡尔曼滤波器计算得到GNSS状态最优估计的过程如下：

GNSS系统运动模型线性化：将系统状态方程按照泰勒公式围绕滤波值展开：

$X_k^G\approx f({\hat{X}}_{k-1}^G,k-1)+\frac{\partial f}{\partial {\hat{X}}_{k-1}^G}(X_{k-1}-{\hat{X}}_{k-1}^G)+\Gamma ({\hat{X}}_{k-1}^G,k-1)W_{k-1}^G$

略去高阶项得：其中：

GNSS观测模型线性化：其中

GNSS状态最优估计和估计的协方差矩阵P_k^G的求解过程如下：

状态一步预测：

一步预测协方差矩阵

滤波增益

GNSS状态最优估计

协方差矩阵

实施例6

如实施例4所述的基于自适应加权混合卡尔曼滤波的多源联合定位方法，其区别在于，步骤3)中RFID状态最优估计和估计的协方差矩阵的计算过程如下：

RFID系统运动模型：

RFID观测模型：

其中，分别是四个标签的坐标；

k-1时刻RFID状态最优估计RFID估计的协方差矩阵为通过扩展卡尔曼滤波器计算得到k时刻RFID状态最优估计估计的协方差矩阵是

实施例7

如实施例4所述的基于自适应加权混合卡尔曼滤波的多源联合定位方法，其区别在于，步骤4)中IMU状态最优估计和估计的协方差矩阵的计算过程如下：

IMU系统运动模型：

IMU状态观测模型：

其中，ΔT是k时刻与k-1时刻的时间差；

k-1时刻IMU状态最优估计IMU估计的协方差矩阵为通过扩展卡尔曼滤波器计算得到k时刻的IMU状态最优估计估计的协方差矩阵

实施例8

如实施例1所述的基于自适应加权混合卡尔曼滤波的多源联合定位方法，其区别在于，步骤5)中确定RFID的权重系数和IMU的权重系数的方法如下：

k时刻RFID的权重系数：

k时刻IMU的权重系数：

实施例9

如实施例4所述的基于自适应加权混合卡尔曼滤波的多源联合定位方法，其区别在于，步骤7)中在数据库中分别搜索RFID的权重系数和IMU的权重系数的具体方法如下：

其中，i＝1,2,…k,…搜索数据库中的确定使最小的则其对应的为RFID的权重系数和IMU的权重系数。

实施例10

如实施例4所述的基于自适应加权混合卡尔曼滤波的多源联合定位方法，其区别在于，步骤10)中确定全局状态最优估计的具体方法如下：

首先建立目标误差函数：

使目标误差函数最小的为全局状态最优估计：

解方程得：

${\hat{X}}_o=\frac{w_i^R{\left(P_i^R\right)}^{-1}{\hat{X}}_i^R+w_i^I{\left(P_i^I\right)}^{-1}{\hat{X}}_i^I}{w_i^R{\left(P_i^R\right)}^{-1}+w_i^I{\left(P_i^I\right)}^{-1}}$

$P_o={(w_i^R{\left(P_i^R\right)}^{-1}+w_i^I{\left(P_i^I\right)}^{-1})}^{-1}$

融合后的全局状态最优估计和误差最优估计P_o分别为：

${\hat{x}}_o=\frac{w_i^R{\left(P_i^R\right)}^{-1}{\hat{x}}_i^R+w_i^I{\left(P_i^I\right)}^{-1}{\hat{x}}_i^I}{w_i^R{\left(P_i^R\right)}^{-1}+w_i^I{\left(P_i^I\right)}^{-1}}$

${\hat{y}}_o=\frac{w_i^R{\left(P_i^R\right)}^{-1}{\hat{y}}_i^R+w_i^I{\left(P_i^I\right)}^{-1}{\hat{y}}_i^I}{w_i^R{\left(P_i^R\right)}^{-1}+w_i^I{\left(P_i^I\right)}^{-1}}$

$P_o={(w_i^R{\left(P_i^R\right)}^{-1}+w_i^I{\left(P_i^I\right)}^{-1})}^{-1}$

全局状态最优估计是i时刻在GNSS没有信号时，最终的经过校正后输出的定位信息。