CN104977001A - 一种应用于个人室内导航***的相对导航方法 - Google Patents

Abstract

本发明属于导航方法，具体涉及一种应用于个人室内导航***的相对导航方法。它包括：步骤一：初始化，步骤二：惯性相对导航解算，计算速度、位置信息；步骤三：保存必要信息；步骤四：计算投影值积分；步骤五：检测是否静止；步骤六：修正输出；对速度、位置、加速度进行修正，修正后的数据作为本方法的输出，并循环执行步骤二～步骤六，直到导航结束。本发明的效果是：本发明的方法可以准确有效的解算出实验人员行进的轨迹，说明本方法正确、有效。

Description

一种应用于个人室内导航***的相对导航方法

技术领域

本发明属于导航方法，具体涉及一种应用于个人室内导航***的相对导航方法。

背景技术

由于卫星导航所具有的高精度、低功耗、易携带等独特优势，其在日常生活中的应用日益广泛，然而在室内、地下及工作厂房等密闭空间中由于卫星信号受遮挡卫星用户机不能定位而无法满足用户的导航需求。

个人室内导航***由于其不依赖卫星导航、完全自主的独特优势而受到越来越多的重视，以美国为首的欧美国家对该***的研发起步较早，但尚未出现货架产品，为了填补国内对该技术与产品的研发空白，对个人室内导航技术进行研究。

发明内容

本发明的目的是针对现有技术的缺陷，提供一种应用于个人室内导航***的相对导航方法。

本发明是这样实现的：一种应用于个人室内导航***的相对导航方法，包括下述步骤：

步骤一：初始化

对速度、位置、姿态进行初始化，在第一次计算时使用初始化数据，在后续循环计算中使用外部传感器给出的实时信息；

步骤二：惯性相对导航解算

计算速度、位置信息；

步骤三：保存必要信息

保存加速度计陀螺敏感信息以及加速度计信息在导航坐标系中投影值

步骤四：计算投影值积分

计算加速度计、陀螺敏感信息均方差以及加速度计信息在导航坐标系中投影值积分；

步骤五：检测是否静止

当***同时满足下述三个条件时，判定***静止

1）三轴加速度计敏感信息的均方差均小于0.05m/s^2；

2）三轴陀螺敏感信息的均方差均小于0.001rad/s；

3）一个滤波周期内加速度积分绝对值均小于0.01m/s；

如果判断结果是***处于静止状态，则执行下面步骤六，否则执行步骤二；

步骤六：修正输出

对速度、位置、加速度进行修正，修正后的数据作为本方法的输出，并循环执行步骤二～步骤六，直到导航结束。

如上所述的一种应用于个人室内导航***的相对导航方法，其中，所述的步骤一的初始化包括给定初始化数据，包括

a)初始位置：X=0，Y=0，Z=0；

b)初始速度：Vx=0，Vy=0，Vz=0；

c)初始姿态：俯仰角滚动角航向角Yaw＝0.0，其中的asin表示反正弦函数

其中为1秒内加速度的平均值，计算公式为：

${\stackrel{\‾}{A}}_x=\underset{i=1}{\overset{200}{\mathrm{\Σ}}}{f}_{\mathrm{ibx}}^b\left(i\right),{\stackrel{\‾}{A}}_z=\underset{i=1}{\overset{200}{\mathrm{\Σ}}}{f}_{\mathrm{ibz}}^b\left(i\right),$

其中，和是加速度计敏感参数 $f_{\mathrm{ib}}^b=\left[\begin{array}{ccc}{f}_{\mathrm{ibx}}^b& f_{\mathrm{iby}}^b& f_{\mathrm{ibz}}^b\end{array}\right]$ 的两个输出值，该加速度计敏感参数由外部的加速度计给出。

如上所述的一种应用于个人室内导航***的相对导航方法，其中，所述的步骤二惯性相对导航解算，包括

a)姿态更新

设表征载体姿态的四元数：Q＝[q₀ q₁ q₂ q₃]^T，四元数更新的解析式为：

$Q(k+1)=\{\cos \frac{{\mathrm{\Δ\θ}}_0}{2}I+\sin \frac{{\mathrm{\Δ\θ}}_0}{2}[\mathrm{\Δ\θ}]/\mathrm{\Δ}{\mathrm{\θ}}_0\}Q\left(k\right)---\left(1\right)$

其中：I为4×4阶单位矩阵； ${\mathrm{\Δ\θ}}_0=\sqrt{{\mathrm{\Δ\θ}}_{\mathrm{ibx}}^b\×{\mathrm{\Δ\θ}}_{\mathrm{ibx}}^b+{\mathrm{\Δ\θ}}_{\mathrm{iby}}^b\×\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{iby}}^b+{\mathrm{\Δ\θ}}_{\mathrm{ibz}}^b\×{\mathrm{\Δ\θ}}_{\mathrm{ibz}}^b}$ $\left[\mathrm{\Δ\θ}\right]=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{ibx}}^b& -\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{iby}}^b& -\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{ibz}}^b\\ \mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{ibx}}^b& 0& {\mathrm{\Δ\θ}}_{\mathrm{ibz}}^b& -{\mathrm{\Δ\θ}}_{\mathrm{iby}}^b\\ {\mathrm{\Δ\θ}}_{\mathrm{iby}}^b& -{\mathrm{\Δ\θ}}_{\mathrm{ibz}}^b& 0& {\mathrm{\Δ\θ}}_{\mathrm{ibx}}^b\\ {\mathrm{\Δ\θ}}_{\mathrm{ibz}}^b& {\mathrm{\Δ\θ}}_{\mathrm{iby}}^b& -{\mathrm{\Δ\θ}}_{\mathrm{ibx}}^b& 0\end{array}\right],$ 为k到k+1时刻陀螺敏感到的角增量，由下式获得： $\left\{\begin{array}{c}{\mathrm{\Δ\θ}}_{\mathrm{ibx}}^b={\mathrm{\ω}}_{\mathrm{ibx}}^b\×T_n\\ {\mathrm{\Δ\θ}}_{\mathrm{iby}}^b={\mathrm{\ω}}_{\mathrm{iby}}^b\×T_n\\ {\mathrm{\Δ\θ}}_{\mathrm{ibz}}^b={\mathrm{\ω}}_{\mathrm{ibz}}^b\×T_n\end{array}\right.,$ 其中T_n＝0.005s，即导航解算周期，姿态矩阵的更新计算公式为：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(2\right)$

b)速度、位置更新

相对导航速度更新算法为：

$V(K+1)=V\left(K\right)+f_{\mathrm{ib}}^n*T_n---\left(3\right)$

相对导航位置更新算法为：

P(K+1)＝P(K)+0.5*(V(K+1)+V(K))*T_n  (4)

其中为加速度计敏感信息在导航系中投影：V(K)、P(K)为K时刻的速度、位置信息，所述的T_n为导航周期，一般取50毫秒。

如上所述的一种应用于个人室内导航***的相对导航方法，其中，所述的步骤六修正输出中，使用的卡尔曼滤波***方程为 $\left\{\begin{array}{c}\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)\\ Z=\mathrm{HX}\left(t\right)+V\left(t\right)\end{array}\right.$

其中，公式（8）中的第一个方程为***误差状态方程，具体形式见步骤（6.1），X(t)为***误差状态量X(t)＝[δP,δX,φ,▽,ε]^T，F(t)为***状态转移矩阵 $F\left(t\right)=\left[\begin{array}{ccccc}{I}_{3\×3}& 0& 0& 0& 0\\ 0& F_V& 0& C_b^n& 0\\ 0& 0& 0_{3\×3}& 0& C_b^n\\ 0& 0& 0& 0_{3\×3}& 0\\ 0& 0& 0& 0& 0_{3\×3}\end{array}\right],$ $F_V=\left[\begin{array}{ccc}0& 0& g\\ 0& 0& 0\\ -g& 0& 0\end{array}\right],$ W(t)为***噪声矢量，g为当地重力加速度，

公式（8）中的第二个方程为***观测方程，具体形式见步骤（6.2）， $Z={\left[\begin{array}{ccc}{\stackrel{\‾}{V}}_x& {\stackrel{\‾}{V}}_y& {\stackrel{\‾}{V}}_z\end{array}\right]}^T$ 为零速观测量，H＝[0_3×3 I_3×3 0_3×3 0_3×3 0_3×3]，V(t)为观测噪声矢量，

（6.1）***误差方程

a)位置误差方程

$\mathrm{\δ}\stackrel{\·}{P}=\mathrm{\δV}---\left(9\right)$

b)速度误差方程

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}_x=g\·{\mathrm{\φ}}_z+T_{11}\·{\▿}_x+T_{21}\·{\▿}_y+T_{31}\·{\▿}_z\\ \mathrm{\δ}{\stackrel{\·}{V}}_y=T_{12}\·{\▿}_x+T_{22}\·{\▿}_y+T_{32}\·{\▿}_z\\ \mathrm{\δ}{\stackrel{\·}{V}}_z=-g\·{\mathrm{\φ}}_x+T_{13}\·{\▿}_x+T_{23}\·{\▿}_y+T_{33}\·{\▿}_z\end{array}\right.---\left(10\right)$

c)姿态误差方程

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_x=-(T_{11}\·{\mathrm{\ϵ}}_x+T_{21}\·{\mathrm{\ϵ}}_y+T_{31}\·{\mathrm{\ϵ}}_z)\\ {\stackrel{\·}{\mathrm{\φ}}}_y=-(T_{12}\·{\mathrm{\ϵ}}_x+T_{22}\·{\mathrm{\ϵ}}_y+T_{32}\·{\mathrm{\ϵ}}_z)\\ {\stackrel{\·}{\mathrm{\φ}}}_z=-(T_{13}\·{\mathrm{\ϵ}}_x+T_{23}\·{\mathrm{\ϵ}}_y+T_{33}\·{\mathrm{\ϵ}}_z)\end{array}\right.---\left(11\right)$

d)加速度计零位误差方程

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_x=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z=0\end{array}\right.---\left(12\right)$

e)陀螺漂移误差方程

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_x=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z=0\end{array}\right.---\left(13\right)$

其中，δP=[δP_x δP_y δP_z]^T、δV=[δV_x δV_y δV_z]^T为位置、速度误差，φ=[φ_x φ_y φ_z]^T为姿态误差，▽=[▽_x ▽_y ▽_z]^T为机体系加速度计零位误差，ε=[ε_x ε_y ε_z]^T为机体系陀螺漂移误差， $C_n^b=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right],$

（6.2）零速匹配观测方程

在***检测到人体处于静止状态下，利用下式求观测量：

$Z=\stackrel{\‾}{V}-0.0---\left(14\right)$

其中，Z为零速匹配卡尔曼滤波器观测量，为零速状态下滤波周期内速度平均值，在此设定卡尔曼滤波周期为50ms，即10个导航周期滤波一次，

观测方程：

$\mathrm{\δV}=Z=\stackrel{\‾}{V}-0.0---\left(15\right).$

本发明的效果是：本发明的方法可以准确有效的解算出实验人员行进的轨迹，说明本方法正确、有效。

附图说明

附图1是工作人员进行实验时行走路线的示意图。

附图2是利用本方法导航得到的轨迹图。

具体实施方式

一种应用于个人室内导航***的相对导航方法，包括下述步骤：

步骤一：初始化

给定初始化数据，包括

d)初始位置：X=0，Y=0，Z=0；

e)初始速度：Vx=0，Vy=0，Vz=0；

f)初始姿态：俯仰角滚动角航向角Yaw＝0.0。其中的asin表示反正弦函数

其中为1秒内加速度的平均值，计算公式为： ${\stackrel{\‾}{A}}_x=\underset{i=1}{\overset{200}{\mathrm{\Σ}}}{f}_{\mathrm{ibx}}^b\left(i\right),{\stackrel{\‾}{A}}_z=\underset{i=1}{\overset{200}{\mathrm{\Σ}}}{f}_{\mathrm{ibz}}^b\left(i\right).$

其中，和是加速度计敏感参数 $f_{\mathrm{ib}}^b=\left[\begin{array}{ccc}{f}_{\mathrm{ibx}}^b& f_{\mathrm{iby}}^b& f_{\mathrm{ibz}}^b\end{array}\right]$ 的两个输出值，该加速度计敏感参数由外部的加速度计给出。初始化之后，在后续计算过程中使用到的速度、姿态、加速度等信息，均由外部相应传感器给出。

步骤二：惯性相对导航解算

c)姿态更新

设表征载体姿态的四元数：Q＝[q₀ q₁ q₂ q₃]^T，四元数更新的解析式为：

$Q(k+1)=\{\cos \frac{{\mathrm{\Δ\θ}}_0}{2}I+\sin \frac{{\mathrm{\Δ\θ}}_0}{2}[\mathrm{\Δ\θ}]/\mathrm{\Δ}{\mathrm{\θ}}_0\}Q\left(k\right)---\left(1\right)$

其中：I为4×4阶单位矩阵； ${\mathrm{\Δ\θ}}_0=\sqrt{{\mathrm{\Δ\θ}}_{\mathrm{ibx}}^b\×{\mathrm{\Δ\θ}}_{\mathrm{ibx}}^b+{\mathrm{\Δ\θ}}_{\mathrm{iby}}^b\×\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{iby}}^b+{\mathrm{\Δ\θ}}_{\mathrm{ibz}}^b\×{\mathrm{\Δ\θ}}_{\mathrm{ibz}}^b}$ $\left[\mathrm{\Δ\θ}\right]=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{ibx}}^b& -\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{iby}}^b& -\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{ibz}}^b\\ \mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{ibx}}^b& 0& {\mathrm{\Δ\θ}}_{\mathrm{ibz}}^b& -{\mathrm{\Δ\θ}}_{\mathrm{iby}}^b\\ {\mathrm{\Δ\θ}}_{\mathrm{iby}}^b& -{\mathrm{\Δ\θ}}_{\mathrm{ibz}}^b& 0& {\mathrm{\Δ\θ}}_{\mathrm{ibx}}^b\\ {\mathrm{\Δ\θ}}_{\mathrm{ibz}}^b& {\mathrm{\Δ\θ}}_{\mathrm{iby}}^b& -{\mathrm{\Δ\θ}}_{\mathrm{ibx}}^b& 0\end{array}\right],$ 为k到k+1时刻陀螺敏感到的角增量，由下式获得： $\left\{\begin{array}{c}{\mathrm{\Δ\θ}}_{\mathrm{ibx}}^b={\mathrm{\ω}}_{\mathrm{ibx}}^b\×T_n\\ {\mathrm{\Δ\θ}}_{\mathrm{iby}}^b={\mathrm{\ω}}_{\mathrm{iby}}^b\×T_n\\ {\mathrm{\Δ\θ}}_{\mathrm{ibz}}^b={\mathrm{\ω}}_{\mathrm{ibz}}^b\×T_n\end{array}\right.,$ 其中T_n＝0.005s，即导航解算周期，姿态矩阵的更新计算公式为：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(2\right)$

即用公式（1）计算q0～q3，计算出这个周期的q0～q3，继而得到这个周期的姿态矩阵。下个周期的姿态矩阵再利用这个公式计算。

d)速度、位置更新

相对导航速度更新算法为：

$V(K+1)=V\left(K\right)+f_{\mathrm{ib}}^n*T_n---\left(3\right)$

相对导航位置更新算法为：

P(K+1)＝P(K)+0.5*(V(K+1)+V(K))*T_n  (4)

其中为加速度计敏感信息在导航系中投影：V(K)、P(K)为K时刻的速度、位置信息。所述的T_n为导航周期，一般取50毫秒。

步骤三：保存必要信息

保存加速度计陀螺敏感信息以及加速度计信息在导航坐标系中投影值三类信息。其中加速度计陀螺敏感信息由外部给出，加速度计信息在导航坐标系中投影值由步骤三计算得到。

步骤四：计算投影值积分

计算加速度计、陀螺敏感信息均方差以及加速度计信息在导航坐标系中投影值积分。其中投影值是用原始信息与姿态矩阵的转置矩阵相乘得到，积分是用投影值积分得到的。

步骤五：检测是否静止

由于个人室内导航***采用的是微观性测量单元器件精度较差，短时间内纯惯性导航导航误差发散较快，因此需要通过一定的修正算法抑制误差的快速发散，达到高精度导航的目的，在个人室内导航导航***中，我们充分利用人体行走过程中脚部运动动静交替切换的特点，在脚部处于静止的时间段内，采用零速修正算法对***各项导航误差进行修正。

零速修正的前提是静止状态的检测，零速检测算法需要既能够满足隔离***的运动状态，又能够对震动、扰动等特殊运动状态有一定容忍度，因此检测算法设计如下：

1）每个导航周期实时滑动保存1个滤波周期内(10个导航周期)三轴加速度计陀螺敏感信息以及三轴加速度计信息在导航坐标系中投影

2）每个导航周期实时计算三轴加速度计、陀螺敏感信息的均方差及一个滤波周期内三轴加速度计信息在导航坐标系中投影的积分值。其中，加速度均方差计算公式为：

$\mathrm{Std}\left(f_{\mathrm{ibj}}^b\right)=\sqrt{\underset{i=1}{\overset{10}{\mathrm{\Σ}}}{(f_{\mathrm{ibj}}^b-\frac{1}{10}\underset{i=1}{\overset{10}{\mathrm{\Σ}}}{f}_{\mathrm{ibj}}^b)}^2},j=x,y,z---\left(5\right)$

角速度均方差计算公式为：

$\mathrm{Std}\left(W_{\mathrm{ibj}}^b\right)=\sqrt{\underset{i=1}{\overset{10}{\mathrm{\Σ}}}{(w_{\mathrm{ibj}}^b-\frac{1}{10}\underset{i=1}{\overset{10}{\mathrm{\Σ}}}{w}_{\mathrm{ibj}}^b)}^2},j=x,y,z---\left(6\right)$

三轴加速度计信息在导航坐标系中投影的积分值计算公式为：

$\underset{i=1}{\overset{10}{\∫}}{f}_{\mathrm{ibj}}^n=T_n\·\underset{i=1}{\overset{10}{\mathrm{\Σ}}}{f}_{\mathrm{ibj}}^n,j=x,y,z---\left(7\right)$

如果同时满足以下三个条件，则可以视该滤波周期内***处于静止状态，否则***处于运动状态：

4）三轴加速度计敏感信息的均方差均小于0.05m/s^2；

5）三轴陀螺敏感信息的均方差均小于0.001rad/s；

6）一个滤波周期内加速度积分绝对值均小于0.01m/s；

如果判断结果是***处于静止状态，则执行下面步骤六，否则执行步骤二。

步骤六：修正输出

卡尔曼滤波***方程

$\left\{\begin{array}{c}\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)\\ Z=\mathrm{HX}\left(t\right)+V\left(t\right)\end{array}\right.---\left(8\right)$

其中，公式（8）中的第一个方程为***误差状态方程，具体形式见步骤（6.1），X(t)为***误差状态量X(t)＝[δP,δX,φ,▽,ε]^T，F(t)为***状态转移矩阵 $F\left(t\right)=\left[\begin{array}{ccccc}{I}_{3\×3}& 0& 0& 0& 0\\ 0& F_V& 0& C_b^n& 0\\ 0& 0& 0_{3\×3}& 0& C_b^n\\ 0& 0& 0& 0_{3\×3}& 0\\ 0& 0& 0& 0& 0_{3\×3}\end{array}\right],$ $F_V=\left[\begin{array}{ccc}0& 0& g\\ 0& 0& 0\\ -g& 0& 0\end{array}\right],$ W(t)为***噪声矢量，g为当地重力加速度。

公式（8）中的第二个方程为***观测方程，具体形式见步骤（6.2）， $Z={\left[\begin{array}{ccc}{\stackrel{\‾}{V}}_x& {\stackrel{\‾}{V}}_y& {\stackrel{\‾}{V}}_z\end{array}\right]}^T$ 为零速观测量，H＝[0_3×3 I_3×3 0_3×3 0_3×3 0_3×3]，V(t)为观测噪声矢量。

（6.1）***误差方程

f)位置误差方程

$\mathrm{\δ}\stackrel{\·}{P}=\mathrm{\δV}---\left(9\right)$

g)速度误差方程

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}_x=g\·{\mathrm{\φ}}_z+T_{11}\·{\▿}_x+T_{21}\·{\▿}_y+T_{31}\·{\▿}_z\\ \mathrm{\δ}{\stackrel{\·}{V}}_y=T_{12}\·{\▿}_x+T_{22}\·{\▿}_y+T_{32}\·{\▿}_z\\ \mathrm{\δ}{\stackrel{\·}{V}}_z=-g\·{\mathrm{\φ}}_x+T_{13}\·{\▿}_x+T_{23}\·{\▿}_y+T_{33}\·{\▿}_z\end{array}\right.---\left(10\right)$

h)姿态误差方程

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_x=-(T_{11}\·{\mathrm{\ϵ}}_x+T_{21}\·{\mathrm{\ϵ}}_y+T_{31}\·{\mathrm{\ϵ}}_z)\\ {\stackrel{\·}{\mathrm{\φ}}}_y=-(T_{12}\·{\mathrm{\ϵ}}_x+T_{22}\·{\mathrm{\ϵ}}_y+T_{32}\·{\mathrm{\ϵ}}_z)\\ {\stackrel{\·}{\mathrm{\φ}}}_z=-(T_{13}\·{\mathrm{\ϵ}}_x+T_{23}\·{\mathrm{\ϵ}}_y+T_{33}\·{\mathrm{\ϵ}}_z)\end{array}\right.---\left(11\right)$

i)加速度计零位误差方程

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_x=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z=0\end{array}\right.---\left(12\right)$

j)陀螺漂移误差方程

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_x=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z=0\end{array}\right.---\left(13\right)$

其中，δP=[δP_x δP_y δP_z]^T、δV=[δV_x δV_y δV_z]^T为位置、速度误差，φ=[φ_x φ_y φ_z]^T为姿态误差，▽=[▽_x ▽_y ▽_z]^T为机体系加速度计零位误差，ε=[ε_x ε_y ε_z]^T为机体系陀螺漂移误差， $C_n^b=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right].$

（6.2）零速匹配观测方程

在***检测到人体处于静止状态下，利用下式求观测量：

$Z=\stackrel{\‾}{V}-0.0---\left(14\right)$

其中，Z为零速匹配卡尔曼滤波器观测量，为零速状态下滤波周期内速度平均值，在此设定卡尔曼滤波周期为50ms，即10个导航周期滤波一次。

观测方程：

$\mathrm{\δV}=Z=\stackrel{\‾}{V}-0.0---\left(15\right)$

本领域的技术人员利用本领域的卡尔曼滤波基本知识，结合本申请公开的***方程、***误差方程和零速匹配观测方程可以计算出速度、位置、加速度的误差，然后再利用本领域的通用修正方法，可以得到修正后的速度、位置、加速度。所述修正后的速度、位置、加速度即为本方法的输出值。但是因为导航时持续进行的，因此重复执行步骤二～步骤六，直到导航结束。

Claims (4)

1.一种应用于个人室内导航***的相对导航方法，其特征在于，包括下述步骤： 

步骤一：初始化 

对速度、位置、姿态进行初始化，在第一次计算时使用初始化数据，在后续循环计算中使用外部传感器给出的实时信息； 

步骤二：惯性相对导航解算 

计算速度、位置信息； 

步骤三：保存必要信息 

保存加速度计陀螺敏感信息以及加速度计信息在导航坐标系中投影值

步骤四：计算投影值积分 

计算加速度计、陀螺敏感信息均方差以及加速度计信息在导航坐标系中投影值积分； 

步骤五：检测是否静止 

当***同时满足下述三个条件时，判定***静止 

1）三轴加速度计敏感信息的均方差均小于0.05m/s^2； 

2）三轴陀螺敏感信息的均方差均小于0.001rad/s； 

3）一个滤波周期内加速度积分绝对值均小于0.01m/s； 

如果判断结果是***处于静止状态，则执行下面步骤六，否则执行步骤二； 

步骤六：修正输出 

对速度、位置、加速度进行修正，修正后的数据作为本方法的输出， 并循环执行步骤二～步骤六，直到导航结束。 

2.如权利要求1所述的一种应用于个人室内导航***的相对导航方法，其特征在于：所述的步骤一的初始化包括给定初始化数据，包括 

a)初始位置：X=0，Y=0，Z=0； 

b)初始速度：Vx=0，Vy=0，Vz=0； 

c)初始姿态：俯仰角滚动角 航向角Yaw＝0.0，其中的asin表示反正弦函数 

其中为1秒内加速度的平均值，计算公式为： 

其中，和是加速度计敏感参数的两个输出值，该加速度计敏感参数由外部的加速度计给出。 

3.如权利要求2所述的一种应用于个人室内导航***的相对导航方法，其特征在于：所述的步骤二惯性相对导航解算，包括 

a)姿态更新 

设表征载体姿态的四元数：Q＝[q₀ q₁ q₂ q₃]^T，四元数更新的解析式为： 

其中：I为4×4阶单位矩阵； 为k到k+1时刻陀螺敏感到的角增量，由下式获得：其中T_n＝0.005s，即导航解算周 期，姿态矩阵的更新计算公式为： 

b)速度、位置更新 

相对导航速度更新算法为： 

相对导航位置更新算法为： 

P(K+1)＝P(K)+0.5*(V(K+1)+V(K))*T_n   (4) 

其中为加速度计敏感信息在导航系中投影：V(K)、P(K)为K时刻的速度、位置信息，所述的T_n为导航周期，一般取50毫秒。 

4.如权利要求3所述的一种应用于个人室内导航***的相对导航方法，其特征在于：所述的步骤六修正输出中，使用的卡尔曼滤波***方程为

其中，公式（8）中的第一个方程为***误差状态方程，具体形式见步骤（6.1），X(t)为***误差状态量X(t)＝[δP,δX,φ,▽,ε]^T，F(t)为***状态转移矩阵 W(t)为***噪声矢量，g为当地重力加速度， 

公式（8）中的第二个方程为***观测方程，具体形式见步骤（6.2）， 为零速观测量，H＝[0_3×3 I_3×3 0_3×3 0_3×3 0_3×3]，V(t)为观测噪声矢量， 

（6.1）***误差方程 

a)位置误差方程 

b)速度误差方程 

c)姿态误差方程 

d)加速度计零位误差方程 

e)陀螺漂移误差方程 

其中，δP=[δP_x δP_y δP_z]^T、δV=[δV_x δV_y δV_z]^T为位置、速度误差，φ=[φ_x φ_y φ_z]^T为姿态误差，▽=[▽_x ▽_y ▽_z]^T为机体系加速度计零位误差，ε=[ε_x ε_y ε_z]^T为机体系陀螺漂移误差，

（6.2）零速匹配观测方程 

在***检测到人体处于静止状态下，利用下式求观测量： 

其中，Z为零速匹配卡尔曼滤波器观测量，为零速状态下滤波周期内速度平均值，在此设定卡尔曼滤波周期为50ms，即10个导航周期滤波一次， 

观测方程：