[发明专利]一种基于反馈互补滤波和代数逼近的行人定位方法

说明书

本发明提供了一种基于反馈互补滤波和代数逼近的行人定位方法，涉及行人定位领域，本发明用逻辑值判断行人的静止状态，然后根据逻辑值，进行代数逼近姿态解算，速度递推更新和位置递推更新，且行人在长时间静止时，对代数逼近姿态、速度和位置进行修正后再计算，本发明反馈互补滤波用于行人长期静止下的三轴陀螺仪输出角速度漂移估计，较为准确估计陀螺漂移；按照比例反馈上一时刻三轴陀螺仪输出角速度漂移补偿量，更快调节三轴陀螺仪输出角速度漂移；行人长期静止状态下的位置估计中，将姿态解算与速度更新及位置更新分离，降低卡尔曼滤波维数，降低计算量，有利于节约电源消耗；总之本发明实现简单，可移植性好。

技术领域

本发明涉及行人定位领域，尤其是一种利用惯性传感器实现行人定位的方法。

背景技术

互补滤波经简单计算获取陀螺仪输出的修正值，间接校正载体的姿态，广泛用于小型无人机、机器人、无人车辆领域，代数逼近法通过有理式的形式求解微分方程， 以优秀的正交性，广泛用于固体力学、流体力学、姿态求解等领域。

目前，南京航空航天大学专利公开号为CN106123900A的专利中用互补滤波辅助实现行人的磁航向解算。中国兵器工业集团第二一四研究所苏州研发中心专利号为CN105241454A的专利中用磁力计辅助微机电系统(Micro-Electro-Mechanical System，MEMS)进行磁航向和水平姿态的修正。当前行人定位中的姿态的解算方法有欧拉角 法、四元数法、方向余弦法。互补滤波器利用重力加速度作为参考信息估计陀螺仪输 出的角速度零偏，一般通过四元数法或方向余弦矩阵求解姿态信息。

行人长时间静止时(大于10秒)，传统基于卡尔曼滤波的估计方法，难以估计 MEMS陀螺仪的角速度漂移，进而导致行人静止时姿态角缓慢漂移。互补滤波器可用 于准确估计行人静止时MEMS陀螺仪角速度漂移，进而抑制姿态角的缓慢漂移。

传统的基于互补滤波器的姿态解算存在的问题包括，行人姿态对应的姿态矩阵或四元数需要在每次计算后正交化，同时传统的互补滤波器中获取加速度计输出的比力 和姿态矩阵确定的体坐标系上的参考重力获取三轴陀螺仪输出角速度漂移补偿量时， 未充分利用上一时刻的三轴陀螺仪输出角速度漂移补偿量信息。

发明内容

为了克服现有技术的不足，本发明提供一种基于反馈互补滤波和代数逼近的行人定位方法。

本发明解决其技术问题所采用的技术方案具体包含如下步骤：

步骤1：本发明所用MEMS传感器包含三轴加速度计和三轴陀螺仪的惯性测量单元，行人静止m秒以上定义为长时间静止，m取10秒，用四个逻辑值确定行人是否 处于长时间静止状态，分别为三轴加速度计输出比力模值确定的逻辑值C₁，三轴加速 度计输出比力滑动方差确定的逻辑值C₂，三轴陀螺仪输出角速度模值确定的逻辑值C₃和时间判断逻辑值C₄，总的条件逻辑值C由C₁，C₂，C₃，C₄之间利用逻辑与运算确 定，当C＝1表示行人处于长时间静止状态，C＝0表示行人处于非长时间静止状态；

步骤2：根据步骤1获取的总逻辑值C，对于行人静止时间进行零速判断，即当 C＝0时，根据三轴加速度计输出的比力和三轴陀螺仪输出的角速度，利用递推的思 想求解行人位置，包括代数逼近姿态解算，速度递推更新和位置递推更新；当C＝1时， 则用反馈互补滤波估计的三轴陀螺仪角速度漂移修正代数逼近姿态解算结果，用卡尔 曼滤波估计的速度误差和位置误差修正速度和位置，经过分别修正后，再进行代数逼 近姿态解算，速度递推更新和位置递推更新。

步骤2中所述代数逼近姿态解算为在方向余弦矩阵微分方程基础上，用代数逼近的方法求离散解，微分方程离散解用有理分式表示，具体计算公式如下：