中投影值 /2立类信息。其中加速度计/^、巧螺敏感信息64由外部给出，加速度计信息在导航坐标 系中投影值為由步骤H计算得到。
[0088] 步骤四：计算投影值积分
[0089] 计算加速度计、巧螺敏感信息均方差W及加速度计信息在导航坐标系中投影值积 分。其中投影值是用原始信息与姿态矩阵的转置矩阵相乘得到，积分是用投影值积分得到 的。
[0090] 步骤五；检测是否静止
[0091]由于个人室内导航系统采用的是微观性测量单元器件精度较差，短时间内纯惯性 导航导航误差发散较快，因此需要通过一定的修正算法抑制误差的快速发散，达到高精度 导航的目的，在个人室内导航导航系统中，我们充分利用人体行走过程中脚部运动动静交 替切换的特点，在脚部处于静止的时间段内，采用零速修正算法对系统各项导航误差进行 修正。
[0092] 零速修正的前提是静止状态的检测，零速检测算法需要既能够满足隔离系统的运 动状态，又能够对震动、扰动等特殊运动状态有一定容忍度，因此检测算法设计如下：
[0093] 1)每个导航周期实时滑动保存1个滤波周期内（10个导航周期）H轴加速度计 巧螺敏感信息严￡W及H轴加速度计信息在导航坐标系中投影爲.
[0094] 2)每个导航周期实时计算H轴加速度计、巧螺敏感信息的均方差及一个滤波周期 内H轴加速度计信息在导航坐标系中投影的积分值。其中，加速度均方差计算公式为：
[0098]H轴加速度计信息在导航坐标系中投影的积分值计算公式为：
[0099] 口）
[0100] 如果同时满足W下H个条件，则可W视该滤波周期内系统处于静止状态，否则系 统处于运动状态：
[010。 4)H轴加速度计敏感信息的均方差SM(./二.）均小于0. 05m/s'2 ;
[0102] 5)H轴巧螺敏感信息的均方差均小于0.OOlrad/s;
[0103] 6)-个滤波周期内加速度积分绝对值
均小于0.Olm/s;
[0104] 如果判断结果是系统处于静止状态，则执行下面步骤六，否则执行步骤二。
[0105] 步骤六：修正输出
[0106] 卡尔曼滤波系统方程
[0107] 锭）
[0108] 其中，公式（8)中的第一个方程为系统误差状态方程，具体形式见步骤（6.1)， x(t)为系统误差状态量X(t)= [SP，5X，（35,▽，e]T，F(t)为系统状态转移矩阵
Kt)为系统噪声矢量，g为当地重力 加速度。
[0109] 公式（8)中的第二个方程为系统观测方程，具体形式见步骤（6.2)， Z=[K KEJ为零速观测量，H= [03X3l3X3 03X3 03X3 03X3]，V(t)为观测噪声矢量。
[0110] (6.1)系统误差方程
[0111] f)位置误差方程
[0112] SP=SV (9)
[0113]g)速度误差方程
[0119]j)巧螺漂移误差方程
[0120] (13)
[0121] 其中，5p=[SPx5Py5Pz]t、5V=[SVx5Vy5Vz]t为位置、速度误差，cj5 =[cK 为姿态误差，VyVJ为机体系加速度计零位误差，e=[e,ej T为机体系巧螺漂移误差，
[0122] (6. 2)零速匹配观测方程
[0123] 在系统检测到人体处于静止状态下，利用下式求观测量：
[0124] Z=V-0.0 (14)
[0125] 其中，Z为零速匹配卡尔曼滤波器观测量，为零速状态下滤波周期内速度平均 值，在此设定卡尔曼滤波周期为50ms，即10个导航周期滤波一次。
[0126] 观测方程：
[0127] 經=Z=f-0.0 (巧）
[0128] 本领域的技术人员利用本领域的卡尔曼滤波基本知识，结合本申请公开的系统方 程、系统误差方程和零速匹配观测方程可W计算出速度、位置、加速度的误差，然后再利用 本领域的通用修正方法，可W得到修正后的速度、位置、加速度。所述修正后的速度、位置、 加速度即为本方法的输出值。但是因为导航时持续进行的，因此重复执行步骤二~步骤六， 直到导航结束。
【主权项】
1. 一种应用于个人室内导航系统的相对导航方法，其特征在于，包括下述步骤： 步骤一：初始化 对速度、位置、姿态进行初始化，在第一次计算时使用初始化数据，在后续循环计算中 使用外部传感器给出的实时信息； 步骤二：惯性相对导航解算 计算速度、位置信息； 步骤三：保存必要信息 保存加速度计Λ、陀螓敏感信息4以及加速度计信息在导航坐标系中投影值 步骤四：计算投影值积分 计算加速度计、陀螺敏感信息均方差以及加速度计信息在导航坐标系中投影值积分； 步骤五：检测是否静止 当系统同时满足下述三个条件时，判定系统静止 1) 三轴加速度计敏感信息的均方差均小于0. 〇5m/s~2 ; 2) 三轴陀螺敏感信息的均方差均小于0. 001rad/s ; 3) -个滤波周期内加速度积分绝对值均小于0. 01m/s ; 如果判断结果是系统处于静止状态，则执行下面步骤六，否则执行步骤二； 步骤六：修正输出 对速度、位置、加速度进行修正，修正后的数据作为本方法的输出，并循环执行步骤 二~步骤六，直到导航结束。2. 如权利要求1所述的一种应用于个人室内导航系统的相对导航方法，其特征在于： 所述的步骤一的初始化包括给定初始化数据，包括 a) 初始位置：X=0, Y=0, Z=O ; b) 初始速度：Vx=O，Vy=O，Vz=O ; c) 初始姿态：俯仰角航向角Yaw = 0· 0,其中的asin表示反正弦函数 其中,孑为1秒内加速度/Ul的平均值，计算公式为： * * ' *， ^t(I)白勺胃亥加M 度计敏感参数由外部的加速度计给出。3. 如权利要求2所述的一种应用于个人室内导航系统的相对导航方法，其特征在于： 所述的步骤二惯性相对导航解算，包括 a)姿态更新 设表征载体姿态的四元数：Q= [qtl qi q2 q3]T，四元数更新的解析式为：其中：I为4X4阶单位矩阵为k到k+Ι时刻陀螺敏感到 的角增量，由下式获得：其中Tn = 0.005s，即导航解算周期，姿态矩 阵的更新计算公式为：b)速度、位置更新 相对导航速度更新算法为：相对导航位置更新算法为： P (K+l) = P (K) +0· 5* (V (K+l) +V (K)) *Tn (4) 其中K为加速度计敏感信息,M在导航系中投影：，V(K)、P(K)为K时刻 的速度、位置信息，所述的Tn为导航周期，一般取50毫秒。4.如权利要求3所述的一种应用于个人室内导航系统的相对导航方法，其特征在于： 所述的步骤六修正输出中，使用的卡尔曼滤波系统方程为其中，公式（8)中的第一个方程为系统误差状态方程，具体形式见步骤（6. 1)， X(t)为系统误差状态量X(t) = [δΡ，δΧ，φ，▽，ε]τ，F(t)为系统状态转移矩阵为系统噪声矢量，g为当地重力 加速度， 公式（8)中的第二个方程为系统观测方程，具体形式见步骤为零速观测量，H= [03X3 I3x3 O3x3 O3x3 03X3]，V(t)为观测噪声矢量， (6.1)系统误差方程 a)位置误差方程其中，6?=[6?!￡6?7 6卩」'6￥=[6￥!￡6￥7 6\]1为位置、速度误差，（]5 = [(]5!￡(^ Φζ]τ为姿态误差，V=IiVxVy VJtS机体系加速度计零位误差，ε=[εχ ey εζ]τ为 机体系陀螺漂移误_(6. 2)零速匹配观测方程 在系统检测到人体处于静止状态下，利用下式求观测量：其中，Z为零速匹配卡尔曼滤波器观测量，为零速状态下滤波周期内速度平均值， 在此设定卡尔曼滤波周期为50ms，即10个导航周期滤波一次， 观测方程：
【专利摘要】本发明属于导航方法，具体涉及一种应用于个人室内导航系统的相对导航方法。它包括：步骤一：初始化，步骤二：惯性相对导航解算，计算速度、位置信息；步骤三：保存必要信息；步骤四：计算投影值积分；步骤五：检测是否静止；步骤六：修正输出；对速度、位置、加速度进行修正，修正后的数据作为本方法的输出，并循环执行步骤二～步骤六，直到导航结束。本发明的效果是：本发明的方法可以准确有效的解算出实验人员行进的轨迹，说明本方法正确、有效。
【IPC分类】G01C21/16
【公开号】CN104977001
【申请号】CN201410130948
【发明人】姜述明, 王根, 吴亮华, 徐海刚, 李瑞贤, 郭元江
【申请人】北京自动化控制设备研究所
【公开日】2015年10月14日
【申请日】2014年4月2日