[发明专利]基于多信息融合补偿的行人高精度足部导航方法

权利要求书

1.一种基于多信息融合补偿的行人高精度足部导航方法，其特征在于：实时采集陀螺仪和加速度计的原始测量数据，初始陀螺仪的误差补偿为零，加速度计的误差补偿也为零，通过捷联惯性导航解算算法进行捷联解算；在上述基础上，构建卡尔曼滤波器修正模型进行滤波修正，状态方程一致，而量测方程随多维量测信息动态调整，在不同的多维信息检测下，构建对应的算法，量测方程实时变换，估计、修正导航定位结果误差和传感器误差，返回陀螺仪的误差补偿和加速度计的误差补偿；最后输出导航定位结果，即行人的位置、速度、姿态；

所述捷联惯性导航解算算法如下：

惯性传感器由加速度计与陀螺仪组成，加速度计获取运动载体的比力信息，通过一次积分获得速度，通过二次积分获得位置；陀螺仪测量机体系相对于惯性系的角速度，通过一次积分获得角度；最基本的捷联惯性导航解算算法包括姿态解算、速度解算、位置解算；于是由下列公式计算姿态、速度、位置：

其中，b为机体坐标系，n为导航坐标系，e为地球坐标系，i为惯性坐标系，f为比力，g为重力加速度，为机体坐标系到导航坐标系的姿态转移矩阵，为x、y、z三轴机体系相对于惯性系角速率在机体系上的投影，为三轴导航系相对于惯性系角速率在机体系上的投影，为地球自转角速率在导航系下的投影，为导航坐标系相对于地球坐标系的角速度在导航坐标系轴向的分量构成的列矢量，v为三轴速度矢量，p为三轴位置矢量；为关于时间t的导数，为vⁿ关于时间t的导数，为pⁿ关于时间t的导数，因此上式是常微分方程，在已知初始姿态、速度、位置的基础上，迭代求解；

所述卡尔曼滤波修正模型的状态方程如下：

卡尔曼滤波模型选用“东北天”地理坐标系，构建18阶状态模型如下：

其中，δφ＝[δφ_e δφ_n δφ_u]为平台角误差；δν＝[δv_e δv_n δv_u]为速度误差；δp＝[δλδL δh]为位置误差即经度、纬度、高度误差；ε_b＝[ε_bx ε_by ε_bz]为三轴陀螺随机常数；ε_r＝[ε_rx ε_ry ε_rz]为三轴陀螺仪一阶马尔科夫过程，为三轴加速度计一阶马尔科夫过程；

不同地理坐标系下姿态转移矩阵不一样，状态方程里的A、G、W系数也不一样，在“东北天”地理坐标系下，A、G、W设置如下列四式所示：A为系统矩阵，G为系统噪声矩阵，W＝[ω_gxω_gy ω_gz ω_rx ω_ry ω_rz ω_ax ω_ay ω_az]为系统噪声，根据传感器本身特性设置；其中ω_g为三轴陀螺随机白噪声漂移，其均方差为σ_g，ω_r是三轴陀螺均方差为σ_r的马尔科夫过程驱动白噪声，ω_a是三轴加速度计均方差为σ_a的马尔科夫过程驱动白噪声；

是对应9个基本导航参数的矩阵，T是马尔科夫过程的相关时间；

当只有零速检测成功时，采用伪量测信息构建基于零速卡尔曼滤波修正模型算法；当有零速检测成功和初始磁航向误差自观测时，构建行人初始静止状态下磁航向误差自观测算法；当有零速检测成功和动态状态下零速航向误差自观测时，构建行人运动状态下零速航向误差自观测算法；当有零速检测成功、运动状态下零速航向误差自观测和地磁匹配时，构建基于地磁匹配和捷联惯性导航解算算法的融合算法，其中每个算法的状态方程和量测方程共同作用构建了卡尔曼滤波器修正模型；

所述基于零速卡尔曼滤波修正模型算法如下：

在只有零速检测成功时，利用伪量测信息构建零速状态下的六维量测模型；令veloP为3×1阶的零矩阵，posiP(t)＝posiN(t-1)，则基于零速的卡尔曼滤波量测模型如下式所示：

其中，Z₁(t)为速度和位置的观测量，veloN为捷联惯导系统实时解算的速度，posiN为捷联惯导系统实时解算的位置，M_v为行人零速状态下速度误差，M_p为行人零速状态下位置误差，V₁为量测噪声，H₁为量测矩阵，如下列两式所示：

R_m为子午圈地球曲率半径，R_n为卯酉圈地球曲率半径，L为纬度；

所述行人初始静止状态下磁航向误差自观测算法如下：

行人初始静止状态下，磁航向角具有较强的稳定性，因此当有零速检测成功和初始磁航向误差自观测时，构建一维量测方程，如下式所示：

其中，Z₂(t)为初始航向角的观测量，为航向角误差，ψ_INS为捷联解算的航向角，由捷联惯性导航解算算法中矩阵进行三角函数转换获得，令则ψ_mg为磁航向角，由磁传感器测量得到的磁信息解算获得，为磁航向角解算误差，V₂为量测噪声；

行人初始静止状态下，利用下式实现对传感器误差的进一步估计：

Z₃(t)为速度、位置和初始航向角的观测量；

所述行人运动状态下零速航向误差自观测算法如下：

行人运动状态下，每一步经历一段零速时刻，在零速阶段航向基本不变，基于零速航向不变理论，当有零速检测成功和动态状态下零速航向误差自观测时，记该步第一零速时刻点的航向角为ψ_k-initial，记该步其余零速时刻点的航向角为ψ_k-others；

构建一维零速航向自观测量测方程如下：

其中：Z₄(t)为运动状态下航向角的观测量，为航向角误差；为零速航向角误差，V₄为量测噪声；因此：

θ为俯仰角，由捷联惯性导航解算算法中矩阵进行三角函数转换获得，

于是得运动状态下加入零速航向误差自观测算法的7维量测模型为：

其中，Z₅(t)为速度、位置和运动状态下航向角的观测量；

所述基于地磁匹配和捷联导航解算算法的融合算法如下：

当同时检测到零速及地磁匹配成功时，使用集中滤波器构建10维量测方程，辅助修正位置误差、速度误差及传感器误差，如下列三式所示，其中posiM为地磁匹配位置，由地磁匹配直接得到，M₆为地磁匹配位置误差，V₆为量测噪声，posiN为惯导实时解算的位置，δp为位置误差，HI如基于零速卡尔曼滤波修正模型算法中所示：

Z₆(t)为地磁匹配位置观测量，Z₇(t)为速度、位置、运动状态下航向角以及地磁匹配位置观测量。