(12)发明专利申请
(10)申请公布号 CN 107966143 A(43)申请公布日 2018.04.27
(21)申请号 201711123461.8(22)申请日 2017.11.14
(71)申请人 济南大学
地址 250022 山东省济南市南辛庄西路336
号(72)发明人 徐元 李岳炀 王宜敏 部丽丽
吴玉涛 (74)专利代理机构 济南圣达知识产权代理有限
公司 37221
代理人 董雪(51)Int.Cl.
G01C 21/16(2006.01)G01C 21/20(2006.01)
权利要求书3页 说明书7页 附图2页
(54)发明名称
一种基于多窗口的自适应EFIR数据融合方法
(57)摘要
本发明公开了一种基于多窗口的自适应EFIR数据融合方法，包括：通过UWB系统和惯性导航器件INS系统分别测量参考节点到目标节点之间的距离；在此基础上，将两种系统测量得到的距离信息的平方作差，差值作为数据融合算法所使用的滤波模型的观测量；在此基础上，对传统的扩展有限脉冲响应(EFIR)滤波器进行改进，以在离线阶段选择的不同局部滤波窗口构建不同的子EFIR滤波器，并通过IMM方式对构建的子EFIR滤波器的输出进行融合，得到当前时刻的惯性导航器件INS测量系统测量的行人最优的位置误差预估；在此基础上，将惯性导航器件INS测量得到的行人位置与EFIR滤波器得到的位置误差预估作差，最终得到当前时刻最优的行人位置预估。
CN 107966143 ACN 107966143 A
权 利 要 求 书
1/3页
1.一种基于多窗口的自适应EFIR数据融合方法，其特征在于，包括：
(1)以惯性导航器件INS在k时刻在东向和北向的位置误差和速度误差作为状态量，以惯性导航器件INS与UWB分别测量的目标节点与参考节点之间的距离的平方的差值作为系统观测量，构建滤波模型；
(2)在扩展有限脉冲响应滤波器的基础上，根据在离线阶段选择的不同局部滤波窗口，构建P个不同的子EFIR滤波器，P为设定值；
(3)通过IMM方式对构建的子EFIR滤波器的输出进行融合，得到当前时刻的惯性导航器件INS测量系统测量的行人最优的位置误差预估；
(4)将惯性导航器件INS测量得到的行人位置与EFIR滤波器得到的位置误差预估作差，最终得到当前时刻最优的行人位置预估。
2.如权利要求1所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述步骤(2)中，子EFIR滤波器的状态方程为：
其中，(δPEast,k+1,δPNorth,k+1)、(δPEast,k,δPNorth,k)分别为k+1和k时刻惯性导航器件INS解算的东向和北向的位置误差；(δVEast,k+1,δVNorth,k+1)、(δVEast,k,δVNorth,k)为k+1和k时刻惯性导航器件INS解算的东向和北向的速度误差；T为采样周期；wk为k时刻的系统噪声。
3.如权利要求1所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述步骤(2)中，子EFIR滤波器的观测方程为：
其中，为k时刻惯性导航器件INS和UWB分别测量得到的参考节点与未知节
为k时刻惯性导航器件INS测为k时刻UWB测量得到的参考节点
点之间的距离的平方的差；n为参考节点的数目；量得到的参考节点与未知节点之间的距离；与未知节点之间的距离；
其中，(xi,yi),i∈[1,n]为参考节点的位置；η其协方差矩阵k为系统k时刻的观测噪声，为Rk；
分别为k时刻惯性导航器件INS测量得到的东向和北向位置。
4.如权利要求1所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述
2
CN 107966143 A
权 利 要 求 书
2/3页
步骤(2)中，在离线阶段选择的不同局部滤波窗口构建不同的子EFIR滤波器的数量根据对算法性能的需求自行选择。
5.如权利要求1所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述步骤(2)中，各子EFIR滤波器通过一组独立的数据，将能获取到的均方误差最小的P个局部滤波窗口N，分别作为子EFIR滤波器的局部滤波窗口。
6.如权利要求1所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，子EFIR滤波器的迭代算法为：
设定中间变量m和s：m＝k-N+1,s＝m+M-1其中，N为子EFIR滤波器的局部滤波窗口的大小；M为状态向量的维数；
其中，为s时刻的状态向量、为s时刻的状态向量预估、ys为s时刻的观测向量；设定中间变量j的范围为从s+1时刻到k时刻，子EFIR滤波器进行的迭代如下：
在此基础上，
其中，Fj和Fj-1分别为j时刻和j-1时刻带观测时滞模型的系统矩阵；和刻和j-1时刻带观测时滞模型的状态向量迭代中间量；
分别为j时
为由j-1时刻得到的j时刻带观
测时滞模型的状态向量迭代中间量；Gj和Gj-1分别为j和j-1时刻的广义噪声功率增益；Kj为j时刻带观测时滞模型的偏差校正增益；yj为带观测时滞模型在j时刻的观测向量；为j时刻带观测时滞模型下的状态向量预估值；
为从j-1到j时刻的误差矩阵、
差、
为关于
的函数；
为从j-1时刻的误差矩阵、Q为系统噪声wk的协方
Rj为观测噪声的协方差；
为第i个子EFIR滤波器的状态预估，为状态预估中间变量，
为第i个子
EFIR滤波器的误差矩阵，为协方差矩阵中间变量。
7.如权利要求1所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述
3
CN 107966143 A
权 利 要 求 书
3/3页
步骤(3)中，通过IMM方式对构建的子EFIR滤波器的输出进行融合具体为：
1)构建马尔可夫的转移概率矩阵，在此基础上，引入权重矩阵；2)计算各子EFIR滤波器的权重；
3)根据各子EFIR滤波器的权重以及子EFIR滤波器的状态向量预估，得到数据融合滤波器的总输出。
8.如权利要求7所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述
EFIR#χ
步骤2)中，各子EFIR滤波器的权重θ的计算方法如下：
其中，为上一时刻的权重值；χ＝1,2,3,4，…g；g为子EFIR滤波器个数；
yk为k时刻的观测向量，Rk为系统k时刻的观测噪声的协方差矩阵，
为第χ个EFIR滤波器的一步状态预估，第χ个EFIR滤波器通过一步预估获取到的观测向量预估
为第χ个EFIR滤波器的误差矩阵。
9.如权利要求7所述的一种基于多窗口的自适应EFIR数据融合方法，其特征在于，所述步骤3)中，数据融合滤波器的总输出具体为：
其中，数；
为k时刻第χ个子EFIR滤波器的权重，χ＝1,2,3,4，…P；P为子EFIR滤波器个
为子EFIR滤波器的状态向量预估。
4
CN 107966143 A
说 明 书
一种基于多窗口的自适应EFIR数据融合方法
1/7页
技术领域
[0001]本发明涉及复杂环境下组合定位技术领域，尤其涉及一种基于多窗口的自适应EFIR数据融合方法。
背景技术
[0002]近年来，行人导航(Pedestrian Navigation,PN)作为导航技术应用的新兴领域，正越来越受到各国学者的重视，并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理论意义和实际应用价值。
[0003]在现有的定位方式中，全球卫星导航系统(Global Navigation Satellite System,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。近年来，UWB(Ultra Wideband)以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜力。学者们提出将基于UWB的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，UWB信号十分容易受到干扰而导致定位精度下降甚至失锁；与此同时，由于UWB采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟踪在室内导航领域仍旧面临很多挑战。
发明内容
[0004]本发明的目的就是为了解决由于在实时系统中很难获取对扩展有限状态响应(Extended finite impulse response，EFIR)滤波器的滤波性能有较大影响的最优滤波窗口N的问题，提出了一种采用IMM(交互多模型)窗口的自适应EFIR数据融合方法，该方法对传统的扩展有限脉冲响应(EFIR)滤波器进行改进，以在离线阶段选择的不同局部滤波窗口构建不同的子EFIR滤波器，并通过IMM方式对构建的子EFIR滤波器的输出进行融合，最终得到当前时刻最优的行人位置预估。[0005]为实现上述目的，本发明的具体方案如下：
[0006]本发明公开了一种基于多窗口的自适应EFIR数据融合方法，包括：[0007](1)以惯性导航器件INS在k时刻在东向和北向的位置误差和速度误差作为状态量，以惯性导航器件INS与UWB分别测量的目标节点与参考节点之间的距离的平方的差值作为系统观测量，构建滤波模型；[0008](2)在扩展有限脉冲响应滤波器的基础上，根据在离线阶段选择的不同局部滤波
5
CN 107966143 A
说 明 书
2/7页
窗口，构建P个不同的子EFIR滤波器，P为设定值；[0009](3)通过IMM方式对构建的子EFIR滤波器的输出进行融合，得到当前时刻的惯性导航器件INS测量系统测量的行人最优的位置误差预估；[0010](4)将惯性导航器件INS测量得到的行人位置与EFIR滤波器得到的位置误差预估作差，最终得到当前时刻最优的行人位置预估。[0011]进一步地，所述步骤(2)中，子EFIR滤波器的状态方程为：
[0012]
[0013]
其中，(δPEast,k+1,δPNorth,k+1)、(δPEast,k,δPNorth,k)分别为k+1和k时刻惯性导航器件INS解算的东向和北向的位置误差；(δVEast,k+1,δVNorth,k+1)、(δVEast,k,δVNorth,k)为k+1和k时刻惯性导航器件INS解算的东向和北向的速度误差；T为采样周期；wk为k时刻的系统噪声。[0014]进一步地，所述步骤(2)中，子EFIR滤波器的观测方程为：
[0015]
[0016]其中，为k时刻惯性导航器件INS和UWB分别测量得到的参考节点与
为k时刻惯性导航器件为k时刻UWB测量得到的参考
未知节点之间的距离的平方的差；n为参考节点的数目；INS测量得到的参考节点与未知节点之间的距离；节点与未知节点之间的距离；
[0017]
[0018]
其中，(xi,yi),i∈[1,n]为参考节点的位置；η其协方差k为系统k时刻的观测噪声，
分别为k时刻惯性导航器件INS测量得到的东向和北向位置。
矩阵为Rk；
[0019]
进一步地，所述步骤(2)中，在离线阶段选择的不同局部滤波窗口构建不同的子EFIR滤波器的数量根据对算法性能的需求自行选择。[0020]进一步地，所述步骤(2)中，各子EFIR滤波器通过一组独立的数据，将能获取到的均方误差最小的P个局部滤波窗口N，分别作为子EFIR滤波器的局部滤波窗口。[0021]进一步地，子EFIR滤波器的迭代算法为：[0022]设定中间变量m和s：[0023]m＝k-N+1,s＝m+M-1[0024]其中，N为子EFIR滤波器的局部滤波窗口的大小；M为状态向量的维数；
6
CN 107966143 A
说 明 书
3/7页
[0025][0026][0027][0028][0029][0030][0031][0032][0033][0034][0035]
其中，为s时刻的状态向量、为s时刻的状态向量预估、ys为s时刻的观测向量；设定中间变量j的范围为从s+1时刻到k时刻，子EFIR滤波器进行的迭代如下：
Kj＝Gj(Hj)T
在此基础上，
其中，Fj和Fj-1分别为j时刻和j-1时刻带观测时滞模型的系统矩阵；和
分别
为j时刻和j-1时刻带观测时滞模型的状态向量迭代中间量；为由j-1时刻得到的j时
刻带观测时滞模型的状态向量迭代中间量；Gj和Gj-1分别为j和j-1时刻的广义噪声功率增
益；Kj为j时刻带观测时滞模型的偏差校正增益；yj为带观测时滞模型在j时刻的观测向量；为j时刻带观测时滞模型下的状态向量预估值；
[0036]
为关于的函数；
为从j-1到j时刻的误差矩阵、为从j-1时刻的误差矩阵、Q为系统噪声wk
的协方差、
[0037]
Rj为观测噪声的协方差；
为第i个子EFIR滤波器的状态预估，为状态预估中间变量，
为第i
个子EFIR滤波器的误差矩阵，为协方差矩阵中间变量。
[0038]
进一步地，所述步骤(3)中，通过IMM方式对构建的子EFIR滤波器的输出进行融合具体为：
[0039]1)构建马尔可夫的转移概率矩阵，在此基础上，引入权重矩阵；[0040]2)计算各子EFIR滤波器的权重；
[0041]3)根据各子EFIR滤波器的权重以及子EFIR滤波器的状态向量预估，得到数据融合滤波器的总输出。
EFIR#χ[0042]进一步地，所述步骤2)中，各子EFIR滤波器的权重θ的计算方法如下：
[0043]
[0044]其中，为上一时刻的权重值；χ＝1,2,3,4，…g；g为子EFIR滤波器个数；
7
CN 107966143 A[0045]
说 明 书
4/7页
[0046][0047][0048]
[0049]yk为k时刻的观测向量，Rk为系统k时刻的观测噪声的协方差矩
为第χ个EFIR滤波器的一步状态预估，第χ个EFIR滤波器通过一步预估获取到的
为第χ个EFIR滤波器的误差矩阵。
阵，
观测向量预估
[0050][0051][0052]
进一步地，所述步骤3)中，数据融合滤波器的总输出具体为：
其中，为k时刻第χ个子EFIR滤波器的权重，χ＝1,2,3,4，…P；P为子EFIR滤为子EFIR滤波器的状态向量预估。
波器个数；
[0053]
本发明的有益效果：
[0054]1、通过在离线阶段通过独立的数据获取的子EFIR滤波器的滤波窗口长度，构建各子EFIR滤波器，在此基础上，通过交互多模型(IMM)算法，将各子滤波器的输出进行融合，最终得到最优的惯性导航器件INS位置解算误差预估，并进而得到惯性导航器件INS的最优行人位置预估。[0055]2、可用于室内环境下的中高精度定位。附图说明
[0056]图1为一种基于多窗口的自适应EFIR数据融合系统的示意图；[0057]图2为本发明构建滤波模型进行数据融合示意图；
[0058]图3为本发明通过IMM方式对构建的子EFIR滤波器的输出进行融合示意图。具体实施方式：
[0059]下面结合附图对本发明进行详细说明：
[0060]本发明一种基于多窗口的自适应EFIR数据融合系统如图1所示，包括：组合导航算法采用UWB和INS两种导航系统，其中，UWB包括UWB参考节点和UWB定位标签，UWB参考节点预先固定在已知坐标上，UWB定位标签固定在目标行人上。INS主要由固定在目标行人足部的IMU组成。
[0061]基于上述系统，本发明公开了一种基于多窗口的自适应EFIR数据融合方法，包括：[0062](1)如图2所示，以惯性导航器件INS在k时刻在东向和北向的位置误差和速度误差
8
CN 107966143 A
说 明 书
5/7页
作为状态量，以惯性导航器件INS与UWB分别测量的目标节点与参考节点之间的距离的平方的差值作为系统观测量，构建滤波模型进行数据融合；[0063](2)在扩展有限脉冲响应滤波器的基础上，根据在离线阶段选择的不同局部滤波窗口，构建不同的子EFIR滤波器；
[0064]子EFIR滤波器的状态方程为：
[0065]
其中，(δPEast,k+1,δPNorth,k+1)、(δPEast,k,δPNorth,k)为k+1和k时刻惯性导航器件INS解
算的东向和北向的位置误差；(δVEast,k+1,δVNorth,k+1)、(δVEast,k,δVNorth,k)为k+1和k时刻惯性导航器件INS解算的东向和北向的速度误差；T为采样周期；wk为k时刻的系统噪声；[0067]子EFIR滤波器的观测方程为：
[0066]
[0068]
[0069]其中，为k时刻惯性导航器件INS和UWB分别测量得到的参考节点与
为k时刻惯性导航器件为k时刻UWB测量得到的参考
未知节点之间的距离的平方的差；n为参考节点的数目；INS测量得到的参考节点与未知节点之间的距离；节点与未知节点之间的距离；
[0070]
[0071]
其中，(xi,yi),i∈[1,n]为参考节点的位置；ηk为系统k时刻的观测噪声；[0072](3)如图3所示，通过IMM方式对构建的子EFIR滤波器的输出进行融合，得到当前时刻的惯性导航器件INS测量系统测量的行人最优的位置误差预估；
[0073]在离线阶段选择的不同局部滤波窗口构建不同的子EFIR滤波器的数量可以根据对算法性能的需求自行选择，本发明以离线阶段选择4个不同的局部滤波窗口构建4个子EFIR滤波器为例进行介绍。
[0074]各子EFIR滤波器通过一组独立的数据，将能获取到的最小均方误差的4个局部滤波窗口N，分别作为子EFIR滤波器的局部滤波窗口；其中，对子EFIR滤波器，其均方根误差的表达式如下：
[0075]
[0076]
EFIR算法的迭代方程为：
9
CN 107966143 A[0077][0078][0079][0080][0081][0082][0083][0084][0085][0086][0087][0088][0089]
说 明 书
6/7页
m＝k-N+1,s＝m+M-1其中，N为EFIR滤波器的局部滤波窗口的大小；M为状态向量的维数；
从s+1时刻到k时刻，EFIR滤波器进行的迭代如下：
Gj＝[(Hl)THj+(FjGj-1(Fj)T)-1]-1Kj＝Gj(Hj)T
在此基础上，
IMM算法具体为：
首先构建马尔可夫的转移概率矩阵：
[0090]
[0091]
其中，aij,ij∈[1,4]表示4个子EFIR滤波器之间的转移概率；在此基础上，引入权
重矩阵θ如下：k，
[0092]
[0093][0094][0095][0096]
对各子EFIR滤波器(编号为#i,i∈[1,4])的权重计算如下：
[0097][0098][0099]
其中，为上一时刻的权重值；
10
CN 107966143 A
说 明 书
7/7页
[0100]
[0101][0102]
故，数据融合滤波器的总输出为：
(4)将惯性导航器件INS测量得到的行人位置与EFIR滤波器得到的位置误差预估
作差，最终得到当前时刻最优的行人位置预估。
[0104]上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。
[0103]
11
CN 107966143 A
说 明 书 附 图
1/2页
图1
12
CN 107966143 A
说 明 书 附 图
2/2页
图2
图3
13
因篇幅问题不能全部显示，请点此查看更多更全内容