[发明专利]基于增广拉格朗日条件的MIMU与GPS组合行人导航法

说明书

技术领域

本发明涉及的是一种行人导航系统信息融合方法，尤其涉及一种基于增广拉格朗日条件的信息融合方法。

背景技术

基于微惯性测量单元MIMU(Micro Inertial Measurement Unit)惯性解算的行人导航自主定位装置工作时，微惯性器件工作时误差发散严重，惯性解算算法得到的行人航位推算结果验证了在导航阶段若微惯性器件误差不能得到有效补偿，位置误差会以时间三次方的趋势发散，系统最终将丧失导航功能。因此，将微惯性系统应用于行人导航定位的最大难点在于有效的修正误差。

从目前来看，为减小微惯性系统的导航误差，市场上绝大数具有行人导航定位功能的产品主要依靠GPS或地图匹配MM（Map Matching）来辅助惯导系统进行定位，除此之外磁力计、气压计、UWB无线设备、图像传感器、多普勒雷达、以及超声波传感器均可作为组合系统的选择。然而，它们都存在着动态性能差、非自主等缺点，易受客观因素影响，在处于复杂的周边环境会产生信号丢失，提供定位信息功能失效等问题。

在滤波方面，目前在多数行人导航定位功能的产品中所应用的滤波对约束条件的处理方法不够理想，使得信息融合存在一定的误差，导航结果精度不高。

总体来说现有的室内行人组合导航系统稳定性差，滤波效果不够理想，难以满足行人导航精确可靠的要求。

发明内容

本发明的目的在于提供一种能最大限度实现系统之间的相互校正，导航精度高的基于增广拉格朗日条件的MIMU与GPS组合行人导航法。

本发明的目的是这样实现的：

步骤1：将MIMU1和MIMU2分别固定在行人一只脚的脚跟和脚尖上，将射频信号接收机固定在行人的肩膀上且与固定有两个MIMU的脚同侧，实时采集两个MIMU和射频信号接收机的数据；

步骤2：对MIMU1和MIMU2采集到的数据进行惯导解算，得到惯导行人行进时的速度、位置以及姿态，同时利用射频信号接收机测到的射频信号进行定位，得到接收机位置；

步骤3：根据步骤2中惯导解算出的速度信息对MIMU1和MIMU2进行零速检测，并对MIMU1和MIMU2进行零速校正；

步骤4：根据微惯性系统以及射频信号的运算结果进行约束条件检验，对于检验结果合格的点则进入下一循环；若不满足约束条件则利用增广拉格朗日方程将不满足条件的状态变量投影，使其投射到约束解空间，重新投影后则所有状态变量满足约束方程，重新进入下一滤波循环中。

本发明还可以包括：

1、步骤4中对于状态变量不满足约束的情况构造增广拉格朗日方程将其投影至解空间，具体包括：

（1）约束的数学模型为

令L1=defI303×12-I303×12]]>