CN112946711B - 一种gnss/ins组合导航系统的导航方法 - Google Patents

Abstract

本发明提供一种GNSS/INS组合导航系统的导航方法，应用于GNSS/INS组合导航系统工作在紧组合模式下。技术方案是：利用GNSS接收机的伪码跟踪环路锁定值、载波相位跟踪环路锁定值、导航信息校验值构造联合权重矩阵，对GNSS接收机伪距测量值的协方差矩阵进行修正；利用修正的GNSS接收机伪距测量值的协方差矩阵，通过因子图计算得到GNSS/INS组合导航系统的导航结果。本发明导航精度高，且不需要增加额外的传感器，因此具有低成本的特点，在工程实践中具有很好的应用价值。

Description

一种GNSS/INS组合导航系统的导航方法

技术领域

本发明涉及卫星导航技术领域，特别是涉及GNSS(Global Navigation SatelliteSystem,全球卫星导航系统)与INS(Inertial Navigation System，惯性导航系统)组合导航系统的紧组合模式下，利用测量信号进行导航的方法。

背景技术

GNSS以其全天时、全球覆盖、高精度的定位性能，成为当今主流的导航手段，然而GNSS落地信号功率低(约-160dBw)，且易受各种干扰及遮挡，导致在某些复杂环境下可用性差，如城市峡谷。INS以其不与外界发生信息交换的特点，恰好解决了GNSS易受干扰的问题。将上述两种导航设备组合在一起的 GNSS/INS组合导航系统，取二者之优点，已成为未来导航技术发展的趋势。

GNSS/INS组合导航系统通常有三种组合模式，分别为：松组合模式、紧组合模式和深组合模式。其中，紧组合模式具有适中的计算量和良好的定位性能，是目前GNSS/INS组合导航技术研究的热点。GNSS/INS组合导航系统紧组合模式通常采用GNSS接收机输出的原始观测信息(如伪距、伪距率、多普勒等) 作为测量值，通过贝叶斯滤波器，和INS输出的测量值进行融合滤波，得到最优的系统状态估计量。这种模式下，GNSS/INS组合导航系统是非线性的，需要使用非线性的融合滤波方法来进行系统状态量估计。非线性融合滤波方法分为基于模型状态的非线性融合滤波方法和基于小波分析、神经网络、模糊控制等的非线性融合滤波方法。在基于模型状态的非线性融合滤波方法中，通常是在 Kalmam滤波方法基础上变化的扩展算法，参见文献[1]-[3]。这些扩展算法使用数值积分或线性逼近的方法，对系统状态的后验概率密度分布函数进行逼近，并基于某种最优化准则，对系统状态量进行最优估计。使用上述方法时，存在的问题是：当GNSS/INS组合导航系统运行在卫星信号频繁短暂失锁、强多径干扰等复杂环境下时，由于GNSS测量噪声变化剧烈、测量值缺失，即测量模型改变，上述的非线性融合滤波方法，对系统状态量发生突变的适应能力差，不具有应对GNSS/INS组合导航系统模型不确定的鲁棒性，在噪声统计值未知或者时变情况下易出现滤波精度下降甚至发散，导致计算失败。

为了解决上述问题，文献[4]提出了一种基于因子图的优化方法，对GNSS 接收机中接收的每一颗卫星都设置可靠性因子，可靠性因子表示卫星信号的质量好坏，当某颗卫星的伪距、伪距率等测量值缺失时，对应的可靠性因子为0，表示该卫星不可见，当某颗卫星测量值正常时，可靠性因子为1，表示该卫星信号可用，但是文献[4]的方法，没有解决卫星信号在各种遮挡、干扰环境下伪距测量值变差而导致的模型不准确问题。文献[5]提出了一种基于鱼眼相机辅助的，在因子图模型下的GNSS/INS组合导航系统紧组合导航方法，该文献增加了鱼眼相机作为系统的辅助传感器，实时获取运动载体周围的可见光图像，从而判断载体目前在哪些方向上有建筑物等的遮挡，再结合星历给出的卫星位置，来判断哪些卫星不可用。另外，利用GNSS接收机输出的信噪比、卫星方位角等测量信息，使用拟合公式计算卫星可用性的权值，将此权值作为对应卫星伪距测量的方差参与系统状态量的计算。文献[5]相比文献[4]的方法更具完整性和准确性，然而新增的鱼眼相机增加了系统成本和复杂性，同时拟合公式也是由经验值计算而来，不同的传感器设备，不同的应用场景，拟合公式的参数不同，需要用户自定义，不具通用性。

发明内容

针对现有技术中存在的问题，本发明提供一种应用于GNSS/INS组合导航系统工作在紧组合模式下的导航方法。相比现有的方法，在不增加额外传感器资源的基础上，提高了GNSS/INS紧组合导航系统在复杂环境下的导航精度和鲁棒性，具有较低的计算复杂度，能适应GNSS/INS紧组合导航系统在各种噪声模型下的定位解算。

本发明的技术方案是：一种GNSS/INS组合导航系统的导航方法，应用于 GNSS/INS组合导航系统工作在紧组合模式下，其特征在于，利用GNSS接收机的伪码跟踪环路锁定值、载波相位跟踪环路锁定值、导航信息校验值构造联合权重矩阵，对GNSS接收机伪距测量值的协方差矩阵进行修正；利用修正的GNSS接收机伪距测量值的协方差矩阵，通过因子图计算得到GNSS/INS 组合导航系统的导航结果。

进一步，在因子图计算过程中，将IMU(Inertial Measurement Unit，导航测量单元)测量值进行预积分，其预积分时间间隔与GNSS接收机输出测量值间隔一致。

本发明的技术效果是：通过利用GNSS接收机的内部参数(伪码跟踪环路锁定值、载波相位跟踪环路锁定值、导航信息校验值)构造联合权重矩阵，使联合权重矩阵的取值实时反映接收的不同卫星信号的质量情况，从而可以使本发明可以适应各种不同信号环境，提高组合导航系统的鲁棒性和可靠性。利用联合权重矩阵对GNSS接收机伪距测量值的协方差矩阵进行修正，从而提高系统在各种复杂环境下的导航精度和鲁棒性。通过对IMU测量值进行预积分，经过一段时间间隔积累后再进行坐标系的转换、输出导航解，可以在保证导航精度的同时减小计算量。本发明不需要增加额外的传感器，因此具有低成本的特点，在工程实践中具有很好的应用价值。

附图说明

图1是本发明的流程示意图；

图2是GNSS/INS组合系统的因子图模型图；

图3是仿真的载体运动轨迹图；

图4是本发明与卡尔曼滤波、经典因子图算法的定位、测速误差对比图；

图5是本发明与文献[4]的方法在定位、测速的误差对比图；

表1是仿真数据的初始参数设置表；

具体实施方式

下面结合图1对本发明做进一步介绍。

本发明提供一种GNSS/INS组合导航系统的导航方法,包括：

第一步，构造联合权重矩阵

假设在某个时刻，GNSS接收机接收到的I支路信号经过相干积分后，得到的超前支路信号、即时支路信号、滞后支路信号分别为I_E、I_P、I_L；Q支路信号经过相干积分后，得到的超前支路信号、即时支路信号、滞后支路信号分别为Q_E、 Q_P、Q_L。

利用下式得到鉴相器输出的伪码相位误差控制量DLI为：

其中，d_c为相干器间隔。

利用下式计算归一化的伪码相位锁定检测值D：

根据上式可知，当伪码环路锁定时，D取值为(0,1]，当环路失锁时D取ε，ε为接近零的正小数，在实际计算中取值小于10^-5即可。

利用下式计算接收机对信号的载波相位锁定值PLI：

其中

表示对I支路连续N个相干积分后的即时支路信号进行累加，

表示对Q支路连续N个相干积分后的即时支路信号进行累加。N的取值根据实际情况确定，与接收机设计有关。在载波相位锁定时，PLI的值将趋于1.0。

再利用下式计算载波相位锁定检测值P：

其中，α为锁定门限，通常取值大于0.85，小于0.95。当PLI＞α时认为相位锁定，否则失锁。

GNSS接收机对接收信号锁定的另外一个判据是导航电文正确接收，利用下式计算电文校验正确标志C：

综上，对GNSS接收机内部正确锁定信号的判决，可以由三个参数来表示，分别是接收信号的伪码相位锁定检测值D，载波相位锁定检测值P和电文校验正确标志C。这三个参数的组合，可以用来表征伪距测量值不确定度，即GNSS 接收机锁定信号的质量好坏，锁定信号好则伪距测量值准确，锁定信号差则伪距测量值不准确，后续导航定位时利用伪距测量值进行定位。所以伪距测量值的精度影响导航定位精度。

对于GNSS接收机接收的第j颗可见卫星的导航信号，构造联合权重系数w_j如下：

w_j＝D_j·P_j·C_j

其中，D_j为第j颗可见卫星的伪码相位锁定检测值，P_j为第j颗可见卫星的载波相位锁定检测值，C_j为第j颗可见卫星的电文校验正确标志。

联合权重矩阵W＝diag(w₁,w₂,…,w_j,…,w_J)，W是以w_j为对角线元素的对角矩阵(J为可见卫星数目)，表征GNSS接收机对各颗可见卫星的信号锁定性能，同时也体现了伪距测量值的不确定度，当w_j越小，则对应卫星的伪距测量值的不确定度越大，测量效果越差，反之则越好。

第二步，构造因子图

构造因子图如图2所示，具体构建过程参照文献[6]：在因子图中，包括两种因子节点；一种是过程模型因子节点，即INS节点；一种是测量模型因子节点，即GNSS节点。在任意一个测量时刻i，因子图系统状态量为X_i， X_i＝{x₁,x₂…x_i}，x₁表示第一个测量时刻，GNSS/INS组合导航系统状态量，x₂表示第二个测量时刻，GNSS/INS组合导航系统状态量，依次论推，则可知X_i包括了截至到时刻iGNSS/INS组合导航系统状态量的所有值。

首先，计算过程模型因子节点。

假设在测量时刻i，已知IMU在B系(Body frame，载体坐标系)的输出测量值为加速度f_i ^b和角度

令

为IMU在B系三个坐标轴上的加速度计测量偏差，ε^b _i为IMU在B系三个坐标轴上的陀螺仪测量偏差；IMU输出测量值的时间间隔为Δt。为了降低计算负担，采用预积分的方法。选定INS因子节点的更新周期为时间间隔T，时间间隔T的大小为GNSS接收机输出测量值的时间间隔。在时间间隔T内对IMU测量值在B系下的速度、姿态角变化量分别进行预积分，得到下述结果：

其中，

分别为B系下在测量时刻i，经过时间间隔T积分后得到的 GNSS/INS组合导航系统的速度、姿态角的变化量；M＝T/Δt，根据接收机的参数设置可知M的取值必定为正整数。由以上预积分求出的变化量，进一步可以求得测量时刻i+1时，GNSS/INS组合导航系统的速度、位置、姿态角的估计值：

其中,

分别为GNSS/INS组合导航系统在E系(Earth-CenteredEarth-Fixed,地球地固坐标系)下测量时刻i时的位置、速度、姿态角，由INS 递推的原理可知这些为已知量；

表示从测量时刻i到测量时刻i+1，由B 系变换到E系的旋转矩阵；g^e表示重力矢量，在时间间隔T内为一个常量。值得说明的是，测量时刻i到测量时刻i+1的时间间隔是T。

设过程模型因子节点(即INS节点)的状态量

上标“T”表示转置运算，则由前面的计算得到测量时刻i+1的INS节点的状态量的估计值

其次，构造测量模型因子节点，即GNSS节点。

已知测量时刻i，GNSS接收机的测量值

其中

ρ_i,j为接收机在测量时刻i对第j颗可见卫星的伪距测量值，

为ρ_i,j对应的伪距率。

由导航电文可以得到测量时刻i第j颗卫星的在E系下的位置

及对应的速度

则测量时刻i所有可见卫星的位置矢量和速度矢量为

和

设GNSS接收机在测量时刻i的状态量为

其中，δ_i为GNSS接收机的本地时间和GPS系统时间的差，

为δ_i的变化率。

则GNSS接收机的测量方程可以写为：

其中，c表示光速，

表示接收机与卫星在视线方向上的位置大小，

表示接收机与卫星在视线方向上的速度大小。

上式即表示估计的伪距与伪距率。

第三步，因子图的计算由上述推导得，测量时刻i+1，因子图系统状态量X_i+1的最大后验概率值X_i+1 ^MAP的表达式为：

其中，

表示求矩阵的马氏距离(mahalanobis distance)的平方。当GNSS 接收机被短时遮挡或干扰时，输出的伪距测量值噪声变大，此时用接收机输出的联合权重矩阵W来调整矩阵

对应的协方差矩阵Λ_k，即使用协方差矩阵Λ′_k求矩阵

的马氏距离的平方，Λ′_k＝W^-1·Λ_k。通过这个调整，得使协方差中的元素大小实时对应实际接收机的工作环境，从而获得更符合实际测量值噪声特性的数据。

利用基于平滑和建图的标准C++的库GTSAM(Georgia Tech Smoothing andMapping)，采用第二版增量平滑与地图构建(Incremental Smoothing And Mapping 2，iSAM2)算法(见参考文献7)对上述公式(1)进行求解计算，可以得到系统导航解。

下面将通过GNSS/INS紧组合实验对所提方法进行验证。实验数据采用模拟生成的数据，其中GNSS接收机的输出速率为1Hz，IMU输出速率为100Hz，时间间隔T＝1s。GNSS与INS具体的仿真参数如表1所示。

表1

¹PSD：功率谱密度

GNSS/INS组合导航系统放在载体上，载体为汽车，载体的真实运动轨迹如图3所示，仿真轨迹坐标采用E系，从坐标原点出发，运动持续时间为200秒，整个轨迹分别在70秒和120秒处进行了两次45度转弯。

仿真实验一将本发明与现有的卡尔曼滤波、因子图方法进行比较，载体运动轨迹选取0-200秒区间，为模拟复杂环境下多星受遮挡或干扰时的定位情况，在80-95秒时间增加第1-5颗可见卫星的基带导航信号噪声分量3-5dB不等，使卫星锁定结果变差，伪距测量误差增加。实验结果如图4所示，首先，因子图方法由于使用了所有历史观测数据，定位误差比卡尔曼更小；另外，在GNSS 数据出现异常时，本发明由于能自适应调节测量噪声协方差矩阵，因此各方向上的定位结果皆优于卡尔曼滤波算法与因子图方法。图4(b)为三种方法速度误差实验结果，与因子图方法和卡尔曼滤波相比，本发明速度估计误差更小。总的来说，本发明在位置与速度估计方面都体现出更好性能。

仿真实验二将本发明与文献[4]提供的方法进行比较。载体的运动轨迹与仿真实验一相同，GNSS与INS各参数保持不变，不同的是可观测卫星为4颗。在 80-95秒时间内加大第一颗可见卫星基带信号对应的噪声分量3bB，使得测量的伪距与伪距率误差变大。在实验中，文献[4]的第一颗可见卫星的伪距与伪距率的权重设为ω，其余可见卫星权重值均取1。分别取ω为0、0.5和1进行仿真实验，定位和测速实验结果分别如图5(a)与(b)所示。当ω＝0时表示此时 GNSS因子中对于故障卫星测量值不可见，参与状态估计的卫星数为3，定位结果出现发散。ω＝1是表示对故障不作任何处理，而ω＝0.5则是降低故障在因子中的权重，所以权重值为0.5时的结果较优。另外，图中点画线为本发明的结果，与文献[4]中方法相比，本发明在三个坐标轴方向的定位和速度误差均小于文献[4]。原因是本文直接调整测量的协方差矩阵，而文献[4]的协方差矩阵不变，只是调整了参与代价函数计算的数据大小，且没有说明权重函数ω如何取值，因此无法得到确切的最优结果。所以总体来说，本发明在系统导航性能上优于文献[4]的方法。

参考文献

[1]J.L Crassidis,Sigma-point kalman filtering for integrated GPS andinertial navigation[C].IEEE Transactions on Aerospace and Electronic Systems,vol.42,no.2,pp.750-756,2006.

[2]秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航原理[M].第3版.西安:西北工业大学出版社，2015.

[3]王小旭,赵琳.自适应融合滤波算法及其在INS/GPS组合导航中的应用[J]. 宇航学报,2010,31(11):22-28.

[4]W Li,X Cui,and M.Lu.A robust graph optimization realization oftightly coupled GNSS/INS integrated navigation system for urbanvehicles.Tsinghua Science and Technology,vol.23,no.6,pp.724-732,2018.

[5]W Wen,X Bai,and Y Kan,et al.Tightly coupled integration via factorgraph and aided by fish-eye camera.IEEE Transactions on Vehicular Technology,vol.68,no.11,pp.10651-10662,Nov,2019.

[6]V Indelman,S Williams,M Kaess,F Dellaert.Information fusion innavigation systems via factor graph based incremental smoothing[J]. Roboticsand Autonomous Systems.61,pp.721-738,2013.

[7]M Kaess,H Johannsson,R Roberts,V Indelman,J Leonard,F Dellaert.iSAM2:Incremental smoothing and mapping using the Bayes tree[J].TheInternational Journal of Robotics Research,vol.31,no.2,pp.216-235, 2012.

Claims (3)

1.一种GNSS/INS组合导航系统的导航方法，应用于GNSS/INS组合导航系统工作在紧组合模式下，其特征在于，利用GNSS接收机的伪码相位锁定检测值D、载波相位锁定检测值P和电文校验正确标志C构造联合权重矩阵，对GNSS接收机伪距测量值的协方差矩阵进行修正；利用修正的GNSS接收机伪距测量值的协方差矩阵，通过因子图计算得到GNSS/INS组合导航系统的导航结果；其中，GNSS是指全球卫星导航系统，INS是指惯性导航系统，符号“/”是指与或者和的意思；

采用下述过程计算任意一颗可见卫星的伪码相位锁定检测值D：

假设在某个时刻，GNSS接收机接收到的I支路信号经过相干积分后，得到的超前支路信号、即时支路信号、滞后支路信号分别为I_E、I_P、I_L；Q支路信号经过相干积分后，得到的超前支路信号、即时支路信号、滞后支路信号分别为Q_E、Q_P、Q_L；

利用下式得到鉴相器输出的伪码相位误差控制量DLI为：

其中，d_c为相干器间隔；

利用下式计算归一化的伪码相位锁定检测值D：

其中，ε为接近零的正小数；

采用下述过程计算任意一颗可见卫星的载波相位锁定检测值P：

利用下式计算接收机对信号的载波相位锁定值PLI：

其中

表示对I支路连续N个相干积分后的即时支路信号进行累加，

表示对Q支路连续N个相干积分后的即时支路信号进行累加，N的取值根据实际情况确定；

再利用下式计算载波相位锁定检测值P：

其中，α为锁定门限，取值大于0.85且小于0.95；

采用下述过程计算任意一颗可见卫星的电文校验正确标志C：

采用下述过程构造联合权重矩阵：

对于GNSS接收机接收的第j颗可见卫星的导航信号，构造联合权重系数w_j如下：

w_j＝D_j·P_j·C_j

其中，D_j为第j颗可见卫星的伪码相位锁定检测值，P_j为第j颗可见卫星的载波相位锁定检测值，C_j为第j颗可见卫星的电文校验正确标志；

联合权重矩阵W＝diag(w₁,w₂,…,w_j,…,w_J)，W是以w_j为对角线元素的对角矩阵，J为可见卫星数目；

用接收机输出的联合权重矩阵W来调整矩阵

对应的协方差矩阵，求解因子图系统状态量的最大后验概率值的最小值，得到组合导航的结果；

其中，GNSS接收机在测量时刻i的状态量为

其中，δ_i为GNSS接收机的本地时间和GPS系统时间的差，

为δ_i的变化率；

分别为GNSS/INS组合导航系统在地球地固坐标系下测量时刻i时的位置、速度；

测量时刻k，GNSS接收机的测量值

其中，ρ_k＝(ρ_k,1,ρ_k,2,…,ρ_k,j…,ρ_k,J)^T，ρ_k,j为接收机在测量时刻k对第j颗可见卫星的伪距测量值，

为ρ_k,j对应的伪距率；

其中，c表示光速，

表示接收机与卫星在视线方向上的位置大小，

表示接收机与卫星在视线方向上的速度大小；测量时刻i所有可见卫星的位置矢量和速度矢量为

和

2.根据权利要求1所述的GNSS/INS组合导航系统的导航方法，其特征在于，计算过程模型因子节点时，采用预积分的方法，选定INS因子节点的更新周期为时间间隔T，时间间隔T的大小为GNSS接收机输出测量值的时间间隔。

3.一种GNSS/INS组合导航系统，其特征在于，采用权利要求1至2任一项所述的导航方法进行导航。

Images