[发明专利]一种应用于个人室内导航系统的相对导航方法

权利要求书

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个导航周期滤波一次， 

观测方程：