CN116772837A - 基于交互式多模型的gnss/sins组合导航方法 - Google Patents

Abstract

本发明公开了一种基于交互式多模型的GNSS/SINS组合导航方法，包括如下步骤：在无人机上搭载SINS子系统，结合GNSS子系统建立GNSS/SINS组合导航系统，利用RAUKF算法对无人机的状态输入和协方差矩阵进行跟踪滤波处理；判断GNSS和SINS数据的有效性，比较数据的可靠性，分别调整系统的协方差以提供可靠的导航信号；进行交互式多模型估计，根据组合导航系统的观测数据，更新导航状态；根据交互式多模型估计的导航状态，输出精确的位置和速度信息，用于导航控制和定位应用。本发明所公开的方法可以保证无人机实时地感知和纠正自身的姿态、位置和速度误差，能够确保导航的准确性和稳定性。

Description

基于交互式多模型的GNSS/SINS组合导航方法

技术领域

本发明涉及导航方法领域，特别涉及一种基于交互式多模型的GNSS/SINS组合导航方法。

背景技术

随着城市现代化发展，复杂的城市区域对导航精度提出了很大的挑战，即建筑物和树木的存在会引起信号的反射和衰减，从而引发受误差影响的观测。随着对运动载体的导航定位精度和可靠性要求的提高，单一的导航系统已经难以满足用户的需求。由于GNSS和SINS具有良好的优势互补性，因此GNSS/SINS组合可以提高系统的整体导航性能及导航精度。

Kalman滤波(KF)作为线性随机系统的最优估计器，在数据融合算法中得到了广泛的应用。然而对于导航来说，各方面都可能造成位置偏移。多传感器集成可能会增加系统的非线性，这超出了传统KF的能力。因此，为了处理非线性系统产生了扩展卡尔曼滤波(Extended Kalman Filter，EKF)和无迹卡尔曼滤波(Unscented Kalman Filter，UKF)等滤波器。UKF基于无迹变换(Unscented Transform，UT)所提出的。无迹变换研究的是如何通过确定的采样点捕获经非线性变换的高斯随机变量的后验分布的问题。通过无迹变换得到相应的统计特性后，再结合标准卡尔曼滤波框架，便得到无迹卡尔曼滤波。但UKF作为Kalman滤波器的一个变种，需要对系统噪声特性有准确的先验知识。在这其中，噪声协方差Q和测量噪声协方差R是最重要的，因为它们直接调节预测值和测量值，会对系统状态估计产生影响从而导致性能下降。所以自适应的调整系统的协方差矩阵是卡尔曼滤波器发展的关键。针对以上情况亟需一种有效的解决方法。

发明内容

为解决上述技术问题，本发明提供了一种基于交互式多模型的GNSS/SINS组合导航方法，以达到实现精确的导航功能的目的。

为达到上述目的，本发明的技术方案如下：

一种基于交互式多模型的GNSS/SINS组合导航方法，包括如下步骤：

步骤1：在无人机上搭载SINS子系统，结合GNSS子系统建立GNSS/SINS组合导航系统，其中包含系统的离散状态方程、观测方程的建立；

步骤2：基于所述离散状态方程和观测方程，利用RAUKF算法对无人机的状态输入和协方差矩阵进行跟踪滤波处理；

步骤3：判断GNSS和SINS数据的有效性，比较数据的可靠性，分别调整系统的协方差以提供可靠的导航信号；

步骤4：进行交互式多模型估计，根据组合导航系统的观测数据，更新导航状态；根据交互式多模型估计的导航状态，输出精确的位置和速度信息，用于导航控制和定位应用。

上述方案中，步骤1中，在无人机上搭载SINS子系统，并建立其姿态、位置、速度的误差方程，表达式为：

其中，δVⁿ、δPⁿ分别为n系下的姿态误差、速度误差和位置误差；n系指导航坐标系；fⁿ为n系下的比力；/>为地球自转角速度在n系的投影，/>为n系相对于地球坐标系的角速度在n系的投影，/>为n系相对于惯性坐标系的角速度在n系的投影；/>为b系转换到n系的方向余弦矩阵；b系指载体坐标系；δω^b、δf^b分别为载体坐标系下的加速度计和陀螺仪的测量误差；/>为无人机在n系下的姿态；对无人机移动轨迹进行实时的跟踪、预测、记录。

上述方案中，步骤1中，系统的离散状态方程建立如下：

X(t)＝F(t)x(t)+G(t)W(t)

其中，X(t)表示系统的离散状态参数，F(t)为系统非线性转移矩阵，x(t)为SINS子系统误差状态量，G(t)为系统状态噪声输入矩阵，W(t)为一个随机过程，代表状态噪声向量；

系统的观测方程是由伪距测量组成的系统观测方程，具体如下：

Z_k＝(δρ¹,δρ²,…,δρ^m)^T＝H(t)X(t)+V(t)

其中，Z_k为系统观测值，δρ^m为相对于第m颗卫星的伪距，V(t)为观测噪声，H(t)为系统测量矩阵。

进一步的，x(t)的表达式如下：

其中，分别为系统三个方向的姿态失准角；δv_E,δv_N,δv_U为惯性坐标系下三轴方向的速度误差；δL,δλ,δh为发射惯性坐标系下三轴方向的位置误差；ε_bx,ε_by,ε_bz为加速度计常值偏置；/>为陀螺常值漂移；b_p,b_f为GNSS接收机时钟相关的距离偏差。

上述方案中，步骤2中，通过在RAUKF计算过程的预测状态协方差矩阵中引入时变衰落因子λ_k来膨胀预测信息，时变衰落因子λ_k设置为λ_k∈(0,1)，系统的预测协方差矩阵更新为：

其中，P_k/k-1为预测协方差，F_k/k-1为离散时间转移矩阵，Q_k-1为系统噪声协方差。

上述方案中，步骤2中，在RAUKF计算过程的观测协方差矩阵中引入一个时变衰落因子σ_k来膨胀观测信息，时变衰落因子σ_k设置为σ_k∈(0,1)；系统的观测协方差矩阵更新为：

其中，T为转置操作，为观测协方差，/>为无迹变换所选容积点经非线性变换所得的新点集；R_k为系统观测噪声协方差矩阵，/>为k时刻系统的预测观测值，通过将此值与系统k时刻的实际观测值进行进一步的度量。

上述方案中，步骤3中，引入新息向量来衡量系统状态量和观测量的比重，新息向量/>定义为观测值和其前一时刻对当前的预测值的差值，它是在估计过程中新信息的度量值，如下式所示：

其中，Z_k为系统观测值，为系统k-1时刻对于k时刻的状态预测值。

进一步的，使用新息向量与坐标原点的马氏距离来判决是否存在误差；如果在系统不涉及建模误差时，则新息向量应满足零均值且具有以下概率密度函数的高斯分布：

其中，为矩阵/>的逆矩阵，/>为矩阵/>的模；/>表示概率密度函数，/>为系统的观测协方差矩阵，/>表示多元高斯分布；

定义新息向量到坐标原点的马氏距离ξ_k作为判据：

当系统不存在建模误差时，每个新息向量满足高斯分布，即判据ξ_k应满足自由度为m的卡方分布，对于给定的显著性水平α,α∈(0,1)和其所对应的分位数应满足：

其中，表示随机变量ξ_k小于等于卡方分布的临界值的概率。

上述方案中，步骤4中，选择不同性质的UKF滤波器对无人机不同运动状态进行分别估计，并在交互式多模型输入时进行交互：

X^j(k-1/k-1)＝∑Xⁱ(k-1/k-1)μ_ij(k-1/k-1)

P^j(k-1/k-1)＝∑M_i(k-1){Pⁱ(k-1/k-1)+[X_i(k-1/k-1)-X^j(k-1/k-1)][X_i(k-1/k-1)-X^j(k-1/k-1)]^T}

其中，X^j(k-1/k-1)代表k-1时刻第j种运动状态模型的状态输入，P^j(k-1/k-1)为k-1时刻第j种运动状态模型的输入协方差，μ_ij(k)为k时刻运动状态模型i向运动状态模型j运动状态模型转移的概率，M_i(k-1)表示k-1时刻起作用的运动状态模型，X_i(k-1/k-1)表示在k-1时刻下的状态估计向量，p_ij是运动状态模型i转移到运动状态模型j的转移概率，为归一化函数。

进一步的，对应于模型交互后的输入通过RAUKF算法对系统姿态、位置、速度并行滤波，并且对不同模型的输出概率进行更新；并对交互式多模型不同模型概率更新，采用贝叶斯假设检验方法对不同模型权重选择更新；

μ_j(k)＝P{M_j(k)/Z^k}＝P{Z(k)/M_j(k),Z^k-1}P{M_j(k)/Z^k-1}

其中，μ_j(k)为第j种运动状态模型在k时刻的概率，P{·}为条件慨率，M_j(k)表示k时刻起作用的运动状态模型，Z^k为k时刻的观测值，Z^k-1为k-1时刻的观测值；

最后对交互式多模型的卡尔曼滤波结果输出进行交互，将滤波结果作为GNSS/SINS组合导航系统的输出。

通过上述技术方案，本发明提供的一种基于交互式多模型的GNSS/SINS组合导航方法具有如下有益效果：

经过对GNSS和SINS两个子系统的融合和优化，为无人机提供精确的姿态、位置和速度信息。滤波结果通过多传感器数据的整合和滤波算法来修正和校准无人机的导航状态，实现精确的导航功能。无人机实时地感知和纠正自身的姿态、位置和速度误差，能够确保导航的准确性和稳定性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。

图1为本发明所公开的基于交互式多模型的GNSS/SINS组合导航方法框图；

图2为交互式多模型原理图；

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

本发明提供了一种基于交互式多模型的GNSS/SINS组合导航方法，如图1所示，其特征在于，包括如下步骤：

步骤1：在无人机上搭载SINS子系统，结合GNSS子系统建立GNSS/SINS组合导航系统，其中包含系统的离散状态方程、观测方程的建立。

具体的，在无人机上搭载SINS子系统，并建立其姿态、位置、速度的误差方程，表达式为：

其中，δVⁿ、δPⁿ分别为n系下的姿态误差、速度误差和位置误差；n系指导航坐标系；fⁿ为n系下的比力；/>为地球自转角速度在n系的投影，/>为n系相对于地球坐标系的角速度在n系的投影，/>为n系相对于惯性坐标系的角速度在n系的投影；/>为b系转换到n系的方向余弦矩阵；b系指载体坐标系；δω^b、δf^b分别为载体坐标系下的加速度计和陀螺仪的测量误差；/>为无人机在n系下的姿态；对无人机移动轨迹进行实时的跟踪、预测、记录。

将SINS子系统和GNSS子系统进行组合，建立GNSS/SINS组合导航系统，系统的离散状态方程建立如下：

X(t)＝F(t)x(t)+G(t)W(t)

其中，X(t)表示组合导航系统的离散状态参数，F(t)为系统非线性转移矩阵，x(t)为SINS子系统误差状态量，G(t)为系统状态噪声输入矩阵，W(t)为一个随机过程，代表状态噪声向量；

系统的观测方程是由伪距测量组成的系统观测方程，具体如下：

Z_k＝(δρ¹,δρ²,…,δρ^m)^T＝H(t)X(t)+V(t)

其中，Z_k为系统观测值，δρ^m为相对于第m颗卫星的伪距，V(t)为观测噪声，H(t)为系统测量矩阵。

其中，选取传统15维惯导系统误差状态量作为SINS子系统状态参量，x(t)的表达式如下：

其中，分别为系统三个方向的姿态失准角；δv_E,δv_N,δv_U为惯性坐标系下三轴方向的速度误差；δL,δλ,δh为发射惯性坐标系下三轴方向的位置误差；ε_bx,ε_by,ε_bz为加速度计常值偏置；/>为陀螺常值漂移；b_p,b_f为GNSS接收机时钟相关的距离偏差。

步骤2：基于离散状态方程和观测方程，利用RAUKF算法对无人机的状态输入和协方差矩阵进行跟踪滤波处理。

具体的，通过在RAUKF(无迹卡尔曼滤波)计算过程的预测状态协方差矩阵中引入时变衰落因子λ_k来膨胀预测信息，时变衰落因子λ_k设置为λ_k∈(0,1)，用来确保更新强度。因此，系统的预测协方差矩阵更新为：

其中，P_k/k-1为预测协方差，F_k/k-1为离散时间转移矩阵，Q_k-1为系统噪声协方差。

在RAUKF计算过程的观测协方差矩阵中引入一个时变衰落因子σ_k来膨胀观测信息，时变衰落因子σ_k设置为σ_k∈(0,1)，用来确保更新强度；因此，系统的观测协方差矩阵更新为：

其中，T为转置操作，为观测协方差，/>为无迹变换所选容积点经非线性变换所得的新点集；R_k为系统观测噪声协方差矩阵，/>为k时刻系统的预测观测值，通过将此值与系统k时刻的实际观测值进行进一步的度量。

通过上述步骤分别引入λ_k、σ_k，两个因子分别控制系统的状态和观测方程，当系统出现运动误差和观测误差时，能够自适应鲁棒的调整系统预测量，以达到对误差的消除。并后续通过多模型交互原理将具有不同性质的改进无迹卡尔曼滤波器进行融合，在系统出现不同类型的误差时达到最优解。

步骤3：判断GNSS和SINS数据的有效性，比较数据的可靠性，分别调整系统的协方差以提供可靠的导航信号。

具体地，通过引入新息向量来衡量系统状态量和观测量的比重，新息向量/>定义为观测值和其前一时刻对当前的预测值的差值，它是在估计过程中新信息的度量值，如下式所示：

其中，Z_k为系统观测值，为系统k-1时刻对于k时刻的状态预测值。

使用新息向量与坐标原点的马氏距离来判决是否存在误差；如果在系统不涉及建模误差时，则新息向量应满足零均值且具有以下概率密度函数的高斯分布：

其中，为矩阵/>的逆矩阵，/>为矩阵/>的模；/>表示概率密度函数，/>为系统的观测协方差矩阵，/>表示多元高斯分布；

定义新息向量到坐标原点的马氏距离ξ_k作为判据：

当系统不存在建模误差时，每个新息向量满足高斯分布，即判据ξ_k应满足自由度为m的卡方分布，对于给定的显著性水平α,α∈(0,1)和其所对应的分位数应满足：

其中，表示随机变量ξ_k小于等于卡方分布的临界值的概率。

当时，则判断为组合导航系统存在误差，将UKF中的协方差更新方程更换为步骤2中所使用的加入了时变衰落因子的协方差更新方程，以避免误差带来的影响。

步骤4：进行交互式多模型估计，根据组合导航系统的观测数据，更新导航状态；并且调整导航模型的权重；根据交互式多模型估计的导航状态，输出精确的位置和速度信息，用于导航控制和定位应用。

具体地，如图2所示，选择不同性质的UKF滤波器对无人机不同运动状态进行分别估计，并在交互式多模型输入时进行交互：本实施例中，选择RUKF，CUKF，UKF滤波器进行交互。

X^j(k-1/k-1)＝∑Xⁱ(k-1/k-1)μ_ij(k-1/k-1)

P^j(k-1/k-1)＝∑M_i(k-1){Pⁱ(k-1/k-1)+[X_i(k-1/k-1)-X^j(k-1/k-1)][X_i(k-1/k-1)-X^j(k-1/k-1)]^T}

其中，X^j(k-1/k-1)代表k-1时刻第j种运动状态模型的状态输入，P^j(k-1/k-1)为k-1时刻第j种运动状态模型的输入协方差，μ_ij(k)为k时刻运动状态模型i向运动状态模型j运动状态模型转移的概率，M_i(k-1)表示k-1时刻起作用的运动状态模型，X_i(k-1/k-1表示在k-1时刻下的状态估计向量，p_ij是运动状态模型i转移到运动状态模型j的转移概率，为归一化函数。

对应于模型M_j(k)，μ_ij(k)为模型j在k时刻的概率，以X^oj(k-1/k-1)，P^oj(k-1/k-1)及Z(k)作为输入进行滤波。

对交互式多模型不同模型概率更新，采用贝叶斯假设检验方法进行不同模型权重选择更新。

μ_j(k)＝P{M_j(k)/Z^k}＝P{Z(k)/M_j(k),Z^k-1}P{M_j(k)/Z^k-1}

其中，μ_j(k)为第j种运动状态模型在k时刻的概率，P{·}为条件慨率，M_j(k)表示k时刻起作用的运动状态模型，Z^k为k时刻的观测值，Z^k-1为k-1时刻的观测值；

最后对交互式多模型的卡尔曼滤波结果输出进行交互：

滤波结果作为GNSS/SINS组合导航系统的输出，经过对两个子系统的融合和优化，为无人机提供精确的姿态、位置和速度信息。滤波结果通过多传感器数据的整合和滤波算法来修正和校准无人机的导航状态，实现精确的导航功能。无人机实时地感知和纠正自身的姿态、位置和速度误差，确保导航的准确性和稳定性。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种基于交互式多模型的GNSS/SINS组合导航方法，其特征在于，包括如下步骤：

步骤1：在无人机上搭载SINS子系统，结合GNSS子系统建立GNSS/SINS组合导航系统，其中包含系统的离散状态方程、观测方程的建立；

步骤2：基于所述离散状态方程和观测方程，利用RAUKF算法对无人机的状态输入和协方差矩阵进行跟踪滤波处理；

步骤3：判断GNSS和SINS数据的有效性，比较数据的可靠性，分别调整系统的协方差以提供可靠的导航信号；

步骤4：进行交互式多模型估计，根据组合导航系统的观测数据，更新导航状态；根据交互式多模型估计的导航状态，输出精确的位置和速度信息，用于导航控制和定位应用。

2.根据权利要求1所述的一种基于交互式多模型的GNSS/SINS组合导航方法，其特征在于，步骤1中，在无人机上搭载SINS子系统，并建立其姿态、位置、速度的误差方程，表达式为：

其中，δVⁿ、δPⁿ分别为n系下的姿态误差、速度误差和位置误差；n系指导航坐标系；fⁿ为n系下的比力；/>为地球自转角速度在n系的投影，/>为n系相对于地球坐标系的角速度在n系的投影，/>为n系相对于惯性坐标系的角速度在n系的投影；/>为b系转换到n系的方向余弦矩阵；b系指载体坐标系；δω^b、δf^b分别为载体坐标系下的加速度计和陀螺仪的测量误差；/>为无人机在n系下的姿态；对无人机移动轨迹进行实时的跟踪、预测、记录。

3.根据权利要求1所述的一种基于交互式多模型的GNSS/SINS组合导航方法，其特征在于，步骤1中，系统的离散状态方程建立如下：

X(t)＝F(t)x(t)+G(t)W(t)

其中，X(t)表示系统的离散状态参数，F(t)为系统非线性转移矩阵，x(t)为SINS子系统误差状态量，G(t)为系统状态噪声输入矩阵，W(t)为一个随机过程，代表状态噪声向量；

系统的观测方程是由伪距测量组成的系统观测方程，具体如下：

Z_k＝(δρ¹,δρ²,…,δρ^m)^T＝H(t)X(t)+V(t)

其中，Z_k为系统观测值，δρ^m为相对于第m颗卫星的伪距，V(t)为观测噪声，H(t)为系统测量矩阵。

4.根据权利要求3所述的一种基于交互式多模型的GNSS/SINS组合导航方法，其特征在于，x(t)的表达式如下：

其中，分别为系统三个方向的姿态失准角；δv_E,δv_N,δv_U为惯性坐标系下三轴方向的速度误差；δL,δλ,δh为发射惯性坐标系下三轴方向的位置误差；ε_bx,ε_by,ε_bz为加速度计常值偏置；/>为陀螺常值漂移；b_p,b_f为GNSS接收机时钟相关的距离偏差。

5.根据权利要求1所述的一种基于交互式多模型的GNSS/SINS组合导航方法，其特征在于，步骤2中，通过在RAUKF计算过程的预测状态协方差矩阵中引入时变衰落因子λ_k来膨胀预测信息，时变衰落因子λ_k设置为λ_k∈(0,1)，系统的预测协方差矩阵更新为：

其中，P_k/k-1为预测协方差，F_k/k-1为离散时间转移矩阵，Q_k-1为系统噪声协方差。

6.根据权利要求1所述的一种基于交互式多模型的GNSS/SINS组合导航方法，其特征在于，步骤2中，在RAUKF计算过程的观测协方差矩阵中引入一个时变衰落因子σ_k来膨胀观测信息，时变衰落因子σ_k设置为σ_k∈(0,1)；系统的观测协方差矩阵更新为：

其中，T为转置操作，为观测协方差，/>为无迹变换所选容积点经非线性变换所得的新点集；R_k为系统观测噪声协方差矩阵，/>为k时刻系统的预测观测值，通过将此值与系统k时刻的实际观测值进行进一步的度量。

7.根据权利要求1所述的一种基于交互式多模型的GNSS/SINS组合导航方法，其特征在于，步骤3中，引入新息向量来衡量系统状态量和观测量的比重，新息向量/>定义为观测值和其前一时刻对当前的预测值的差值，它是在估计过程中新信息的度量值，如下式所示：

其中，Z_k为系统观测值，为系统k-1时刻对于k时刻的状态预测值。

8.根据权利要求7所述的一种基于交互式多模型的GNSS/SINS组合导航方法，其特征在于，使用新息向量与坐标原点的马氏距离来判决是否存在误差；如果在系统不涉及建模误差时，则新息向量应满足零均值且具有以下概率密度函数的高斯分布：

其中，为矩阵/>的逆矩阵，/>为矩阵/>的模；/>表示概率密度函数，/>为系统的观测协方差矩阵，/>表示多元高斯分布；

定义新息向量到坐标原点的马氏距离ξ_k作为判据：

当系统不存在建模误差时，每个新息向量满足高斯分布，即判据ξ_k应满足自由度为m的卡方分布，对于给定的显著性水平α,α∈(0,1)和其所对应的分位数应满足：

其中，表示随机变量ξ_k小于等于卡方分布的临界值的概率。

9.根据权利要求1所述的一种基于交互式多模型的GNSS/SINS组合导航方法，其特征在于，步骤4中，选择不同性质的UKF滤波器对无人机不同运动状态模型进行分别估计，并在交互式多模型输入时进行交互：

其中，X^j(k-1/k-1)代表k-1时刻第j种运动状态模型的状态输入，P^j(k-1/k-1)为k-1时刻第j种运动状态模型的输入协方差，μ_ij(k)为k时刻运动状态模型i向运动状态模型j运动状态模型转移的概率，M_i(k-1)表示k-1时刻起作用的运动状态模型，X_i(k-1/k-1)表示在k-1时刻下的状态估计向量，p_ij是运动状态模型i转移到运动状态模型j的转移概率，为归一化函数。

10.根据权利要求1所述的一种基于交互式多模型的GNSS/SINS组合导航方法，其特征在于，步骤4中，对应于模型交互后的输入通过RAUKF算法对系统姿态、位置、速度并行滤波，并且对不同模型的输出概率进行更新；并对交互式多模型不同模型概率更新，采用贝叶斯假设检验方法对不同模型权重选择更新；

μ_j(k)＝P{M_j(k)/Z^k}＝P{Z^k/M_j(k),Z^k-1}P{M_j(k)/Z^k-1}

其中，μ_j(k)为第j种运动状态模型在k时刻的概率，P{·}为条件慨率，M_j(k)表示k时刻起作用的运动状态模型，Z^k为k时刻的观测值，Z^k-1为k-1时刻的观测值；

最后对交互式多模型的卡尔曼滤波结果输出进行交互，将滤波结果作为GNSS/SINS组合导航系统的输出。