采用伪距和位置信息的室内行人uwb/ins紧组合导航系统及方法
【专利摘要】本发明公开了一种采用伪距和位置信息的室内行人UWB/INS紧组合导航系统及方法，包括：惯性导航器件INS、UWB定位标签、UWB参考节点、数据处理系统；所述惯性导航器件INS和UWB定位标签分别设置在行人的鞋子和身体上，所述UWB参考节点设置在任意位置，所述惯性导航器件INS和UWB定位标签分别与数据处理系统连接。本发明有益效果：通过将参考节点的位置信息引入状态变量中，并通过滤波器完成对参考节点位置信息的预估，减少了组合导航系统需要预先获取参考节点位置信息以及组合导航系统的定位精度依赖于预先获取到的参考节点位置信息精度的缺点。
【专利说明】
采用伪距和位置信息的室内行人UWB/1 NS紧组合导航系统及 方法
技术领域
[0001] 本发明涉及复杂环境下组合定位技术领域，尤其涉及一种采用伪距信息和位置信 息的室内行人UWB/INS紧组合导航系统及方法。
【背景技术】
[0002] 近年来，行人导航(Pedestrian Navigation，PN)作为导航技术应用的新兴领域， 正越来越受到各国学者的重视，并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地 下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航 信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行 有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理 论意义和实际应用价值。
[0003] 在现有的定位方式中，全球卫星导航系统（Global Navigation Satellite SyStem，GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是 其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道 等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。近年来，UWB (Ultra Wideband)以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很 大的潜力。学者们提出将基于UWB的目标跟踪应用于GNSS失效环境下的行人导航。这种方式 虽然能够实现室内定位，但是由于室内环境复杂多变，UWB信号十分容易受到干扰而导致定 位精度下降甚至失锁；与此同时，由于UWB采用的通信技术通常为短距离无线通信技术，因 此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络 组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟 踪在室内导航领域仍旧面临很多挑战。
[0004] 在导航模型方面，目前在室内行人组合导航领域应用较多的为松组合导航模型。 该模型具有容易实现的优点，但是需要指出的是，该模型的实现需要参与组合导航的多种 技术能够独立的完成导航定位。例如，需要UWB设备能够提供行人的导航信息，这就要求目 标行人所处的环境必须能够获取至少3个参考节点信息，这大大的降低了组合导航模型的 应用范围，与此同时，参与导航的子技术独立完成定位，也引入了新的误差，不利于组合导 航技术精度的提高。为了克服这一问题，学者们提出将紧组合模型应用于室内行人导航领 域，紧组合模型直接将参与组合导航的子技术的原始传感器数据应用于最后的导航信息的 解算，减少了子技术自行解算引入新误差的风险，提高了组合导航的精度，但是需要指出的 是，紧组合导航模型需要提前获取参与组合的参考节点的精确位置信息，这限制了导航模 型的灵活性。
【发明内容】
[0005] 本发明的目的就是为了解决上述难题，提供了一种采用伪距和位置信息的室内行 人UWB/INS紧组合导航系统及方法，该系统及方法无需提前获取参考节点的位置信息，而是 将参考节点的位置信息引入状态变量中，并通过滤波器完成对参考节点位置信息的预估。
[0006] 为实现上述目的，本发明的具体方案如下：
[0007] 一种采用伪距和位置信息的室内行人UWB/INS紧组合导航系统，包括：
[0008]惯性导航器件INS:用于测量行人的位置、速度和姿态导航信息；
[0009] UWB定位标签：用于测量UWB定位标签与UWB参考节点之间的距离信息，即伪距信 息；
[0010] UWB参考节点:预先放置在设定的位置，便于测量与定位标签之间的距离；
[0011] 数据处理系统:用于对采集到的传感器数据进行数据融合；
[0012] 所述惯性导航器件INS和UWB定位标签分别设置在行人的鞋子和身体上，所述UWB 参考节点设置在任意位置，所述惯性导航器件INS和UWB定位标签分别与数据处理系统连 接。
[0013] 进一步地，所述数据处理系统包括迭代扩展卡尔曼滤波器，通过所述迭代扩展卡 尔曼滤波器对惯性导航器件INS在本地相对坐标系中获取的目标行人的导航信息以及UWB 定位标签在本地相对坐标系中获取的伪距信息进行数据融合。
[0014] 一种采用伪距和位置信息的室内行人UWB/INS紧组合导航方法，包括：
[0015] (1)以惯性导航器件INS的误差向量和各UWB参考节点的东向和北向位置作为状态 量，以惯性导航器件INS测量得到的目标行人的东向和北向位置信息的平方和与UWB定位标 签测量得到的伪距的平方之差作为系统观测量，构建UWB/INS组合导航模型；
[0016] (2)利用所述UWB/INS组合导航模型，通过迭代扩展卡尔曼滤波器将惯性导航器件 INS在本地相对坐标系中获取的目标行人的导航信息以及UWB定位标签在本地相对坐标系 中获取的伪距信息进行数据融合；
[0017] 最终得到当前时刻目标行人最优的导航信息以及参考节点位置的最优估计。
[0018]进一步地，作为状态量的惯性导航器件INS的误差向量以及各UWB参考节点的东向 和北向位置的初值根据实际需要自行设定。
[0019]进一步地，所述迭代扩展卡尔曼滤波器的状态方程为：
h gpn.
[0021] 其中，^了 为15维INS误差向量； Y /V(wA-+l)2Aixl
[0022] = [[if、Pf、],[/T2 f ]，…，[^f]]二为2M维参考节点坐标向量； M为参考节点数目； 0 am
[0023] 4^')= ~aDk 0 (4 ;嶙4]为惯性导航器件INS测量得到的导航坐标 _ClM ~^Ek 〇 系下目标行人的东向、北向和天向三个方向的加速度误差；
[0024]巾k、巾k+1分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下目标 行人的横摇、纵摇和航向角误差yvr、分别为k时刻和k+i时刻ins测量得到的导航坐 标系下目标行人的东向、北向和天向三个方向的速度误差;▽丨、分别为k时刻和k+1时刻 导航坐标系下目标行人的东向、北向和天向三个方向的加速度误差;4、分别为k时刻 和k+i时刻导航坐标系下目标行人的东向、北向和天向三个方向的角速度误差；、#/分 别为k时刻和k+1时刻导航坐标系下目标行人的东向、北向和天向三个方向的位置误差；
[0025] ? k为状态噪声;T为采样周期；I为单位矩阵;C!为从载体系到导航系的状态转移矩 阵。
[0026] 进一步地，所述迭代扩展卡尔曼滤波器的观测方程为：
[0028] 其中，(巧,〇为k时亥lj INS解算的东向和北向位置;(咕叱:;)为k时亥lj INS解算的 东向和北向位置误差;…，岣为k时刻UWB测量得到的未知节点到第i个参考节点之 间的伪距；= 1,2, 为第i个参考节点的坐标。
[0029] 本发明的有益效果：
[0030] 1、组合导航方法采用改进的INS/UWB紧组合模型，该模型以INS解算得到目标行人 的东向和北向位置信息的平方和与UWB定位标签测量得到的伪距的平方之差作为系统观测 量;在此基础上，通过迭代扩展卡尔曼滤波将INS和UWB获取的导航信息进行数据融合，最终 得到当前时刻最优的导航信息和对参考节点位置信息的预估。
[0031] 2、现有的紧组合定位方法需要预先获取参考节点的位置信息，在此基础上再做定 位，那这就会出现一个问题，就是参考节点的位置信息的精度会对组合导航系统的精度有 一定的影响。本发明方法将参考节点的位置信息加入到状态变量中，人工设定初值即可，不 需要提前获取参考节点的位置信息就能够直接进行预估。
[0032] 3、本发明通过将参考节点的位置信息引入状态变量中，并通过滤波器完成对参考 节点位置信息的预估，减少了组合导航系统需要预先获取参考节点位置信息以及组合导航 系统的定位精度依赖于预先获取到的参考节点位置信息精度的缺点。
[0033] 4、可用于室内环境下的智能鞋子的中高精度定位。
【附图说明】
[0034]图1为采用伪距和位置信息的室内行人UWB/INS紧组合导航系统示意图；
[0035]图2为采用伪距和位置信息的室内行人UWB/INS紧组合导航方法的示意图。
【具体实施方式】：
[0036]下面结合附图对本发明进行详细说明：
[0037] 一种采用伪距信息和位置信息的室内行人UWB/INS紧组合导航，如图1所示，包括： 惯性导航器件INS、UWB定位标签、UWB参考节点和数据处理系统；
[0038]惯性导航器件INS和UWB定位标签分别设置在行人的鞋子和身体上，UWB参考节点 设置在任意位置，惯性导航器件INS和UWB定位标签分别与数据处理系统连接。
[0039] 其中，惯性导航器件INS:用于测量行人的位置、速度和姿态等导航信息；
[0040] UWB定位标签:用于测量UWB定位标签与参考节点之间的距离信息，即伪距信息； [0041] UWB参考节点:预先放置在已知的位置，便于测量与定位标签之间的距离；
[0042]数据处理系统：用于对采集到的传感器数据进行数据融合。
[0043]数据处理系统包括迭代扩展卡尔曼滤波器，通过所述迭代扩展卡尔曼滤波器对惯 性导航器件INS在本地相对坐标系中获取的目标行人的导航信息以及UWB定位标签在本地 相对坐标系中获取的伪距信息进行数据融合。
[0044] -种面向室内行人的UWB/INS紧组合导航方法，如图2所示，包括：
[0045] (1)以惯性导航器件INS的误差向量和各UWB参考节点的东向和北向位置作为状态 量，以惯性导航器件INS测量得到的目标行人的东向和北向位置信息的平方和与UWB定位标 签测量得到的伪距的平方之差作为系统观测量，构建INS/UWB组合SLAM模型；
[0046] 作为状态量的惯性导航器件INS的误差向量以及各UWB参考节点的东向和北向位 置的初值根据实际需要自行设定，因此不需要预先获知各UWB参考节点的位置信息，避免了 组合导航系统的定位精度依赖于预先获取到的参考节点位置信息精度的问题。
[0047] (2)通过迭代扩展卡尔曼滤波器将惯性导航器件INS和UWB定位标签在本地相对坐 标系中获取的目标行人的导航信息进行数据融合;迭代扩展卡尔曼滤波器的输出即得到当 前时刻目标行人最优的导航信息以及参考节点位置的最优估计。
[0048] 根据INS/UWB组合SLAM模型，得到的迭代扩展卡尔曼滤波器的状态方程为：
-I -
[0050] 其中，为15维INS误差向量； Y k^l
[0051] X,則=[[#' Pf:],.?.,[#? 户 M为参考节点数目； 0 4 -a'k
[0052] ^(f；^ -a'pi 0 ; [4 4k 4]为惯性导航器件INS测量得到的导航坐标 _ a\k ~aa 0 系下的东向、北向和天向三个方向的加速度误差；
[0053] (i>k、(i)k+1分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下的横 摇、纵摇和航向角误差;、（?￥二分别为k时刻和k+1时刻INS测量得到的导航坐标系下的 东向、北向和天向三个方向的速度误差;g、VL,分别为k时刻和k+1时刻导航坐标系下的东 向、北向和天向三个方向的加速度误差；4、. 分别为k时刻和k+1时刻导航坐标系下的 东向、北向和天向三个方向的角速度误差;spr、分别为k时刻和k+i时刻导航坐标系下 的东向、北向和天向三个方向的位置误差；
[0054] co k为状态噪声；T为采样周期；I为单位矩阵；C:为从载体系到导航系的状态转移 矩阵。
[0055] 根据INS/UWB组合SLAM模型，得到的迭代扩展卡尔曼滤波器的观测方程为：
[0057] 其中，(松,〇为k时亥ljINS解算的东向和北向位置;(叱';，机::)为k时亥ljINS解算的 东向和北向位置误差；…，W为k时刻UWB测量得到的未知节点到第i个参考节点 之间的伪距;)(/ = 1,2,..为第i个参考节点的坐标。
[0058] 上述虽然结合附图对本发明的【具体实施方式】进行了描述，但并非对本发明保护范 围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不 需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。
【主权项】
1. 一种采用伪距和位置信息的室内行人UWB/INS紧组合导航系统，其特征是，包括： 惯性导航器件INS:用于测量行人的位置、速度和姿态导航信息； UWB定位标签:用于测量UWB定位标签与UWB参考节点之间的距离信息，即伪距信息； UWB参考节点:预先放置在设定的位置，便于测量与定位标签之间的距离； 数据处理系统:用于对采集到的传感器数据进行数据融合； 所述惯性导航器件INS和UWB定位标签分别设置在行人的鞋子和身体上，所述UWB参考 节点设置在任意位置，所述惯性导航器件INS和UWB定位标签分别与数据处理系统连接。2. 如权利要求1所述的一种采用伪距和位置信息的室内行人UWB/INS紧组合导航系统， 其特征是，所述数据处理系统包括迭代扩展卡尔曼滤波器，通过所述迭代扩展卡尔曼滤波 器对惯性导航器件INS在本地相对坐标系中获取的目标行人的导航信息以及UWB定位标签 在本地相对坐标系中获取的伪距信息进行数据融合。3. -种采用伪距和位置信息的室内行人UWB/INS紧组合导航方法，其特征是，包括： (1) 以惯性导航器件INS的误差向量和各UWB参考节点的东向和北向位置作为状态量， 以惯性导航器件INS测量得到的目标行人的东向和北向位置信息的平方和与UWB定位标签 测量得到的伪距的平方之差作为系统观测量，构建UWB/INS组合导航模型； (2) 利用所述UWB/INS组合导航模型，通过迭代扩展卡尔曼滤波器将惯性导航器件INS 在本地相对坐标系中获取的目标行人的导航信息以及UWB定位标签在本地相对坐标系中获 取的伪距信息进行数据融合； 最终得到当前时刻目标行人最优的导航信息以及参考节点位置的最优估计。4. 如权利要求3所述的一种采用伪距和位置信息的室内行人UWB/INS紧组合导航方法， 其特征是，作为状态量的惯性导航器件INS的误差向量以及各UWB参考节点的东向和北向位 置的初值根据实际需要自行设定。5. 如权利要求3所述的一种采用伪距和位置信息的室内行人UWB/INS紧组合导航方法， 其特征是，所述迭代扩展卡尔曼滤波器的状态方程为：圳加丨=[[f Ακν]，.·.5[4~ir' 考节点数目； 为惯性导航器件INS测量得到的导航坐标系下目标行人的东向、北向和天向三个方向的加速度误差； (K、(i)k+1分别为k时刻和k+Ι时刻惯性导航器件INS测量得到的导航坐标系下目标行人 的横摇、纵摇和航向角误差、?ΛΙ分别为k时刻和k+Ι时刻INS测量得到的导航坐标系 下目标行人的东向、北向和天向三个方向的速度误差；▽〗、巧 +1分别为k时刻和k+Ι时刻导航 坐标系下目标行人的东向、北向和天向三个方向的加速度误差;4分别为k时刻和k+ 1时刻导航坐标系下目标行人的东向、北向和天向三个方向的角速度误差;#Γ、分别为k 时刻和k+1时刻导航坐标系下目标行人的东向、北向和天向三个方向的位置误差； ω k为状态噪声;T为采样周期；I为单位矩阵;C:为从载体系到导航系的状态转移矩阵。6.如权利要求3所述的一种采用伪距和位置信息的室内行人UWB/INS紧组合导航方法， 其特征是，所述迭代扩展卡尔曼滤波器的观测方程为：其中，为k时亥UINS解算的东向和北向位置;为k时亥UINS解算的东向 和北向位置误差;为k时刻UWB测量得到的未知节点到第i个参考节点之间的 伪距；= 为第i个参考节点的坐标。
【文档编号】G01C21/16GK105928518SQ201610231406
【公开日】2016年9月7日
【申请日】2016年4月14日
【发明人】徐元, 张勇, 程金, 赵钦君, 王滨, 王宜敏, 马思源
【申请人】济南大学