CN109945860B - 一种基于卫星紧组合的ins和dr惯性导航方法与系统 - Google Patents

Abstract

本发明公开了一种基于卫星紧组合的INS和DR惯性导航方法与系统，其包括有：INS系统误差分析单元；航位推算误差分析单元；GBDT算法处理单元，用于处理卫星信号失锁情况下的Kalman观测值；紧组合导航滤波方程建立单元；基于GBDT算法改进的紧组合导航位置误差实验单元，用于在卫星信号丢失的情况下，进行基于GBDT算法改进的紧组合导航位置误差实验，检验系统在东、北、天等三个方向的位置误差。本发明将GBDT算法应用于紧组合导航系统，GBDT算法构建模型所需时间短，在卫星信号失锁情况下，仍有效预测紧组合导航系统滤波的观测值，确保导航系统的实时性，提高定位精度，可有效解决复杂环境下载车定位时间长而影响导航性能的问题。

Description

一种基于卫星紧组合的INS和DR惯性导航方法与系统

技术领域

本发明涉及惯性导航系统，尤其涉及一种基于卫星紧组合的INS和DR惯性导航方法与系统。

背景技术

惯性导航系统(Inertial Navigation System，INS)和全球导航卫星系统(GlobalNavigation Satellite System，GNSS)的组合是最常见的组合导航系统，其有效利用了GNSS和INS各自的优点，进行系统间的取长补短，能减小系统误差，提高系统精度。组合方式有松组合、紧组合、深组合等三种，目前大多数INS与GNSS组合导航是以松组合的方式，在GNSS信号正常时，组合导航系统能够提供良好的导航性能，但在可见卫星数目低于4颗、卫星信号失锁的情况下，GNSS无法提供载体的位置、速度，从而无法与INS组合导航，且滤波器输出发散，降低导航性能，同时地，随着测量时间增加，会出现惯导定位误差累积、陀螺仪、加速度传感器等器件测量精度下降而导致的INS性能变差等问题。而紧组合采用伪距及伪距率组合，能够得到稳定可靠的连续定位精度，导航实时性高，定位精度高。

紧组合导航中，只需对惯导输出的位置信息和GNSS接收机提供星历数据进行解算得出伪距和伪距率，所以在复杂环境下卫星信号短暂缺失时，紧组合仍能继续导航，但位置误差随时间增加而增大，滤波器输出逐渐发散，长时精度不高，在复杂环境下提升导航性能仍存在局限性。

现有紧组合导航技术中，请参见申请号为201310462886、公开号为103487820A、名称为“一种车载捷联_卫星紧组合无缝导航方法”的中国发明专利申请文件，该技术方案的缺陷在于，在高动态的复杂环境中，卫星信号易丢失，当卫星数小于4颗时，运用智能组合算法，其算法中卫星双频恢复技术，无法消除多路径误差，在城市建筑密集区定位精度较弱，尤其是双频定位所需定位时间较长，影响载车高速运行时的定位导航性能。

GBDT(Gradient Boosting Decision Tree，梯度提升决策树)算法，是一种迭代的决策树算法，其本质是基于残差学习，在逐次迭代中，残差向梯度方向不断减小，最终将所有迭代产生的模型累加得到最终的预测模型。GBDT可广泛应用于各种类型数据的处理，可用于非线性和线性的回归问题，与SVM(Support Vector Machine，支持向量机)相比有较高的准确率，对异常值的鲁棒性强，GBDT算法构建模型所需时间短，不需要花费大量时间进行数据预处理和仔细调整学习过程，能提供与现有数据相媲美的最佳预测能力，有效解决复杂环境下载车定位时间长而影响导航性能的问题。

发明内容

本发明要解决的技术问题在于，针对现有技术的不足，提供一种设计合理、实时性高、定位精度高、鲁棒性好、能有效改善复杂环境下载车高速运行时的导航性能的基于卫星紧组合的INS和DR惯性导航方法与系统。

为解决上述技术问题，本发明采用如下技术方案。

一种基于卫星紧组合的INS和DR惯性导航方法，其包括有：步骤S1，INS系统误差分析，选取东北天地理坐标系为导航参考坐标系，记为n系，当惯性模块按一定的方位安装到载车上时，认为载车坐标系与惯性坐标系重合，记为b系，惯导系统姿态、速度和位置误差方程经过推导并整理得到姿态误差方程、速度误差方程、位置误差方程；

步骤S2，航位推算误差分析；

步骤S3，GBDT算法；GBDT算法的过程包括有训练过程和预测过程两个部分。系统开始工作后，建立训练模型。

训练过程：

(1)在卫星信号正常(可见卫星数目≥4)时，采集确定GNSS数据集，初始化首层回归树。

γ＝ave(y_i|x_i∈R_j0)

式中，γ为首层回归树叶子节点，ave(·)为平均值函数，R_j0为首层回归树第j个节点，y_i为回归树输出值，x_i表示第j个节点区域值。

设训练样本个数为N，得到首层回归树函数f₀(x)：

其中，argmin表示使损失函数取最小值时的γ值，L(·,·)为损失函数，采用回归树输出值方差作为损失函数，如下所示。

L(y_i,fx_i))＝∑(y_i-f(x_i))²

(2)遍历回归树m＝1,2,……,M。M为回归树层数。

第一步，遍历训练数据集i＝1,2,……,N，并计算伪残差：

第二步，对叶子节点区域j＝1,2,……,J_m。J_m为第m层回归树的大小即节点个数，R_jm以r_im目标梯度提升拟合回归树；

第三步，遍历j＝1,2,……,J_m,计算使得损失函数最小化后的叶节点区域值：

式中，γ_im为第m层回归树叶子节点，f_m(x)为第m层回归树函数，R_jm为第m层回归树第j个节点。

第四步，更新回归树函数：

I(x∈R_jm)表示第j个节点的节点区域值

回归树函数定义为

其中T(x；Θ_m)表示回归树的回归函数，Θ_m表示回归函数输入参数，该参数定义为

各参数更新值如下：

(3)输出回归树叠加组合函数：

当卫星信号失锁时，以上一迭代过程的训练模型为预测模型。

预测过程：GBDT的预测输出公式如下：

步骤S4，建立紧组合导航滤波方程，建立状态方程、量测方程和卡尔曼滤波算法；

步骤S5，常规紧组合、基于GBDT算法改进的紧组合导航位置误差对比实验。

优选地，所述步骤S1中，导航参考坐标系是以载车为中心，将X轴和Y轴设置在载车上，X轴和Y轴分别指向东向和北向，Z轴沿天垂直方向。

优选地，所述步骤S1中，载车坐标系原点为载车的质心，X轴指向载车前端，Y轴沿载车的侧面方向，Z轴指向载车的上侧。

优选地，所述步骤S1中，惯性坐标系选择地球的中心做为坐标系原点，X轴和Y轴位于赤道平面中指向相对惯性空间不动的方向，Z轴沿着北极轴线方向。

优选地，所述步骤S5中，常规紧组合、基于GBDT算法改进的紧组合导航位置误差对比实验过程包括：采用MATLAB对紧组合导航系统和基于GBDT算法改进的紧组合导航系统进行对比测试研究，设系统工作时间为400秒，在150秒时人为进行卫星信号丢失(如：可见卫星数目小于正常值4，为3、2或1的某一数值)，分别进行常规紧组合导航系统位置误差测试实验与基于GBDT算法改进的紧组合导航位置误差测试实验，实验数据为东、北、天三个方向上的位置误差。

一种基于卫星紧组合的INS和DR惯性导航系统，其包括有：INS系统误差分析单元，用于对惯导系统姿态、速度和位置误差方程进行推导并整理得出姿态误差方程、速度误差方程和位置误差方程；航位推算误差分析单元，用于提供航位推算的位置误差方程式；GBDT算法处理单元，用于处理卫星信号失锁情况下的Kalman观测值。系统开始工作后，建立GBDT训练模型，当卫星信号正常时，GNSS接收机数据与惯导数据作为训练模型的输入值，在训练模型中进行数据分类回归学习；当卫星信号失锁时，以上一迭代后的训练模型为预测模型，惯导数据作为预测模型的输入值，预测GNSS接收机数据，输出紧组合导航滤波的观测值；紧组合导航滤波方程建立单元，用于建立状态方程、量测方程和卡尔曼滤波算法；基于GBDT算法改进的紧组合导航位置误差实验单元，用于在卫星信号丢失的情况下，进行基于GBDT算法改进的紧组合导航位置误差实验，检验系统的在东、北、天等三个方向的位置误差。

本发明公开的基于卫星紧组合的INS和DR惯性导航方法中，利用惯导系统的信号作辅助信息可以改善卫星在城市复杂电磁干扰信号环境中的抗干扰能力，同时利用航位推算以及卫星的位置、速度信息可以校正惯导系统的积累误差；当卫星信号失锁时，利用GBDT算法预测模型，处理紧组合导航滤波模型的观测值，INS惯导系统在DR、GBDT辅助下能继续工作，提供高精度的导航信息。将GBDT(梯度提升决策树)算法应用于紧组合导航系统，GBDT算法构建模型所需时间短，不需要花费大量时间进行数据预处理和仔细调整学习过程，在卫星信号失锁情况下，能提供与现有数据相媲美的最佳预测能力，有效预测出紧组合模式下组合导航系统滤波的观测值，保证导航系统定位的实时性的同时，提高预测精度，可有效解决复杂环境下GNSS信号丢失和载车定位时间长而影响导航性能的问题。

附图说明

图1为本发明基于卫星紧组合的INS和DR惯性导航方法与系统的原理图；

图2为本发明基于卫星紧组合的INS和DR惯性导航方法的流程图；

图3为卫星信号正常时，GBDT算法训练原理图；

图4为卫星信号失锁时，GBDT算法预测原理图；

图5为GBDT算法流程图；

图6为常规紧组合、基于GBDT算法改进的紧组合对比实验的东向位置误差；

图7为常规紧组合、基于GBDT算法改进的紧组合对比实验的北向位置误差；

图8为常规紧组合、基于GBDT算法改进的紧组合对比实验的天向位置误差。

具体实施方式

下面结合附图和实施例对本发明作更加详细的描述。

本发明公开了一种基于卫星紧组合的INS和DR惯性导航方法，请参见图2，其包括有：

步骤S1，INS系统误差分析，选取东北天地理坐标系为导航参考坐标系，记为n系，当惯性模块按一定的方位安装到载车上时，认为载车坐标系与惯性坐标系重合，记为b系，惯导系统姿态、速度和位置误差方程经过推导并整理得到：

姿态误差方程：

式中，f¹为测量的载车比力矢量，

为导航参考坐标系n系相对于载车坐标系b系的转动角速率，δvⁿ是地球参考速度误差矢量，

是载车坐标系到导航坐标系的方向余弦矩阵，称为姿态矩阵，e^b是载车坐标系的偏心率；

速度误差方程：

其中：δp＝[δL δλ δh]^T，式中，δL是纬度微分，δλ是经度微分，δh是高程微分，A₁～A₇为矩阵；

式中，δp是位置误差，fⁿ是n系的比力矢量，

为加速度计偏置在载车坐标系上的投影；

位置误差方程：

δp＝A₅δvⁿ+A₇δp；

其中：δp＝[δL δλ δh]^T；A₁～A₇为矩阵；

A₁₃＝A₁+A₃；

A₅＝vⁿ×(A₁+A₃)；

式中R_M为子午半径，R_N卯酉半径，h为海拔，λ为经度，L是纬度，ω_ie是地球的自转角速率，ω_iesinL和ω_iecosL是地球旋转分量，

为地球坐标系相对于惯导坐标系i系的转动角速率，

为导航坐标系n系相对于地球坐标系e系的转动角速率，vⁿ是地球参考速度，

分别为其在东北方向的分量；

步骤S2，航位推算误差分析，将里程计坐标系记为m系，其oy轴位于与载车车轮相接触的地平面内，并且指向载车行驶正前方，oz轴垂直于地平面向上为正，ox轴指向右方，里程计在一段时间Δt＝t_j-t_j-1；t＝0,1,2,...,j-1,j,...∞内测量的里程增量ΔS_j在m系的投影写成矢量形式为：

设定载车坐标系轴与里程计坐标系对应轴相平行，可得里程增量ΔS_j在导航坐标系n系上的投影为：

其中

是t_j-1时刻的姿态矩阵；

求得航位推算的纬度(LA)、经度(LD)和高程(HD)递推算法：

式中，L_A(j-1)、L_D(j-1)、H_D(j-1)分别为j-1时刻航位推算的纬度、经度、高程，

分别为里程增量在导航坐标系上的投影在东、北、天方向的分量；

航位推算误差源于姿态角误差f和里程计刻度系数误差δK_D；

由里程计增量式得出里程计增量的实际计算公式：

其中，I_3×3表示三阶单位阵，展开上式并忽略关于f和δK_D的高阶小量得到：

比较里程计增量式和上式可得t_j-1时刻到t_j时刻航位推算误差方程：

步骤S3，GBDT算法；GBDT算法分为训练过程、预测过程两部分。系统开始工作后，建立GBDT训练模型，卫星信号正常时，以卫星接收机语句数据与惯导系统的数据作为训练模型的输入，在训练模型内部学习；卫星信号失锁(接收机无可用数据输出，如卫星数目<4时)时，以上一迭代过程的训练模型为预测模型，以惯导系统的数据作为预测模型的输入，预测模型输出值作为紧组合导航滤波方程的观测值。

训练过程：

(1)在卫星信号正常(可见卫星数目≥4)时，采集确定GNSS数据集，初始化首层回归树。

γ＝ave(y_i|x_i∈R_j0)

式中，γ为首层回归树叶子节点，ave(·)为平均值函数，R_j0为首层回归树第j个节点，y_i为回归树输出值，x_i表示第j个节点区域值。

设训练样本个数为N，得到首层回归树函数f₀(x)：

其中，argmin表示使损失函数取最小值时的γ值，L(·,·)为损失函数，采用回归树输出值方差作为损失函数，如下所示。

L(y_i,f(x_i))＝∑(y_i-f(x_i))²

(2)遍历回归树m＝1,2,……,M。M为回归树层数。

第一步，遍历训练数据集i＝1,2,……,N，并计算伪残差：

第二步，对叶子节点区域j＝1,2,……,J_m。J_m为第m层回归树的大小即节点个数，R_jm以r_im目标梯度提升拟合回归树；

第三步，遍历j＝1,2,……,J_m,计算使得损失函数最小化后的叶节点区域值：

式中，γ_im为第m层回归树叶子节点，f_m(x)为第m层回归树函数，R_jm为第m层回归树第j个节点。

第四步，更新回归树函数：

I(x∈R_jm)表示第j个节点的节点区域值

回归树函数定义为

其中T(x；Θ_m)表示回归树的回归函数，Θ_m表示回归函数输入参数，该参数定义为

各参数更新值如下：

(3)输出回归树叠加组合函数：

预测过程：GBDT的预测输出公式如下：

步骤S4，建立紧组合导航滤波方程；

步骤S5，常规紧组合、基于GBDT算法改进的紧组合导航位置误差对比实验。

上述方法中，将GBDT(梯度提升决策树)算法应用于组合导航系统，基于紧组合的Kalman滤波算法与GBDT相结合，在卫星信号失锁情况下，能有效预测紧组合模式下组合导航系统滤波的观测值，保证导航系统定位的实时性的同时，提高预测精度，有效解决了复杂环境下动态载体的GNSS信号丢失时的导航问题，可以改善卫星在城市复杂电磁干扰信号环境中的抗干扰能力。紧组合导航系统可以有效地利用各自的优点，进行系统间的取长补短以减少系统误差影响，提高导航系统的性能。

作为一种优选方式，所述步骤S1中，导航参考坐标系是以载车为中心，将X轴和Y轴设置在载车上，X轴和Y轴分别指向东向和北向，Z轴沿天垂直方向。

进一步地，所述步骤S1中，载车坐标系原点为载车的质心，X轴指向载车前端，Y轴沿载车的侧面方向，Z轴指向载车的上侧。

本实施例中，所述步骤S1中，惯性坐标系选择地球的中心做为坐标系原点，X轴和Y轴位于赤道平面中指向相对惯性空间不动的方向，Z轴沿着北极轴线方向。

作为一种优选方式，所述步骤S2中，为了把位置误差表示成地理坐标的形式，将纬度(LA)、经度(LD)和高程(HD)的表达式进行微分并整理得到：

其中：δP_Dj＝[δL_Aj δL_Dj δH_Dj]^T；A_D1、A_D2为矩阵，

L_Aj-1、H_Dj-1分别是t_j-1时刻的纬度、高程。

本实施例中，所述步骤S4中，建立紧组合导航滤波方程的过程包括：

步骤S40，建立状态方程，对惯导系统误差方程进行离散化，选取惯导状态变量：

则惯导系统误差方程如下：

式中，A_INS为惯导系统参数矩阵，w_INS为惯导系统速度矩阵：

式中，

为基于惯导系统参数转置；

通过典型的离散化处理得到惯导离散系统误差方程：

x_INS(k)＝A_INS(k,k-1)x_INS(k-1)+w_INS(k)；

E[w_INS(k)]＝0_15×1；

设定航位推算更新周期与惯导更新周期一致，并且卡尔曼滤波周期和航位推算更新周期一致，选取航位推算状态变量：

此处假定里程计表度系数误差δK_D是随机常值误差，所以有：

GNSS状态方程：

式中，W_G(t)为系统状态白噪声，G_G(t)为系统状态噪声，X_G(t)为状态参数阵，A_G(t)为系统状态的转移矩阵。

综合整理后可得INS/DR和GNSS紧组合导航系统的状态方程：

x(k)＝A(k,k-1)x(k-1)+ω(k)；

其中，

T_clk为接收机时钟漂移相关时间常数；

步骤S41，量测方程的建立，以校正后的惯导和GNSS接收机的伪距、伪距率之差作为卡尔曼滤波器的量测，即有：

z(k)＝p_INS-p_DR-p_GNSS＝δp_k-δp_Dk-rⁿ-δp_uk-εⁿ

＝H(k)x(k)+v(k)+ε_ρ(k)；

式中，p_INS为INS惯性导航解算的伪距，p_DR为DR航位推算测得的伪距，p_GNSS为GNSS接收机的伪距，δp_k为惯导伪距量测值，δp_Dk为航位推算伪距量测值，rⁿ为卫星与接收机之间的几何距离，δp_uk为GNSS伪距量测值，εⁿ为剩余残差，H(k)为k时刻的量测矩阵，x(k)为k时刻的实际量测值，v(k)为k时刻的量测噪声，ε_ρ(k)为k时刻的伪距量测值。

步骤S42，卡尔曼滤波算法，根据上述建立的状态方程和量程方程，可以得到标准的卡尔曼滤波过程，即时间传播方程：

P(k|k-1)＝A(k-1)P(k-1|k-1)A^T(k-1)+Q(k-1)；

量测更新方程：

K(k)＝P(k|k-1)H^T(k)/[H(k)P(k|k-1)H^T(k)+R(k)]；

P(k|k)＝[I-K(k)H(k)]P(k|k-1)；

其中：

为k时刻的预测值，A(k-1)为k-1时刻的状态转移矩阵矩阵，

为k-1时刻的最优估计值，P(k|k-1)为

的协方差矩阵，P(k-1|k-1)为

的协方差矩阵，Q(k-1)为状态噪声ω(k-1)的协方差矩阵，K(k)为k时刻的系统增益矩阵，H(k)为k时刻的量测矩阵，R(k)为量测噪声v(k)的协方差矩阵，

为k时刻的最优估计值，z(k)为k时刻的量测值，P(k|k)为

的协方差矩阵；I为单位矩阵。

本实施例中，所述步骤S5中，常规紧组合、基于GBDT算法改进的紧组合导航位置误差对比实验过程包括：采用MATLAB对紧组合导航系统和基于GBDT算法改进的紧组合导航系统进行对比测试研究，设系统工作时间为400秒，在150秒时人为进行卫星信号丢失(如：可见卫星数目小于正常值4，为3、2或1的某一数值)，分别进行常规紧组合导航系统位置误差测试实验与基于GBDT算法改进的紧组合导航位置误差测试实验，实验数据为东、北、天三个方向上的位置误差。

为了更好地描述本发明的技术方案，本发明还公开了一种基于卫星紧组合的INS和DR惯性导航系统，其原理方框图，请参见图1，基于卫星紧组合的INS和DR惯性导航系统，将GNSS接收机的伪距、伪距率与惯导解算出的伪距、伪距率进行组合，系统开始工作后，建立GBDT训练模型，卫星信号正常时，数据输入GBDT训练模型进行数据回归分类；卫星信号失锁时，以上一迭代后的训练模型为GBDT预测模型，预测模型输出值作为Kalman滤波的预测观测值，经过紧组合导航滤波算法，计算得系统的误差估计值，将误差估计值反馈到惯性导航系统中，对惯导进行校零，以提高导航系统的定位精度。

本发明公开的基于卫星紧组合的INS和DR惯性导航系统，其系统结构，请参见图1和图2，其包括有：

INS系统误差分析单元，用于对惯导系统姿态、速度和位置误差方程进行推导并整理得出姿态误差方程、速度误差方程和位置误差方程；

航位推算误差分析单元，用于提供航位推算的位置误差方程式；

GBDT算法处理单元，用于处理卫星信号失锁情况下的Kalman观测值。系统开始工作后，建立GBDT训练模型，当卫星信号正常时，GNSS接收机数据与惯导数据作为训练模型的输入值，在训练模型中进行数据分类回归学习；当卫星信号失锁时，以上一时刻训练完成的训练模型为预测模型，以惯导数据作为预测模型的输入值，输出观测值，该观测值作为紧组合导航滤波的输入观测值；

紧组合导航滤波方程建立单元，用于建立状态方程、量测方程和卡尔曼滤波算法；

基于GBDT算法改进的紧组合导航位置误差实验单元，用于在卫星信号丢失的情况下，进行基于GBDT算法改进的紧组合导航位置误差实验，检验系统的在东、北、天等三个方向的位置误差。

本发明公开的基于卫星紧组合的INS和DR惯性导航系统，卫星信号正常时，GBDT算法训练原理图，请参见图3。GNSS接收机和INS/DR惯导系统的伪距、伪距率之差作为GBDT训练模型的训练输入值，模型逐层迭代计算伪残差，梯度提升决策能力，为变更为预测模型作准备，伪距和伪距率之差作为紧组合导航滤波算法的观测值，滤波输出误差估计值对系统进行校零。

本发明公开的基于卫星紧组合的INS和DR惯性导航系统，卫星信号失锁时，GBDT算法预测原理图，请参见图4。解算INS/DR惯导系统的速度、姿态、位置信息得到伪距、伪距率，作为GBDT预测模型的输入值，以上一迭代后得到的GBDT训练模型为预测模型，预测模型的输出值作为紧组合导航滤波器的观测值，滤波器输出校正惯导系统误差。

本发明公开的基于卫星紧组合的INS和DR惯性导航系统，GBDT算法流程图，请参见图5。系统开始工作后，建立GBDT训练模型，GNSS信号正常时采集确定数据集，首先初始化首层回归树，其次迭代所有回归树，遍历逐层训练数据集计算伪残差，梯度提升拟合回归树，且遍历回归树叶子节点同时计算叶子节点区域值，更新回归树函数，直至已遍历所有回归树，输出训练模型；GNSS信号失锁时，以上一迭代后的训练模型作为预测模型，以惯导系统伪距、伪距率作为GBDT预测模型的输入，从而输出预测观测值。

本发明公开的基于卫星紧组合的INS和DR惯性导航系统，其常规紧组合、基于GBDT改进的紧组合导航系统对比实验，请参见图6至图8，比较分析两个实验结果，在150秒时刻人为设定进行卫星信号丢失的情况下，对比常规紧组合导航系统以及基于GBDT的紧组合导航系统的东、北、天向位置误差。

东向位置误差曲线，请参见图6，卫星失锁后常规紧组合的东向位置误差波动较大，在150s时误差逐渐上升，160s时误差达到峰值，直到170s才趋于稳定，花费时间较长。而基于GBDT的紧组合在卫星失锁时波动较小，失锁前后误差均维持在一定水平，稳定性较高；北向位置误差曲线图，请参见图7，卫星失锁后常规紧组合的北向位置误差较大，在150s-200s区间内误差变化较大，200s后两者的误差趋于一致；天向位置误差曲线图，请参见图8，卫星失锁后常规紧组合的天向位置误差波动较大，从150s开始，常规紧组合的误差值比基于GBDT的紧组合误差值波动增大，在150s-200s区间内多次出现较大值，210s后常规紧组合的误差值逐渐回归正常。基于GBDT改进的紧组合导航系统位置误差在卫星丢失时也有波动的情况，但波动幅度较常小，在一定时间内，相比常规紧组合导航位置误差小很多，定位信息比较精确。

综上，可以得出当卫星信号丢失时，基于GBDT改进的紧组合导航系统仍能继续工作，在有效时间内提供高精度的导航信息。

本发明公开的基于卫星紧组合的INS和DR惯性导航方法与系统，为方便阅读，其将上述过程整合后，可参考如下实施例：

实施例一

步骤S1，INS系统误差分析：

选取东北天(Earth-fixed Coordinate System，ENU)地理坐标系为导航参考坐标系(此坐标系是以载车为中心，X轴和Y轴在载车上。X轴和Y轴分别指向东向和北向，Z轴沿天垂直方向)，记为n系(Navigation Frame)，当惯性模块按一定的方位安装到载车上时，认为载车坐标系(载车坐标系原点为载车的质心，X轴指向载车前端，Y轴沿载车的侧面方向，Z轴指向载车的上侧。)与惯性坐标系(惯性坐标系选择地球的中心做为坐标系的原点，X轴和Y轴位于赤道平面中指向相对惯性空间不动的方向，Z轴沿着北极轴线方向。这个坐标系是不转动的。)重合，记为b系(Body Frame)。惯导系统姿态、速度和位置误差方程经过推导并整理可得，姿态误差方程：

式中，f为测量的载车比力矢量；

为导航参考坐标系相对于惯性坐标系的转动角速率；

δvⁿ是地球参考速度误差矢量；

是载车坐标系到导航坐标系的方向余弦矩阵，称为姿态矩阵；

e^b是载车坐标系的偏心率。

速度误差方程：

式中，δp是位置误差；

fⁿ是n系的比力矢量；

为加速度计偏置在载车坐标系上的投影。

位置误差方程：

δp＝A₅δvⁿ+A₇δp..............(74)

其中：δp＝[δL δλ δh]^T；A₁～A₇为矩阵；

A₁₃＝A₁+A₃；

A₅＝vⁿ×(A₁+A₃)；

式中R_M为子午半径；R_N卯酉半径；h为海拔；λ为经度；L是纬度；ω_ie是地球的自转角速率；ω_iesinL和ω_iecosL是地球旋转分量；

为地球坐标系相对于惯导坐标系i系的转动角速率；

为导航坐标系n系相对于地球坐标系e系的转动角速率；vⁿ是地球参考速度(地速)，

分别为其在东北方向的分量。

步骤S2，航位推算误差分析：

里程计坐标系记为m系，它的oy轴在和载车车轮相接触的地平面内，并且指向载车行驶正前方，oz轴垂直于地平面向上为正，ox轴指向右方。里程计在一小段时间Δt＝t_j-t_j-1内测量的里程增量ΔS_j在m系的投影写成矢量形式为：

假设载车坐标系轴和里程计坐标系对应轴相平行，可得里程增量ΔS_j在导航坐标系n系上的投影为：

其中

是t_j-1时刻的姿态矩阵。

和惯导位置更新算法类似，可求得航位推算的纬度(LA)、经度(LD)和高程(HD)递推算法：

式中，L_A(j-1)、L_D(j-1)、H_D(j-1)分别为j-1时刻航位推算的纬度、经度、高程。

分别为里程增量在导航坐标系上的投影在东、北、天方向的分量。

航位推算误差主要源于姿态角误差f和里程计刻度系数误差δK_D，其中里程计刻度系数误差与车轮气压和路面状况等因素有关。

由里程计增量(85)式里程计增量的实际计算公式：

其中I_3×3表示三阶单位阵，展开上式并忽略关于f和δK_D的高阶小量得：

比较(85)和(90)式可得t_j-1时刻到t_j时刻航位推算误差方程：

此式即是航位推算的位置误差方程式，为了把位置误差表示成地理坐标的形式将(86)(87)(88)式微分并整理得：

其中：δP_Dj＝[δL_Aj δL_Dj δH_Dj]^T；A_D1、A_D2为矩阵，

L_Aj-1、H_Dj-1分别是t_j-1时刻的纬度、高程。

步骤S3，GBDT算法；GBDT算法分为训练过程、预测过程两部分。系统开始工作后，建立GBDT训练模型，当卫星信号失锁时，以上一迭代过程的训练模型为GBDT预测模型。

训练过程：

(1)在卫星信号正常(可见卫星数目大于等于4)时，采集确定GNSS数据集，初始化首层回归树。

γ＝ave(y_i|x_i∈R_j0)

式中，γ为首层回归树叶子节点，ave(·)为平均值函数，R_j0为首层回归树第j个节点，y_i为回归树输出值，x_i表示第j个节点区域值

设训练样本个数为N，得到首层回归树函数f₀(x)：

其中，argmin表示使损失函数取最小值时的γ值，L(·,·)为损失函数，采用回归树输出值方差作为损失函数，如下所示。

L(y_i,f(x_i))＝∑(y_i-f(x_i))²

(2)遍历回归树m＝1,2,……,M。M为回归树层数。

第一步，遍历训练数据集i＝1,2,……,N，并计算伪残差：

第二步，对叶子节点区域j＝1,2,……,J_m。J_m为第m层回归树的大小即节点个数，R_jm以r_im目标梯度提升拟合回归树；

第三步，遍历j＝1,2,……,J_m,计算使得损失函数最小化后的叶节点区域值：

式中，γ_im为第m层回归树叶子节点，f_m(x)为第m层回归树函数，R_jm为第m层回归树第j个节点。

第四步，更新回归树函数：

I(x∈R_jm)表示第j个节点的节点区域值

回归树函数定义为

其中T(x；Θ_m)表示回归树的回归函数，Θ_m表示回归函数输入参数，该参数定义为

各参数更新值如下：

(3)输出回归树叠加组合函数：

预测过程：GBDT的预测输出公式如下：

步骤S4，建立紧组合导航滤波方程：

步骤S40，状态方程的建立；

上述(72)(73)(74)和(92)式就是INS/DR组合导航系统的状态方程，其中惯导误差方程是连续方程，而航位推算误差方程是离散方程。为了应用离散卡尔曼滤波方程，需对惯导系统误差方程进行离散化，选取惯导状态变量：

则惯导系统误差方程如下：

式中，

通过典型的离散化处理得到惯导离散系统误差方程：

x_INS(k)＝A_INS(k,k-1)x_INS(k-1)+w_INS(k).............(103)

E[w_INS(k)]＝0_15×1.........................................(104)

假设航位推算更新周期是惯导更新周期的一致，并且卡尔曼滤波周期和航位推算更新周期一致，选取航位推算状态变量：

这里假定里程计表度系数误差δK_D是随机常值误差，所以：

GNSS状态方程：

式中，W_G(t)为系统状态白噪声，G_G(t)为系统状态噪声，X_G(t)为状态参数阵，A_G(t)为系统状态的转移矩阵。

综合(92)式和(103)式可得INS/DR和GNSS紧组合导航系统的状态方程：

x(k)＝A(k,k-1)x(k-1)+ω(k).........(108)

其中，

T_clk为接收机时钟漂移相关时间常数；

步骤S41量测方程的建立，以航位推算后惯导和GNSS接收机伪距、伪距率之差作为卡尔曼滤波器的量测，即有：

z(k)＝p_INS-p_DR-p_GNSS＝δp_k-δp_Dk-rⁿ-δp_uk-εⁿ

＝H(k)x(k)+v(k)+ε_ρ(k)......(115)

式中，p_INS为INS惯性导航解算的伪距，p_DR为DR航位推算测得的伪距，p_GNSS为GNSS接收机的伪距，δp_k为惯导伪距量测值，δp_Dk为航位推算伪距量测值，rⁿ为卫星与接收机之间的几何距离，δp_uk为GNSS伪距量测值，εⁿ为剩余残差，H(k)为k时刻的量测矩阵，x(k)为k时刻的实际量测值，v(k)为k时刻的量测噪声，ε_ρ(k)为k时刻的伪距量测值。

(108)式和(115)式就是紧组合导航系统的卡尔曼滤波方程。

步骤S42，卡尔曼滤波算法，根据上述建立的状态方程和量程方程，可以得到标准的卡尔曼滤波过程，即时间传播方程：

P(k|k-1)＝A(k-1)P(k-1|k-1)A^T(k-1)+Q(k-1)....(120).

量测更新方程：

K(k)＝P(k|k-1)H^T(k)/[H(k)P(k|k-1)H^T(k)+R(k)]……(121)

P(k|k)＝[I-K(k)H(k)]P(k|k-1).........(123)

其中：

为k时刻的预测值；A(k-1)为k-1时刻的状态转移矩阵矩阵；

为k-1时刻的最优估计值；P(k|k-1)为

的协方差矩阵；P(k-1|k-1)为

的协方差矩阵；Q(k-1)为状态噪声ω(k-1)的协方差矩阵；K(k)为k时刻的系统增益矩阵；H(k)为k时刻的量测矩阵；R(k)为量测噪声v(k)的协方差矩阵；

为k时刻的最优估计值；z(k)为k时刻的量测值；P(k|k)为

的协方差矩阵；I为单位矩阵。

步骤S5，常规紧组合、基于GBDT算法改进的紧组合导航位置误差对比实验：本实施例采用了采用MATLAB对紧组合导航系统和基于GBDT算法改进的紧组合导航系统进行对比测试研究，设系统工作时间为400秒，在150秒时人为进行卫星信号丢失(如：可见卫星数目小于正常值4，为3、2或1的某一数值)，分别进行常规紧组合导航系统位置误差测试实验与基于GBDT算法改进的紧组合导航位置误差测试实验，实验数据为东、北、天三个方向上的位置误差。

本发明公开的基于卫星紧组合的INS和DR惯性导航方法与系统，其相比现有技术而言的有益效果在于，卫星接收到的定位数据能够对INS进行误差校正，使传统的惯性导航系统能够持续输出高精度的导航信息；INS/DR惯性导航系统与GNSS卫星系统采用紧组合模式，具有高抗干扰能力、高伪距测量精度及高导航定位能力；在可用星数目小于4的情况下，非紧组合导航必须转为纯惯性，而紧组合方式下，仍能提供有用定位数据对惯导系统进行误差校正，一定程度上提高了系统的冗余度；将GBDT(梯度提升决策树)算法应用于组合导航系统，在卫星信号失锁情况下，能有效地预测出紧组合模式下组合导航系统滤波的观测值，保证导航系统定位的实时性的同时，提高预测精度，有效解决了复杂环境下动态载体的GNSS信号丢失时的导航问题。

以上所述只是本发明较佳的实施例，并不用于限制本发明，凡在本发明的技术范围内所做的修改、等同替换或者改进等，均应包含在本发明所保护的范围内。

Claims (10)

1.一种基于卫星紧组合的INS和DR惯性导航方法，其特征在于，包括有：

步骤S1，INS系统误差分析，选取东北天地理坐标系为导航参考坐标系，记为n系，当惯性模块按一定的方位安装到载车上时，认为载车坐标系与惯性坐标系重合，记为b系，惯导系统姿态、速度和位置误差方程经过推导并整理得到：

姿态误差方程：

式中，f¹为测量的载车比力矢量，

为导航参考坐标系n系相对于载车坐标系b系的转动角速率，

是载车坐标系到导航坐标系的方向余弦矩阵，称为姿态矩阵，e^b是载车坐标系的偏心率；

速度误差方程：

其中：δp＝[δL δλ δh]^T，式中，δL是纬度微分，δλ是经度微分，δh是高程微分，A₁～A₇为矩阵；

位置误差方程：

δp＝A₅δvⁿ+A₇δp；

其中：δp＝[δL δλ δh]^T；A₁～A₇为矩阵；

A₁₃＝A₁+A₃；

A₅＝vⁿ×(A₁+A₃)；

式中，R_M为子午半径，R_N卯酉半径，h为海拔，λ为经度，L是纬度，ω_ie是地球的自转角速率，ω_ie sin L和ω_ie cos L是地球旋转分量，

为地球坐标系相对于惯导坐标系i系的转动角速率，

为导航坐标系n系相对于地球坐标系e系的转动角速率，vⁿ是地球参考速度，

分别为其在东北方向的分量；

步骤S2，航位推算误差分析，将里程计坐标系记为m系，其oy轴位于与载车车轮相接触的地平面内，并且指向载车行驶正前方，oz轴垂直于地平面向上为正，ox轴指向右方，里程计在一段时间Δt内测量的里程增量ΔS_j在m系的投影写成矢量形式为：

其中，Δt＝t_j-t_j-1；t＝0，1，2，...，j-1，j，...∞；

设定载车坐标系轴与里程计坐标系对应轴相平行，可得里程增量ΔS_j在导航坐标系n系上的投影为：

其中

是t_j-1时刻的姿态矩阵；

求得航位推算的纬度、经度和高程递推算法：

式中，L_A(j-1)、L_D(j-1)、H_D(j-1)分别为j-1时刻航位推算的纬度、经度、高程，

分别为里程增量在导航坐标系上的投影在东、北、天方向的分量；

航位推算误差源于姿态角误差f和里程计刻度系数误差δK_D；

由里程计增量式得出里程计增量的实际计算公式：

其中，I_3×3表示三阶单位矩阵，展开上式并忽略关于f和δK_D的高阶小量得到：

比较里程计增量式和上式可得t_j-1时刻到t_j时刻航位推算误差方程：

步骤S3，利用GBDT算法进行数据训练；

步骤S4，建立紧组合导航滤波方程；

步骤S5，常规紧组合、基于GBDT算法改进的紧组合导航位置误差对比实验。

2.如权利要求1所述的基于卫星紧组合的INS和DR惯性导航方法，其特征在于，所述步骤S1中，导航参考坐标系是以载车为中心，将X轴和Y轴设置在载车上，X轴和Y轴分别指向东向和北向，Z轴沿天垂直方向。

3.如权利要求1所述的基于卫星紧组合的INS和DR惯性导航方法，其特征在于，所述步骤S1中，载车坐标系原点为载车的质心，X轴指向载车前端，Y轴沿载车的侧面方向，Z轴指向载车的上侧。

4.如权利要求1所述的基于卫星紧组合的INS和DR惯性导航方法，其特征在于，所述步骤S1中，惯性坐标系选择地球的中心作为坐标系原点，X轴和Y轴位于赤道平面中指向相对惯性空间不动的方向，Z轴沿着北极轴线方向。

5.如权利要求1所述的基于卫星紧组合的INS和DR惯性导航方法，其特征在于，所述步骤S2中，为了把位置误差表示成地理坐标的形式，将纬度、经度和高程的表达式进行微分并整理得到：

其中：δP_Dj＝[δL_AjδL_Dj δH_Dj]^T；A_D1、A_D2为矩阵，

L_Aj-1、H_Dj-1分别是t_j-1时刻的纬度、高程。

6.如权利要求1所述的基于卫星紧组合的INS和DR惯性导航方法，其特征在于，所述步骤S3中，GBDT算法的过程包括有训练过程和预测过程两个部分，系统开始工作后，建立训练模型；

训练过程：

(1)在卫星信号正常时，采集确定GNSS数据集，初始化首层回归树；

γ＝ave(y_i|x_i∈R_j0)

式中，γ为首层回归树叶子节点，ave(·)为平均值函数，R_j0为首层回归树第j个节点，y_i为回归树输出值，x_i表示第i个节点区域值；

设训练样本个数为N，得到首层回归树函数f₀(x)：

其中，argmin表示使损失函数取最小值时的γ值，L(·，·)为损失函数，采用回归树输出值方差作为损失函数，如下所示：

L(y_i，f(x_i))＝∑(y_i-f(x_i))²；

(2)遍历回归树m＝1，2，......，M，M为回归树层数；

第一步，遍历训练数据集i＝1，2，......，N，并计算伪残差：

第二步，对叶子节点区域j＝1，2，......，J_m，J_m为第m层回归树的大小即节点个数，R_jm以r_im目标梯度提升拟合回归树；

第三步，遍历j＝1，2，......，J_m，计算使得损失函数最小化后的叶节点区域值：

式中，γ_im为第m层回归树叶子节点，f_m(x)为第m层回归树函数，R_jm为第m层回归树第j个节点；

第四步，更新回归树函数：

I(x∈R_jm)表示第j个节点的节点区域值；

回归树函数定义为

其中T(x；Θ_m)表示回归树的回归函数，Θ_m表示回归函数输入参数，该参数定义为

各参数更新值如下：

(3)输出回归树叠加组合函数：

当卫星信号失锁时，以上一迭代过程的训练模型为预测模型；

预测过程：GBDT的预测输出公式如下：

7.如权利要求1所述的基于卫星紧组合的INS和DR惯性导航方法，其特征在于，所述步骤S3中，系统开始工作后，建立GBDT训练模型，卫星信号正常时，以卫星接收机语句数据与惯导系统的数据作为训练模型的输入，在训练模型内部学习；卫星信号失锁时，以上一迭代过程的训练模型为预测模型，以INS/DR惯导系统的数据作为预测模型的输入值，预测模型的输出作为紧组合导航滤波方程的观测值。

8.如权利要求1所述的基于卫星紧组合的INS和DR惯性导航方法，其特征在于，所述步骤S4中，建立紧组合导航滤波方程的过程包括：

步骤S40，建立状态方程，对惯导系统误差方程进行离散化，选取惯导状态变量：

则惯导系统误差方程如下：

式中，A_INS为惯导系统参数矩阵，ω_INS为惯导系统速度矩阵：

式中，

为基于惯导系统参数转置；

通过典型的离散化处理得到惯导离散系统误差方程：

x_INS(k)＝A_INS(k，k-1)x_INS(k-1)+ω_INS(k)；

E[w_INS(k)]＝0_15×1；

设定航位推算更新周期与惯导更新周期一致，并且卡尔曼滤波周期和航位推算更新周期一致，选取航位推算状态变量：

此处假定里程计表度系数误差δK_D是随机常值误差，所以有：

GNSS状态方程：

式中，W_G(t)为系统状态白噪声，G_G(t)为系统状态噪声，X_G(t)为状态参数阵，A_G(t)为系统状态的转移矩阵；

综合整理后可得INS/DR和GNSS紧组合导航系统的状态方程：

x(k)＝A(k，k-1)x(k-1)+ω(k)；

其中，

T_clk为接收机时钟漂移相关时间常数；

步骤S41，量测方程的建立，以航位推算校正后惯导和GNSS接收机的伪距、伪距率之差作为卡尔曼滤波器的量测，即有：

z(k)＝p_INS-p_DR-p_GNSS＝δp_k-δp_Dk-rⁿ-δp_uk-εⁿ；

＝H(k)x(k)+v(k)+ε_ρ(k)；

式中，p_INS为INS惯性导航解算的伪距，p_DR为DR航位推算测得的伪距，p_GNSS为GNSS接收机的伪距，δp_k为惯导伪距量测值，δp_Dk为航位推算伪距量测值，rⁿ为卫星与接收机之间的几何距离，δp_uk为GNSS伪距量测值，εⁿ为剩余残差，H(k)为k时刻的量测矩阵，x(k)为k时刻的实际量测值，v(k)为k时刻的量测噪声，ε_ρ(k)为k时刻的伪距量测值；

步骤S42，卡尔曼滤波算法，根据上述建立的状态方程和量程方程，得到标准的卡尔曼滤波过程，即时间传播方程：

P(k|k-1)＝A(k-1)P(k-1|k-1)A^T(k-1)+Q(k-1)；

量测更新方程：

K(k)＝P(k|k-1)H^T(k)/[H(k)P(k|k-1)H^T(k)+R(k)]；

P(k|k)＝[I-K(k)H(k)]P(k|k-1)；

其中：

为k时刻的预测值，A(k-1)为k-1时刻的状态转移矩阵，

为k-1时刻的最优估计值，P(k|k-1)为

的协方差矩阵，P(k-1|k-1)为

的协方差矩阵，Q(k-1)为状态噪声ω(k-1)的协方差矩阵，K(k)为k时刻的系统增益矩阵，H(k)为k时刻的量测矩阵，R(k)为量测噪声v(k)的协方差矩阵，

为k时刻的最优估计值，z(k)为k时刻的量测值，P(k|k)为

的协方差矩阵；I为单位矩阵。

9.如权利要求1所述的基于卫星紧组合的INS和DR惯性导航方法，其特征在于，所述步骤S5中，常规紧组合、基于GBDT算法改进的紧组合导航位置误差对比实验过程包括：采用MATLAB对紧组合导航系统和基于GBDT算法改进的紧组合导航系统进行对比测试研究，系统工作时间为400秒，在150秒时人为进行卫星信号丢失，分别进行常规紧组合导航系统位置误差测试实验与基于GBDT算法改进的紧组合导航位置误差测试实验，实验数据为东、北、天三个方向上的位置误差。

10.一种基于卫星紧组合的INS和DR惯性导航系统，其特征在于，包括有：

INS系统误差分析单元，用于对惯导系统姿态、速度和位置误差方程进行推导并整理得出姿态误差方程、速度误差方程和位置误差方程；

航位推算误差分析单元，用于提供航位推算的位置误差方程式；

GBDT算法处理单元，用于处理卫星信号失锁情况下的Kalman观测值，系统开始工作后，建立GBDT训练模型，当卫星信号正常时，GNSS接收机数据与惯导数据作为训练模型的输入值，在训练模型中进行数据分类回归学习；当卫星信号失锁时，以上一时刻学习后的训练模型为预测模型，惯导数据作为预测模型的输入值，预测GNSS接收机数据，输出紧组合导航滤波的观测值；

紧组合导航滤波方程建立单元，用于建立状态方程、量测方程和卡尔曼滤波算法；

基于GBDT算法改进的紧组合导航位置误差实验单元，用于在卫星信号丢失的情况下，进行基于GBDT算法改进的紧组合导航位置误差实验，检验系统在东、北、天三个方向的位置误差。

Images