一种基于等式约束卡尔曼滤波的单兵导航方法
【专利摘要】本发明涉及一种基于等式约束卡尔曼滤波的单兵导航方法，包括：单兵导航系统配有一个小型GPS接收机和两个MIMU传感器。GPS与双MIMU系统在人体安置的位置选择以便最大限度的克服GPS动态性能不良以及MEMS随时间导航精度急剧降低的缺点。利用各导航器件之间的位置关系，构造两个非线性等式约束方程。将约束方程与卡尔曼滤波相结合，摆脱微弱信号下GPS定位精度不够的问题。在恶劣的封闭室内导航环境下，采用本发明可以充分增加微弱GPS信号的可利用性，在一定程度上补偿MEMS的累积误差，并且在卫星信号失锁的紧急时刻也能利用MEMS独立辅助单兵执行任务，因此大大的提高了组合导航系统的可靠性。
【专利说明】一种基于等式约束卡尔曼滤波的单兵导航方法
【技术领域】:
[0001]本发明涉及的是一种导航方法，特别是涉及一种基于等式约束卡尔曼滤波的单兵导航方法。
【背景技术】:
[0002]对于单兵导航来说，一个既可靠又准确且能够实现导航模式由室外到室内无缝切换的定位系统是必不可少的。由于综合导航环境的限制，需要的传感器件必须具有轻便、简小、价格低的优点，但须至少提供米级以上的精度。因此小型GPS接收机与惯导器件必不可少，除此之外磁力计、气压计、UWB无线设备、图像传感器、多普勒雷达、以及超声波传感器均可作为多传感器融合系统的选择。
[0003]多种传感器信息可供选择，本发明选取其中最基本的两项，即惯导与卫导结合。将多传感器信息进行高精度融合成为一个亟待解决的问题。因此如何改进滤波方法，使得这两种导航系统的精度更高成为研究重点。本发明基于此目的利用单兵的人体特征，如单脚的长度、身高的最大值等构造约束方程，抑制获取优于直接卡尔曼滤波的结果。
【发明内容】
:
[0004]本发明的目的在于提供一种基于等式约束卡尔曼滤波的单兵导航方法，以解决现有技术中MEMS传感器在导航过程中误差积累较大，造成随时间导航精度急剧降低以及在室内GPS信号过于微弱造成输出结果可信度不高以及信号失锁后由于动态性能差造成的搜星困难等问题。
[0005]为了解决【背景技术】所存在的问题，本发明采用以下技术方案:
[0006]一种基于等式约束卡尔曼滤波的单兵导航方法，它包括如下步骤:
[0007](I)将GPS安置在单兵头部，两个MIMU导航器件分别放置在单兵的单脚脚尖与脚后跟；
[0008](2)测量三个导航子系统所构成的空间三角形边长；
[0009](3)根据三边关系，构造两个非线性等式约束方程，其中第一个等式约束即为双MU之间的相对距离固定；
[0010](4)运用基于等式约束的卡尔曼滤波方法，对各导航子系统分别校正与解算。
[0011]在所述步骤(4)中，当单兵行走时，脚部接触地面的瞬间，传感器对地绝对速度为零，则可以将对地绝对零速设为观测量，进行滤波估计，实现对陀螺加速度计零位误差的反馈矫正，即触发两个MIMU的零速校正。
[0012]在所述步骤(4)中，初始对准可通过采集陀螺x、y、ζ轴前100个角速率求得，SP
[0013]fx=mean (gyro (1,1:100))；
[0014]y，ζ 轴类似。
[0015]在所述步骤(3)中，，双MMU系统会相互抑制其误差发散，脚负载双MMU系统，其相对位置固定，即相对距离的二范数为定值。在单兵运动过程中，经过零速校正的双MMU分别绘制出两条轨迹，但是其相对距离却可能并不满足条件，因此可以将等式约束与卡尔曼滤波相结合，将双MMU系统的位置状态变量利用投影法进行二次估计，即为双MIMU系统会相互抑制其误差发散。
[0016]在所述步骤(4)中，，GPS信号微弱时定位误差较大，类似于双MIMU系统相互抑制误差发散，对于相对位置可以近似为固定的GPS天线和脚部负载MMU也可以构造基于等式约束卡尔曼滤波，将含有较大误差的卫星信号与一个MIMU的位置状态变量用投影法进行估计，可对GPS位置、速度信息进行进一步估计。
[0017]在所述步骤(4)中，，当GPS信号完全失锁时可利用惯导系统独立导航，即两个MIMU进行单等式约束卡尔曼滤波，同时惯导信息也可输出用于辅助GPS信号的再次捕获。
[0018]本发明对比现有技术有如下的有益效果:利用本发明中的基于等式约束卡尔曼滤波进行估计的过程中，既可对零速校正后的MEMS器件的位置进行约束，减小其误差漂移，同时又可以对输出结果稳定但是误差较大的GPS进行校正，并且能够保证实现由信号微弱至信号失锁的无缝切换。
【专利附图】
【附图说明】:
[0019]图1为本发明中的单兵导航示意图。
[0020]图2示为本发明中采用的XSENS公司生产的MT1-G型IMU0
[0021]图3为MEMS在单兵脚上的实例图，单脚负载双MMU示意图
[0022]图4为双MIMU的位置约束关系。
[0023]图5为单兵导航的坐标系与轨迹。
[0024]图6为惯导辅助卫星导航的原理图。
[0025]图7为单MIMU进行单兵导航解算定位图。
[0026]图8为采用基于等式约束卡尔曼滤波的单兵导航解算定位图。
[0027]图9为本发明方法流程图。
【具体实施方式】:
[0028]下面结合附图和【具体实施方式】对本发明作进一步描述:
[0029]针对单兵导航精度高、可靠性好等要求，本发明提出一种基于等式约束卡尔曼滤波的单兵导航方法。具体结构装置为将GPS安置在单兵头部，两个MIMU导航器件分别放置在单兵的单脚脚尖与脚后跟，如图1为本发明提出的单兵导航示意图。单兵导航进行时，与传统的导航要得到精确的经度与纬度不同。封闭的室内导航空间有限，若单兵移动100米左右，那么对应的经度、纬度的变化几乎为零，因此图5表示的导航坐标系可以选取为室内平面的延墙面的基线分别为X、1、z轴，根据单兵导航需求设置初始位置为原点0(0，0，0)。图2为本发明所采用XSENS公司生产的的MIMU传感器示意图，设绕Z轴逆时针旋转为航向正方向。图3为单脚双MMU示意图，双MMU均固连在载体上，在运动过程中MMUl的三轴陀螺输出三个方向的角速率，三轴加速度计输出沿载体系I的线加速度，MMU2同理。图4为单脚双MIMU构成的等式约束示意图，通过图中可以直接看出两个IMU之间的位置约束关系，在卡尔曼滤波过程中构造等式约束方程。图6为惯导辅助卫星导航的原理图，在卫星信号失锁的时候，单兵导航系统只有依靠自主式的惯性单元完成导航任务，MMUl与MMU2在基于等式约束的卡尔曼滤波估计后分别可以得到两个位置，由于其与GPS天线的距离均为定值，则以两个微惯导IMU组件为圆心作圆的交点即为GPS预测位置。根据GPS天线预测位置可以辅助卫导快速搜星，大大提高室内单兵导航的可靠性与可利用性。
[0030]首先，分析等式约束与卡尔曼结合的滤波过程，具体分析如下:
[0031]步骤一:双MIMU与GPS构造联合状态向量
[0032]如上图所示,两个IMU与GPS卫星导航系统相互独立,令
【权利要求】
1.一种基于等式约束卡尔曼滤波的单兵导航方法，其特征在于，包括以下步骤: (1)将GPS安置在单兵头部，两个MIMU导航器件分别放置在单兵的单脚脚尖与脚后跟； (2)测量三个导航子系统所构成的空间三角形边长； (3)根据三边关系，构造两个非线性等式约束方程，其中第一个等式约束即为双IMU之间的相对距离固定； (4)运用基于等式约束的卡尔曼滤波方法，对各导航子系统分别校正与解算。
2.根据权利要求1所述的一种基于等式约束卡尔曼滤波的单兵导航方法，其特征在于，在所述步骤(4)中，当单兵行走时，脚部接触地面的瞬间，传感器对地绝对速度为零，则可以将对地绝对零速设为观测量，进行滤波估计，实现对陀螺加速度计零位误差的反馈矫正，即触发两个MIMU的零速校正。
3.根据权利要求1所述的一种基于等式约束卡尔曼滤波的单兵导航方法，其特征在于，在所述步骤(4)中，初始对准可通过采集陀螺x、y、z轴前100个角速率求得，SP
fx=mean(gyro(1,1: 100))； y，z轴类似。
4.根据权利要求1所述的一种基于等式约束卡尔曼滤波的单兵导航方法，在所述步骤(3)中，其特征在于，双MIMU系统会相互抑制其误差发散，单脚负载双MIMU系统，其相对位置固定，即相对距离的二范数为定值；在单兵运动过程中，经过零速校正的双MMU分别绘制出两条轨迹，但是其相对距离却可能并不满足条件，因此可以将等式约束与卡尔曼滤波相结合，将双MMU系统的位置状态变量利用投影法进行二次估计，即为双MMU系统会相互抑制其误差发散。
5.根据权利要求1所述的一种基于等式约束卡尔曼滤波的单兵导航方法，在所述步骤(4)中,其特征在于,GPS信号微弱时定位误差较大,类似于双MIMU系统相互抑制误差发散，对于相对位置可以近似为固定的GPS天线和脚部负载MMU也可以构造基于等式约束卡尔曼滤波，将含有较大误差的卫星信号与一个MIMU的位置状态变量用投影法进行估计，可对GPS位置、速度信息进行进一步估计。
6.根据权利要求 1所述的一种基于等式约束卡尔曼滤波的单兵导航方法，在所述步骤(4)中，其特征在于，当GPS信号完全失锁时可利用惯导系统独立导航，即两个MIMU进行单等式约束卡尔曼滤波，同时惯导信息也可输出用于辅助GPS信号的再次捕获。
【文档编号】G01S19/49GK103744099SQ201310581902
【公开日】2014年4月23日 申请日期:2013年11月20日 优先权日:2013年11月20日
【发明者】高伟, 李佳璇, 周广涛, 于春阳, 兰海钰, 林萌萌, 刘晓旭, 白红美, 梁宏, 孙梦 申请人:哈尔滨工程大学