[发明专利]基于零速修正与姿态自观测的惯性行人导航算法

权利要求书

1.基于零速修正与姿态自观测的惯性行人导航算法，其特征处在于包括以下步骤：

行人运动过程中的数据被MIMU惯性传感器接收，并传输到导航计算机中，导航计算机对采集的数据进行捷联惯导解算，得到行人运动的速度、位置以及传感器姿态信息，由于采用捷联惯导解算时，导航误差会随时间逐渐积累，因此需要对导航误差进行修正，对采集的数据同时进行零速检测，检测出行走时的零速区间，设计卡尔曼滤波器，在零速区间内运用零速修正与姿态自观测算法估计出导航姿态、位置以及速度误差，并对惯导解算结果进行误差修正，从而实现对行人的精确定位与导航；

所述零速检测之后，在检测到的零速区间内，利用重力加速度分量计算出俯仰角θ_ACC和滚转角γ_ACC，捷联惯导解算之后能获得解算俯仰角θ_INS、滚转角γ_INS以及航向角φ，θ_ACC和γ_ACC分别与θ_INS和γ_INS作差获得俯仰角和滚转角误差观测量，而当前时刻的航向角φ与前两个时刻的航向角平均值作差得到航向角误差观测量，俯仰角、滚转角和航向角误差观测量共同构成姿态误差观测量；同时，在零速区间内，惯导解算速度v_INS就作为速度误差观测量，对上述速度误差观测量与姿态误差观测量，运用卡尔曼滤波进行状态估计，就得到姿态、位置和速度误差的估计值，利用误差估计值就实现系统的误差修正；

所述姿态自观测算法包括以下步骤：当行人处于零速区间时，由于脚部与地面保持静止，因此惯性传感器只受到重力加速度的作用，则下列等式在零速区间成立：

此时通过三轴加速度计输出确定出惯性传感器的俯仰角和滚转角：

此时，利用惯导解算出的俯仰角θ_INS和滚转角γ_INS与通过加速度确定的俯仰角θ_ACC和滚转角γ_ACC作差，构造俯仰角和滚转角误差观测量，但由于无法通过加速度计算航向角，基于上述条件无法构造航向角误差观测量，因此无法通过卡尔曼滤波修正航向角误差，而通过进一步分析零速区间发现，脚部与地面处于相对静止时，同一零速区间内的航向角也应具有不变的特性，因此将当前采样点航向角解算值与前两个采样点航向角平均值之差作为航向角误差的观测值

由此，得到姿态自观测方法的观测方程为：

其中，

将零速修正算法与姿态自观测算法的观测方程进行组合，得到系统观测方程为：

其中，

根据上述零速修正与姿态自观测算法，通过卡尔曼滤波就得到系统误差的估计值从而实现对捷联惯导解算参数的误差修正，