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

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

背景技术：

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

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

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

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

技术实现要素：

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

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

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

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

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

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

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

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

δk＝kpek+ki[ek+ek-1]·h/2(4)

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

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

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

vk＝vk-1+0.5·(ak+a′k)·h-δvk(5)

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

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

rk＝rk-1+0.5·(vk+vk-1)·h-δrk(8)

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

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

pk|k-1＝φkpk-1φk^t+qk-1(9)

kk＝pk|k-1h^t(hpk|k-1h^t+rk)^-1(10)

xk＝φkxk-1-kk(zk-hφkxk-1)(11)

pk＝(i6×6-kkh)pk|k-1(i6×6-kkh)^t+kkrkkk^t(12)

其中，pk|k-1是状态预测协方差矩阵，φk是k-1时刻至k时刻的一步转移矩阵，pk-1是k-1时刻协方差矩阵，qk-1是k-1时刻零均值状态噪声方差阵，kk是滤波增益矩阵，h是量测矩阵，rk是k时刻零均值量测噪声方差阵，是k时刻滤波状态量，xk-1是k-1时刻滤波状态量，量测量zk＝vk-[000]^t，pk为k时刻估计协方差矩阵用于衡量滤波状态量xk的估计，i6×6为6阶单位矩阵，从滤波状态量xk中提取，即可得到速度误差δvk和位置误差δrk；

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

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

附图说明

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

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

具体实施方式

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

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

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

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

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

c＝c1andc2andc3andc4(17)

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

其中，m1是滑动窗口长度m1+m2的左偏移量，m2是滑动窗口长度m1+m2的右偏移；

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

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

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

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

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

δk＝kpek+ki[ek+ek-1]·h/2(4)

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

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

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

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

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

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

vk＝vk-1+0.5·(ak+a′k)·h-δvk(5)

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

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

rk＝rk-1+0.5·(vk+vk-1)·h-δrk(8)

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

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

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

xk＝φkxk-1+wk-1(20)

zk＝hxk+vk(21)

其中，xk是k时刻状态向量，φk是k-1时刻至k时刻的一步转移矩阵，wk-1是k-1时刻状态噪声矩阵为qk的零均值噪声序列，zk是k时刻观测向量，h是量测矩阵，vk是k时刻量测噪声矩阵为rk的零均值噪声序列。

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

pk|k-1＝φkpk-1φk^t+qk-1(9)

kk＝pk|k-1h^t(hpk|k-1h^t+rk)^-1(10)

xk＝φkxk-1-kk(zk-hφkxk-1)(11)

pk＝(i6×6-kkh)pk|k-1(i6×6-kkh)^t+kkrkkk^t(12)

其中，pk|k-1是状态预测协方差矩阵，φk是k-1时刻至k时刻的一步转移矩阵，pk-1是k-1时刻协方差矩阵，qk-1是k-1时刻零均值状态噪声方差阵，kk是滤波增益矩阵，h是量测矩阵，rk是k时刻零均值量测噪声方差阵，是k时刻滤波状态量，xk-1是k-1时刻滤波状态量，量测量zk＝vk-[000]^t，pk为k时刻估计协方差矩阵用于衡量滤波状态量xk的估计，i6×6为6阶单位矩阵，从滤波状态量xk中提取，即可得到速度误差δvk和位置误差δrk。具体滤波参数如下：

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

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

δk＝[000]^t

δvk＝[000]^t

δrk＝[000]^t