CN103900581A - 基于增广拉格朗日条件的mimu与gps组合行人导航法 - Google Patents

Abstract

本发明提供的是一种基于增广拉格朗日条件的MIMU与GPS组合行人导航法。行人导航系统配有一个射频信号接收机和两个MIMU，射频信号接收机固定在行人的肩膀上，两个MIMU分别固定在一只脚的脚尖和脚跟上，实时采集系统数据；分别对两个MIMU进行导航解算得到惯导行人位置、速度、姿态信息，同时利用射频信号进行定位，得到接收机行人位置信息；分别对两个MIMU进行零速校正；构造一个非线性等式约束方程和一个非线性不等式约束方程；将约束方程与卡尔曼滤波相结合，利用增广拉格朗日方程对系统的状态变量进行校正，摆脱微弱信号下射频信号定位精度不够的问题。在恶劣的封闭室内导航环境下，采用本发明可以充分增加微弱射频信号的可利用性。

Description

基于增广拉格朗日条件的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）约束的数学模型为

令 $L_1\stackrel{\mathrm{def}}{=}\left[\begin{array}{cccc}{I}_3& 0_{3\×12}& {-I}_3& 0_{3\×12}\end{array}\right]$

令 $L_2\stackrel{\mathrm{def}}{=}\left[\begin{array}{cccc}{I}_3& 0_{3\×12}& 0_3& 0_{3\×12}\end{array}\right]$

其中L1为行人脚的长度，L2为两个MIMU到射频信号接收机的位置、取行人身高，I_i，0_j×_k分别表示i×i阶的单位阵和j×k阶的零矩阵；

1）等式约束的数学模型为：

${\left|\right|L_{1}X\left|\right|}^2=d_1^2$

式中 $X={\left[\begin{array}{cc}{X}_1^T& X_2^T\end{array}\right]}^T$ 为滤波估计状态向量，其中 $X_1={\left[\begin{array}{ccccc}{P}_1^T& V_1^T& {\mathrm{\φ}}_1^T& {\mathrm{\Δa}}_1^T& {\mathrm{\Δ\ω}}_1^T\end{array}\right]}^T$ 为MIMU1的状态估计向量，

为MIMU2的状态估计向量，P_1,2V_1,2φ_1,2Δa_1,2Δω_1,2则分别MIMU1与MIMU2的位置、速度、姿态以及加速度计和陀螺误差，T表示转置；d₁为行人脚长；

2）不等式约束的数学模型为：

${\left|\right|L_{2}X-\mathrm{\γ}\left|\right|}^2\≤d_2^2$

式中γ=[xyz]^T为射频信号接收机定位得到的位置向量，d₂为行人肩膀到脚的高度；

（2）根据约束数学模型列写增广拉格朗日方程式

1）增广拉格朗日方程

$\stackrel{\‾}{L}(X,\mathrm{\μ};\mathrm{\ρ})=f\left(X\right)+\underset{i=1}{\overset{l}{\mathrm{\Σ}}}({\mathrm{\μ}}_i{g}_i\left(X\right)+\frac{\mathrm{\ρ}}{2}{g}_i{\left(X\right)}^2)+\frac{1}{2\mathrm{\ρ}}\underset{i=l+1}{\overset{m}{\mathrm{\Σ}}}\left(\max \right\{0,{\mathrm{\μ}}_i+{\mathrm{\ρg}}_i\left(X\right){\}}^2-{\mathrm{\μ}}_i^2)$

式中

为上一时刻状态向量，投影矩阵P为正定的对称阵,取为卡尔曼滤波的协方差矩阵；i=1,2,......,l时g_i(x)=0为等式约束条件，i=l,l+1,......,m时g_i(x)≤0为第i个不等式约束条件；μ_i为第i个约束条件的增广拉格朗日乘子；ρ为惩罚系数；

（3）构造增广拉格朗日方程对状态变量进行约束

采用增广拉格朗日法对状态量进行约束，具体方法为：

①先找到满足条件的X

$X=\min \stackrel{\‾}{L}(X,{\mathrm{\μ}}^k;\mathrm{\ρ});$

②确定等式约束的乘子值

${\mathrm{\μ}}_1^{k+1}=u_1^k+{\mathrm{\ρ}}_k{g}_1\left(X\right)$

③确定不等式约束的乘子值

${\mathrm{\μ}}_2^{k+1}=\max \{0,{\mathrm{\μ}}_2^k+{\mathrm{\ρ}}_k{g}_2\left(X\right)\}$

其中

表示i时刻的等式约束乘子，

表示i时刻的不等式约束乘子。

2、步骤3中所述对MIMU1和MIMU2进行零速校正的数学模型为，

$\left\{\begin{array}{c}{X}_{\mathrm{1,2}}={\mathrm{AX}}_{\mathrm{1,2}}+W\\ Z_{\mathrm{1,2}}={\mathrm{HX}}_{\mathrm{1,2}}+V\end{array}\right.$

这里X_1,2为MIMU1与MIMU2系统的状态变量，依次表示位置，速度姿态以及加速度计和陀螺误差，为15维；A为状态转移矩阵，W和V分别为系统与两侧噪声驱动阵；对于零速校正来说其观测量为所能观测的速度误差值，因而其观测方程如下：

$\begin{array}{c}{Z}_{3\×1}={\mathrm{HX}}_{\mathrm{1,2}}+V\\ ={\left[\begin{array}{ccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×9}\end{array}\right]}_{3\×15}{X}_{\mathrm{1,2}}+V\end{array}.$

本发明针对行人运动状态下的位置误差的抑制，提出了一种利用增广拉格朗日方程求解多约束条件下的最优解的方法。本发明方法依据行人的人体结构特征及人体运动特性，即单脚的长度和人体身高的最大值等构造约束方程。为了提高求解的精度，针对本发明中二阶非线性等式约束为非标准凸优化模型的问题，利用目标函数和约束条件构造增广拉格朗日方程，将存在误差的状态量投影到满足约束条件的范围内，最大限度实现系统之间的相互校正，提高导航精度。

在恶劣的封闭室内导航环境下，采用本发明可以充分增加微弱射频信号的可利用性，在一定程度上补偿微惯性器件的累积误差，并且在卫星信号失锁的紧急时刻也能利用微惯性系统独立辅助行人执行任务，因此大大的提高了组合导航系统的可靠性。

附图说明

图1是本发明的基于增广拉格朗日条件的行人导航方法的流程图；

图2是本发明的基于增广拉格朗日条件的行人导航方法中约束条件建立原理图；

图3是本发明的整体流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

步骤1：将MIMU1和MIMU2分别固定在行人一只脚的脚跟和脚尖上，将射频信号接收机固定在行人的肩膀上（与固定有双MIMU的脚同侧），实时的采集数据以进行信息融合。

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

步骤3：根据步骤2中惯导解算出的速度信息对MIMU1和MIMU2进行零速检测，并进行零速校正，下式为零速校正数学模型。

$\left\{\begin{array}{c}{X}_{\mathrm{1,2}}={\mathrm{AX}}_{\mathrm{1,2}}+W\\ Z_{\mathrm{1,2}}={\mathrm{HX}}_{\mathrm{1,2}}+V\end{array}\right.$

这里X_1,2为MIMU1与MIMU2系统的状态变量，依次表示位置，速度姿态以及加速度计和陀螺误差，为15维。A为状态转移矩阵，W和V分别为系统与两侧噪声驱动阵。对于零速校正来说其观测量为所能观测的速度误差值，因而其观测方程如下：

$\begin{array}{c}{Z}_{3\×1}={\mathrm{HX}}_{\mathrm{1,2}}+V\\ ={\left[\begin{array}{ccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×9}\end{array}\right]}_{3\×15}{X}_{\mathrm{1,2}}+V\end{array}$

步骤4：对于状态变量不满足条件的情况构造增广拉格朗日方程将其投影至解空间。

1.根据人体的结构特性，脚长为定值。可以构造两个MIMU的距离等于脚长的等式约束关系。根据人体运动特性，行人导航过程中人体肩膀到脚的高度不小于人体肩膀到脚的直线距离，如图2所示。可以构造人体肩膀到脚的高度不小于射频接收机到MIMU1的距离的不等式约束关系。

由上可看出约束关系，建立约束的数学模型。

令 $L_1\stackrel{\mathrm{def}}{=}\left[\begin{array}{cccc}{I}_3& 0_{3\×12}& {-I}_3& 0_{3\×12}\end{array}\right]$

令 $L_2\stackrel{\mathrm{def}}{=}\left[\begin{array}{cccc}{I}_3& 0_{3\×12}& 0_3& 0_{3\×12}\end{array}\right]$

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

（1）等式约束的数学模型为：

${\left|\right|L_{1}X\left|\right|}^2=d_1^2$

式中 $X={\left[\begin{array}{cc}{X}_1^T& X_2^T\end{array}\right]}^T$ 为滤波估计状态向量，其中 $X_1={\left[\begin{array}{ccccc}{P}_1^T& V_1^T& {\mathrm{\φ}}_1^T& {\mathrm{\Δa}}_1^T& {\mathrm{\Δ\ω}}_1^T\end{array}\right]}^T$ 为MIMU1的状态估计向量，

为MIMU2的状态估计向量，P_1,2V_1,2φ_1,2Δa_1,2Δω_1,2则分别MIMU1与MIMU2的位置、速度、姿态以及加速度计和陀螺误差，T表示转置；d₁为行人脚长。

（2）不等式约束的数学模型为：

${\left|\right|L_{2}X-\mathrm{\γ}\left|\right|}^2\≤d_2^2$

式中γ=[xyz]^T为射频信号接收机定位得到的位置向量，d₂为行人肩膀到脚的高度。

2.根据约束数学模型列写增广拉格朗日方程式。

（1）增广拉格朗日方程定义为：

$\stackrel{\‾}{L}(X,\mathrm{\μ};\mathrm{\ρ})=f\left(X\right)+\underset{i=1}{\overset{l}{\mathrm{\Σ}}}({\mathrm{\μ}}_i{g}_i\left(X\right)+\frac{\mathrm{\ρ}}{2}{g}_i{\left(X\right)}^2)+\frac{1}{2\mathrm{\ρ}}\underset{i=l+1}{\overset{m}{\mathrm{\Σ}}}\left(\max \right\{0,{\mathrm{\μ}}_i+{\mathrm{\ρg}}_i\left(X\right){\}}^2-{\mathrm{\μ}}_i^2)$

式中

为上一时刻状态向量，投影矩阵P为正定的对称阵,通常可取为卡尔曼滤波的协方差矩阵；i=1,2,......,l时g_i(x)=0为等式约束条件，i=l,l+1,......,m时g_i(x)≤0为第i个不等式约束条件；μ_i为第i个约束条件的增广拉格朗日乘子；ρ为惩罚系数。

将上式综合化简可得增广拉格朗日函数为：

$\left\{\begin{array}{c}\stackrel{\‾}{L}(X,\mathrm{\μ};\mathrm{\ρ})=f\left(X\right)+({\mathrm{\μ}}_1{g}_1\left(X\right)+\frac{\mathrm{\ρ}}{2}{g}_1{\left(X\right)}^2)+\frac{1}{2\mathrm{\ρ}}(\max {\{0,{\mathrm{\μ}}_2+{\mathrm{\ρg}}_2\left(X\right)\}}^2-{\mathrm{\μ}}_2^2)\\ g_1\left(X\right)={\left|\right|L_{1}X\left|\right|}^2-d_1^2\\ g_2\left(x\right)\≤{\left|\right|L_{2}X-\mathrm{\γ}\left|\right|}^2-d_2^2\\ f\left(x\right)=\mathrm{atg}\min {(X-\hat{X})}^{T}P(X-\hat{X})\end{array}\right.$

式中g₁(x)为等式约束条件；g₂(x)为不等式约束条件。

3.利用增广拉格朗日方程对状态进行校正。

当状态变量不能满足约束条件时，需要利用增广拉格朗日方程对状态进行约束。关于约束存在条件下求极值是典型的凸优化问题，但是由于本发明中的等式约束具有二阶形式，并不满足凸优化标准型。利用增广拉格朗日方程求解也是约束求极值的一种方法，对比凸优化问题其对约束方程的限制较松，需满足二阶可导，即可解。具体解算方法如下：

①先找到满足条件的X。

$X=\min \stackrel{\‾}{L}(X,{\mathrm{\μ}}^k;\mathrm{\ρ})$

②确定等式约束的乘子值

${\mathrm{\μ}}_1^{k+1}=u_1^k+{\mathrm{\ρ}}_k{g}_1\left(X\right)$

③确定不等式约束的乘子值

${\mathrm{\μ}}_2^{k+1}=\max \{0,{\mathrm{\μ}}_2^k+{\mathrm{\ρ}}_k{g}_2\left(X\right)\}$

以上求解过程涉及到乘子的迭代过程，其中

表示i时刻的等式约束乘子，

表示i时刻的不等式约束乘子。

Claims (3)

1.一种基于增广拉格朗日条件的MIMU与GPS组合行人导航法，其特征是：

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

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

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

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

2.根据权利要求1所述的基于增广拉格朗日条件的MIMU与GPS组合行人导航法，其特征是步骤4中对于状态变量不满足约束的情况构造增广拉格朗日方程将其投影至解空间，具体包括：

（1）约束的数学模型为

令 $L_1\stackrel{\mathrm{def}}{=}\left[\begin{array}{cccc}{I}_3& 0_{3\×12}& {-I}_3& 0_{3\×12}\end{array}\right]$

令 $L_2\stackrel{\mathrm{def}}{=}\left[\begin{array}{cccc}{I}_3& 0_{3\×12}& 0_3& 0_{3\×12}\end{array}\right]$

其中L₁为行人脚的长度，L₂为两个MIMU到射频信号接收机的位置、取行人身高，I_i，0_j×k分别表示i×i阶的单位阵和j×k阶的零矩阵；

1）等式约束的数学模型为：

${\left|\right|L_{1}X\left|\right|}^2=d_1^2$

式中 $X={\left[\begin{array}{cc}{X}_1^T& X_2^T\end{array}\right]}^T$ 为滤波估计状态向量，其中 $X_1={\left[\begin{array}{ccccc}{P}_1^T& V_1^T& {\mathrm{\φ}}_1^T& {\mathrm{\Δa}}_1^T& {\mathrm{\Δ\ω}}_1^T\end{array}\right]}^T$ 为MIMU1的状态估计向量，

为MIMU2的状态估计向量，P_1,2V_1,2φ_1,2Δa_1,2Δω_1,2则分别MIMU1与MIMU2的位置、速度、姿态以及加速度计和陀螺误差，T表示转置；d₁为行人脚长；

2）不等式约束的数学模型为：

${\left|\right|L_{2}X-\mathrm{\γ}\left|\right|}^2\≤d_2^2$

式中γ=[xyz]^T为射频信号接收机定位得到的位置向量，d₂为行人肩膀到脚的高度；

（2）根据约束数学模型列写增广拉格朗日方程式

1）增广拉格朗日方程

$\stackrel{\‾}{L}(X,\mathrm{\μ};\mathrm{\ρ})=f\left(X\right)+\underset{i=1}{\overset{l}{\mathrm{\Σ}}}({\mathrm{\μ}}_i{g}_i\left(X\right)+\frac{\mathrm{\ρ}}{2}{g}_i{\left(X\right)}^2)+\frac{1}{2\mathrm{\ρ}}\underset{i=l+1}{\overset{m}{\mathrm{\Σ}}}\left(\max \right\{0,{\mathrm{\μ}}_i+{\mathrm{\ρg}}_i\left(X\right){\}}^2-{\mathrm{\μ}}_i^2)$

式中 为上一时刻状态向量，投影矩阵P为正定的对称阵,取为卡尔曼滤波的协方差矩阵；i=1,2,......,l时g_i(x)=0为等式约束条件，i=l,l+1,......,m时g_i(x)≤0为第i个不等式约束条件；μ_i为第i个约束条件的增广拉格朗日乘子；ρ为惩罚系数；

（3）构造增广拉格朗日方程对状态变量进行约束

采用增广拉格朗日法对状态量进行约束，具体方法为：

①先找到满足条件的X

$X=\min \stackrel{\‾}{L}(X,{\mathrm{\μ}}^k;\mathrm{\ρ});$

②确定等式约束的乘子值

${\mathrm{\μ}}_1^{k+1}=u_1^k+{\mathrm{\ρ}}_k{g}_1\left(X\right)$

③确定不等式约束的乘子值

${\mathrm{\μ}}_2^{k+1}=\max \{0,{\mathrm{\μ}}_2^k+{\mathrm{\ρ}}_k{g}_2\left(X\right)\}$

其中

表示i时刻的等式约束乘子，

表示i时刻的不等式约束乘子。

3.根据权利要求1或2所述的基于增广拉格朗日条件的MIMU与GPS组合行人导航法，其特征是步骤3中所述对MIMU1和MIMU2进行零速校正的数学模型为，

$\left\{\begin{array}{c}{X}_{\mathrm{1,2}}={\mathrm{AX}}_{\mathrm{1,2}}+W\\ Z_{\mathrm{1,2}}={\mathrm{HX}}_{\mathrm{1,2}}+V\end{array}\right.$

这里X_1,2为MIMU1与MIMU2系统的状态变量，依次表示位置，速度姿态以及加速度计和陀螺误差，为15维；A为状态转移矩阵，W和V分别为系统与两侧噪声驱动阵；对于零速校正来说其观测量为所能观测的速度误差值，因而其观测方程如下：

$\begin{array}{c}{Z}_{3\×1}={\mathrm{HX}}_{\mathrm{1,2}}+V\\ ={\left[\begin{array}{ccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×9}\end{array}\right]}_{3\×15}{X}_{\mathrm{1,2}}+V\end{array}.$

Images