CN108444467A - 一种基于反馈互补滤波和代数逼近的行人定位方法 - Google Patents

Abstract

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

Description

一种基于反馈互补滤波和代数逼近的行人定位方法

技术领域

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

背景技术

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

目前，南京航空航天大学专利公开号为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中所述代数逼近姿态解算为在方向余弦矩阵微分方程基础上，用代数逼近的方法求离散解，微分方程离散解用有理分式表示，具体计算公式如下：

其中，是k时刻的方向余弦矩阵，其中矩阵上标n表示导航坐标系，下标b 表示载体坐标系，是k-1时刻的方向余弦矩阵，I是3阶单位矩阵，Ω_k是三轴陀 螺仪输出角速度构成的反对称矩阵和k时刻三轴陀螺仪输出角速度漂移补偿量构成的 反对称矩阵的差值组成的三阶矩阵，T_s是三轴陀螺仪输出角速度以及加速度计输出比 力的采样周期，k时刻三轴陀螺仪输出角速度为δ_k为三轴陀 螺仪输出角速度漂移补偿量；

其中，所述三轴陀螺仪输出角速度漂移补偿量δ_k的求解方法如下：

当C＝1时，对三轴加速度计输出的比力单位化，用k-1时刻的方向余弦矩阵将标准重力加速度转换到载体坐标系，单位化后的比力和载体系下的重力加速度矢量叉乘 获取误差角，并减去k-1时刻的三轴陀螺仪输出角速度漂移后获取角度误差角，获取 的角度误差角经过比例和积分环节后获取角速度漂移，反馈互补滤波具体计算公式如 下：

δ_k＝k_pe_k+k_I[e_k+e_k-1]·h/2 (4)

其中，姿态矩阵是的转置，是k时刻加速度计输出的比力，gⁿ＝[0 01]^T是标准重力参考量，为加速度计输出比力滑动方差确定的比例系数，参数值k_P是比例系数，k_I是积分系数，δ_k三轴陀螺仪k时刻输出角速度漂移补偿量，δ_k-1三轴陀 螺仪k-1时刻输出角速度漂移补偿量,e_k角增量误差项；

所获取的三轴陀螺仪输出角速度漂移补偿量δ_k在行人处于长期零速状态下，即 C＝1时使用；当C＝0时，即行人非长期静止，则三轴陀螺仪输出角速度漂移补偿量δ_k是零向量。

所述速度递推更新为在速度微分方程的基础上，采用梯形积分法求离散速度解，梯形积分法为用k时刻剩余加速度和k-1时刻剩余加速度的均值作为平均加速度，k 时刻的速度由k-1时刻速度叠加平均加速度与更新周期相乘后的速度增量获得，剩余 加速度是三轴加速度计输出的比力补偿重力后剩下的加速度，速度递推更新的具体计 算公式如下，

v_k＝v_k-1+0.5·(a_k+a′_k)·h-δv_k (5)

其中，v_k是k时刻行人速度，v_k-1是k-1时刻行人速度，h是速度递推更新周期， 也是姿态更新周期和位置递推更新周期，δv_k是k时刻速度误差，g₀为是重力加速度 常量；

所述的位置递推更新的具体计算公式如下：

r_k＝r_k-1+0.5·(v_k+v_k-1)·h-δr_k (8)

其中，v_k是k时刻行人速度，v_k-1是k-1时刻行人速度，r_k是k时刻的行人位置， r_k-1是k-1时刻的行人位置，δr_k是k时刻位置误差。

所述卡尔曼滤波所用的速度误差δv_k和位置误差δr_k的具体形式如下，

P_k|k-1＝Φ_kP_k-1Φ_k ^T+Q_k-1 (9)

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1 (10)

X_k＝Φ_kX_k-1-K_k(Z_k-HΦ_kX_k-1) (11)

P_k＝(I_6×6-K_kH)P_k|k-1(I_6×6-K_kH)^T+K_kR_kK_k ^T (12)

其中，P_k|k-1是状态预测协方差矩阵，Φ_k是k-1时刻至k时刻的一步转移矩阵，P_k-1是k-1时刻协方差矩阵，Q_k-1是k-1时刻零均值状态噪声方差阵，K_k是滤波增益矩 阵，H是量测矩阵，R_k是k时刻零均值量测噪声方差阵，是k时刻滤 波状态量，X_k-1是k-1时刻滤波状态量，量测量Z_k＝v_k-[0 0 0]^T，P_k为k时刻估计协方 差矩阵用于衡量滤波状态量X_k的估计，I_6×6为6阶单位矩阵，从滤波状态量X_k中提取， 即可得到速度误差δv_k和位置误差δr_k；

其中当C＝0时，即行人非长期静止，则速度误差δv_k和位置误差δr_k都是零向量。

本发明的有益效果在于提出的基于反馈互补滤波和代数逼近的行人定位方法相比 传统定位方法，一是反馈互补滤波用于行人长期静止下的三轴陀螺仪输出角速度漂移估计，较为准确估计陀螺漂移，传统的基于外部零速度参考的卡尔曼滤波难以估计三 轴陀螺仪输出角速度漂移；二是按照比例反馈上一时刻三轴陀螺仪输出角速度漂移补 偿量，更快调节三轴陀螺仪输出角速度漂移；三是行人长期静止状态下的位置估计中， 将姿态解算与速度更新及位置更新分离，降低卡尔曼滤波维数，降低计算量，有利于 节约电源消耗；四是本发明实现简单，可移植性好。

附图说明

图1是基于反馈互补滤波和代数逼近的行人定位原理图。

图2是反馈互补滤波和代数逼近姿态解算原理图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

本发明中提出的反馈互补滤波器在传统的互补滤波器基础上，添加上一时刻加速度计输出的比力和姿态矩阵确定的体坐标系上的参考重力获取的三轴陀螺仪输出角速 度漂移补偿量，作为反馈信息，通过正交化计算形式的代数逼近法求解行人姿态，再 根据行人零速度信息，通过卡尔曼滤波估计出行人的速度误差和位置误差，修正航位 推算中的速度和位置。

如图1所示，一种基于反馈互补滤波和代数逼近的行人定位方法，具体包含如下步骤：

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

本发明中，行人长时间静止(大于10秒)状态的判断具体如下：

C＝C₁and C₂and C₃and C₄ (17)

其中，是k时刻三轴加速度计输出的比力模值，g₀是重力加速度模值，Gate_a是差值阈值常量，是k时刻三轴加速度计输出比力的滑动方差，Gate_var是三轴加速 度计输出比力的滑动方差阈值常量，是三轴陀螺仪输出的角速度模值，Gate_ω是 三轴陀螺仪输出的角速度模值阈值常量，Δt是两段时间差值，C_i,(i＝1,2,3,4)是逻辑 值条件，“and”是逻辑与，C是总逻辑值，C＝1表示行人处于长时间静止状态，C＝0 表示行人处于非长时间静止状态，的具体计算如下，

其中，M₁是滑动窗口长度M₁+M₂的左偏移量，M₂是滑动窗口长度M₁+M₂的 右偏移；

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

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

其中，是k时刻的方向余弦矩阵，其中矩阵上标n表示导航坐标系，下标b 表示载体坐标系，是k-1时刻的方向余弦矩阵，I是3阶单位矩阵，Ω_k是三轴陀 螺仪输出角速度构成的反对称矩阵和k时刻三轴陀螺仪输出角速度漂移补偿量构成的 反对称矩阵的差值组成的三阶矩阵，T_s是三轴陀螺仪输出角速度以及加速度计输出比 力的采样周期，k时刻三轴陀螺仪输出角速度为δ_k为三轴陀 螺仪输出角速度漂移补偿量；

其中，所述三轴陀螺仪输出角速度漂移补偿量δ_k的求解方法如下：

当C＝1时，对三轴加速度计输出的比力单位化，用k-1时刻的方向余弦矩阵将标准重力加速度转换到载体坐标系，单位化后的比力和载体系下的重力加速度矢量叉乘 获取误差角，并减去k-1时刻的三轴陀螺仪输出角速度漂移后获取角度误差角，获取 的角度误差角经过比例和积分环节后获取角速度漂移，如图2所示，反馈互补滤波具 体计算公式如下：

δ_k＝k_pe_k+k_I[e_k+e_k-1]·h/2 (4)

其中，姿态矩阵是的转置，是k时刻加速度计输出的比力，gⁿ＝[0 0 1]^T是 标准重力参考量，为加速度计输出比力滑动方差确定的比例系数，具体计 算为：

其中，a根据实验测定，本发明中a＝0.1。

参数值k_P是比例系数，k_I是积分系数，δ_k三轴陀螺仪k时刻输出角速度漂移补偿量，δ_k-1三轴陀螺仪k-1时刻输出角速度漂移补偿量,e_k角增量误差项；

三轴加速度计输出的k时刻比力单位化后与标准重力向量gⁿ＝[0 0 1]^T在载体系 下的投影量叉乘，再反馈k-1时刻的部分三轴陀螺仪输出角速度漂移补偿量获得k时刻的三轴陀螺仪输出角速度漂移补偿量δ_k，三轴陀螺仪输出角 速度补偿掉δ_k后，用代数逼近法姿态解算，获取k时刻的方向余弦矩阵

所获取的三轴陀螺仪输出角速度漂移补偿量δ_k在行人处于长期零速状态下，即 C＝1时使用；当C＝0时，即行人非长期静止，则三轴陀螺仪输出角速度漂移补偿量δ_k是零向量。

所述速度递推更新为在速度微分方程的基础上，采用梯形积分法求离散速度解，梯形积分法为用k时刻剩余加速度和k-1时刻剩余加速度的均值作为平均加速度，k 时刻的速度由k-1时刻速度叠加平均加速度与更新周期相乘后的速度增量获得，剩余 加速度是三轴加速度计输出的比力补偿重力后剩下的加速度，速度递推更新的具体计 算公式如下，

v_k＝v_k-1+0.5·(a_k+a′_k)·h-δv_k (5)

其中，v_k是k时刻行人速度，v_k-1是k-1时刻行人速度，h是速度递推更新周期， 也是姿态更新周期和位置递推更新周期，δv_k是k时刻速度误差，g₀为是重力加速度 常量，取g₀＝9.79m/s²；

所述的位置递推更新的具体计算公式如下：

r_k＝r_k-1+0.5·(v_k+v_k-1)·h-δr_k (8)

其中，v_k是k时刻行人速度，v_k-1是k-1时刻行人速度，r_k是k时刻的行人位置， r_k-1是k-1时刻的行人位置，δr_k是k时刻位置误差。

所述卡尔曼滤波所用的速度误差δv_k和位置误差δr_k的具体形式如下：

速度误差δv_k和位置误差δr_k的获取通过卡尔曼滤波实现的，系统的状态空间方程如下：

X_k＝Φ_kX_k-1+W_k-1 (20)

Z_k＝HX_k+V_k (21)

其中，X_k是k时刻状态向量，Φ_k是k-1时刻至k时刻的一步转移矩阵，W_k-1是 k-1时刻状态噪声矩阵为Q_k的零均值噪声序列，Z_k是k时刻观测向量，H是量测矩阵， V_k是k时刻量测噪声矩阵为R_k的零均值噪声序列。

对于系统状态方程，卡尔曼滤波具体形式如下：

P_k|k-1＝Φ_kP_k-1Φ_k ^T+Q_k-1 (9)

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1 (10)

X_k＝Φ_kX_k-1-K_k(Z_k-HΦ_kX_k-1) (11)

P_k＝(I_6×6-K_kH)P_k|k-1(I_6×6-K_kH)^T+K_kR_kK_k ^T (12)

其中，P_k|k-1是状态预测协方差矩阵，Φ_k是k-1时刻至k时刻的一步转移矩阵，P_k-1是k-1时刻协方差矩阵，Q_k-1是k-1时刻零均值状态噪声方差阵，K_k是滤波增益矩 阵，H是量测矩阵，R_k是k时刻零均值量测噪声方差阵，是k时刻滤 波状态量，X_k-1是k-1时刻滤波状态量，量测量Z_k＝v_k-[0 0 0]^T，P_k为k时刻估计协方 差矩阵用于衡量滤波状态量X_k的估计，I_6×6为6阶单位矩阵，从滤波状态量X_k中提取， 即可得到速度误差δv_k和位置误差δr_k。具体滤波参数如下：

从滤波状态量X_k中提取速度误差和位置误差，即可用于步骤2中计算。

其中当C＝0时，即行人非长期静止，则三轴陀螺仪输出角速度漂移补偿量δ_k，速度误差δv_k和位置误差δr_k都是零向量，即：

δ_k＝[0 0 0]^T

δv_k＝[0 0 0]^T

δr_k＝[0 0 0]^T

Claims (3)

1.一种基于反馈互补滤波和代数逼近的行人定位方法，其特征在于包括下述步骤：

步骤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.根据权利要求1所述的一种基于反馈互补滤波和代数逼近的行人定位方法，其特征在于：

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

其中，是k时刻的方向余弦矩阵，其中矩阵上标n表示导航坐标系，下标b表示载体坐标系，是k-1时刻的方向余弦矩阵，I是3阶单位矩阵，Ω_k是三轴陀螺仪输出角速度构成的反对称矩阵和k时刻三轴陀螺仪输出角速度漂移补偿量构成的反对称矩阵的差值组成的三阶矩阵，T_s是三轴陀螺仪输出角速度以及加速度计输出比力的采样周期，k时刻三轴陀螺仪输出角速度为δ_k为三轴陀螺仪输出角速度漂移补偿量；

所述三轴陀螺仪输出角速度漂移补偿量δ_k的求解方法如下：

当C＝1时，对三轴加速度计输出的比力单位化，用k-1时刻的方向余弦矩阵将标准重力加速度转换到载体坐标系，单位化后的比力和载体系下的重力加速度矢量叉乘获取误差角，并减去k-1时刻的三轴陀螺仪输出角速度漂移后获取角度误差角，获取的角度误差角经过比例和积分环节后获取角速度漂移，反馈互补滤波具体计算公式如下：

δ_k＝k_pe_k+k_I[e_k+e_k-1]·h/2 (4)

其中，姿态矩阵是的转置，是k时刻加速度计输出的比力，gⁿ＝[0 0 1]^T是标准重力参考量，为加速度计输出比力滑动方差确定的比例系数，参数值k_P是比例系数，k_I是积分系数，δ_k三轴陀螺仪k时刻输出角速度漂移补偿量，δ_k-1三轴陀螺仪k-1时刻输出角速度漂移补偿量,e_k角增量误差项；

所获取的三轴陀螺仪输出角速度漂移补偿量δ_k在行人处于长期零速状态下，即C＝1时使用；当C＝0时，即行人非长期静止，则三轴陀螺仪输出角速度漂移补偿量δ_k是零向量。

3.根据权利要求1所述的一种基于反馈互补滤波和代数逼近的行人定位方法，其特征在于：

步骤2中所述速度递推更新为在速度微分方程的基础上，采用梯形积分法求离散速度解，梯形积分法为用k时刻剩余加速度和k-1时刻剩余加速度的均值作为平均加速度，k时刻的速度由k-1时刻速度叠加平均加速度与更新周期相乘后的速度增量获得，剩余加速度是三轴加速度计输出的比力补偿重力后剩下的加速度，速度递推更新的具体计算公式如下，

v_k＝v_k-1+0.5·(a_k+a′_k)·h-δv_k (5)

其中，v_k是k时刻行人速度，v_k-1是k-1时刻行人速度，h是速度递推更新周期，也是姿态更新周期和位置递推更新周期，δv_k是k时刻速度误差，g₀为是重力加速度常量；

步骤2中所述的位置递推更新的具体计算公式如下：

r_k＝r_k-1+0.5·(v_k+v_k-1)·h-δr_k (8)

其中，v_k是k时刻行人速度，v_k-1是k-1时刻行人速度，r_k是k时刻的行人位置，r_k-1是k-1时刻的行人位置，δr_k是k时刻位置误差；

所述卡尔曼滤波所用的速度误差δv_k和位置误差δr_k的具体形式如下，

P_k|k-1＝Φ_kP_k-1Φ_k ^T+Q_k-1 (9)

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1 (10)

X_k＝Φ_kX_k-1-K_k(Z_k-HΦ_kX_k-1) (11)

P_k＝(I_6×6-K_kH)P_k|k-1(I_6×6-K_kH)^T+K_kR_kK_k ^T (12)

其中，P_k|k-1是状态预测协方差矩阵，Φ_k是k-1时刻至k时刻的一步转移矩阵，P_k-1是k-1时刻协方差矩阵，Q_k-1是k-1时刻零均值状态噪声方差阵，K_k是滤波增益矩阵，H是量测矩阵，R_k是k时刻零均值量测噪声方差阵，是k时刻滤波状态量，X_k-1是k-1时刻滤波状态量，量测量Z_k＝v_k-[0 0 0]^T，P_k为k时刻估计协方差矩阵用于衡量滤波状态量X_k的估计，I_6×6为6阶单位矩阵，从滤波状态量X_k中提取，即可得到速度误差δv_k和位置误差δr_k；

其中当C＝0时，即行人非长期静止，则速度误差δv_k和位置误差δr_k都是零向量。