法律状态公告日
法律状态信息
法律状态
2020-05-05
授权
授权
2018-11-30
实质审查的生效 IPC(主分类):G01C21/16 申请日:20180806
实质审查的生效
2018-11-06
公开
公开
技术领域
本发明涉及复杂环境下组合定位技术领域,尤其涉及面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及系统。
背景技术
近年来,行人导航(Pedestrian Navigation,PN)作为导航技术应用的新兴领域,正越来越受到各国学者的重视,并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地下停车场等室内环境下,外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行有效的融合以消除室内复杂环境影响,保证行人导航精度的持续稳定,具有重要的科学理论意义和实际应用价值。
在现有的定位方式中,全球卫星导航系统(Global Navigation SatelliteSystem,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息,但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围,特别是在室内、地下巷道等一些密闭的、环境复杂的场景,GNSS信号被严重遮挡,无法进行有效的工作。近年来,UWB(Ultra Wideband)以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜力。学者们提出将基于UWB的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位,但是由于室内环境复杂多变,UWB信号十分容易受到干扰而导致定位精度下降甚至失锁;与此同时,由于UWB采用的通信技术通常为短距离无线通信技术,因此若想完成大范围的室内目标跟踪定位,需要大量的网络节点共同完成,这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟踪在室内导航领域仍旧面临很多挑战。
发明内容
本发明的目的就是为了解决由于在实时系统中UWB由于受到室内环境的影响不能得到正常的距离信息的问题,提出了一种面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及系统,该方法对传统的自适应Kalman滤波算法进行改进,引入变量
为实现上述目的,本发明的具体方案如下:
本发明的第一目的是公开一种面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法,包括:
以惯性导航器件INS在t时刻在导航系下的位置误差、速度误差、姿态误差、加速度误差和角速度误差作为状态量,以INS与UWB分别测量的目标节点与参考节点之间距离的差值作为系统观测量,构建滤波模型;
利用自适应Kalman滤波算法对位置误差进行预估,预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失,如果有,对缺失的距离信息进行预估;
最终得到当前时刻目标行人最优的导航信息。
进一步地,所述自适应Kalman预估滤波器的状态方程为:
其中,
进一步地,所述从载体系到导航系的姿态转移矩阵
其中,(γ ψ θ)为t时刻载体的横摇、纵摇和航向角。
进一步地,所述自适应Kalman预估滤波器的的观测方程为:
其中,Xt|t-1为由t-1时刻得到的t时刻的状态向量;δdi,t,i∈(1,2,...,g)为t时刻惯性导航器件INS和UWB分别测量得到的参考节点与未知节点之间的距离的差;g为参考节点的数目;
PE,t为t时刻的东向位置,PN,t为t时刻的北向位置。
进一步地,所述的预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失,如果有,对缺失的距离信息进行预估,具体为:
引入变量
进一步地,对缺失数据进行预估后,自适应Kalman预估滤波器的观测方程变为:
进一步地,所述利用自适应Kalman滤波算法对位置误差进行预估,具体为:
Pt=(I-KtHt)Pt|t-1;
其中,Kt表示KF在t时刻的误差增益矩阵;I表示单位矩阵,Pt|t-1表示KF由t-1时刻到t时刻的最小预测均方误差矩阵,
本发明的第二目的是公开一种面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波系统,包括服务器,所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时实现以下步骤:
以惯性导航器件INS在t时刻在导航系下的位置误差、速度误差、姿态误差、加速度误差和角速度误差作为状态量,以INS与UWB分别测量的目标节点与参考节点之间距离的差值作为系统观测量,构建滤波模型;
利用自适应Kalman滤波算法对位置误差进行预估,预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失,如果有,对缺失的距离信息进行预估;
最终得到当前时刻目标行人最优的导航信息。
本发明的第三目的是公开一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时执行以下步骤:
以惯性导航器件INS在t时刻在导航系下的位置误差、速度误差、姿态误差、加速度误差和角速度误差作为状态量,以INS与UWB分别测量的目标节点与参考节点之间距离的差值作为系统观测量,构建滤波模型;
利用自适应Kalman滤波算法对位置误差进行预估,预估过程中实时判断UWB测量得到的参考节点与未知节点之间的距离信息是否有缺失,如果有,对缺失的距离信息进行预估;
最终得到当前时刻目标行人最优的导航信息。
本发明的有益效果:
1、通过引入变量
2、可用于室内环境下的中高精度定位。
附图说明
图1为一种面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法的实现系统的示意图;
图2为本发明构建滤波模型进行数据融合示意图;
图3为自适应预估Kalman滤波算法流程图。
具体实施方式:
下面结合附图对本发明进行详细说明:
本发明一种面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法的实现统如图1所示,包括:组合导航算法采用UWB和INS两种导航系统,其中,UWB包括UWB参考节点和UWB定位标签,UWB参考节点预先固定在已知坐标上,UWB定位标签固定在目标行人上。INS主要由固定在目标行人足部的IMU组成。
基于上述系统,本发明公开了具有数据缺失INS/UWB紧组合行人导航的自适应预估Kalman滤波算法,包括:
(1)如图2所示,以惯性导航器件INS在t时刻在导航系下的位置误差、速度误差、姿态误差、加速度误差和角速度误差作为状态量,以INS与UWB分别测量的目标节点与参考节点之间距离的差值作为系统观测量,构建滤波模型进行数据融合;
(2)利用自适应Kalman滤波算法对位置误差进行预估,自适应Kalman预估滤波器的状态方程为:
其中,
其中,(γ ψ θ)为t时刻的横摇、纵摇和航向角;
其中,
自适应Kalman预估滤波器的的观测方程为:
其中,Xt|t-1为由t-1时刻得到的t时刻的状态向量;δdi,t,i∈(1,2,...,g)为t时刻惯性导航器件INS和UWB分别测量得到的参考节点与未知节点之间的距离的差;g为参考节点的数目;
PE,t为t时刻的东向位置,PN,t为t时刻的北向位置。
其中,(δPE,t,δPN,t)为东向和北向的位置误差;νt为系统t时刻的观测噪声,其协方差矩阵为Rt。在此基础上,判断一下距离信息是否可用,引入变量
进一步地自适应Kalman预估滤波算法的步骤如图3所示:
首先,进行一步预估
判断一下距离信息是否可用,引入变量
其中,HtXt|t-1(i,1)表示用矩阵HtXt|t-1的第i行第1列替代不可用的距离信息。
Pt=(I-KtHt)Pt|t-1
上述虽然结合附图对本发明的具体实施方式进行了描述,但并非对本发明保护范围的限制,所属领域技术人员应该明白,在本发明的技术方案的基础上,本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。
机译: 基于Kalman滤波算法的预测和更新行为来确定自动车辆状态信息的系统
机译: 行人路线引导导航系统,行人路线引导导航系统和行人路线引导导航程序
机译: 行人导航设备,行人导航系统,行人导航方法和程序