采用固定区间crts平滑的ins/uwb紧组合导航系统及方法

采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
【技术领域】
[0001]本发明涉及复杂环境下组合定位技术领域，尤其涉及一种采用固定区间CRTS平滑 的INS/UWB紧组合导航系统及方法。
【背景技术】
[0002] 近年来，行人导航(Pedestrian Navigation，PN)作为导航技术应用的新兴领域， 正越来越受到各国学者的重视，并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地 下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航 信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行 有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理 论意义和实际应用价值。
[0003] 在现有的定位方式中，全球卫星导航系统（Global Navigation Satellite SyStem，GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是 其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道 等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。近年来，UWB 以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜力。学者们提 出将基于UWB的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内 定位，但是由于室内环境复杂多变，UWB信号十分容易受到干扰而导致定位精度下降甚至失 锁;与此同时，由于UWB采用的通信技术通常为短距离无线通信技术，因此若想完成大范围 的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、 多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟踪在室内导航领域 仍旧面临很多挑战。
[0004] 在导航模型方面，目前在室内行人组合导航领域应用较多的为松组合导航模型。 该模型具有容易实现的优点，但是需要指出的是，该模型的实现需要参与组合导航的多种 技术能够独立的完成导航定位。例如，需要UWB设备能够提供行人的导航信息，这就要求目 标行人所处的环境必须能够获取至少3个参考节点信息，这大大的降低的组合导航模型的 应用范围，与此同时，参与导航的子技术独立完成定位，也引入了新的误差，不利于组合导 航技术精度的提高。为了克服这一问题，学者们提出将紧组合模型应用于室内行人导航领 域，紧组合模型直接将参与组合导航的子技术的原始传感器数据应用于最后的导航信息的 解算，减少了子技术自行解算引入新误差的风险，提高了组合导航的精度，但是需要指出的 是，传统的紧组合导航模型都是建立在一阶泰勒展开的基础上的，这不可避免的引入了系 统的截断误差，这在普遍采用低成本传感器的行人导航领域，严重影响了组合导航系统的 精度。

【发明内容】

[0005] 本发明的目的就是为了解决上述难题，提供了一种采用固定区间CRTS平滑的INS/ UWB紧组合导航系统及方法，采用改进的INS/UWB紧组合模型，该模型以INS和UWB分别测量 得到的伪距的平方之差作为系统观测量;在此基础上，通过容积卡尔曼滤波(CKF)将INS和 UWB获取的导航信息进行数据融合；当到达输出时间时，将CKF得到的上一输出时间到现输 出时间之间的INS误差预估先通过RTS平滑之后再进行均值滤波，最后将数据输出，与当前 时刻INS解算的导航信息做差，最终得到当前时刻最优的导航信息。该系统及方法能满足室 内行人导航的中高精度定位和定向的要求。
[0006] 为实现上述目的，本发明的具体方案如下：
[0007] -种采用固定区间CRTS平滑的INS/UWB紧组合导航系统，包括：
[0008] 惯性导航器件INS、UWB无线标签、UWB无线参考节点、参考系统和数据处理系统； [0009]所述惯性导航器件INS和UWB无线标签分别设置在行人的鞋子上，所述UWB无线参 考节点和参考系统分别设置在设定位置，所述惯性导航器件INS、UWB无线标签和参考系统 分别与数据处理系统连接；
[0010]所述数据处理系统包括:局部数据融合滤波器、容积卡尔曼滤波器、伪距数据处理 模块、RTS平滑模块和均值滤波模块；
[0011] 所述惯性导航器件INS与局部数据融合滤波器连接，所述惯性导航器件INS和UWB 无线标签分别与伪距数据处理模块连接，伪距数据处理模块的输出连接所述容积卡尔曼滤 波器;所述容积卡尔曼滤波器的输出依次连接RTS平滑模块和均值滤波模块后与减法器的 一个输入端连接;所述局部数据融合滤波器的输出与减法器的另一个输入端连接。
[0012] 进一步地，所述伪距数据处理模块求取惯性导航器件INS和UWB无线标签测量得到 的伪距的平方和之差。
[0013] 进一步地，所述参考系统包括码盘和电子罗盘。
[0014] -种采用固定区间CRTS平滑的INS/UWB紧组合导航方法，包括：
[0015] (1)将人的运行状态分为静止和运动两个状态，通过惯性导航器件INS对行人的当 前运动状态进行判断；
[0016] (2)在鞋子处于静止状态时，通过局部数据融合滤波器对惯性导航器件INS解算的 导航信息误差进行预估和补偿，得到当前时刻惯性导航器件INS解算的位置、速度和姿态的 导航信息的最优估计；
[0017] (3)以惯性导航器件INS的误差向量作为状态量，以惯性导航器件INS和UWB无线标 签分别测量得到的伪距的平方之差作为系统观测量，构建INS/UWB紧组合模型；
[0018] (4)通过容积卡尔曼滤波器将惯性导航器件INS和UWB无线标签在本地相对坐标系 中获取的导航信息进行数据融合；
[0019] (5)当到达输出时间时，将容积卡尔曼滤波器得到的上一输出时间到现输出时间 之间的INS误差预估通过RTS平滑之后再进行均值滤波处理；
[0020] (6)将经过均值滤波处理后的数据与当前时刻INS解算的导航信息做差，最终得到 当前时刻最优的导航信息。
[0021] 进一步地，所述步骤（1)中，通过惯性导航器件INS对行人的当前运动状态进行判 断的方法为：
[0022]通过惯性导航器件INS自身携带的3个加速度传感器采集到的数据，得到当前时刻 行人的加速度模值，通过加速度模值的大小判断鞋子的运动状态：
[0023]
[0024] 其中，加速度模值：
，ax、ay、a z分别为在载体坐标系下的x、y
[0025] 进一步地，所述步骤(2)中，通过局部数据融合滤波器对惯性导航器件INS解算的 导航信息误差进行预估和补偿；
[0026] 局部数据融合滤波器的状态方程为：
[0027]
L s7.i+l」L ?3,3 ?3.3 _|:1_ %」
[0028] 其中，$为12维INS误差向量； 为惯性导 Jl _ 航器件INS测量得到的导航坐标系下的东向、北向和天向三个方向的加速度误差；Φζα、 Φζ,μ分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下的纵摇、横摇和航 向角误差；
[0029] 5?.、分别为k时刻和k+Ι时刻惯性导航器件INS测量得到的导航坐标系下 的东向、北向和天向三个方向的速度误差；VL分别为k时刻和k+Ι时刻导航坐标系下 的东向、北向和天向三个方向的加速度误差；、4#分别为k时刻和k+Ι时刻导航坐标系 下的东向、北向和天向三个方向的角速度误差；《z,k为状态噪声;T为采样周期；I为单位矩 阵；为从载体系到导航系的状态转移矩阵，
其中（γ，θ，φ)分别为纵摇、横摇和航向角；coz,k为状态噪声;Τ为采样周期；I为单位矩阵；
[0030] 局部数据融合滤波器的观测方程为：
[0031]