一种应用于个人室内导航系统的相对导航方法
【技术领域】
[0001] 本发明属于导航方法，具体涉及一种应用于个人室内导航系统的相对导航方法。
【背景技术】
[0002] 由于卫星导航所具有的高精度、低功耗、易携带等独特优势，其在日常生活中的应 用日益广泛，然而在室内、地下及工作厂房等密闭空间中由于卫星信号受遮挡卫星用户机 不能定位而无法满足用户的导航需求。
[0003] 个人室内导航系统由于其不依赖卫星导航、完全自主的独特优势而受到越来越多 的重视，W美国为首的欧美国家对该系统的研发起步较早，但尚未出现货架产品，为了填补 国内对该技术与产品的研发空白，对个人室内导航技术进行研究。
【发明内容】
[0004] 本发明的目的是针对现有技术的缺陷，提供一种应用于个人室内导航系统的相对 导航方法。
[0005] 本发明是该样实现的：一种应用于个人室内导航系统的相对导航方法，包括下述 步骤：
[000引步骤一；初始化
[0007] 对速度、位置、姿态进行初始化，在第一次计算时使用初始化数据，在后续循环计 算中使用外部传感器给出的实时信息；
[0008] 步骤二：惯性相对导航解算
[0009] 计算速度、位置信息；
[0010] 步骤H;保存必要信息
[0011] 保存加速度计说、巧螺敏感信息W及加速度计信息在导航坐标系中投影值 fl. *
[0012] 步骤四：计算投影值积分
[0013] 计算加速度计、巧螺敏感信息均方差W及加速度计信息在导航坐标系中投影值积 分；
[0014] 步骤五；检测是否静止
[0015] 当系统同时满足下述H个条件时，判定系统静止
[0016] 1)H轴加速度计敏感信息的均方差均小于0. 05m/s'2 ;
[0017] 2)H轴巧螺敏感信息的均方差均小于0.OOlrad/s;
[001引3)-个滤波周期内加速度积分绝对值
均小于O.Olm/s;
[0019] 如果判断结果是系统处于静止状态，则执行下面步骤六，否则执行步骤二；
[0020] 步骤六：修正输出
[0021] 对速度、位置、加速度进行修正，修正后的数据作为本方法的输出，并循环执行步 骤二~步骤六，直到导航结束。
[0022] 如上所述的一种应用于个人室内导航系统的相对导航方法，其中，所述的步骤一 的初始化包括给定初始化数据，包括
[0023] a)初始位置；X=0,Y=0,Z=0;
[0024] b)初始速度；Vx=0,Vy=0,Vz=0;
[0025]c)初始姿态：俯仰角巧cl/! =gsin化/各），滚动角 敌W/ =osin(-X《g.cos(巧6姑说，航向角Yaw=0.0,其中的asin表示反正弦函数
[0026]其中为1砂内加速度的平均值，计算公式为：
[0027]
[002引其中，.成巧和记:巧是加速度计敏感参数'冷=以点乂；，.说]的两个输出值，该 加速度计敏感参数由外部的加速度计给出。
[0029] 如上所述的一种应用于个人室内导航系统的相对导航方法，其中，所述的步骤二 惯性相对导航解算，包括
[0030] a)姿态更新
[0031] 设表征载体姿态的四元数；Q= [q。屯Q3]t，四元数更新的解析式为：
[0033] 其中；1为4X4阶单位矩阵；
[0032] (1)
句k至IJk+1时刻巧螺敏感到
的角增量，由下式获得； 痒中T。= 0. 005s，即导航解算周期，姿态矩阵 仁:; 的更新计算公式为：
[0034]
[00对b)速度、位置更新
[0036] 相对导航速度更新算法为：
[0037] riK + l)^nK) + fi*T" (3)
[0038] 相对导航位置更新算法为：
[0039] P化+1) =P化)+0. 5*(V(K+1)+V化)）*Tn(4)
[0040] 其中iJ为加速度计敏感信息篇在导航系中投影，V似、P似为K时 刻的速度、位置信息，所述的T。为导航周期，一般取50毫砂。
[0041] 如上所述的一种应用于个人室内导航系统的相对导航方法，其中，所述的步骤六 修正输出中，使用的卡尔曼滤波系统方程为
[0042] 其中，公式（8)中的第一个方程为系统误差状态方程，具体形式见步骤（6. 1)， X(t)为系统误差状态量X(t) = [SP，5X，（35, ▽，e]T，F(t)为系统状态转移矩阵
W(t)为系统噪声矢量，g为当地重力 加速度，
[0043] 公式（8)中的第二个方程为系统观测方程，具体形式见步骤（6. 2)， Z=[v, KFJ^为零速观测量，H= [03x3l3X3 03X3 03X3 03X3]，^t)为观测噪声矢量'
[0044] (6.1)系统误差方程
[0045] a)位置误差方程
[0046] SP=S￥ (9)
[0047] b)速度误差方程
[0051]d)加速度计零位误差方程
[0052]
(域
[0053] e)巧螺漂移误差方程
[0054] (13)
[005引其中，5p=[SPx5Py5Pz]t、5V=[SVx5Vy5Vz]t为位置、速度误差，cj5 =[cK为姿态误差，VyVJ为机体系加速度计零位误差，e=[e,ej T为机体系巧螺漂移误差，
[0056] (6. 2)零速匹配观测方程
[0057] 在系统检测到人体处于静止状态下，利用下式求观测量：
[0058] Z=V-0.0 (14)
[0059] 其中，Z为零速匹配卡尔曼滤波器观测量，沒而为零速状态下滤波周期内速度平均 值，在此设定卡尔曼滤波周期为50ms，即10个导航周期滤波一次，
[0060] 观测方程：
[0061] sv=z=v-m (巧）。
[0062] 本发明的效果是：本发明的方法可W准确有效的解算出实验人员行进的轨迹，说 明本方法正确、有效。
【附图说明】
[0063] 附图1是工作人员进行实验时行走路线的示意图。
[0064] 附图2是利用本方法导航得到的轨迹图。
【具体实施方式】
[0065] 一种应用于个人室内导航系统的相对导航方法，包括下述步骤：
[006引步骤一；初始化
[0067] 给定初始化数据，包括
[0068] d)初始位置；X=0,Y=0,Z=0 ;
[0069] e)初始速度；Vx=0,Vy=0,Vz=0 ;
[0070] f)初始姿态：俯仰角巧c如=0sin化/幻，滚动角 货W/ =asin(-I%.cos(巧幻々))），航向角Yaw= 0.0。其中的asin表示反正弦函数
[0071]其中4为1砂内加速度./;L，./i的平均值，计算公式为：
[007引其中，施(0和./;化)是加速度计敏感参数
的两个输出值，该 加速度计敏感参数由外部的加速度计给出。初始化之后，在后续计算过程中使用到的速度、 姿态、加速度等信息，均由外部相应传感器给出。
[0073] 步骤二：惯性相对导航解算
[0074]C)姿态更新
[0075] 设表征载体姿态的四元数；Q= [q。屯Q3]t，四元数更新的解析式为：
[0077] 其中；1为4X4阶单位矩阵；
[0076] (1)
为k至IJk+1时刻巧螺敏感到
的角增量，由下式获得： 其中T。= 0. 005s，即导航解算周期，姿态矩阵 巧 的更新计算公式为：
[0078]
[0079] 即用公式（1)计算qO~q3,计算出该个周期的qO~q3,继而得到该个周期的姿 态矩阵。下个周期的姿态矩阵再利用该个公式计算。
[0080]d)速度、位置更新
[0081] 相对导航速度更新算法为：
[0082]
(3)
[0083] 相对导航位置更新算法为：
[0084] P化+1) =P化)+0. 5*(V(K+1)+V化)）*Tn(4)
[00财其中4"为加速度计敏感信息錢在导航系中投影：.觀=记，V化)、P似为K时 刻的速度、位置信息。所述的T。为导航周期，一般取50毫砂。
[0086] 步骤H;保存必要信息
[0087] 保存加速度计巧螺敏感信息W及加速度计信息在导航坐标系