CN110487275A - 一种基于最小上限滤波的gps/ins组合导航定位方法 - Google Patents

Abstract

本发明涉及一种基于最小上限滤波的GPS/INS组合导航定位方法，包括下述步骤：步骤1、导航信息获取阶段；步骤2、导航模型建立阶段；步骤3、导航误差判断阶段；步骤4、导航信息校正；步骤5、最优状态估计信息输出。本发明方法首先将GPS量测信号中失灵偏差和干扰部分建模为传感器量测中未知输入项，并在此基础上设计了一种基于最小上限滤波的GPS/INS组合导航定位方法。该方法可以在GPS信号短时受扰或失灵时，有效抑制定位误差而实现精确自身定位与导航。具体为在组合导航滤波算法中的位置估计方差中引入了自由参数，并构造位置估计方差上限表达式，通过实时凸优化动态寻优得到其估计方差优化值，输出精确的导航信息，进而保证了组合导航系统的稳定运行。

Description

一种基于最小上限滤波的GPS/INS组合导航定位方法

技术领域

本发明属于无人机组合导航技术领域，涉及一种基于最小上限滤波的GPS/INS组合导航定位方法。

背景技术

无人机定位的方法多种多样，其中较为常见的方法是基于卡尔曼滤波的GPS/INS组合导航。该种方法通过卡尔曼滤波器，将GPS和INS的数据进行融合，最终得到最优估计的位置信息实现无人机的精确定位。由于该方法既有INS的特点－—高动态范围，又有GPS的特点——位置信息不随时间发散，即实现了GPS定位和INS定位的优势互补，因此基于卡尔曼滤波的组合导航定位方法在无人机定位领域中应用广泛。

前基于卡尔曼滤波的组合导航能够稳定运行的前提是GPS信号不受干扰。但是，在某些特定场合，譬如建筑物密集或植被茂密等信号遮挡区、室内等信号阻隔区、甚至极端战场信号欺骗干扰区，GPS信号会不可避免地受到扰动出现时断时续甚至短时完全中断现象，此时导航系统因只能单纯依靠惯性导航，受到惯导输出漂移持续积累出现定位精度下降甚至输出无法定位现象，进而导致基于卡尔曼滤波的组合导航系统失效。

经查阅，针对上述问题，专利公开号CN107390246A，涉及一种基于遗传神经网络的GPS/INS组合导航方法，该方法是在基于卡尔曼滤波的GPS/INS组合导航的基础上，在GPS信号失锁时，利用训练好的神经网络模型来预测INS的输出误差并随之进行误差补偿和修正，进而输出精确的位置信息。对于专利公开号CN109521454A，涉及一种基于自学习的容积卡尔曼滤波的GPS/INS组合导航方法，该方法利用两个循环滤波子系统构成的自学习卡尔曼滤波器，在GPS信号失锁时进行误差补偿，最终输出位置信息。

以上两种方法设计思想是通过遗传算法或者自学习卡尔曼滤波器，对INS数据进行校正，最后输出位置信息。存在的问题是在组合导航过程中，需要先经过数据积累和学习，进而训练出系统模型参数，一旦数据积累不足将影响参数训练精度进而引发系统建模偏差，导致算法失效。本发明提出一种基于最小上限滤波器的组合导航定位方法，引入自由参数构造滤波残差方差上限，不需要数据训练而通过凸优化得到参数最优值，实现GPS信号中断或受扰时导航系统实时高精度定位。

发明内容

本发明针对GPS信号受到扰动或遮挡出现时断时续甚至短时中断时GPS/INS组合导航定位精度下降甚至系统失效问题，提出了一种基于最小上限滤波的GPS/INS组合导航定位方法。

本发明包括如下步骤：

一种基于最小上限滤波的GPS/INS组合导航定位方法，包括下述步骤：

步骤1、导航信息获取阶段：

由安装在无人机上的加速度计实时测得的无人机的加速度，通过导航解算计算机，将测得的加速度信息进行积分解算，得到无人机的经度、纬度、高度位置信息；安装在无人机上的GPS接收机通过卫星，得到无人机的经度、纬度、高度位置信息；

步骤2、导航模型建立阶段：

首先，将GPS解算位置信息与INS量测信息之差作为系统状态，则定位系统的状态向量写为x＝[δL δλ δh]，其中δL.δλ.δh分别为INS与GPS的纬度、经度、高度之差，即纬度误差、经度误差和高度误差；

系统量测方程为

z_k+1＝H_k+1x_k+1|k+A_k+1δ_k+1+v_k+1 (1)

在式(1)中，x_k+1为状态向量，z_k+1为量测向量；H_k+1和A_k+1分别为量测矩阵和扰动矩阵；v_k+1为量测噪声；δ_k+1为GPS受到的广义未知扰动；

系统状态方程为

x_k+1＝F_kx_k+w_k (2)

在式(2)中，F_k为状态转移矩阵，w_k为过程噪声。状态噪声和量测噪声均为高斯白噪声；

步骤3、导航误差判断阶段：

对比上一时刻一步预测估计和当前时刻最优估计，当其差值大于阈值时，则判定当前GPS信号发生异常，即信号发生跳变或失灵，此时进入步骤4，否则采用经典基于卡尔曼的组合导航算法对信号进行融合处理；

步骤4、导航信息校正：

当GPS信号发生跳变或失灵时，启动最小上限滤波器得出的状态误差最优估计，在建立系统偏差模型基础上利用反馈校正，修正由INS解算所造成的累计误差，输出导航信息；

步骤5、最优状态估计信息输出。

进一步的，步骤4中，具体的修正步骤如下：

步骤401：由当前时刻状态估计和协方差估计P^* _k|k，计算状态预测和量测预测其中

步骤402：由下一时刻的量测值z_k+1，计算滤波残差γ_k+1，其中滤波残差γ_k+1中存在GPS受到的广义未知扰动δ_k+1；

步骤403：计算新息协方差其中

步骤404：由式P^* _k+1|k＝F_kP^* _k|kF_k ^T+Γ_kQ_kΓ_k ^T计算状态预测协方差P^* _k+1|k，其中Γ_k为状态噪声系数矩阵，Q_k为状态噪声协方差矩阵；

步骤405：凸优化环节

对于最优参数：

这里，定义Ω_k+1＝{τ_k+1|τ_k+1≥0，S^* _k+1|τ_k+1≥S_k+1}

服从如下不等式约束：

在式(4)中，S_k+1是未知量，因此在实际的参数优化中，用它的无偏估计来代替；

步骤406：凸优化判定环节：

通过一个判定环节即对比上一时刻一步预测估计和当前时刻最优估计，当满足不等式关系：其中a是人为设定的值，，重复进行步骤405，直至不满足不等式关系时，调出循环，结束凸优化，得到最优参数τ_k+1；

步骤407：数据优化阶段

在得到最优参数τ_k+1后，可以得出基于MUBF的组合导航中的核心优化公式和步骤：

式中，∑是人为设定的已知量，τ_k+1是经过凸优化后的参数，是由τ_k+1调节出来的最优增益，最终由来保证最优估计，即最优偏差量。通过式(5)最终得到当前时刻的最优的状态误差估计。

步骤408：位置校正阶段

当得到最优的状态误差估计后再通过反馈校正将误差消除，得到校正后的位置信息p_k，INS，其中这里设系统位置向量为p＝[Lλ h]，k时刻修正后的位置向量为p_k，INS，k时刻修正前的位置向量为在k时候最优的状态误差估计为

步骤409：输出最优导航位置估计

最后，将校正后的位置信息p_k，INS作为最终的无人机的定位信息，输出最优状态估计。

与现有技术相比，本发明的有益效果是：

首先将GPS量测信号中失灵偏差和干扰部分建模为传感器量测中未知输入项，并在此基础上设计了一种基于最小上限滤波的GPS/INS组合导航定位方法。该方法可以在GPS信号短时受扰或失灵时，有效抑制定位误差而实现精确自身定位与导航。具体为在组合导航滤波算法中的位置估计方差中引入了自由参数，并构造位置估计方差上限表达式，通过实时凸优化动态寻优得到其估计方差优化值，输出精确的导航信息，进而保证了组合导航系统的稳定运行。

附图说明：

图1是实施例中基于MUBF的组合导航定位方法的步骤图。

图2是步骤4中GPS数据通过最小上限滤波进行修正的流程图。

具体实施方式：

下面将结合附图和实施例对本发明进行详细地说明。

实施例

参见图1，本发明提供了一种基于最小上限滤波的GPS/INS组合导航定位方法，包括下述步骤：

步骤1、导航信息获取阶段：

由安装在无人机上的加速度计实时测得的无人机的加速度，通过导航解算计算机，将测得的加速度信息进行积分解算，得到无人机的位置信息(经度、纬度、高度)。安装在无人机上的GPS接收机通过卫星，得到无人机的位置信息(经度、纬度、高度)。

步骤2、导航模型建立阶段：

首先，将GPS解算位置信息与INS量测信息之差作为系统状态，则定位系统的状态向量写为x＝[δL δλ δh]，其中δL.δλ.δh分别为INS与GPS的纬度、经度、高度之差，即纬度误差、经度误差和高度误差。

系统量测方程为

z_k+1＝H_k+1x_k+1|k+A_k+1δk₊₁+v_k+1 (1)

在式(1)中，x_k+1为状态向量，z_k+1为量测向量。H_k+1和A_k+1分别为量测矩阵和扰动矩阵。v_k+1为量测噪声。δ_k+1为GPS受到的广义未知扰动。

系统状态方程为

x_k+1＝F_kx_k+w_k (2)

在式(1)中，F_k为状态转移矩阵，w_k为过程噪声。状态噪声和量测噪声均为高斯白噪声。

步骤3、导航误差判断阶段：

对比上一时刻一步预测估计和当前时刻最优估计，当其差值大于阈值时，则判定当前GPS信号发生异常，即信号发生跳变或失灵，此时进入步骤4，否则采用经典基于卡尔曼的组合导航算法对信号进行融合处理。

步骤4、导航信息校正：

当GPS信号发生跳变或失灵时，启动最小上限滤波器得出的状态误差最优估计，在建立系统偏差模型基础上利用反馈校正，修正由INS解算所造成的累计误差，输出导航信息。

具体修正步骤如下(参见图2)：

第一步：由当前时刻状态估计和协方差估计P^* _k|k，计算状态预测和量测预测其中

第二步：由下一时刻的量测值z_k+1，计算滤波残差γ_k+1，其中滤波残差γ_k+1中存在GPS受到的广义未知扰动δ_k+1。

第三步：计算新息协方差其中

第四步：由式P^* _k+1|k＝F_kP^* _k|kF_k ^T+Γ_kQ_kΓ_k ^T计算状态预测协方差P^* _k+1|k，其中Γ_k为状态噪声系数矩阵，Q_k为状态噪声协方差矩阵。

第五步：凸优化环节

对于最优参数：

这里，定义Ω_k+1＝{τ_k+1|τ_k+1≥0，S^* _k+1|τ_k+1≥S_k+1}

服从如下不等式约束：

在式(4)中，S_k+1是未知量，因此在实际的参数优化中，用它的无偏估计来代替。

第六步：凸优化判定环节：

通过一个判定环节即对比上一时刻一步预测估计和当前时刻最优估计，当满足不等式关系：(其中a是人为设定的值)，重复进行第五步，直至不满足不等式关系时，调出循环，结束凸优化，得到最优参数τ_k+1。

第七步：数据优化阶段

在得到最优参数τ_k+1后，可以得出基于MUBF的组合导航中的核心优化公式和步骤：

式中，∑是人为设定的已知量，τ_k+1是经过凸优化后的参数，是由τ_k+1调节出来的最优增益，最终由来保证最优估计，即最优偏差量。通过式(5)最终得到当前时刻的最优的状态误差估计。

第八步：位置校正阶段

当得到最优的状态误差估计后再通过反馈校正将误差消除，得到校正后的位置信息p_k，INS，其中这里设系统位置向量为p＝[Lλ h]，k时刻修正后的位置向量为p_k，INS，k时刻修正前的位置向量为在k时候最优的状态误差估计为

第九步：输出最优导航位置估计

最后，将校正后的位置信息p_k，INS作为最终的无人机的定位信息，输出最优状态估计。继续进入步骤3导航误差判断阶段进行下一步信息处理。至此，步骤4结束。

步骤5、最优状态估计信息输出。

对于本技术领域的普通技术人员来说，在不脱离本发明所属原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (2)

1.一种基于最小上限滤波的GPS/INS组合导航定位方法，其特在于，包括下述步骤：

步骤1、导航信息获取阶段：

由安装在无人机上的加速度计实时测得的无人机的加速度，通过导航解算计算机，将测得的加速度信息进行积分解算，得到无人机的经度、纬度、高度位置信息；安装在无人机上的GPS接收机通过卫星，得到无人机的经度、纬度、高度位置信息；

步骤2、导航模型建立阶段：

首先，将GPS解算位置信息与INS量测信息之差作为系统状态，则定位系统的状态向量写为x＝[δL δλ δh]，其中δL、δλ、δh分别为INS与GPS的纬度、经度、高度之差，即纬度误差、经度误差和高度误差；

系统量测方程为

z_k+1＝H_k+1z_k+1|k+A_k+1δ_k+1+v_k+1 (1)

在式(1)中，x_k+1为状态向量，z_k+1为量测向量；H_k+1和A_k+1分别为量测矩阵和扰动矩阵；v_k+1为量测噪声；δ_k+1为GPS受到的广义未知扰动；

系统状态方程为

x_k+1＝F_kx_k+w_k (2)

在式(2)中，F_k为状态转移矩阵，W_k为过程噪声。状态噪声和量测噪声均为高斯白噪声；

步骤3、导航误差判断阶段：

对比上一时刻一步预测估计和当前时刻最优估计，当其差值大于阈值时，则判定当前GPS信号发生异常，即信号发生跳变或失灵，此时进入步骤4，否则采用经典基于卡尔曼的组合导航算法对信号进行融合处理；

步骤4、导航信息校正：

当GPS信号发生跳变或失灵时，启动最小上限滤波器得出的状态误差最优估计，在建立系统偏差模型基础上利用反馈校正，修正由INS解算所造成的累计误差，输出导航信息；

步骤5、最优状态估计信息输出。

2.根据权利要求1所述基于最小上限滤波的GPS/INS组合导航定位方法，其特在于，步骤4中，具体的修正步骤如下：

步骤401：由当前时刻状态估计和协方差估计P^* _k|k，计算状态预测和量测预测其中

步骤402：由下一时刻的量测值z_k+1，计算滤波残差γ_k+1，其中滤波残差γ_k+1中存在GPS受到的广义未知扰动δ_k+1；

步骤403：计算新息协方差其中

步骤404：由式P^* _k+1|k＝F_kP^* _k|kF_k ^T+Γ_kQ_kΓ_k ^T计算状态预测协方差P^* _k+1|k，其中Γ_k为状态噪声系数矩阵，Q_k为状态噪声协方差矩阵；

步骤405：凸优化环节

对于最优参数：

这里，定义Ω_k+1＝{τ_k+1|τ_k+1≥0，S^* _k+1|τ_k+1≥S_k+1}

服从如下不等式约束：

在式(4)中，S_k+1是未知量，因此在实际的参数优化中，用它的无偏估计来代替；

步骤406：凸优化判定环节：

通过一个判定环节即对比上一时刻一步预测估计和当前时刻最优估计，当满足不等式关系：其中a是人为设定的值,，重复进行步骤405，直至不满足不等式关系时，调出循环，结束凸优化，得到最优参数τ_k+1；

步骤407：数据优化阶段

在得到最优参数τ_k+1后，可以得出基于MUBF的组合导航中的核心优化公式和步骤：

式中，∑是人为设定的已知量，τ_k+1是经过凸优化后的参数，是由τ_k+1调节出来的最优增益，最终由来保证最优估计，即最优偏差量。通过式(5)最终得到当前时刻的最优的状态误差估计。

步骤408：位置校正阶段

当得到最优的状态误差估计后再通过反馈校正将误差消除，得到校正后的位置信息p_k，INS，其中这里设系统位置向量为p＝[Lλ h],k时刻修正后的位置向量为p_k，INS，k时刻修正前的位置向量为在k时候最优的状态误差估计为

步骤409：输出最优导航位置估计

最后，将校正后的位置信息p_k，INS作为最终的无人机的定位信息，输出最优状态估计。