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

权利要求书

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时，则用反馈互补滤波估计的三轴陀螺仪角速度漂移修正代数逼近姿态解算结果，用卡尔曼滤波估计的速度误差和位置误差修正速度和位置，经过分别修正后，再进行代数逼近姿态解算，速度递推更新和位置递推更新；

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

其中，是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-1为k-1时刻角增量误差项，e_k为k时刻角增量误差项，h为速度递推更新周期；

所获取的三轴陀螺仪输出角速度漂移补偿量δ_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₀为是重力加速度常量；

步骤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都是零向量。