技术领域
[0001] 本
发明属于领域，具体涉及一种基于UWB与SINS的组合室内行人导航系统及方法。
背景技术
[0002] 行人导航始于20世纪八、九十年代，发展较晚且速度缓慢，直至1996年美国联邦通信委员会公布E-911的
定位需求后，行人导航才逐渐发展起来。行人导航系统主要用于实时
跟踪个人的
位置信息并提供位置服务，能够为士兵、救援抢险人员提供服务，为高危险工作人员提供了更高级别的安全保障。
[0003] 在室外GPS/LBS定位技术已经十分成熟，已经在车载、手机等设备上成功运用。然而在室内或者
信号严重遮挡的位置，全球定位系统(GPS)或者空间定位服务(LBS)无法有效定位，对于严重依赖于这两种导航技术的导航设备则无法继续使用。随着计算机与通信技术的发展，无线
射频识别、WIFI、超宽带(Ultra Wideband，UWB)技术等在室内定位领域得到了广泛应用。其中超宽带技术利用宽度仅为纳秒级的脉冲作为通信信号，具有非常高的时间
分辨率，能获得高达厘米级的定位
精度，还具有一定的抗多径能
力和穿透性，适用于高精度的室内导航定位服务。但是UWB定位是通过测距或测向完成，在非视距环境下，由于障碍物的遮挡，脉冲信号发生反射，会降低UWB的测距精度，大幅影响其定位精度，分析如下：
[0004] 超宽带技术是一种新型的无线通信技术。它通过对具有很陡上升和下降时间的冲激脉冲进行直接调制，使信号具有GHz量级的带宽。其定位使用的是时间飞行法，通过测量脉冲飞行时间差计算距离，当出现遮挡物，会影响脉冲传播速度；设脉冲飞行距离为S[0005] S＝c·Δt
[0006] 其中c是光速，Δt是时间；由于时间Δt出现误差，c极大，则会严重影响S。因此在测距过程中经常出现野值，造成测距错误。所以对于惯性导航与超宽带定位都不能独自作为定位系统。
[0007] 基于微
机电系统(Micro-Electro-Mechanical System，MEMS)捷联式惯性导航系统(strap-down inertial navigation System)能够为UWB定位提供有效的补充，具有短时精度高，抗干扰性能强且能够实现短时自主定位的功能，基于积分工作方式的导航系统，定位误差会随着时间的推移迅速的累积，定位误差过大甚至不能使用，不能成为一种真正意义上的导航系统，具体分析如下：
[0008] 由惯性导航基本公式：
[0009]
[0010] 其中，vn为载体的速度， 为速度的微分， 为载体
坐标系(b系)至导航坐标系(n系)转换矩阵，fb为
加速度在b系投影、 为哥氏加速度、 为
向心加速度、gn为
重力加速度。考虑到行人导航实际情况，哥氏加速度与向心加速度影响很小，近似为下式：
[0011]
[0012] 设
加速度计实际输出为 是误差，陀螺实际输出为 其中 是陀螺理论输出，εb为陀螺的常值误差，速度与位置更公式为：
[0013]
[0014]
[0015] 式中， 当前时刻速度， 是当前时刻的位置，t是时间；加表误差包含在 中，陀螺误差包含在 中，随着时间推移， 与εb常值算误差会以2次方的增长速度累计，长时间定位误差过大无法使用。
发明内容
[0016] 针对上述问题，本发明提出一种基于UWB与SINS的组合室内行人导航系统及方法，在传统的捷联
算法上引入了零速修正技术用于检测零速时刻，在进入超宽带信号区域，利用超宽带提供位置航向
角，通过联邦kalman滤波融合零速、位置、航向角，对导航系统速度、位置、航向进行校正，实现室内人员定位功能。
[0017] 实现上述技术目的，达到上述技术效果，本发明通过以下技术方案实现：
[0018] 第一方面，本发明提供了一种基于UWB与SINS的组合室内行人导航系统，包括：
[0019] SINS单元，所述SINS单元包括相连的惯性测量单元和导航解算模
块，所述惯性测量单元用于采集人体运动加速度和转动
角速度；所述导航解算模块接收并基于所述人体运动加速度和转动角速度计算出航向角和位置信息；
[0020] 零速检测单元，所述零速检测单元的输入端与所述惯性测量单元的输出端相连，用于接收惯性测量单元输出的人体运动加速度和转动角速度，并基于所述人体运动加速度和转动角速度进行零速判断，获得零速矢量；
[0021] UWB无线定位单元，所述UWB无线定位单元用于输出航向角和位置信息；
[0022] 磁力计，所述磁力计用于输出航向角；
[0023] 联邦kalman滤波单元，所述联邦kalman滤波单元的输入端分别与所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出端相连，并对所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的
输出信号进行数据融合，获得人体运动速度、航向角和位置信息的误差信号，并反馈至导航解算模块进行速度、航向、位置矫正。
[0024] 优选地，所述零速检测单元包括运动模式判断模块、广义似然比检验模块和零速判断模块；
[0025] 所述运动模式判断模块用于根据惯性测量单元输出的人体运动加速度进行运动模式判断；
[0026] 所述广义似然比检验模块用于基于惯性测量单元输出的人体运动加速度和转动角速度计算广义似然比；
[0027] 所述零速判断模块用于根据广义似然比检验模块输出的广义似然比以及各运动模式下的经验
阈值，进行零速判断，获得零速矢量。
[0028] 优选地，所述广义似然比的计算公式具体为：
[0029]
[0030] 其中， 表示惯性测量单元中的加速度计输出的人体运动加速度， 表示惯性测量单元中的
陀螺仪输出的转动角速度，||·||表示矢量的模值，Ωn为惯性测量单元测量时间序列的窗口周期， 分别是惯性测量单元中的加速度计与陀螺仪的观测噪声方差， 表示在窗口周期Ωn内加速度计数据的平均值，N表示窗口长度,g表示当地重力加速度矢量。
[0031] 优选地，所述根据广义似然比检验模块输出的广义似然比以及各运动模式下的经验阈值，进行零速判断采用的判断公式为：
[0032]
[0033] Thi为运动模式i下的经验阈值，zero_flag表示是零速标志，其值为1表示速度为零速，其值为0时表示非零速。
[0034] 优选地，所述联邦kalman
滤波器包括速度子滤波器、航向子滤波器、
姿态子滤波器和主滤波器，所述速度子滤波器、航向子滤波器、姿态子滤波器的数据传输端均与所述主滤波器的数据传输端相连，所述速度子滤波器的输入端分别与所述导航解算模块和零速检测单元的输出端相连，所述航向子滤波器的输入端分别与所述导航解算模块和UWB无线定位单元的输出端相连，所述姿态子滤波器的输入端分别与所述导航解算模块和磁力计的输出端相连。
[0035] 第二方面，本发明提供了一种基于UWB与SINS的组合室内行人导航方法，包括以下步骤：
[0036] (1)利用惯性测量单元采集人体运动加速度和转动角速度，并利用导航解算模块基于所述人体运动加速度和转动角速度计算出航向角和位置信息；
[0037] (2)利用零速检测单元基于惯性测量单元输出的人体运动加速度和转动角速度，并基于所述人体运动加速度和转动角速度进行零速判断，获得零速矢量；
[0038] (3)利用UWB无线定位单元输出人体位置信息和航向角；
[0039] (4)利用联邦Kalman滤波器对所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出信号进行数据融合，获得人体运动速度、航向角和位置信息的误差信号，并反馈至导航解算模块进行速度、航向、位置矫正。
[0040] 优选地，所述利用零速检测单元基于惯性测量单元输出的人体运动加速度和转动角速度，并基于所述人体运动加速度和转动角速度进行零速判断，获得零速矢量，具体包括以下步骤：
[0041] 根据惯性测量单元输出的人体运动加速度进行运动模式判断；
[0042] 基于惯性测量单元输出的人体运动加速度和转动角速度计算广义似然比；
[0043] 根据广义似然比以及各运动模式下的经验阈值，进行零速判断，获得零速矢量。
[0044] 优选地，所述广义似然比的计算公式具体为：
[0045]
[0046] 其中， 表示惯性测量单元中的加速度计输出的人体运动加速度， 表示惯性测量单元中的陀螺仪输出的转动角速度，||·||表示矢量的模值，Ωn为惯性测量单元测量时间序列的窗口周期， 分别是惯性测量单元中的加速度计与陀螺仪的观测噪声方差， 表示在窗口周期Ωn内加速度计数据的平均值，N表示窗口长度,g表示当地重力加速度矢量；
[0047] 所述零速判断采用的判断公式为：
[0048]
[0049] Thi为运动模式i下的经验阈值，zero_flag表示是零速标志，其值为1表示速度为零速，其值为0时表示非零速。
[0050] 优选地，所述步骤(4)包括以下子步骤：
[0051] (4.1)利用速度子滤波器将零速检测单元获得的零速信号与导航解算模块解算速度进行速度融合：
[0052] (4.2)利用航向子滤波器将导航解算模块与UWB无线定位单元中输出的位置、航向测量进行位置和航向融合；
[0053] (4.3)利用姿态子滤波器将导航解算模块与磁力计输出的航向角进行航向融合；
[0054] (4.4)利用上界方差技术，将速度子滤波器、航向子滤波器、姿态子滤波器与主滤波器的初始估计协方差阵Pi,0与过程噪声协方差阵Qi,0设置为联邦kalman滤波器初始估计协方差阵Pg,0和过程噪声协方差阵Qg,0的 倍，保证各个子滤波器互不相关，βi满足信息守恒原则；
[0055] (4.5)将速度子滤波器、航向子滤波器、姿态子滤波器分别独立进行时间更新；
[0056] (4.6)将速度子滤波器、航向子滤波器、姿态子滤波器分别进行量测更新；
[0057] (4.7)将主滤波器进行时间更新；
[0058] (4.8)将各子滤波器与主滤波器的估计信息融合成为新的全局状态估计信息；
[0059] (4.9)将获得的新的全局状态估计信息、误差协方差矩阵Pg以及公共系统噪声矩阵Qg按照设定规则重置各子滤波器与主滤波器，同时获得的新的全局状态估计信息发送至惯性测量单元中去用于进行速度、航向角和位置的修正。
[0060] 优选地，所述设定的规则为：
[0061]
[0062] 式中，k表示k时刻，i表示第i个滤波器， 表示k时刻公共系统的状态估计值，表示第i个滤波器的状态估计值，Pg,k、Qg,k表示k时刻公共系统的过程协方差矩阵与噪声协方差矩阵，Pi,k、Qi,k表示k时刻第i个滤波器过程协方差矩阵与噪声协方差矩阵。
[0064] 本发明针对不依赖GPS或者LBS的室内行人导航定位需求，阐述了一种基于惯性测量单元(IMU)与超宽带(UWB)相结合的室内行人导航装置及方法。由MEMS惯性测量单元与超宽带定位系统组成，MEMS固连在行人
鞋面，UWB固定于
肩膀位置；室内行人导航算法在传统的捷联算法上引入了零速修正技术用于检测零速时刻，在进入超宽带信号区域，利用超宽带提供位置航向角，在非超宽带区域利用磁力计提供航向角，通过联邦kalman滤波融合零速、位置、航向角，对系统速度、位置、航向进行校正，实现室内人员定位功能。
附图说明
[0065] 图1为本发明一种
实施例的原理示意图；
[0066] 图2为本发明一种实施例的结构示意图。
具体实施方式
[0067] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0068] 下面结合附图对本发明的应用原理作详细的描述。
[0069] 实施例1
[0070] 本发明实施例中提供了一种基于UWB与SINS的组合室内行人导航装置，如图1-2所示，具体包括：
[0071] SINS单元，所述SINS单元包括相连的惯性测量单元和导航解算模块，所述惯性测量单元用于采集人体运动加速度和转动角速度；所述导航解算模块接收并基于所述人体运动加速度和转动角速度计算出航向角和位置信息；
[0072] 零速检测单元，所述零速检测单元的输入端与所述惯性测量单元的输出端相连，用于接收惯性测量单元输出的人体运动加速度和转动角速度，并基于所述人体运动加速度和转动角速度进行零速判断；在本发明的一种具体实施例中，所述零速检测单元包括运动模式判断模块、广义似然比检验模块和零速判断模块，其中：
[0073] 所述运动模式判断模块用于根据惯性测量单元输出的人体运动加速度进行运动模式判断；所述运动模式判断模块的工作过程为：
[0074] 设本发明实施例中的惯性测量单元的x轴为人体行走方向的前向轴，通过计算x轴基于
时间窗口的平均加速度来判断人体运动的快慢，设k时刻x轴的平均加速度为：
[0075]
[0076] 根据经验值设置两个阈值Thv1,Thv2划分为三个运动速度模式mod1～mod3，分别代表慢速、中速和快速；具体划分方法如下所示：
[0077]
[0078] modi表示运动模式i。
[0079] 所述广义似然比检验模块的工作过程为：
[0080] 建立广义似然比检验模型
[0081] 定义k时刻惯性测量单元的输出为：
[0082]
[0083] 其中， 表示k时刻惯性测量单元中加速度计的三轴输出， 表示k时刻惯性测2
量单元中陀螺仪的三轴输出，单位分别为m/s、rad/s。
[0084] 设惯性测量单元输出量从n时刻开始，连续采集输出N次(窗口周期)，得到一组连续的量测值{yk|k∈[n,n-N+1]}，k为正整数。基于广义似然比统计的零速检测方法通过对长度为N的惯性测量单元输出数据构建广义似然比统计值，定义广义似然比统计模型如下：
[0085]
[0086] 其中， 表示加速度计输出值， 表示陀螺输出值，||·||表示矢量的模值，Ωn为惯性测量单元测量时间序列的窗口周期， 分别是加速度计与陀螺仪的观测噪声方差， 表示在窗口周期Ωn内加速度计数据的平均值，N表示窗口长度,g表示当地重力加速度矢量。
[0087]
[0088] Thi为运动模式i下的经验阈值，zero_flag表示是零速标志，其值为1表示速度为零速，其值为0时表示非零速，根据不同的运动速度设置不同的阈值可以更加准确的判断零速。
[0089] UWB无线定位单元，所述UWB无线定位单元用于输出人体航向信息和位置信息；所述UWB无线定位单元包括基站和标签，为了安装的便捷性，只考虑平面的情况；本发明实施例中的UWB无线定位单元包括三个基站和一个标签；当所述UWB无线定位单元工作时，所述标签能够输出自身到各个基站的距离，在已知各个基站的位置时，可以利用三点定位的方式完成定位工作；设三个基站A、B、C的位置分别为(xa,ya),(xb,yb),(xc,yc)，标签输出到三个基站的位置为da,db,dc，则标签的坐标可以表示为，标签的坐标就是人体位置：
[0090]
[0091] 在超宽带信号内，UWB无线定位单元提供的位置相对准确，其输出的航向角相对磁力计更加的稳定，利用两点确定直线的方式确定航向角，航向角的计算方程如式(7)所示：
[0092]
[0093] Yn表示时刻n时的纵轴坐标，Xn表示时刻n时的横轴坐标；
[0094] 磁力计，所述磁力计用于输出航向信息；
[0095] 联邦kalman滤波单元，联邦kalman滤波单元，所述联邦kalman滤波单元的输入端分别与所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出端相连，并对所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出信号进行数据融合，获得人体运动速度、航向角和位置信息的误差信号，并反馈至导航解算模块进行速度、航向、位置矫正；具体地，所述联邦kalman滤波器包括速度子滤波器、航向子滤波器、姿态子滤波器和主滤波器，所述速度子滤波器、航向子滤波器、姿态子滤波器的数据传输端均与所述主滤波器的数据传输端相连，所述速度子滤波器的输入端分别与所述导航解算模块和零速检测单元的输出端相连，所述航向子滤波器的输入端分别与所述导航解算模块和UWB无线定位单元的输出端相连，所述姿态子滤波器的输入端分别与所述导航解算模块和磁力计的输出端相连。
[0096] 所述联邦kalman滤波单元的工作过程具体为：
[0097] 利用速度子滤波器将零速检测单元获得的零速信号与惯性测量单元解算速度进行速度融合，具体为：
[0098] 对于速度子滤波器，在不考虑惯性测量单元误差的情况下，选择速度误差、位置误差与姿态误差作为其状态量，即 其中 表示姿态失准角，δvn表示速度误差，δpn表示位置误差；其
状态方程表示为：
[0099]
[0100] 其中， 表示k-1时刻的状态估计量， 表示 的一步预测值，Wk-1和Vk均视为彼此不相关的零均值白噪声序列；k时刻到k-1时刻的状态转移矩阵Fk|k-1与噪声输入矩阵Gk|k-1分根据下面公式确定，。
[0101]
[0102]
[0103] 利用速度子滤波器进行速度融合，将零速检测单元获得的零速信号与导航解算模块解算速度之差作为观测值，通过观测值对导航解算模块中获得的加速度信号进行修正，观测向量及
观测矩阵分别为：
[0104]
[0105] 其中Z＝03×1-vn中03×1表示零速检测获得的零速矢量，vn表示导航解算模块解算获得的速度矢量。
[0106] 利用航向子滤波器将导航解算模块与UWB无线定位单元中输出的位置、航向测量进行位置和航向融合，具体为：
[0107] 对于航向子滤波器，其状态方程与速度子滤波器相同，即航向子滤波器的状态方程也是公式(8)，但是观测方程不同；航向子滤波器用于进行位置和航向融合，将导航解算模块与UWB无线定位单元中输出的位置、航向测量之差作为观测值，通过观测值对导航解算模块中的位置信息和航向角进行修正，观测向量及观测矩阵分别为：
[0108]
[0109] 式中，Hs是导航解算模块解算出来的航向角，Hu是UWB无线定位单元解算出来的航向角，Ps是导航解算模块解算出的位置，Pu是UWB无线定位单元解算得到的位置。
[0110] 利用姿态子滤波器将导航解算模块与磁力计输出的航向角进行航向融合，具体为：
[0111] 对于姿态子滤波器，其状态方程与速度子滤波器相同，即姿态子滤波器的状态方程也是公式(8)，姿态子滤波器用于进行航向融合，将导航解算模块与磁力计输出的航向角之差作为观测值。通过观测值对导航解算模块中的航向角进行修正，观测向量及观测矩阵分别为：
[0112]
[0113] 式中，Hs是导航解算模块解算出来的航向角，Hc是磁力计解算出来的航向角。
[0114] 利用上界方差技术，将速度子滤波器、航向子滤波器、姿态子滤波器与主滤波器的初始估计协方差阵与过程噪声协方差阵设置为联邦kalman滤波器的 倍，保证各个子滤波器互不相关，βi满足信息守恒原则，具体见下式；
[0115]
[0116] 其中 式中i表示第i个子滤波器；Pg,0表示0时刻公共系统的过程协方差矩阵，Qg,0表示0时刻系统噪声协方差矩阵；
[0117] 将速度子滤波器、航向子滤波器、姿态子滤波器分别独立进行时间更新，更新公式为：
[0118]
[0119] 式中， 表示第i个子滤波器k-1时刻状态估计量， 表示 的当前预测向量，Fi,k|k-1、Gi,k|k-1表示第i个子滤波器k-1到k时刻的状态转移矩阵与噪声输入矩阵，Pi,k-1、Pi,k|k-1表示 的协方差矩阵，Qi,k-1表示k-1时刻系统过程协方差矩阵。
[0120] 将速度子滤波器、航向子滤波器、姿态子滤波器分别进行量测更新(所述的量测即为观测量)，更新公式为：
[0121]
[0122] 式中，Ki,k表示k时刻第i个滤波器滤波增益矩阵，Hi,k表示k时刻观测矩阵，Ri,k表示k时刻噪声的方差矩阵，I表示单位矩阵。
[0123] 将主滤波器进行时间更新，所述主滤波器的状态方程也是公式(8)，更新公式为：
[0124]
[0125] 将各子滤波器与主滤波器的估计信息融合成为新的全局状态估计信息，具体为：
[0126]
[0127] 将获得的全局状态估计 误差协方差矩阵Pg以及公共系统噪声矩阵Qg按照下式重置各子滤波器与主滤波器，同时将由步骤(4.8)获得的 发送至导航解算模块中去用于进行速度、航向角和位置的修正，所述公式(19)具体为：
[0128]
[0129] 实施例2
[0130] 本发明实施例提供了一种基于UWB与SINS的组合室内行人导航方法，包括以下步骤：
[0131] 步骤(1)利用惯性测量单元采集人体运动加速度和转动角速度，并利用导航解算模块基于所述人体运动加速度和转动角速度计算出航向角和位置信息；其中，所述航向角是通过对转动角速度信号进行积分获得姿态信号，姿态信号中包含了
俯仰角、
横滚角和航向角，所述的航向角即为航向角；所述位置信息的计算过程采用的也是现有技术，因此，在本发明实施例中不做过多的赘述；
[0132] 步骤(2)利用零速检测单元基于惯性测量单元输出的人体运动加速度和转动角速度，并基于所述人体运动加速度和转动角速度进行零速判断；
[0133] 零速检测是零速修正的前提，现有的零速检测算法大都是基于加速度计与陀螺仪输出信号进行检测，主要包括广义似然比检测(Generalized Likelihood Ratio Test，GLRT)、加速度计量测方差检测(Accelerometer Measurements Variance Test,MV)、加速度计量幅值检测(Accelerometer Measurements Magnitude Test,MAG)、角速度量测
能量检测(Angular Rate Measurement Energy Test,ARE)，其中广义似然比检测效果显著优于其他方法检测，在本发明实施例的一种具体实施方式中，所述零速检测采用的是广义似然比检测，并通过判断运动模式设置不同的阈值来适应不同速度的步态；
[0134] (2.1)判断运动模式
[0135] 设本发明实施例中的惯性测量单元的x轴为人体行走方向的前向轴，通过计算x轴基于时间窗口的平均加速度来判断人体运动的快慢，设k时刻x轴的平均加速度为：
[0136]
[0137] 根据经验值设置两个阈值Thv1,Thv2划分为三个运动速度模式mod1～mod3，分别代表慢速、中速和快速；具体划分方法如下所示：
[0138]
[0139] modi表示运动模式i。
[0140] (2.2)建立广义似然比检验模型
[0141] 定义k时刻惯性测量单元的输出为：
[0142]
[0143] 其中， 表示k时刻惯性测量单元中加速度计的三轴输出， 表示k时刻惯性测量单元中陀螺仪的三轴输出，单位分别为m/s2、rad/s。
[0144] 设惯性测量单元输出量从n时刻开始，连续采集输出N次(窗口周期)，得到一组连续的量测值{yk|k∈[n,n-N+1]}，k为正整数。基于广义似然比统计的零速检测方法通过对长度为N的惯性测量单元输出数据构建广义似然比统计值，定义广义似然比统计模型如下：
[0145]
[0146] 其中， 表示加速度计输出值， 表示陀螺输出值，||·||表示矢量的模值，Ωn为惯性测量单元测量时间序列的窗口周期， 分别是加速度计与陀螺仪的观测噪声方差， 表示在窗口周期Ωn内加速度计数据的平均值，N表示窗口长度,g表示当地重力加速度矢量。
[0147]
[0148] Thi为运动模式i下的经验阈值，zero_flag表示是零速标志，其值为1表示速度为零速，其值为0时表示非零速，根据不同的运动速度设置不同的阈值可以更加准确的判断零速。
[0149] 步骤(3)利用UWB无线定位单元输出人体位置信息和航向角；
[0150] 所述UWB无线定位单元包括基站和标签，为了安装的便捷性，只考虑平面的情况；本发明实施例中的UWB无线定位单元包括三个基站和一个标签；当所述UWB无线定位单元工作时，所述标签能够输出自身到各个基站的距离，在已知各个基站的位置时，可以利用三点定位的方式完成定位工作；设三个基站A、B、C的位置分别为(xa,ya),(xb,yb),(xc,yc)，标签输出到三个基站的位置为da,db,dc，则标签的坐标可以表示为，标签的坐标就是人体位置：
[0151]
[0152] 在超宽带信号内，UWB无线定位单元提供的位置相对准确，其输出的航向角相对磁力计更加的稳定，利用两点确定直线的方式确定航向角，航向角的计算方程如式(7)所示：
[0153]
[0154] Yn表示时刻n时的纵轴坐标，Xn表示时刻n时的横轴坐标。
[0155] 步骤(4)利用联邦Kalman滤波器对所述导航解算模块、零速检测单元、UWB无线定位单元输出和磁力计的信号进行数据融合处理；
[0156] 如图2所示，所述联邦kalman滤波器包括速度子滤波器、航向子滤波器、姿态子滤波器和主滤波器；
[0157] 所述速度子滤波器、航向子滤波器、姿态子滤波器的数据传输端均与所述主滤波器的数据传输端相连，所述速度子滤波器的输入端分别与所述导航解算模块和零速检测单元的输出端相连，所述航向子滤波器的输入端分别与所述导航解算模块和UWB无线定位单元的输出端相连，所述姿态子滤波器的输入端分别与所述导航解算模块和磁力计的输出端相连，其中，图2中的Xi、Pi分别表示各自滤波器的局部状态估计值与协方差矩阵，Xm、Pm分别表示主滤波器的状态估计值与协方差矩阵，Xg、Pg分别表示公共系统的状态估计值与协方差矩阵；βi表示系数。
[0158] 在本发明实施例的一种具体实施方式中，所述步骤(4)包括以下子步骤：
[0159] 步骤(4.1)：利用速度子滤波器将零速检测单元获得的零速信号与导航解算模块解算速度进行速度融合，具体为：
[0160] 对于速度子滤波器，在不考虑导航解算模块误差的情况下，选择速度误差、位置误差与姿态误差作为其状态量，即 其中 表示姿态失准角，δvn表示速度误差，δpn表示位置误差；其状态方程表示为：
[0161]
[0162] 其中， 表示k-1时刻的状态估计量， 表示 的一步预测值，Wk-1和Vk均视为彼此不相关的零均值白噪声序列；k时刻到k-1时刻的状态转移矩阵Fk|k-1与噪声输入矩阵Gk|k-1分根据下面公式确定，H表示观测矩阵，Zk表示k时刻观测向量。
[0163]
[0164]
[0165] 利用速度子滤波器进行速度融合，将零速检测单元获得的零速信号与导航解算模块解算速度之差作为观测值，通过观测值对导航解算模块中获得的加速度信号进行修正，观测向量及观测矩阵分别为：
[0166]
[0167] 其中Z＝03×1-vn中03×1表示零速检测获得的零速矢量，vn表示惯性导航解算获得的速度矢量。
[0168] 步骤(4.2)：利用航向子滤波器将导航解算模块与UWB无线定位单元中输出的位置、航向测量进行位置和航向融合，具体为：
[0169] 对于航向子滤波器，其状态方程与速度子滤波器相同，即航向子滤波器的状态方程也是公式(8)，但是观测方程不同；航向子滤波器用于进行位置和航向融合，将导航解算模块与UWB无线定位单元中输出的位置、航向测量之差作为观测值，通过观测值对导航解算模块中的位置信息和航向角进行修正，观测向量及观测矩阵分别为：
[0170]
[0171] 式中，Hs是导航解算模块解算出来的航向角，Hu是UWB无线定位单元解算出来的航向角，Ps是导航解算模块解算出的位置，Pu是UWB无线定位单元解算得到的位置。
[0172] 步骤(4.3)：利用姿态子滤波器将导航解算模块与磁力计输出的航向角进行航向融合，具体为：
[0173] 对于姿态子滤波器，其状态方程与速度子滤波器相同，即姿态子滤波器的状态方程也是公式(8)，姿态子滤波器用于进行航向融合，将导航解算模块与磁力计输出的航向角之差作为观测值。通过观测值对导航解算模块中的航向角进行修正，观测向量及观测矩阵分别为：
[0174]
[0175] 式中，Hs是导航解算模块解算出来的航向角，Hc是磁力计解算出来的航向角。
[0176] 步骤(4.4)利用上界方差技术，将速度子滤波器、航向子滤波器、姿态子滤波器与主滤波器的初始估计协方差阵与过程噪声协方差阵设置为联邦kalman滤波器的 倍，保证各个子滤波器互不相关，βi满足信息守恒原则，具体见下式；
[0177]
[0178] 其中 式中i表示第i个子滤波器；Pg,0表示0时刻公共系统的过程协方差矩阵，Qg,0表示0时刻系统噪声协方差矩阵；
[0179] 步骤(4.5)将速度子滤波器、航向子滤波器、姿态子滤波器分别独立进行时间更新，更新公式为：
[0180]
[0181] 式中， 表示第i个子滤波器k-1时刻状态估计量， 表示 的当前预测向量，Fi,k|k-1、Gi,k|k-1表示第i个子滤波器k-1到k时刻的状态转移矩阵与噪声输入矩阵，Pi,k-1、Pi,k|k-1表示 的协方差矩阵，Qi,k-1表示k-1时刻系统过程协方差矩阵。
[0182] 步骤(4.6)将速度子滤波器、航向子滤波器、姿态子滤波器分别进行量测更新(所述的量测即为观测量)，更新公式为：
[0183]
[0184] 式中，Ki,k表示k时刻第i个滤波器滤波增益矩阵，Hi,k表示k时刻观测矩阵，Ri,k表示k时刻噪声的方差矩阵，I表示单位矩阵。
[0185] 步骤(4.7)将主滤波器进行时间更新，所述主滤波器的状态方程也是公式(8)，更新公式为：
[0186]
[0187] 步骤(4.8)将各子滤波器与主滤波器的估计信息融合成为新的全局状态估计信息，具体为：
[0188]
[0189] 步骤(4.9)将获得的全局状态估计 误差协方差矩阵Pg以及公共系统噪声矩阵Qg按照下式重置各子滤波器与主滤波器，同时将由步骤(4.8)获得的 发送至导航解算模块中去用于进行速度、航向角和位置的修正，所述公式(19)具体为：
[0190]
[0191] 以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和
说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的
权利要求书及其等效物界定。
