CN103744099A - 一种基于等式约束卡尔曼滤波的单兵导航方法 - Google Patents

Abstract

本发明涉及一种基于等式约束卡尔曼滤波的单兵导航方法，包括：单兵导航系统配有一个小型GPS接收机和两个MIMU传感器。GPS与双MIMU系统在人体安置的位置选择以便最大限度的克服GPS动态性能不良以及MEMS随时间导航精度急剧降低的缺点。利用各导航器件之间的位置关系，构造两个非线性等式约束方程。将约束方程与卡尔曼滤波相结合，摆脱微弱信号下GPS定位精度不够的问题。在恶劣的封闭室内导航环境下，采用本发明可以充分增加微弱GPS信号的可利用性，在一定程度上补偿MEMS的累积误差，并且在卫星信号失锁的紧急时刻也能利用MEMS独立辅助单兵执行任务，因此大大的提高了组合导航系统的可靠性。

Description

一种基于等式约束卡尔曼滤波的单兵导航方法

技术领域：

本发明涉及的是一种导航方法，特别是涉及一种基于等式约束卡尔曼滤波的单兵导航方法。 

背景技术：

对于单兵导航来说，一个既可靠又准确且能够实现导航模式由室外到室内无缝切换的定位系统是必不可少的。由于综合导航环境的限制，需要的传感器件必须具有轻便、简小、价格低的优点，但须至少提供米级以上的精度。因此小型GPS接收机与惯导器件必不可少，除此之外磁力计、气压计、UWB无线设备、图像传感器、多普勒雷达、以及超声波传感器均可作为多传感器融合系统的选择。 

多种传感器信息可供选择，本发明选取其中最基本的两项，即惯导与卫导结合。将多传感器信息进行高精度融合成为一个亟待解决的问题。因此如何改进滤波方法，使得这两种导航系统的精度更高成为研究重点。本发明基于此目的利用单兵的人体特征，如单脚的长度、身高的最大值等构造约束方程，抑制获取优于直接卡尔曼滤波的结果。 

发明内容：

本发明的目的在于提供一种基于等式约束卡尔曼滤波的单兵导 航方法，以解决现有技术中MEMS传感器在导航过程中误差积累较大，造成随时间导航精度急剧降低以及在室内GPS信号过于微弱造成输出结果可信度不高以及信号失锁后由于动态性能差造成的搜星困难等问题。 

为了解决背景技术所存在的问题，本发明采用以下技术方案： 

一种基于等式约束卡尔曼滤波的单兵导航方法，它包括如下步骤： 

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

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

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

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

在所述步骤(4)中，当单兵行走时，脚部接触地面的瞬间，传感器对地绝对速度为零，则可以将对地绝对零速设为观测量，进行滤波估计，实现对陀螺加速度计零位误差的反馈矫正，即触发两个MIMU的零速校正。 

在所述步骤(4)中，初始对准可通过采集陀螺x、y、z轴前100个角速率求得，即 

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

y，z轴类似。 

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

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

在所述步骤(4)中，，当GPS信号完全失锁时可利用惯导系统独立导航，即两个MIMU进行单等式约束卡尔曼滤波，同时惯导信息也可输出用于辅助GPS信号的再次捕获。 

本发明对比现有技术有如下的有益效果：利用本发明中的基于等式约束卡尔曼滤波进行估计的过程中，既可对零速校正后的MEMS器件的位置进行约束，减小其误差漂移，同时又可以对输出结果稳定但是误差较大的GPS进行校正，并且能够保证实现由信号微弱至信号失锁的无缝切换。 

附图说明：

图1为本发明中的单兵导航示意图。 

图2示为本发明中采用的XSENS公司生产的MTi-G型IMU。 

图3为MEMS在单兵脚上的实例图，单脚负载双MIMU示意图 

图4为双MIMU的位置约束关系。 

图5为单兵导航的坐标系与轨迹。 

图6为惯导辅助卫星导航的原理图。 

图7为单MIMU进行单兵导航解算定位图。 

图8为采用基于等式约束卡尔曼滤波的单兵导航解算定位图。 

图9为本发明方法流程图。 

具体实施方式：

下面结合附图和具体实施方式对本发明作进一步描述： 

针对单兵导航精度高、可靠性好等要求，本发明提出一种基于等式约束卡尔曼滤波的单兵导航方法。具体结构装置为将GPS安置在单兵头部，两个MIMU导航器件分别放置在单兵的单脚脚尖与脚后跟，如图1为本发明提出的单兵导航示意图。单兵导航进行时，与传统的导航要得到精确的经度与纬度不同。封闭的室内导航空间有限，若单兵移动100米左右，那么对应的经度、纬度的变化几乎为零，因此图5表示的导航坐标系可以选取为室内平面的延墙面的基线分别为x、y、z轴，根据单兵导航需求设置初始位置为原点O(0，0，0)。图2为本发明所采用XSENS公司生产的的MIMU传感器示意图，设绕Z轴逆时针旋转为航向正方向。图3为单脚双MIMU示意图，双MIMU均固连在载体上，在运动过程中MIMU1的三轴陀螺输出三个方向的 角速率，三轴加速度计输出沿载体系1的线加速度，MIMU2同理。图4为单脚双MIMU构成的等式约束示意图，通过图中可以直接看出两个IMU之间的位置约束关系，在卡尔曼滤波过程中构造等式约束方程。图6为惯导辅助卫星导航的原理图，在卫星信号失锁的时候，单兵导航系统只有依靠自主式的惯性单元完成导航任务，MIMU1与MIMU2在基于等式约束的卡尔曼滤波估计后分别可以得到两个位置，由于其与GPS天线的距离均为定值，则以两个微惯导IMU组件为圆心作圆的交点即为GPS预测位置。根据GPS天线预测位置可以辅助卫导快速搜星，大大提高室内单兵导航的可靠性与可利用性。 

首先，分析等式约束与卡尔曼结合的滤波过程，具体分析如下： 

步骤一：双MIMU与GPS构造联合状态向量 

如上图所示，两个IMU与GPS卫星导航系统相互独立，令 

作为第i个系统在n时刻的状态向量，令其的前三个元素代表第i个传感器的位置信息，因此可定义多维联合导航系统状态向量为： 

${\hat{x}}_k\stackrel{\mathrm{def}}{=}{\left[{\left(x_k^{\left(1\right)}\right)}^T{\left(x_k^{\left(2\right)}\right)}^T{\left(x_k^{\left(3\right)}\right)}^T\right]}^T---\left(1\right)$

的维数为c1+c2+c3。 

步骤二：建立等式约束数学模型 

令 $L_1\stackrel{\mathrm{def}}{=}[I_3{0}_{3\×(c1+c2-3)}-I_3{0}_{3\×(c3-3)}]---\left(2\right)$

令 $L_2\stackrel{\mathrm{def}}{=}[I_3{0}_{3\×(c1-3)}-I_3{0}_{3\×(c2+c3-3)}]---\left(3\right)$

其中I_i，0_j×k分别表示i×i阶的单位阵和j×k阶的零矩阵。 

令GPS与MEMS1之间的距离为γ₁，两个IMU系统之间存在一个 距离定值γ₂时，则非线性等式约束可以表示为： 

$\left\{\begin{array}{c}{\left|\right|L_1{x}_k\left|\right|}^2={{\mathrm{\γ}}_1}^2,\∀k\\ {\left|\right|L_2{x}_k\left|\right|}^2={{\mathrm{\γ}}_2}^2,\∀k\end{array}\right.---\left(4\right)$

令 ${\hat{x}}_k\stackrel{\mathrm{def}}{=}{\left[{\left(x_k^{\left(1\right)}\right)}^T{\left(x_k^{\left(2\right)}\right)}^T{\left(x_k^{\left(3\right)}\right)}^T\right]}^T$ 为滤波估计状态向量。若 

时，则需将导航滤波的解投影到子空间  $\{x\∈R^m:{\left|\right|{\mathrm{Lx}}_k\left|\right|}^2={\mathrm{\γ}}^2\}$ 上。 

步骤三：非线性等式约束卡尔曼滤波 

在解决物理空间中的实际问题时，很多物理过程的数学描述均为非线性。若对方程做线性化处理，则会导致在远离泰勒展开点处的误差较大，此时线性等式约束卡尔曼滤波器并不适用。因此若要获得较高精度，充分利用等式约束滤波误差，应用非线性方法处理滤波过程十分必要。 

常规离散型卡尔曼滤波数学模型： 

$\begin{array}{c}{x}_{k+1}={\mathrm{Ax}}_k+w_k\\ y_{k+1}={\mathrm{Cx}}_k+v_k\end{array}---\left(5\right)$

式中，k为时间指数，x为状态向量，w为系统激励噪声序列，v为量测噪声序列，A为t_k时刻至t_k+1时刻的一步转移阵，C是量测矩阵。 

离散型卡尔曼滤波基本方程： 

$\begin{array}{c}{\stackrel{\‾}{x}}_{k+1}={A\hat{x}}_k\\ {\stackrel{\‾}{p}}_{k+1}={\mathrm{Ap}}_k{A}^T+Q\\ {\hat{x}}_{k+1}={\stackrel{\‾}{x}}_{k+1}+K_{k+1}(y_{k+1}-{C\stackrel{\‾}{x}}_{k+1})\\ P_{k+1}=(I-K_{k+1}C){\stackrel{\‾}{p}}_{k+1}\\ K_{k+1}={\stackrel{\‾}{p}}_{k+1}{C}^T{({C\stackrel{\‾}{p}}_{k+1}{C}^T+R)}^{-1}\end{array}---\left(6\right)$

引入线性约束

就转化为求解条件极值问题， 利用拉格朗日乘数法将条件极值化为无条件极值。 

$z=f\left(x\right)=\arg \underset{x\∈R^3}{\min }{(x-\hat{x})}^{T}P(x-\hat{x})---\left(7\right)$

这里投影矩阵P为正定的对称阵，通常可取为卡尔曼滤波的协方差矩阵P_k，其拉格朗日函数基本模型为： 

基于上述基础，可以构造条件约束方程： 

这里

求解极值点的方法： 

此处为二阶非线性方程，(10)偏导求解非常困难，计算过程中根据个人需要可采用对约束等式

进行Ch0lesky分解或奇异值分解相结合进行一系列的矩阵简化，得出最终的求解λ的二阶等式方程

此时该方程的解很有可能为一个无穷收敛序列，若要求得准确的解值，计算量太大，因此可以用牛顿求解方法近似表示： 

${\mathrm{\λ}}_{k+1}={\mathrm{\λ}}_k-\frac{t\left({\mathrm{\λ}}_k\right)}{\stackrel{\·}{t}\left({\mathrm{\λ}}_k\right)}---\left(10\right)$

从λ₀=0开始，直到|λ_k+1-λ_k|<λ结束。(PS：λ的值视精度而定)。 

步骤四：误差建模 

步骤一详述了联合状态向量的建立过程。单兵作战过程中负载 不宜过重，因此惯导无法采用光纤陀螺、激光陀螺等较高精度的角速度传感器，所以对于单个MEMS惯性器件的建模会对系统的精度造成很大影响。 

单兵运动时会有慢走，慢跑，快跑以及静止等多种运动状态，出于对系统高精度要求的考量，会采用较为全面的MEMS惯导模型。 

联合状态向量为 ${\hat{x}}_k\stackrel{\mathrm{def}}{=}{\left[{\left(x_k^{\left(1\right)}\right)}^T{\left(x_k^{\left(2\right)}\right)}^T{\left(x_k^{\left(3\right)}\right)}^T\right]}^T,$

除了包含基本的位置、速度、姿态信息，还有零偏、刻度因子误差。 

步骤五：反馈过程 

本设计本身所采用的基于等式约束卡尔曼滤波的单兵导航方法 

图9是本发明的整个流程图，一种基于等式约束卡尔曼滤波的单兵导航方法，它包括如下步骤： 

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

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

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

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

在所述步骤(4)中，当单兵行走时，脚部接触地面的瞬间，传 感器对地绝对速度为零，则可以将对地绝对零速设为观测量，进行滤波估计，实现对陀螺加速度计零位误差的反馈矫正，即触发两个MIMU的零速校正。 

在所述步骤(4)中，初始对准可通过采集陀螺x、y、z轴前100个角速率求得，即 

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

y，z轴类似。 

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

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

在所述步骤(4)中，，当GPS信号完全失锁时可利用惯导系统独立导航，即两个MIMU进行单等式约束卡尔曼滤波，同时惯导信息也可输出用于辅助GPS信号的再次捕获。 

试验验证： 

采用XSEN公司生产的MTi-G(集成了MEMS加速度计、陀螺仪、以及小型GPS接收机)搭建真实单兵导航系统模型，如图1所示。惯性传感器规格参数如表1所示，GPS如表2所示。通过合理的对比试验验证本发明所采用的基于等式约束卡尔曼滤波的单兵导航方法在实际导航过程中的可靠性、实用性、准确性。试验场景选在室外空旷的哈尔滨工程大学军工操场。 

表1XSENS MIMU惯性器件组件规格指标 

表2XSENS GPS接收机规格指标 

实验开始前，实验人员需在场地进行3-5s的系统静止运动，用以完成系统姿态初始化，具体计算方法如下。 

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

f_y=mean(gyro(2，1：100))； 

f_z=mean(gyro(3，1：100))； 

roll=atan2(-f_y，-f_z)； 

$\mathrm{pitch}=a\tan 2(f_x,\mathrm{sqrt}(f_y^2+f_z^2));$

实验1： 

单兵导航多为室内进行紧急救援时使用，因此实验1所采用的环境也为室内，即大概率GPS信号失锁。测试者脚部携带双IMU沿着一个矩形行走。实验过程中实时采集并保存MIMU的输出数据。作为参考，地面上会画出一条参考轨迹，没有转弯。因此航向的标准值即为初始设置的0度。 

将实验数据进行离线分析，为了能够验证本发明的实验结果，先对IMU1进行零速校正绘制出粗轨迹。如图7所示为单纯零速校正单兵导航解算定位图。 

从图7可以看出，所绘制的曲线为一个闭合曲线，但是并不严格吻合矩形的形状，因此对于单IMU采用零速校正方案只能实现粗略的吻合实际运动轨迹。 

实验2： 

为了较全面分析本发明所涉及的单兵导航方法，实验2对应的环境为室外开放地区，则GPS信号优良，可以参与滤波。与实验1类似，在地方画出矩形参考轨迹，让实验人员沿着该轨迹运动。在双IMU的相对距离所构造的等式约束1基础上，加入GPS与IMU1之间的距离约束。由于在运动过程中人体肩部与脚之间的距离变化较小，因此可以近似看做定值。同样实验过程中实时采集并保存MIMU的输出数据，在GPS辅助下进行基于两个等式约束的卡尔曼滤波得出轨迹，如图8所示，对图8进行分析。 

如图8所示，采用基于等式约束卡尔曼滤波的单兵导航方法，在零速校正的基础上，首先对双MIMU系统构造等式约束方程，其次对GPS与IMU再进行等式约束与卡尔曼滤波的结合，充分利用卫星位置信息对精度较差的微惯性器件进行校正，能够很好的抑制误差的发散，如图8所示，此时绘制出的单兵位置轨迹已经十分吻合参考轨迹。因此在本发明中当GPS信号可利用时为单兵导航方法的精度最高状态，其次若卫星信号失锁不可用，则需要切换至纯惯导模式，利用双IMU进行数据融合，以得到优于单MEMS导航的位置、速度、姿态等信息。 

Claims (6)

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信号的再次捕获。

Images