[发明专利]一种基于等式约束卡尔曼滤波的单兵导航方法

权利要求书

1.一种基于等式约束卡尔曼滤波的单兵导航方法，其特征在于，包括以下步骤：

(1)将GPS安置在单兵头部，两个MIMU导航器件分别放置在单兵的单脚脚尖与脚后跟；

(2)测量三个导航子系统所构成的空间三角形边长；

(3)根据三边关系，构造两个非线性等式约束方程，其中第一个等式约束即为双IMU之间的相对距离固定；

(4)运用基于等式约束的卡尔曼滤波方法，对各导航子系统分别校正与解算。

2.根据权利要求1所述的一种基于等式约束卡尔曼滤波的单兵导航方法，其特征在于，在所述步骤(4)中，当单兵行走时，脚部接触地面的瞬间，传感器对地绝对速度为零，则可以将对地绝对零速设为观测量，进行滤波估计，实现对陀螺加速度计零位误差的反馈矫正，即触发两个MIMU的零速校正。

3.根据权利要求1所述的一种基于等式约束卡尔曼滤波的单兵导航方法，其特征在于，在所述步骤(4)中，初始对准可通过采集陀螺x、y、z轴前100个角速率求得，即

f_x=mean(gyro(1，1：100))；

y，z轴类似。

4.根据权利要求1所述的一种基于等式约束卡尔曼滤波的单兵导航方法，在所述步骤(3)中，其特征在于，双MIMU系统会相互抑制其误差发散，单脚负载双MIMU系统，其相对位置固定，即相对距离的二范数为定值；在单兵运动过程中，经过零速校正的双MIMU分别绘制出两条轨迹，但是其相对距离却可能并不满足条件，因此可以将等式约束与卡尔曼滤波相结合，将双MIMU系统的位置状态变量利用投影法进行二次估计，即为双MIMU系统会相互抑制其误差发散。

5.根据权利要求1所述的一种基于等式约束卡尔曼滤波的单兵导航方法，在所述步骤(4)中，其特征在于，GPS信号微弱时定位误差较大，类似于双MIMU系统相互抑制误差发散，对于相对位置可以近似为固定的GPS天线和脚部负载MIMU也可以构造基于等式约束卡尔曼滤波，将含有较大误差的卫星信号与一个MIMU的位置状态变量用投影法进行估计，可对GPS位置、速度信息进行进一步估计。

6.根据权利要求1所述的一种基于等式约束卡尔曼滤波的单兵导航方法，在所述步骤(4)中，其特征在于，当GPS信号完全失锁时可利用惯导系统独立导航，即两个MIMU进行单等式约束卡尔曼滤波，同时惯导信息也可输出用于辅助GPS信号的再次捕获。