采用伪距和位置信息的室内行人UWB/INS紧组合导航系统及方法

摘要

本发明公开了一种采用伪距和位置信息的室内行人UWB/INS紧组合导航系统及方法，包括：惯性导航器件INS、UWB定位标签、UWB参考节点、数据处理系统；所述惯性导航器件INS和UWB定位标签分别设置在行人的鞋子和身体上，所述UWB参考节点设置在任意位置，所述惯性导航器件INS和UWB定位标签分别与数据处理系统连接。本发明有益效果：通过将参考节点的位置信息引入状态变量中，并通过滤波器完成对参考节点位置信息的预估，减少了组合导航系统需要预先获取参考节点位置信息以及组合导航系统的定位精度依赖于预先获取到的参考节点位置信息精度的缺点。

说明书

技术领域

本发明涉及复杂环境下组合定位技术领域，尤其涉及一种采用伪距信息和位置信息的室内行人UWB/INS紧组合导航系统及方法。

背景技术

近年来，行人导航(Pedestrian Navigation,PN)作为导航技术应用的新兴领域，正越来越受到各国学者的重视，并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理论意义和实际应用价值。

在现有的定位方式中，全球卫星导航系统(Global Navigation Satellite System,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。近年来，UWB(Ultra Wideband)以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜力。学者们提出将基于UWB的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，UWB信号十分容易受到干扰而导致定位精度下降甚至失锁；与此同时，由于UWB采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟踪在室内导航领域仍旧面临很多挑战。

在导航模型方面，目前在室内行人组合导航领域应用较多的为松组合导航模型。该模型具有容易实现的优点，但是需要指出的是，该模型的实现需要参与组合导航的多种技术能够独立的完成导航定位。例如，需要UWB设备能够提供行人的导航信息，这就要求目标行人所处的环境必须能够获取至少3个参考节点信息，这大大的降低了组合导航模型的应用范围，与此同时，参与导航的子技术独立完成定位，也引入了新的误差，不利于组合导航技术精度的提高。为了克服这一问题，学者们提出将紧组合模型应用于室内行人导航领域，紧组合模型直接将参与组合导航的子技术的原始传感器数据应用于最后的导航信息的解算，减少了子技术自行解算引入新误差的风险，提高了组合导航的精度，但是需要指出的是，紧组合导航模型需要提前获取参与组合的参考节点的精确位置信息，这限制了导航模型的灵活性。

发明内容

本发明的目的就是为了解决上述难题，提供了一种采用伪距和位置信息的室内行人UWB/INS紧组合导航系统及方法，该系统及方法无需提前获取参考节点的位置信息，而是将参考节点的位置信息引入状态变量中，并通过滤波器完成对参考节点位置信息的预估。

为实现上述目的，本发明的具体方案如下：

一种采用伪距和位置信息的室内行人UWB/INS紧组合导航系统，包括：

惯性导航器件INS：用于测量行人的位置、速度和姿态导航信息；

UWB定位标签：用于测量UWB定位标签与UWB参考节点之间的距离信息，即伪距信息；

UWB参考节点：预先放置在设定的位置，便于测量与定位标签之间的距离；

数据处理系统：用于对采集到的传感器数据进行数据融合；

所述惯性导航器件INS和UWB定位标签分别设置在行人的鞋子和身体上，所述UWB参考节点设置在任意位置，所述惯性导航器件INS和UWB定位标签分别与数据处理系统连接。

进一步地，所述数据处理系统包括迭代扩展卡尔曼滤波器，通过所述迭代扩展卡尔曼滤波器对惯性导航器件INS在本地相对坐标系中获取的目标行人的导航信息以及UWB定位标签在本地相对坐标系中获取的伪距信息进行数据融合。

一种采用伪距和位置信息的室内行人UWB/INS紧组合导航方法，包括：

(1)以惯性导航器件INS的误差向量和各UWB参考节点的东向和北向位置作为状态量，以惯性导航器件INS测量得到的目标行人的东向和北向位置信息的平方和与UWB定位标签测量得到的伪距的平方之差作为系统观测量，构建UWB/INS组合导航模型；

(2)利用所述UWB/INS组合导航模型，通过迭代扩展卡尔曼滤波器将惯性导航器件INS在本地相对坐标系中获取的目标行人的导航信息以及UWB定位标签在本地相对坐标系中获取的伪距信息进行数据融合；

最终得到当前时刻目标行人最优的导航信息以及参考节点位置的最优估计。

进一步地，作为状态量的惯性导航器件INS的误差向量以及各UWB参考节点的东向和北向位置的初值根据实际需要自行设定。

进一步地，所述迭代扩展卡尔曼滤波器的状态方程为：

$\left(\begin{array}{c}{\phi }_{k+1}\\ {\mathrm{\delta V}}_{k+1}^n\\ {\mathrm{\delta P}}_{k+1}^n\\ {▿}_{k+1}^b\\ {ϵ}_{k+1}^b\\ X_{(m,k+1)2M\times 1}\end{array}\right)=\left(\begin{array}{cccccc}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& -I_{3\times 3}·C_b^n·T& 0_{3\times 2M}\\ S\left(f_k^n\right)·T& I_{3\times 3}& 0_{3\times 3}& I_{3\times 3}{C}_b^n·T& 0_{3\times 3}& 0_{3\times 2M}\\ 0_{3\times 3}& I_{3\times 3}·T& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 2M}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 2M}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 2M}\\ 0_{2M\times 3}& 0_{2M\times 3}& 0_{2M\times 3}& 0_{2M\times 3}& 0_{2M\times 3}& I_{2M\times 2M}\end{array}\right)\left(\begin{array}{c}{\phi }_k\\ {\mathrm{\delta V}}_k^n\\ {\mathrm{\delta P}}_k^n\\ {▿}_k^b\\ {ϵ}_k^b\\ X_{(m,k)2M\times 1}\end{array}\right)+{\omega }_k$

其中，为15维INS误差向量；

为2M维参考节点坐标向量；M为参考节点数目；

为惯性导航器件INS测量得到的导航坐标系下目标行人的东向、北向和天向三个方向的加速度误差；

φ_k、φ_k+1分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下目标行人的横摇、纵摇和航向角误差；分别为k时刻和k+1时刻INS测量得到的导航坐标系下目标行人的东向、北向和天向三个方向的速度误差；分别为k时刻和k+1时刻导航坐标系下目标行人的东向、北向和天向三个方向的加速度误差；分别为k时刻和k+1时刻导航坐标系下目标行人的东向、北向和天向三个方向的角速度误差；分别为k时刻和k+1时刻导航坐标系下目标行人的东向、北向和天向三个方向的位置误差；

ω_k为状态噪声；T为采样周期；I为单位矩阵；为从载体系到导航系的状态转移矩阵。

进一步地，所述迭代扩展卡尔曼滤波器的观测方程为：

$\begin{array}{c}\left(\begin{array}{c}{\left(P_{E,k}^{INS}\right)}^2+{\left(P_{N,k}^{INS}\right)}^2-{\left({\rho }_{1,k}^{UWB}\right)}^2\\ {\left(P_{E,k}^{INS}\right)}^2+{\left(P_{N,k}^{INS}\right)}^2-{\left({\rho }_{2,k}^{UWB}\right)}^2\\ .\\ .\\ .\\ {\left(P_{E,k}^{INS}\right)}^2+{\left(P_{N,k}^{INS}\right)}^2-{\left({\rho }_{m,k}^{UWB}\right)}^2\end{array}\right)=\\ \left(\begin{array}{c}2(P_{E,k}^{INS}-P_{E,k}^{{\mathrm{RN}}_1}){\mathrm{\delta P}}_{E,k}^n+2(P_{N,k}^{INS}-P_{N,k}^{{\mathrm{RN}}_1}){\mathrm{\delta P}}_{N,k}^n-({\left({\mathrm{\delta P}}_{E,k}^n\right)}^2+({\mathrm{\delta P}}_{N,k}^2\left)\right)+2(P_{E,k}^{INS}{P}_{E,k}^{{\mathrm{RN}}_1}+P_{N,k}^{INS}{P}_{N,k}^{{\mathrm{RN}}_1})-({\left(P_{E,k}^{{\mathrm{RN}}_1}\right)}^2+{\left(P_{N,k}^{{\mathrm{RN}}_1}\right)}^2)\\ 2(P_{E,k}^{INS}-P_{E,k}^{{\mathrm{RN}}_2}){\mathrm{\delta P}}_{E,k}^n+2(P_{N,k}^{INS}-P_{N,2}^{RN}){\mathrm{\delta P}}_{N,k}^n-({\left({\mathrm{\delta P}}_{E,k}^n\right)}^2+{\left({\mathrm{\delta P}}_{N,k}^2\right)}^2)+2(P_{E,k}^{INS}{P}_{E,k}^{{\mathrm{RN}}_2}+P_{N,k}^{INS}{P}_{N,k}^{{\mathrm{RN}}_2})-({\left(P_{E,k}^{{\mathrm{RN}}_2}\right)}^2+{\left(P_{N,k}^{{\mathrm{RN}}_2}\right)}^2)\\ .\\ .\\ .\\ 2(P_{E,k}^{INS}-P_{E,k}^{{\mathrm{RN}}_M}){\mathrm{\delta P}}_{E,k}^n+2(P_{N,k}^{INS}-P_{N,k}^{{\mathrm{RN}}_M}){\mathrm{\delta P}}_{N,k}^n-({\left({\mathrm{\delta P}}_{E,k}^n\right)}^2+{\left({\mathrm{\delta P}}_{N,k}^2\right)}^2)+2(P_{E,k}^{INS}{P}_{E,k}^{{\mathrm{RN}}_M}+P_{N,k}^{INS}{P}_{N,k}^{{\mathrm{RN}}_M})-({\left(P_{E,k}^{{\mathrm{RN}}_M}\right)}^2+{\left(P_{N,k}^{{\mathrm{RN}}_M}\right)}^2)\end{array}\right)\end{array};$

其中，为k时刻INS解算的东向和北向位置；为k时刻INS解算的东向和北向位置误差；为k时刻UWB测量得到的未知节点到第i个参考节点之间的伪距；为第i个参考节点的坐标。

本发明的有益效果：

1、组合导航方法采用改进的INS/UWB紧组合模型，该模型以INS解算得到目标行人的东向和北向位置信息的平方和与UWB定位标签测量得到的伪距的平方之差作为系统观测量；在此基础上，通过迭代扩展卡尔曼滤波将INS和UWB获取的导航信息进行数据融合，最终得到当前时刻最优的导航信息和对参考节点位置信息的预估。

2、现有的紧组合定位方法需要预先获取参考节点的位置信息，在此基础上再做定位，那这就会出现一个问题，就是参考节点的位置信息的精度会对组合导航系统的精度有一定的影响。本发明方法将参考节点的位置信息加入到状态变量中，人工设定初值即可，不需要提前获取参考节点的位置信息就能够直接进行预估。

3、本发明通过将参考节点的位置信息引入状态变量中，并通过滤波器完成对参考节点位置信息的预估，减少了组合导航系统需要预先获取参考节点位置信息以及组合导航系统的定位精度依赖于预先获取到的参考节点位置信息精度的缺点。

4、可用于室内环境下的智能鞋子的中高精度定位。

附图说明

图1为采用伪距和位置信息的室内行人UWB/INS紧组合导航系统示意图；

图2为采用伪距和位置信息的室内行人UWB/INS紧组合导航方法的示意图。

具体实施方式：

下面结合附图对本发明进行详细说明：

一种采用伪距信息和位置信息的室内行人UWB/INS紧组合导航，如图1所示，包括：惯性导航器件INS、UWB定位标签、UWB参考节点和数据处理系统；

惯性导航器件INS和UWB定位标签分别设置在行人的鞋子和身体上，UWB参考节点设置在任意位置，惯性导航器件INS和UWB定位标签分别与数据处理系统连接。

其中，惯性导航器件INS：用于测量行人的位置、速度和姿态等导航信息；

UWB定位标签：用于测量UWB定位标签与参考节点之间的距离信息，即伪距信息；

UWB参考节点：预先放置在已知的位置，便于测量与定位标签之间的距离；

数据处理系统：用于对采集到的传感器数据进行数据融合。

数据处理系统包括迭代扩展卡尔曼滤波器，通过所述迭代扩展卡尔曼滤波器对惯性导航器件INS在本地相对坐标系中获取的目标行人的导航信息以及UWB定位标签在本地相对坐标系中获取的伪距信息进行数据融合。

一种面向室内行人的UWB/INS紧组合导航方法，如图2所示，包括：

(1)以惯性导航器件INS的误差向量和各UWB参考节点的东向和北向位置作为状态量，以惯性导航器件INS测量得到的目标行人的东向和北向位置信息的平方和与UWB定位标签测量得到的伪距的平方之差作为系统观测量，构建INS/UWB组合SLAM模型；

作为状态量的惯性导航器件INS的误差向量以及各UWB参考节点的东向和北向位置的初值根据实际需要自行设定，因此不需要预先获知各UWB参考节点的位置信息，避免了组合导航系统的定位精度依赖于预先获取到的参考节点位置信息精度的问题。

(2)通过迭代扩展卡尔曼滤波器将惯性导航器件INS和UWB定位标签在本地相对坐标系中获取的目标行人的导航信息进行数据融合；迭代扩展卡尔曼滤波器的输出即得到当前时刻目标行人最优的导航信息以及参考节点位置的最优估计。

根据INS/UWB组合SLAM模型，得到的迭代扩展卡尔曼滤波器的状态方程为：

$\left(\begin{array}{c}{\phi }_{k+1}\\ {\mathrm{\delta V}}_{k+1}^n\\ {\mathrm{\delta P}}_{k+1}^n\\ {▿}_{k+1}^b\\ {ϵ}_{k+1}^b\\ X_{(m,k+1)2M\times 1}\end{array}\right)=\left(\begin{array}{cccccc}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& -I_{3\times 3}·C_b^n·T& 0_{3\times 2M}\\ S\left(f_k^n\right)·T& I_{3\times 3}& 0_{3\times 3}& I_{3\times 3}{C}_b^n·T& 0_{3\times 3}& 0_{3\times 2M}\\ 0_{3\times 3}& I_{3\times 3}·T& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 2M}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 2M}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 2M}\\ 0_{2M\times 3}& 0_{2M\times 3}& 0_{2M\times 3}& 0_{2M\times 3}& 0_{2M\times 3}& I_{2M\times 2M}\end{array}\right)\left(\begin{array}{c}{\phi }_k\\ {\mathrm{\delta V}}_k^n\\ {\mathrm{\delta P}}_k^n\\ {▿}_k^b\\ {ϵ}_k^b\\ X_{(m,k)2M\times 1}\end{array}\right)+{\omega }_k$

其中，为15维INS误差向量；

为2M维参考节点坐标向量；M为参考节点数目；

为惯性导航器件INS测量得到的导航坐标系下的东向、北向和天向三个方向的加速度误差；

φ_k、φ_k+1分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下的横摇、纵摇和航向角误差；分别为k时刻和k+1时刻INS测量得到的导航坐标系下的东向、北向和天向三个方向的速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的加速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的角速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的位置误差；

ω_k为状态噪声；T为采样周期；I为单位矩阵；为从载体系到导航系的状态转移矩阵。

根据INS/UWB组合SLAM模型，得到的迭代扩展卡尔曼滤波器的观测方程为：

$\begin{array}{c}\left(\begin{array}{c}{\left(P_{E,k}^{INS}\right)}^2+{\left(P_{N,k}^{INS}\right)}^2-{\left({\rho }_{1,k}^{UWB}\right)}^2\\ {\left(P_{E,k}^{INS}\right)}^2+{\left(P_{N,k}^{INS}\right)}^2-{\left({\rho }_{2,k}^{UWB}\right)}^2\\ .\\ .\\ .\\ {\left(P_{E,k}^{INS}\right)}^2+{\left(P_{N,k}^{INS}\right)}^2-{\left({\rho }_{m,k}^{UWB}\right)}^2\end{array}\right)=\\ \left(\begin{array}{c}2(P_{E,k}^{INS}-P_{E,k}^{{\mathrm{RN}}_1}){\mathrm{\delta P}}_{E,k}^n+2(P_{N,k}^{INS}-P_{N,k}^{{\mathrm{RN}}_1}){\mathrm{\delta P}}_{N,k}^n-({\left({\mathrm{\delta P}}_{E,k}^n\right)}^2+({\mathrm{\delta P}}_{N,k}^2\left)\right)+2(P_{E,k}^{INS}{P}_{E,k}^{{\mathrm{RN}}_1}+P_{N,k}^{INS}{P}_{N,k}^{{\mathrm{RN}}_1})-({\left(P_{E,k}^{{\mathrm{RN}}_1}\right)}^2+{\left(P_{N,k}^{{\mathrm{RN}}_1}\right)}^2)\\ 2(P_{E,k}^{INS}-P_{E,k}^{{\mathrm{RN}}_2}){\mathrm{\delta P}}_{E,k}^n+2(P_{N,k}^{INS}-P_{N,2}^{RN}){\mathrm{\delta P}}_{N,k}^n-({\left({\mathrm{\delta P}}_{E,k}^n\right)}^2+{\left({\mathrm{\delta P}}_{N,k}^2\right)}^2)+2(P_{E,k}^{INS}{P}_{E,k}^{{\mathrm{RN}}_2}+P_{N,k}^{INS}{P}_{N,k}^{{\mathrm{RN}}_2})-({\left(P_{E,k}^{{\mathrm{RN}}_2}\right)}^2+{\left(P_{N,k}^{{\mathrm{RN}}_2}\right)}^2)\\ .\\ .\\ .\\ 2(P_{E,k}^{INS}-P_{E,k}^{{\mathrm{RN}}_M}){\mathrm{\delta P}}_{E,k}^n+2(P_{N,k}^{INS}-P_{N,k}^{{\mathrm{RN}}_M}){\mathrm{\delta P}}_{N,k}^n-({\left({\mathrm{\delta P}}_{E,k}^n\right)}^2+{\left({\mathrm{\delta P}}_{N,k}^2\right)}^2)+2(P_{E,k}^{INS}{P}_{E,k}^{{\mathrm{RN}}_M}+P_{N,k}^{INS}{P}_{N,k}^{{\mathrm{RN}}_M})-({\left(P_{E,k}^{{\mathrm{RN}}_M}\right)}^2+{\left(P_{N,k}^{{\mathrm{RN}}_M}\right)}^2)\end{array}\right)\end{array};$

其中，为k时刻INS解算的东向和北向位置；为k时刻INS解算的东向和北向位置误差；为k时刻UWB测量得到的未知节点到第i个参考节点之间的伪距；为第i个参考节点的坐标。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。