技术领域
[0001] 本
发明涉及复杂环境下组合
定位技术领域，尤其涉及一种基于分布式组合滤波INS/UWB行人导航系统及方法。
背景技术
[0002] 近年来，行人导航(Pedestrian Navigation,PN)作为导航技术应用的新兴领域，正越来越受到各国学者的重视，并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地下
停车场等室内环境下，外界无线电
信号微弱、
电磁干扰强烈等因素都会对目标行人导航信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行有效的融合以消除室内复杂环境影响，保证行人导航
精度的持续稳定，具有重要的科学理论意义和实际应用价值。
[0003] 在现有的定位方式中，全球
卫星导航系统(Global Navigation Satellite System,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的
位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。近年来，UWB(Ultra Wideband)以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜
力。学者们提出将基于UWB的目标
跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，UWB信号十分容易受到干扰而导致定位精度下降甚至失
锁；与此同时，由于UWB采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络
节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟踪在室内导航领域仍旧面临很多挑战。
[0004] 在导航模型方面，目前在室内行人组合导航领域应用较多的为松组合导航模型。该模型具有容易实现的优点，但是需要指出的是，该模型的实现需要参与组合导航的多种技术能够独立的完成导航定位。例如，需要UWB设备能够提供行人的导航信息，这就要求目标行人所处的环境必须能够获取至少3个参考节点信息，这大大的降低了组合导航模型的应用范围，与此同时，参与导航的子技术独立完成定位，也引入了新的误差，不利于组合导航技术精度的提高。为了克服这一问题，学者们提出将紧组合模型应用于室内行人导航领域，紧组合模型直接将参与组合导航的子技术的原始
传感器数据应用于最后的导航信息的解算，减少了子技术自行解算引入新误差的
风险，提高了组合导航的精度，但是需要指出的是，现有紧组合导航模型均使用集中式模式，这一方式系统容错能力差，并不利于日益精确复杂的组合导航模型。
发明内容
[0005] 为了解决
现有技术的不足，本发明提供了一种基于分布式组合滤波INS/UWB行人导航系统，可降低室内复杂导航环境对组合导航精度的影响，得到目标行人导航信息的最优预估。
[0006] 本发明采用的技术方案是：
[0007] 一种基于分布式组合滤波INS/UWB行人导航系统，包括惯性导航器件、伪距检测单元、无线数据传输系统和
数据处理系统；
[0008] 所述惯性导航器件，用于测量行人的导航信息，所述导航信息包括位置、速度和
姿态信息；
[0009] 所述伪距检测单元，用于获取伪距信息：
[0010] 所述无线数据传输系统，与惯性导航器件和伪距检测单元相连，用于将惯性导航器件和伪距检测单元所采集的数据通过无线传输传递到
数据处理系统中，并将数据处理系统发送的控制命令传递到所述惯性导航器件中；
[0011] 所述数据处理系统：具有分布式组合
滤波器，所述分布式组合滤波器包括EKF
扩展卡尔曼滤波器和EFIR扩展
有限脉冲响应滤波器，根据当前无线通信信道的通信
质量选择上述分布式组合滤波器中的一种用于对采集到的数据进行数据融合估计，并向惯性导航器件发送控制命令。
[0012] 进一步的，所述伪距检测单元包括UWB定位标签和UWB参考节点；所述UWB参考节点预先放置在设定的位置，惯性导航器件和UWB定位标签分别固定在行人身上；通过测量UWB定位标签与UWB参考节点之间的距离，得到伪距信息。
[0013] 进一步的，所述UWB参考节点还可放置在任意位置。
[0014] 进一步的，所述分布式组合滤波器包括在各无线通信信道中设置子组合滤波器，子组合滤波器的结果再发送至主滤波器，根据主滤波器的结果和惯性导航器件测量的结果得到当前时刻的目标行人最优的位置信息。
[0015] 进一步的，通过所述分布式扩展卡尔曼滤波器或扩展有限脉冲响应滤波器对惯性导航器件获取的目标行人的导航信息，以及通过UWB定位标签获取的伪距信息在无线通信信道中进行数据融合估计。
[0016] 进一步的，在第i个无线通信信道中，检测当前通信信道的通信质量，若所述通信质量高于设定值时，采用分布式扩展卡尔曼滤波器将目标行人的导航信息和伪距信息进行数据融合估计，若所述通信质量低于设定值时，采用扩展有限脉冲响应滤波器对目标行人的导航信息和伪距信息进行数据融合估计。
[0017] 进一步的，通过所述分布式扩展卡尔曼滤波器或扩展有限脉冲响应滤波器对惯性导航器件获取的目标行人的导航信息，以及通过UWB定位标签获取的伪距信息在无线通信信道中进行数据融合估计。
[0018] 本发明还提出了一种基于分布式组合滤波INS/UWB行人导航方法，采用以下步骤：
[0019] (1)以惯性导航器件的误差向量和各UWB参考节点的位置信息作为状态量，以惯性导航器件采集的目标行人的位置信息和伪距信息作为系统观测量，构建INS/UWB紧组合导航模型；
[0020] (2)根据所述INS/UWB紧组合导航模型，在第i个无线通信信道中，检测当前通信信道的通信质量，根据当前无线通信信道的通信质量选择分布式组合滤波器中的一种用于对采集到的数据进行数据融合估计，得到当前无线通信信道、当前时刻的目标行人的导航信息最优估计，所述分布式组合滤波器包括EKF扩展卡尔曼滤波器和EFIR扩展有限脉冲响应滤波器；
[0021] (3)将各无线通信信道中的得到的最优估计再进行数据融合估计，得到目标行人最优位置信息的误差预估，将惯性导航器件采集到的目标行人位置信息与所述目标行人最优位置信息的误差预估相减，得到当前时刻的目标行人最优的位置信息。
[0022] 进一步的，在第i个无线通信信道中，检测当前通信信道的通信质量，若所述通信质量高于设定值时，采用分布式扩展卡尔曼滤波器将目标行人的导航信息和伪距信息进行数据融合估计，若所述通信质量低于设定值时，采用扩展有限脉冲响应滤波器对目标行人的导航信息和伪距信息进行数据融合估计。
[0023] 进一步的，状态量包括惯性导航器件的误差向量和各UWB参考节点的东向、北向位置信息。
[0024] 进一步的，所述系统观测量为目标行人的东向和北向位置信息的平方和与伪距信息平方之差。
[0025] 进一步的，所述惯性导航器件的误差向量和各UWB参考节点的东向、北向位置信息具有初始值，初始值为自定义值。
[0026] 进一步的，所述第i个无线通信信道中扩展卡尔曼滤波器/扩展有限脉冲响应滤波器的
状态方程为：
[0027]
[0028] 其中， 分别为k时刻和k+1时刻行人导航系统测量得到的导航
坐标系下目标行人的东向和北向两个个方向的速度误差；
[0029] 分别为k时刻和k+1时刻导航坐标系下目标行人的东向和北向两个个方向的位置误差；ωk为状态噪声，其协方差阵为Qith；T为
采样周期。
[0030] 进一步的，所述第i个无线通信信道中扩展卡尔曼滤波器/扩展有限脉冲响应滤波器的观测方程为：
[0031]
[0032] 其中， 为k时刻惯性导航器件IMU解算的东向和北向位置； 为k时刻惯性导航器件测量得到的未知节点到第i个参考节点之间的伪距； 为k时刻UWB测量得到的未知节点到第i个参考节点之间的伪距； 为第i个参考节点的坐标， 为观测噪声，其协方差阵为Qith。
[0033] 进一步的，所述无线通信信道中扩展卡尔曼滤波器/扩展有限脉冲响应滤波器中扩展卡尔曼
算法的
迭代方程为：
[0034]
[0035]
[0036]
[0037]
[0038]
[0039] 其中，
[0040] 进一步的，所述无线通信信道的通信质量检测方法为：
[0041] 首先计算无线通信信道的质量评价指标：
[0042]
[0043] 在此
基础上，判断 与
门限door的关系，如果 则信道质量正常，反之，则信道质量差，进入扩展有限脉冲响应滤波器滤波器。
[0044] 进一步的，所述无线通信信道中扩展卡尔曼滤波器/扩展有限脉冲响应滤波器中有限脉冲响应滤波算法的迭代方程为：
[0045]
[0046] 进一步的，所述主滤波器的迭代方程为：
[0047]
[0048]
[0049] 与现有技术相比，本发明的有益效果是：
[0050] 1、组合导航方法采用改进的INS/UWB紧组合模型，该模型以INS解算得到目标行人的东向和北向位置信息的平方和与UWB定位标签测量得到的伪距的平方之差作为系统观测量；在此基础上，通过EKF/EFIR滤波将INS和UWB获取的导航信息进行数据融合，最终得到当前时刻最优的导航信息和对参考节点位置信息的预估。提高了数据融合滤波器的精度和鲁棒性。
[0051] 2、通过构建分布式组合导航系统，将数据融合滤波器应用于UWB无线通信信道中，增加了系统的容错能力。
[0052] 3、可用于室内环境下的行人中高精度定位。
附图说明
[0053] 构成本
申请的一部分的
说明书附图用来提供对本申请的进一步理解，本申请的示意性
实施例及其说明用于解释本申请，并不构成对本申请的不当限定。
[0054] 图1为一种分布式INS/UWB紧组合导航系统示意图；
[0055] 图2为一种面向INS/UWB行人导航的分布式EKF/EFIR滤波方法的示意图。
[0056] 图3为一种面向INS/UWB行人导航的局部滤波器中的EKF/EFIR滤波算法示意图。
具体实施方式
[0057] 应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。
[0058] 需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
[0059] EKF：扩展卡尔曼滤波器
[0060] EFIR：扩展有限脉冲响应滤波器
[0061] INS：惯性导航系统
[0062] IMU：惯性导航器件
[0063] 实施例1：如图1所示，
[0064] 一种基于分布式组合滤波INS/UWB行人导航系统，包括惯性导航器件、伪距检测单元、无线数据传输系统和数据处理系统；惯性导航器件，用于测量行人的导航信息，所述导航信息包括位置、速度和姿态信息；伪距检测单元，用于获取伪距信息：无线数据传输系统，与惯性导航器件和伪距检测单元相连，用于将惯性导航器件和伪距检测单元所采集的数据通过无线传输传递到数据处理系统中，并将数据处理系统发送的控制命令传递到所述惯性导航器件中；
[0065] 数据处理系统：具有分布式组合滤波器，所述分布式组合滤波器包括包括EKF扩展卡尔曼滤波器和EFIR扩展有限脉冲响应滤波器，根据当前无线通信信道的通信质量选择上述分布式组合滤波器中的一种用于对采集到的数据进行数据融合估计，并向惯性导航器件发送控制命令。
[0066] 伪距检测单元包括UWB定位标签和UWB参考节点；UWB参考节点预先放置在设定的位置，惯性导航器件和UWB定位标签分别固定在行人身上；通过测量UWB定位标签与UWB参考节点之间的距离，得到伪距信息。
[0067] UWB参考节点还可放置在任意位置。
[0068] 分布式组合滤波器包括在各无线通信信道中设置子组合滤波器，子组合滤波器的结果再发送至主滤波器，根据主滤波器的结果和惯性导航器件测量的结果得到当前时刻的目标行人最优的位置信息。
[0069] 分布式组合滤波器选择分布式扩展卡尔曼滤波器/扩展有限脉冲响应滤波器的组合。通过所述分布式扩展卡尔曼滤波器或扩展有限脉冲响应滤波器对惯性导航器件获取的目标行人的导航信息，以及通过UWB定位标签获取的伪距信息在无线通信信道中进行数据融合估计。
[0070] 在第i个无线通信信道中，检测当前通信信道的通信质量，若通信质量高于设定值时，采用分布式扩展卡尔曼滤波器将目标行人的导航信息和伪距信息进行数据融合估计，若通信质量低于设定值时，采用扩展有限脉冲响应滤波器对目标行人的导航信息和伪距信息进行数据融合估计。
[0071] 通过分布式扩展卡尔曼滤波器或扩展有限脉冲响应滤波器对惯性导航器件获取的目标行人的导航信息，以及通过UWB定位标签获取的伪距信息在无线通信信道中进行数据融合估计。
[0072] 实施例2：如图2所示，一种基于分布式组合滤波INS/UWB行人导航方法，采用以下步骤：
[0073] (1)以惯性导航器件的误差向量和各UWB参考节点的位置信息作为状态量，以惯性导航器件采集的目标行人的位置信息和伪距信息作为系统观测量，构建INS/UWB紧组合导航模型；
[0074] (2)根据所述INS/UWB紧组合导航模型，在第i个无线通信信道中，检测当前通信信道的通信质量，根据当前无线通信信道的通信质量选择分布式组合滤波器中的一种用于对采集到的数据进行数据融合估计，得到当前无线通信信道、当前时刻的目标行人的导航信息最优估计，所述分布式组合滤波器包括EKF扩展卡尔曼滤波器和EFIR扩展有限脉冲响应滤波器；
[0075] (3)将各无线通信信道中的得到的最优估计再进行数据融合估计，得到目标行人最优位置信息的误差预估，将惯性导航器件采集到的目标行人位置信息与所述目标行人最优位置信息的误差预估相减，得到当前时刻的目标行人最优的位置信息。
[0076] 在第i个无线通信信道中，检测当前通信信道的通信质量，若所述通信质量高于设定值时，采用分布式扩展卡尔曼滤波器将目标行人的导航信息和伪距信息进行数据融合估计，若所述通信质量低于设定值时，采用扩展有限脉冲响应滤波器对目标行人的导航信息和伪距信息进行数据融合估计。
[0077] 状态量包括惯性导航器件的误差向量和各UWB参考节点的东向、北向位置信息，系统观测量为目标行人的东向和北向位置信息的平方和与伪距信息平方之差。
[0078] 惯性导航器件的误差向量和各UWB参考节点的东向、北向位置信息具有初始值，初始值为自定义值。
[0079] 实施例3：一种改进的基于分布式组合滤波器的INS/UWB行人导航系统包括：惯性导航器件INS、UWB定位标签、UWB参考节点和数据处理系统；
[0080] 惯性导航器件INS和UWB定位标签分别设置在行人的帽子上，UWB参考节点设置在任意位置，惯性导航器件INS和UWB定位标签分别与数据处理系统连接。
[0081] 其中，惯性导航器件INS：用于测量行人的位置、速度和姿态等导航信息；
[0082] UWB定位标签：用于测量UWB定位标签与参考节点之间的距离信息，即伪距信息；
[0083] UWB参考节点：预先放置在已知的位置，便于测量与定位标签之间的距离；
[0084] 数据处理系统：用于对采集到的
传感器数据进行数据融合。
[0085] 数据处理系统包括EKF/EFIR滤波器，通过所述EKF/EFIR对惯性导航器件INS在本地相对坐标系中获取的目标行人的导航信息以及UWB定位标签在本地相对坐标系中获取的伪距信息进行数据融合。
[0086] 实施例4：如图3所示，一种基于分布式组合滤波INS/UWB行人导航方法[0087] (1)以惯性导航器件INS的误差向量作为状态量，以惯性导航器件INS测量得到的目标行人的东向和北向位置信息的平方和与UWB定位标签测量得到的伪距的平方之差作为系统观测量，在通信信道中构建分布式INS/UWB紧组合模型；
[0088] (2)通过EKF/EFIR滤波器将惯性导航器件INS和UWB定位标签在本地相对坐标系中获取的目标行人的导航信息进行数据融合；EKF/EFIR滤波器的输出即得到当前时刻基于无线信道获取的目标行人最优的导航信息以及参考节点位置的最优估计。
[0089] 所述第i个无线通信信道中EKF/EFIR滤波器的状态方程为：
[0090]
[0091] 其中， 分别为k时刻和k+1时刻INS测量得到的导航坐标系下目标行人的东向和北向两个个方向的速度误差； 分别为k
时刻和k+1时刻导航坐标系下目标行人的东向和北向两个个方向的位置误差；ωk为状态噪声，其协方差阵为Qith；T为采样周期。
[0092] 所述第i个无线通信信道中EKF/EFIR滤波器的观测方程为：
[0093]
[0094] 其中， 为k时刻惯性器件IMU解算的东向和北向位置； 为k时刻IMU测量得到的未知节点到第i个参考节点之间的伪距； 为k时刻UWB测量得到的未知节点到第i个参考节点之间的伪距； 为第i个参考节点的坐标， 为观测噪声，其协方差阵为Qith。
[0095] 所述无线通信信道中EKF/EFIR滤波器中EKF算法的迭代方程为：
[0096]
[0097]
[0098]
[0099]
[0100]
[0101] 其中，
[0102] 所述无线通信信道中EKF/EFIR滤波器中EKF算法的性能评估方法为：
[0103] 首先计算信道质量评价指标：
[0104]
[0105] 在此基础上，判断 与门限door的关系，如果 则信道质量正常，反之，则信道质量差，准入FIR滤波器。
[0106] 所述无线通信信道中EKF/EFIR滤波器中FIR算法的迭代方程为：
[0107]
[0108] 所述主滤波器的迭代方程为：
[0109]
[0110]
[0111] 以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何
修改、等同替换、改进等，均应包含在本申请的保护范围之内。
