CN113109848B - 一种载体低速旋转条件下的导航定位方法 - Google Patents

Abstract

本发明公开了一种载体低速旋转条件下的导航定位方法及其应用系统，方法包括：构建卡尔曼滤波模型并设置参数；预测当前历元的状态估计值和状态估计值的均方误差阵；计算观测量残余和测量方差；若观测量残余与测量方差的比值超出门限值且当前历元的滤波深度超过阈值，根据观测量残余与测量方差的比值调整测量方差，并根据调整后的测量方差计算卡尔曼滤波增益，根据卡尔曼滤波增益进行校正；相较于传统的卡尔曼滤波算法可能导致定位结果发生跳变，本发明通过观测量残余与测量均方差的比值，调整超限观测量的方差，减小其滤波增益，达到弱化超限观测值的权重的目的，实现所有可见卫星均参与滤波，使得定位结果不会发生跳变，使得定位结果更加准确。

Description

一种载体低速旋转条件下的导航定位方法

技术领域

本发明涉及卫星导航接收机技术领域，特别涉及一种载体低速旋转条件下的导航定位方法。

背景技术

随着卫星导航接收机在炮弹等载体中的应用，接收机需要考虑载体旋转引起的可见卫星组合的变化带来的定位结果的跳变。以某型探空火箭为例，最低转速可到0.25转/秒，考虑到天线的接收信号范围及其方向图特性，为了保证能够收齐定位解算所需的卫星数，所以在弹体两侧对向各均安装一个天线。为了方便测量，采用双天线合路设计，但双天线合路会导致接收机接收到的信号能量降低约3dB，所以定位精度也会随之变差，这就对导航定位算法提出了更高的要求。

目前传统的导航定位算法有：最小二乘算法和卡尔曼(Kalman)滤波算法。最小二乘算法没有将不同时刻的定位值联系起来相互制约，因而定位结果通常显得粗糙又杂乱。卡尔曼滤波算法考虑了相邻时刻的位置相关性，但它要求外界提供的过程噪声方差和测量噪声方差是准确的，否则卡尔曼增益和状态估计等参量不会达到最优值，甚至还可能引起滤波器发散。例如，如果输入的过程噪声方差值小于真实值，则滤波增益值将会偏小，致使测量值中所含的信息不能被及时、全部地反映在状态估计值上，最终会导致滤波器发散。对各个时刻的过程噪声和测量噪声方差值进行准确估算是卡尔曼滤波算法中重要的一步。

发明内容

本发明旨在至少解决现有技术中存在的技术问题之一。为此，本发明提出一种载体低速旋转条件下的导航定位方法及其应用系统，利用观测量残余与测量均方差的比值，调整超限观测量的方差，减小其滤波增益，达到弱化超限观测值的权重的目的，实现所有可见卫星均参与滤波，使得定位结果不会发生跳变，并且结果更加准确。

本发明的第一方面，提供了一种载体低速旋转条件下的导航定位方法，包括以下步骤：

构建卡尔曼滤波模型并设置系统运行状态变量；

基于系统状态方程以及上一历元的状态估计值和状态估计值的均方误差阵，预测当前历元的状态估计值和状态估计值的均方误差阵；

根据所述当前历元的状态估计值和状态估计值的均方误差阵计算观测量残余和测量方差；

若所述观测量残余与所述测量方差的比值超出门限值且当前历元的滤波深度超过阈值，根据所述观测量残余与所述测量方差的比值调整所述测量方差，并根据调整后的所述测量方差计算卡尔曼滤波增益，根据所述卡尔曼滤波增益校正所述当前历元的状态估计值和状态估计值的均方误差阵；若所述观测量残余与所述测量方差的比值未超出门限值和/或当前历元的滤波深度未超过阈值，根据所述测量方差计算卡尔曼滤波增益，根据所述卡尔曼滤波增益校正所述当前历元的状态估计值和状态估计值的均方误差阵。

根据本发明的实施例，至少具有如下技术效果：

相较于传统的卡尔曼滤波算法直接剔除某一超限的观测量，可能导致定位结果发生跳变；本方法通过利用观测量残余与测量均方差的比值，调整超限观测量的方差，减小其滤波增益，达到弱化超限观测值的权重的目的，实现所有可见卫星均参与滤波，使得定位结果不会发生跳变，使得定位结果更加准确。

本发明的第二方面，提供了一种应用系统，所述应用系统中使用本发明第一方面所述的载体低速旋转条件下的导航定位方法。

根据本发明的实施例，至少具有如下技术效果：

相较于传统的卡尔曼滤波算法直接剔除某一超限的观测量，可能导致定位结果发生跳变；本系统通过利用观测量残余与测量均方差的比值，调整超限观测量的方差，减小其滤波增益，达到弱化超限观测值的权重的目的，实现所有可见卫星均参与滤波，使得定位结果不会发生跳变，使得定位结果更加准确。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为本发明实施例提供的一种载体低速旋转条件下的导航定位方法的流程示意图；

图2为本发明实施例提供的导航定位的卡尔曼算法流程框图；

图3为本发明实施例提供的实验结果示意图；

图4为本发明实施例提供的实验结果示意图；

图5为本发明实施例提供的实验结果示意图；

图6为本发明实施例提供的实验结果示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

第一实施例；

参照图1和图2，本发明的一个实施例，提供了一种载体低速旋转条件下的导航定位方法，本方法根据卫星的测量残余来设置其权重，即当某测量值表现为较大的测量残余时，通过调整它的测量噪声方差值，减小它的权重，实现所有可见卫星以适当的权重参与滤波运算。针对低速旋转条件下的载体，定位可用率提升大约10％，方法包括以下步骤：

步骤S100、构建卡尔曼滤波模型并设置系统运行状态变量。

步骤S100中的系统运行状态变量包括常系数状态转移矩阵A、测量关系矩阵C、过程噪声矩阵Q、状态向量x、观测向量y以及测量噪声R，具体的：

对于低速旋转的载体系统，其运行状态一般由载体的三个坐标分量上的位置、速度、加速度和接收机的钟差、频差所组成的一个11维状态向量x来描述，如下：

x＝[x v_x a_x y v_y a_y z v_z a_z δt_u δf_u] (1)

常系数状态转移矩阵A为：

式(2)中，T_S表示状态方程的差分步长，状态方程用于描述系统状态如何随时间变化。

测量关系矩阵C：

假设某时刻某卫星位置为(x^s,y^s,z^s)，用户位置为(x,y,z)，则星地距离D为

该卫星的测量关系矩阵为：

过程噪声矩阵Q为：

式(5)中，Q_p、Q_v、Q_a、Q_t、Q_f分别表示位置、速度、加速度、钟差、频差的噪声。

步骤S200、基于系统状态方程以及上一历元的状态估计值和状态估计值的均方误差阵，预测当前历元的状态估计值和状态估计值的均方误差阵。

这里给出一种示例：假设

代表第k-1个历元时卡尔曼滤波对系统状态x_k-1的最优估计值，则第k历元卡尔曼滤波对x_k的先验估计值

和先验估计值的均方误差阵

为：

其中，P_k-1表示上一历元的状态估计值的均方误差阵。

步骤S300、根据当前历元的状态估计值和状态估计值的均方误差阵计算观测量残余和测量方差。

计算公式如下：

其中y_k表示当前历元的观测值，R表示测量噪声。

步骤S400、若观测量残余与测量方差的比值超出门限值且当前历元的滤波深度超过阈值，则根据观测量残余与测量方差的比值调整测量方差，并进入步骤S500；否则，直接进入步骤S500。

作为一种可选的实施方式，按照以下公式调整测量方差：

其中σ表示测量方差；C表示测量关系矩阵；

表示当前历元的状态先验估计值的均方误差阵；M表示观测量残余与测量方差的比值，

作为一种可选的实施方式，经过实验发现，门限值设为5；阈值设为20时，得到的效果最佳，可参见后续实施例的实验结果。

步骤S500、根据测量方差计算卡尔曼滤波增益，根据卡尔曼滤波增益校正当前历元的状态估计值和状态估计值的均方误差阵。

校正公式如下：

其中K_k表示卡尔曼滤波增益，I表示单位矩阵。

步骤S600、当前历元的滤波处理完成，滤波深度加1，并跳转至步骤S200进行下一历元的滤波处理。

上述步骤S100至S600中以低速旋转的用户模型为例，与传统的卡尔曼滤波算法相比，定位结果可用率(三维定位误差小于30m)从89％提升到99％，三维测速误差(1σ)从0.19m/s提升到0.11m/s。针对静态低速旋转场景下，由于可见卫星组合发生变化引起的定位误差大的问题，本方法实施例提高了定位结果的正确率。

相较于传统的卡尔曼滤波算法直接剔除某一超限的观测量，可能导致定位结果发生跳变；本方法通过利用观测量残余与测量均方差的比值，调整超限观测量的方差，减小其滤波增益，达到弱化超限观测值的权重的目的，实现所有可见卫星均参与滤波，使得定位结果不会发生跳变，并且结果更加准确。

第二实施例；

如图3至图6，提供了一组转速为4秒每转的场景下，传统的卡尔曼滤波算法与第一实施例所述方法的定位测速的实验结果。

如图3所示，图中横坐标代表东向测速误差，纵坐标代表北向测速误差，单位为m/s。矩形方框表示传统方法测速误差，圆圈框表示本方法测速误差，由图可知，传统方法下，东向测速误差分布在-100m/s～250m/s，北向测速误差分布在±300m/s，本方法得到的测速误差分布在0附近，相较传统方法，本方法优化了测速误差大于1m/s时刻的测速结果，本方法的测速精度明显优于传统方法。

如图4所示，图中横坐标代表周内秒，纵坐标代表三维测速误差，单位为m/s。虚线表示传统方法下的测速误差，实线表示本方法测速误差。由图可知，在周内秒4.02100×10⁵往后的时间段，传统方法下的三维测速误差超出1m/s，本方法得到的测速误差在0.1m/s以内，测速精度明显得到优化。

如图5所示，图中横坐标代表东向定位误差，纵坐标代表北向定位误差，单位为m。矩形方框表示传统方法定位误差，圆圈框表示本方法定位误差，由图可知，传统方法下，东向定位误差分布在-250m～200m，北向定位误差分布在-400m～300m，本方法得到的定位误差分布在±30m，相较传统方法，本方法优化了定位误差大于30m/s时刻的定位结果，本方法的定位精度明显优于传统方法。

如图6所示，图中横坐标代表周内秒，纵坐标代表三维定位误差，单位为m。虚线表示传统方法下的定位误差，实线表示本方法定位误差。由图可知，在周内秒4.02100×10⁵往后的时间段，传统方法下的三维定位误差超出30m，本方法得到的定位误差小于10m，在周内秒4.01700×10⁵～4.02100×10⁵时间段，本方法得到的定位误差比传统方法下的定位误差明显较小，差值约2～10m。本方法较传统方法，定位精度更优。

第三实施例；

提供了一种应用系统，本应用系统中使用第一实施例所述的载体低速旋转条件下的导航定位方法。

例如上述实施例的方法可应用于气象探空火箭，用来收集大气参数。气象探空火箭垂直发射，转速低于0.25r/s。

由于本系统实施例与第一实施例是属于同一个发明构思，因此第一实施例的相应内容同样适应于本系统实施例，本领域技术人员在清楚第一实施例的基础上，可以不付出创造性劳动就能实现本系统实施例的技术方案，因此本系统实施例不再细述。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (4)

1.一种载体低速旋转条件下的导航定位方法，其特征在于，包括以下步骤：

构建卡尔曼滤波模型并设置系统运行状态变量；

基于系统状态方程以及上一历元的状态估计值和状态估计值的均方误差阵，预测当前历元的状态估计值和状态估计值的均方误差阵；

根据所述当前历元的状态估计值和状态估计值的均方误差阵计算观测量残余和测量方差；

若所述观测量残余与所述测量方差的比值超出门限值且当前历元的滤波深度超过阈值，根据所述观测量残余与所述测量方差的比值调整所述测量方差，并根据调整后的所述测量方差计算卡尔曼滤波增益，根据所述卡尔曼滤波增益校正所述当前历元的状态估计值和状态估计值的均方误差阵；若所述观测量残余与所述测量方差的比值未超出门限值和/或当前历元的滤波深度未超过阈值，根据所述测量方差计算卡尔曼滤波增益，根据所述卡尔曼滤波增益校正所述当前历元的状态估计值和状态估计值的均方误差阵。

2.根据权利要求1所述的载体低速旋转条件下的导航定位方法，其特征在于，根据所述观测量残余与所述测量方差的比值调整所述测量方差，包括：

其中所述σ表示所述测量方差；所述C表示测量关系矩阵；所述

表示所述当前历元的状态估计值的均方误差阵；所述M表示所述观测量残余与所述测量方差的比值。

3.根据权利要求2所述的载体低速旋转条件下的导航定位方法，其特征在于，根据所述卡尔曼滤波增益校正所述当前历元的状态估计值和状态估计值的均方误差阵，包括：

其中K_k表示卡尔曼滤波增益，

表示所述当前历元的状态估计值的均方误差阵，

表示所述当前历元的状态估计值，

表示校正后的所述当前历元的状态估计值，Δy_k表示所述观测量残余，P_k表示校正后的所述当前历元的状态估计值的均方误差阵，I表示单位矩阵。

4.根据权利要求1所述的载体低速旋转条件下的导航定位方法，其特征在于，所述门限值设为5，所述阈值设为20。

Images