一种基于平滑变结构滤波的GNSS/SINS/磁力计的组合方法技术

本发明专利技术提供一种基于平滑变结构滤波、利用磁力计和加速度计提供的姿态角差观测量来增强系统模型的可观测性，同时将平滑变结构滤波运用其中以提高状态变量的鲁棒性的GNSS/SINS/磁力计的组合方法，包括步骤：步骤一，在SINS系统中挑选合适的状态变量。步骤二，建立组合导航系统的状态方程。步骤三，建立组合导航系统的量测方程。步骤四，构建系统模型的可观测性矩阵，对系统模型进行可观测性分析；根据分析结果，评价基于GNSS/SINS/磁力计的组合系统是否具备完全可观测性；若是，则进行平滑变结构滤波的应用；若否，则重复步骤一到四，重新挑选状态变量和相应的观测量，设计新的状态方程和量测方程，直至系统为完全可观系统；步骤五，平滑变结构滤波的应用。平滑变结构滤波的应用。平滑变结构滤波的应用。

【技术实现步骤摘要】
一种基于平滑变结构滤波的GNSS/SINS/磁力计的组合方法


[0001]本专利技术属于组合导航定位
，具体涉及一种基于平滑变结构滤波的GNSS/SINS/磁力计的组合方法。

技术介绍

[0002]GNSS/SINS组合导航是常用的导航手段之一：全球导航卫星系统(GNSS)具有误差不易发散，但卫星信号易受遮挡和干扰的特点；捷联惯导系统(SINS)不受外界干扰，自主性强，但误差容易积累。这两种导航系统结合取长补短，能够较好的完成导航任务。卡尔曼滤波是组合导航中常用的数据融合手段，但是该方法要求系统能够提供准确的状态方程和量测方程，且要求系统噪声和量测噪声为零均值协方差已知的高斯白噪声，否则难以满足要求。对于该问题。不少研究学者提出了扩展卡尔曼滤波(EKF)，无迹卡尔曼滤波(UKF)，粒子滤波(PF)等滤波方法。但或多或少存在一些不足，如将模型误差处理为系统噪声，滤波算法的鲁棒性不足等缺陷。
[0003]平滑变结构滤波(SVSF)最早在2007年提出。这种滤波基于滑模变结构控制，采用变结构增益将预测的状态变量纠正到系统真实状态附近，因此是一种纠正预测滤波。该滤波因其引进了变结构控制概念而具有较强的鲁棒性，对噪声和不确定性不敏感。但是该滤波也存在不足之处，它要求被估计的系统具有完全可观可控性。针对此不足，国内一些学者通过对组合导航系统模型的改进，使其完全可控可观。有学者设计了位置速度六维状态变量的组合导航模型；还有学者设计了一种降阶卡尔曼滤波组合对准方法。但是这些组合对准方法不包含完整的基本导航参数(位置、速度、姿态)，无法应用于组合导航定位中。

技术实现思路

[0004]本专利技术所要解决的技术问题在于克服现有技术缺陷，提供了一种基于平滑变结构滤波、利用磁力计和加速度计提供的姿态角差观测量来增强系统模型的可观测性，同时将平滑变结构滤波运用其中以提高状态变量的鲁棒性的GNSS/SINS/磁力计的组合方法。
[0005]为了解决上述技术问题，本专利技术提供的基于平滑变结构滤波的GNSS/SINS/磁力计的组合方法，包括步骤：
[0006]步骤一，在SINS系统中挑选合适的状态变量，包括惯导东北天向速度误差[δV
E δV
N δV
U
]、东北天向姿态角误差[φ
E φ
N φ
U
]、惯导位置误差[δL δλ δh]。
[0007]步骤二，建立组合导航系统的状态方程
[0008]步骤三，建立组合导航系统的量测方程Z＝HX+V。
[0009]步骤四，构建系统模型的可观测性矩阵，对系统模型进行可观测性分析；根据分析结果，评价基于GNSS/SINS/磁力计的组合系统是否具备完全可观测性；若是，则进行平滑变结构滤波的应用；若否，则重复步骤一到四，重新挑选状态变量和相应的观测量，设计新的状态方程和量测方程，直至系统为完全可观系统；
[0010]步骤五，平滑变结构滤波的应用。
[0011]作为改进，步骤二中的组合导航系统的状态方程具体公式为：
[0012][0013]X＝[δV
E δV
N δV
U φ
E φ
N φ
U δL δλ δh][0014][0015]其中，
[0016][0017][0018][0019][0020]上述公式中，F为状态转移矩阵，G为系统噪声分配矩阵，W为系统噪声向量；V
E
、V
N
、V
U
为惯导东北天向速度分量；ω
N
、ω
U
为北向和天向陀螺移角速率输出，f
E
、f
N
、f
U
为东北天向加速度计比力输出；L为地理纬度；h为地理高度；R
Mh
＝R
M
+h，其中R
M
为子午圈主曲率半径；R
Nh
＝R
N
+h，其中R
N
为卯酉圈主曲率半径；为卯酉圈主曲率半径；β为重力扁率，f为椭球形状扁率；μ为地心引力常数，R为地球平均半径，β2取值为3.08
×
10
‑6s
‑2；β3取值为8.08
×
10
‑9s
‑2；将g0常记为g
e
，g
e
为地球赤道重力加速度。
[0021]作为改进，将加速度计偏置和陀螺漂移ε从基本状态变量中删去，用等效白噪声取代，作为状态方程中的新增系统噪声，如下式：
[0022][0023][0024]状态方程中由系统噪声表达式推得G和W，如下式：
[0025][0026][0027]系统噪声中含有：陀螺零偏等效白噪声w
g
、加速度计偏置等效白噪声w
a
、陀螺仪测量白噪声加速度计测量白噪声
[0028]作为改进，步骤三的量测方程具体构建步骤为：
[0029]加速度计和磁力计分别沿着载体坐标系三轴方向安装，在构造观测量之前，首先应该知道由加速度计计算出的俯仰角θ、横滚角γ，由磁力计输出的航向角ψ。在载体非加速运动状态下并且载体坐标系和参考坐标系重合时，可以得到加速度计在北东地参考坐标系下的比力输出为
[0030]f
n
＝[0 0 g][0031]地磁场强度在北东地参考坐标系下的输出为
[0032][0033]当载体做非加速度运动时，加速度计在载体坐标系下的输出为
[0034][0035]地磁场强度在载体坐标系下的输出为
[0036][0037]其中f
n
为北东地坐标系下的加速度计比力输出，g为当地重力加速度，H
n
为北东地坐标系下的磁力计三轴磁分量，上面四式存在以下关系：
[0038][0039][0040]上式中，q0、q1、q2、q3为四元数，
[0041]由上面两个方程可以得出以下方程组
[0042][0043]对于上式，可得到q0、q1、q2、q3。
[0044]根据四元数和姿态角的转换关系，得到载体的俯仰角θ、横滚角γ和航向角ψ。
[0045][0046]陀螺仪输出的载体系相对于惯性系的角速率为与导航系相对于惯性系的角速率相减后得到载体系相对于导航系的角速率一次积分后的得到载体在导航系下的载体姿态角其中i表示惯性坐标系，b表示载体坐标系，n表示导航坐标系，下标g表示该姿态角为陀螺输出的。
[0047]对于已经求出的两组姿态角观测量对其进行求差组成姿态角差观测量：
[0048][0049]其次SINS编排输出的位置和速度观测量与GPS输出的位置和速度观测量的差值作为另一组观测量：
[0050][0051][0052]上式中，为惯导速度分量；为GPS的速度分量；L
sins
、λ
sins
、h
sins...

【技术保护点】

【技术特征摘要】
1.一种基于平滑变结构滤波的GNSS/SINS/磁力计组合方法，其特征在于，包括以下步骤：步骤一，在SINS系统中挑选状态变量，包括惯导速度误差[δV
E δV
N δV
U
]、姿态角误差[φ
E φ
N φ
U
]、惯导位置误差[δL δλ δh]；步骤二，建立组合导航系统的状态方程步骤三，建立组合导航系统的量测方程Z＝HX+V；步骤四，构建系统模型的可观测性矩阵，对系统模型进行可观测性分析；根据分析结果，评价基于GNSS/SINS/磁力计的组合系统是否具备完全可观测性；若是，则进行平滑变结构滤波的应用；若否，则重复步骤一到四，重新挑选状态变量和相应的观测量，设计新的状态方程和量测方程，直至系统为完全可观系统；步骤五，平滑变结构滤波的应用。2.根据权利要求1所述的基于平滑变结构滤波的GNSS/SINS/磁力计组合方法，其特征在于：所述步骤二的组合导航系统的状态方程中：其中，其中，其中，其中，其中，3.根据权利要求2所述的基于平滑变结构滤波的GNSS/SINS/磁力计组合方法，其特征在于：将加速度计偏置和陀螺漂移ε从基本状态变量中删去，用等效白噪声取代，作为状态
方程中的新增系统噪声，如下式：方程中的新增系统噪声，如下式：状态方程中可由系统噪声表达式推得G和W，如下式：状态方程中可由系统噪声表达式推得G和W，如下式：系统噪声中含有：陀螺零偏等效白噪声w
g
、加速度计偏置等效白噪声w
a
、陀螺仪测量白噪声加速度计测量白噪声4.根据权利要求1所述的基于平滑变结构滤波的GNSS/SINS/磁力计组合方法其特征在于，所述步骤三的量测方程的具体构建步骤为：加速度计和磁力计分别沿着载体坐标系三轴方向安装，在构造观测量之前，确定俯仰角θ、横滚角γ及航向角ψ；在载体非加速运动状态下并且载体坐标系和参考坐标系重合时，得到加速度计在北东地参考坐标系下的比力输出为f
n
＝[0 0 g]地磁场强度在北东地参考坐标系下的输出为当载体运动时，加速度计在载体坐标系下的输出为地磁场强度在载体坐标系下的输出为其中f
n
为北东地坐标系下的加速度计比力输出，g为当地重力加速度，H
n
为北东地坐标系下的磁力计三轴磁分量，上面四式存在以...

【专利技术属性】
技术研发人员：张秋昭，黄伟，孙澳，王方田，郑南山，李增科，
申请(专利权)人：中国矿业大学，
类型：发明
国别省市：