CN106017452B - 双陀螺抗扰动寻北方法 - Google Patents

Abstract

本发明公开了一种双陀螺抗扰动寻北方法，其步骤包括：①初始化寻北***；②陀螺/加速度计数据采集；③初始装订俯仰角、横滚角，构建天向虚拟信息，实现全姿态惯性器件信息的构建；④利用惯性对准技术，计算并输出方位角的信息，结束寻北。本发明在寻北过程中出现外界扰动的情况下，仍能保证寻北***工作正常，寻北结果有效，真正做到扰动下的寻北；省去了转位机构等机械结构件，降低寻北***内部结构复杂度、减轻寻北***重量；还能实时给出载体的姿态和方位信息。

Description

双陀螺抗扰动寻北方法

技术领域

本发明属于惯性寻北技术领域，具体涉及一种双陀螺抗扰动的寻北方法。

背景技术

陀螺寻北***是惯性技术应用的重要成果之一，它通过敏感地球自转来确定北向，能全天候可靠地工作，不受外部环境如地球磁场、地形、气候等的影响，可以在没有任何外部方位信息的情况下实现自主寻北功能，测出当地地理北向。目前的寻北***，绝大多数采用单陀螺，依靠转位机构实现位置转动后结算相应的北向。按转动位置可以分为二位置法、四位置法、多位置法，以及连续旋转调试。无论采用什么寻北方式，寻北***在使用的过程中总会受到各种扰动的影响，对于车载寻北***扰动源主要有阵风、发动机的振动、人员的走动、地基的沉降等，基座的扰动会严重降低***的寻北精度。为了减小各种扰动对寻北精度的影响，目前采用的方法都局限于对惯性仪表即陀螺、加速度计输出值的滤波及数字处理，这些方法可以有效消除高频扰动对寻北***的影响。在实际应用中，尤其是基座扰动中包含较大低频噪声时，使用直接对惯性仪表输出值滤波及数字处理的方法往往得不到比较理想的效果。

发明内容

针对现有技术所存在的不足，本发明所解决的问题在于，怎样提供一种在存在扰动的情况下仍能正常工作，输出有效结果，保证寻北精度的寻北方法。

为实现上述目的，本发明采用如下方案：

双陀螺抗扰动寻北方法，具体步骤如下：

步骤1：对寻北***进行初始化；

步骤2：采集X轴、Y轴光纤陀螺与加速度计输出信息，标定惯性器件的参数；具体来说，1)在一个纬度已知，且标定的转台下，通过采集均匀分布的12个水平位置的X轴、Y轴的陀螺输出数据，标定出双光纤陀螺误差模型中的8个参数；2)在转台下，通过敏感正负地球重力加速度信息，采集6种条件下，X轴、Y轴加速度计输出数据，标定出双加速度计误差模型中的8个参数；

步骤3a：对俯仰角和横滚角两个参数是否已初始装订过进行判断，若判断为否，则初始计算装订俯仰角和横滚角两个参数；若判断为是，则采用更新后的俯仰角和横滚角参数；

步骤3b：将步骤2中标定的16个参数与步骤3a中所述的俯仰角、横滚角两个参数一同构建天向虚拟信息，即天向陀螺虚拟信息和天向加速度计虚拟信息，从而实现对全姿态惯性器件信息的构建；

步骤4：利用惯性系对准技术，计算方位角、俯仰角、横滚角的信息；用计算出的横滚角和俯仰角信息更新步骤3a中所述的横滚角和俯仰角参数；输出计算出的方位角信息，寻北过程结束。

进一步地，所述步骤4在寻北过程结束前，设置判断条件来重复执行步骤2～4以获得符合要求的寻北结果；若判断为是，则寻北过程结束，若判断为否，则返回执行步骤2。

另外，步骤2中对陀螺输出数据的采集位置可不限于12个，也可以是4个、8个等等。由于采用线性回归来拟合参数，在0-360°的范围内，数据分布越均匀，则拟合的参数越准确。若增加采集位置，一是耗时，二是过多的采集位置对于拟合而言意义不大；若采集位置过少，则拟合出的参数精度不高。综合考虑以上因素，要实现本发明的目的，采集位置应不少于4个。

进一步地，所述步骤2中惯性器件的具体标定方法如下：

相应的姿态矩阵为：

1)光纤陀螺参数标定：

双光纤陀螺误差模型:

X轴陀螺:

Y轴陀螺：

上面两式中，W_bx，W_by分别是X轴和Y轴陀螺输出信息，W_0x，W_0y分别是X轴和Y轴陀螺输出零位，是陀螺对载体角速度的测量值，K_xx，K_yy分别是X轴和Y轴陀螺刻度系数，k_xy，k_zx，k_xy，k_zy分别是陀螺交叉耦合系数；

在纬度已知，且水平静止条件下，有如下三式：

上式中，ω_ie是地球自转角速度，为常量；将(4)式带入(2)、(3)式，有

W_bx＝W_0x+K_xx(-ω_iesinψcosL)+k_yx(ω_iecosψcosL)+k_zx(ω_iesinL) (5)

W_by＝W_0y+k_xy(-ω_iesinψcosL)+K_yy(ω_iecosψcosL)+k_zy(ω_iesinL) (6)

标定陀螺过程中，始终保持水平静止，标定方位点依次选取相应的X轴陀螺和Y轴陀螺输出为

构建如下两个矩阵，

X矩阵中的8个参数，即为双光纤陀螺误差模型(2)、(3)式的参数，所以在一个纬度已知，且标定的转台下，通过采集12水平位置的陀螺输出数据，可标定出以上8个参数；

2)加速度计标定

双加速度计误差模型如下：

上面两式中，V_ax，V_ay分别是X轴和Y轴加速度计输出信息，K_ax0，K_ax0分别是X轴和Y轴加速度计输出零位，是加速度计对载体比力的测量值，K_axx，K_ayy分别是X轴和Y轴加速度计刻度系数，k_axy，k_azx，k_axy，k_azy分别是加速度计交叉耦合系数；

加速度计标定时候，静止条件下，依次选取X轴、Y轴、Z轴加速度计敏感正负重力加速，相应的X轴加速度计和Y轴加速度计输出为

构建如下两个矩阵，

X矩阵中的8个参数，即为双加速度计误差模型(8)式的参数，所以在转台下，通过敏感正负地球重力加速度信息，采集6种条件下，X轴Y轴加速度计输出数据，可标定出以上8个参数。

进一步地，所述步骤3b中天向虚拟信息的具体构建方法如下：

1)天向陀螺虚拟信息构建

根据欧拉方程有：

考虑角速度动态约束条件，载体方位上角速度为零，即

带入上式有，得到姿态角速度约束方程：

整理得到：

考虑到有下式成立

根据(2)、(3)中的误差模型知道，X轴陀螺和Y轴陀螺的输出为

将(14)带入(15)，得到：

其中

由于寻北时***在导航坐标系中是静止的，导航坐标系相对地球坐标系没有转动，所以为零，因此在东北天坐标系下

因此有：

2)天向加速度计虚拟信息构建

根据比力方程有：

考虑载体线运动的动态约束条件，垂直方向上的速度和加速度为零，以及水平方向上速度为0，即

带入上式有

整理得天向加速度信息为：

Z轴加速度计输出为：

根据(8)式中的误差模型知道，X加速度计和Y加速度计的输出为

将(24)带入(25)，得到：

因此有：

根据上面(19)和(27)构造的数据，就构成完整的三自由度惯性器件数据。

进一步地，所述步骤4中利用惯性系对准计算方位角的方法如下：

姿态矩阵的四元数形式如下

其中

初始时刻，寻北过程中，载体处于相对静止，及没有速度，此时 为(19)式中的陀螺数据，即对(29)式采用四元数微分方程的毕卡求解法可以实时更新得到 和

由于有

将(28)式带入(30)式得：

整理得到：

式中左核函数和右核函数分别为：

为求解转换成求矩阵最小特征值对应的特征矢量即可，将求解得到后的带入(28)式得到新的就可得到方位、俯仰、横滚信息；

方位：

俯仰：θ＝asin(2(q₂q₃+q₀q₁)) (34)

横滚：

其中

进一步地，所述步骤3a中俯仰角与滚动角的初始装订的方法如下：

对照(27)式，不考虑天向相关参数，有：

此时利用水平加速计输出信息计算俯仰角和横滚角：

在已经成熟的抗扰动初始对准技术中，及惯性系下对准技术，需要全姿态的三轴陀螺输出信息，三轴加速度计输出信息，对准完毕后，方位信息就是寻北***最终的输出结果。在双陀螺抗扰动寻北方法，利用一定的公式，构造出天向角速率信息和加速度信息(均为虚拟信息)，这样虚拟构造出全姿态惯性信息，再利用惯性系下的对准技术，就可以实现抗扰动下寻北技术。

相比于现有技术，本发明具有如下有益效果：

1、本发明的优点在于在寻北过程中出现阵风、发动机振动、人员走动、地基沉降等扰动的情况下，尤其是基座扰动中包含较大低频噪声时，仍能保证寻北***工作正常，寻北结果有效，真正做到扰动下的寻北；

2、传统寻北仪***中，依靠转位机构，实现单陀螺在不同时间段内，分别敏感相互正交4个位置上的地球自转角速度分量信息进行寻北解算；本发明的优点在于省去了转位机构等机械结构件，降低寻北***内部结构复杂度、减轻寻北***重量；

3、传统寻北仪***中，只能在最后寻北结束完毕后给出方位信息；本发明的优点在于具备可以实时给出载体的姿态和方位信息。

附图说明

图1为双陀螺抗扰动寻北方法实施例的流程图。

具体实施方式

在本发明的标定以及计算过程中，均采用东北天坐标系，312转序，即绕-Z轴转动ψ(方位角)，然后绕X轴转动θ(俯仰角)，最后绕Y轴转动γ(横滚角)，惯性器件标定位置的纬度为L。下面结合附图对本发明作进一步详细描述，但本发明的实施方式不限于此。

实施例：

双陀螺抗扰动寻北方法，如图1所示，具体步骤如下：

步骤1：对寻北***进行初始化。

步骤2：采集X轴、Y轴光纤陀螺与加速度计输出信息，标定惯性器件的参数；具体来说，1)在一个纬度已知，且标定的转台下，通过采集均匀分布的12个水平位置的X轴、Y轴的陀螺输出数据，标定出双光纤陀螺误差模型中的8个参数；2)在转台下，通过敏感正负地球重力加速度信息，采集6种条件下，X轴、Y轴加速度计输出数据，标定出双加 速度计误差模型中的8个参数。

步骤3a：对俯仰角和横滚角两个参数是否已初始装订过进行判断，若判断为否，则初始计算装订俯仰角和横滚角两个参数；若判断为是，则采用更新后的俯仰角和横滚角参数。

步骤3b：将步骤2中标定的16个参数与步骤3a中所述的俯仰角、横滚角两个参数一同构建天向虚拟信息，即天向陀螺虚拟信息和天向加速度计虚拟信息，从而实现对全姿态惯性器件信息的构建。

步骤4：利用惯性对准技术，计算方位角、俯仰角、横滚角的信息；用计算出的横滚角和俯仰角信息更新步骤3a中所述的横滚角和俯仰角参数；输出计算出的方位角信息，寻北过程结束；在结束寻北之前，可通过设置判断条件来重复执行步骤2～4以获得符合要求的寻北结果，比如将“寻北时间<3min”作为判断条件，若判断为是，则寻北流程结束，若判断为否，则返回执行步骤2。

以下是各步骤中数据计算公式的具体推演以及与现有技术的试验数据对比：

(一)所述步骤2中惯性器件的具体标定方法如下：

相应的姿态矩阵为：

1)光纤陀螺参数标定：

双光纤陀螺误差模型：

X轴陀螺：

Y轴陀螺：

上面两式中，W_bx，W_by分别是X轴和Y轴陀螺输出信息，W_0x，W_0y分别是X轴和Y轴陀螺输出零位，是陀螺对载体角速度的测量值，K_xx，K_yy分别是X轴和Y轴陀螺刻度系数，k_xy，k_zx，k_xy，k_zy分别是陀螺交叉耦合系数。

在纬度已知，且水平静止条件下，有如下三式：

上式中，ω_ie是地球自转角速度，为常量。将(4)式带入(2)、(3)式，有

W_bx＝W_0x+K_xx(-ω_iesinψcosL)+k_yx(ω_iecosψcosL)+k_zx(ω_iesinL) (5)

W_by＝W_0y+k_xy(-ω_iesinψcosL)+K_yy(ω_iecosψcosL)+k_zy(ω_iesinL) (6)

标定陀螺过程中，始终保持水平静止，标定方位点依次选取相应的X轴陀螺和Y轴陀螺输出为

构建如下两个矩阵，

X矩阵中的8个参数，即为双光纤陀螺误差模型(2)、(3)式的参数，所以在一个纬度已知，且标定的转台下，通过采集12水平位置的陀螺输出数据，可标定出以上8个参数。

2)加速度计标定

双加速度计误差模型如下：

上面两式中，V_ax，V_ay分别是X轴和Y轴加速度计输出信息，K_ax0，K_ax0分别是X轴和Y轴加速度计输出零位，是加速度计对载体比力的测量值，K_axx，K_ayy分别是X轴和Y轴加速度计刻度系数，k_axy，k_azx，k_axy，k_azy分别是加速度计交叉耦合系数。

加速度计标定时候，静止条件下，依次选取X轴、Y轴、Z轴加速度计敏感正负重力加速，相应的X轴加速度计和Y轴加速度计输出为

构建如下两个矩阵，

X矩阵中的8个参数，即为双加速度计误差模型(8)式的参数，所以在转台下，通过敏感正负地球重力加速度信息，采集6种条件下，X轴Y轴加速度计输出数据，可标定出以上8个参数。

(二)所述步骤3b中天向虚拟信息具体构建方法如下：

1)天向陀螺虚拟信息构建

根据欧拉方程有：

考虑角速度动态约束条件，载体方位上角速度为零，即

带入上式有，得到姿态角速度约束方程：

整理得到：

考虑到有下式成立

根据(2)、(3)中的误差模型知道，X轴陀螺和Y轴陀螺的输出为

将(14)带入(15)，得到：

其中

由于寻北时***在导航坐标系中是静止的，导航坐标系相对地球坐标系没有转动，所以为零，因此在东北天坐标系下

因此有：

2)天向加速度计虚拟信息构建

根据比力方程有：

考虑载体线运动的动态约束条件，垂直方向上的速度和加速度为零，以及水平方向上速度为0，即

带入上式有

整理得天向加速度信息为：

Z轴加速度计输出为：

根据(8)式中的误差模型知道，X加速度计和Y加速度计的输出为

将(24)带入(25)，得到：

因此有：

根据上面(19)和(27)构造的数据，就构成完整的三自由度惯性器件数据。

(三)所述步骤4中利用惯性系对准计算方位角的方法如下：

姿态矩阵的四元数形式如下

其中

初始时刻，寻北过程中，载体处于相对静止，及没有速度，此时 为(19)式中的陀螺数据，即对(29)式采用四元数微分方程的毕卡求解法可以实时更新得到 和

由于有

将(28)式带入(30)式得：

整理得到：

式中左核函数和右核函数分别为：

为求解转换成求矩阵最小特征值对应的特征矢量即可。具体求解过程可以参考清华大学出版社《C常用算法程序集—第二版》中的第3.6节：“求实对称矩阵特征值与特征向量的雅克比过关法”。将求解得到后的带入(28)式得到新的就可得到方位、俯仰、横滚信息。

方位：

俯仰：θ＝asin(2(q₂q₃+q₀q₁)) (34)

横滚：

其中

(四)所述步骤3a中俯仰角与滚动角的初始装订的方法如下：

对照(27)式，不考虑天向相关参数，有：

此时利用水平加速计输出信息计算俯仰角和横滚角：

在构造数据之前，通过(19)式和(27)式可知，需要知道陀螺的8个标定参数、加速度计的8个标定参数，以及俯仰角、横滚角，共计18个参数。其中，前16个参数在惯性器件标定试验中可以计算得到；后两个参数(俯仰角、横滚角)需要寻北***初始计算装订，后续俯仰角、横滚角可以利用(34)式和(35)式计算结果参与后期运算。俯仰角和横滚角初始计算装订利用上述公式计算。

(五)本发明与传统寻北方法的对比：

传统方法寻北***中，采用挠性陀螺2位置寻北技术，抗扰动寻北方法采用双陀螺双加速计寻北，某次对比试验结果见表1。

表1数据对比

上面试验数据表明，采用本发明的双陀螺抗扰动寻北方法不仅在静态测试中与传统寻北***寻北精度相当；当存在外界扰动情况下时，传统寻北***寻北精度大大降低，输出结果根本不能使用，而采用双陀螺抗扰动寻北方法依旧可以保证寻北精度(1σ<0.1°)。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (6)

1.双陀螺抗扰动寻北方法，其特征在于具体步骤如下：

步骤1：对寻北***进行初始化；

步骤2：采集X轴、Y轴光纤陀螺与加速度计输出信息，标定惯性器件的参数；具体来说，1)在一个纬度已知，且标定的转台下，通过采集均匀分布的12个水平位置的X轴、Y轴的陀螺输出数据，标定出双光纤陀螺误差模型中的8个参数，所述双光纤陀螺误差模型中的8个参数分别为W_0x、W_0y、K_xx、K_yy、k_xy、k_zx、k_xy及k_zy，其中W_0x，W_0y分别是X轴和Y轴陀螺输出零位，K_xx，K_yy分别是X轴和Y轴陀螺刻度系数，k_xy，k_zx，k_xy，k_zy分别是陀螺交叉耦合系数；2)在转台下，通过敏感正负地球重力加速度信息，采集6种条件下，X轴、Y轴加速度计输出数据，标定出双加速度计误差模型中的8个参数，所述6种条件包括X轴加速度计分别敏感±1g，Y轴加速度计分别敏感±1g，虚拟轴Z轴的加速度计分别敏感±1g下的加速度计输出信息，所述双加速度计误差模型中的8个参数包括K_ax0、K_ay0、K_axx、K_ayy、k_axy、k_azx、k_axy及k_azy，其中，K_ax0、K_ay0分别是X轴和Y轴加速度计输出零位，K_axx、K_ayy分别是X轴和Y轴加速度计刻度系数，K_axy、k_azx、k_axy及k_azy分别是加速度计交叉耦合系数；

所述双光纤陀螺误差模型如下：

X轴陀螺：

Y轴陀螺：

所述双加速度计误差模型如下：

步骤3a：对俯仰角和横滚角两个参数是否已初始装订过进行判断，若判断为否，则初始计算装订俯仰角和横滚角两个参数；若判断为是，则采用更新后的俯仰角和横滚角参数；

步骤3b：将步骤2中标定的16个参数与步骤3a中所述的俯仰角、横滚角两个参数一同构建天向虚拟信息，所述天向虚拟信息包括天向陀螺虚拟信息和天向加速度计虚拟信息，从而实现对全姿态惯性器件信息的构建；

步骤4：利用惯性系对准技术，计算方位角、俯仰角、横滚角的信息；用计算出的横滚角和俯仰角信息更新步骤3a中所述的横滚角和俯仰角参数；输出计算出的方位角信息，寻北过程结束。

2.根据权利要求1所述的双陀螺抗扰动寻北方法，其特征在于：所述步骤4在寻北过程结束前，设置判断条件来重复执行步骤2～4以获得符合要求的寻北结果；若判断为是，则寻北过程结束，若判断为否，则返回执行步骤2；所述判断条件包括，寻北时间<3min。

3.根据权利要求1所述的双陀螺抗扰动寻北方法，其特征在于：所述步骤2中惯性器件的具体标定方法如下：

相应的姿态矩阵为：

1)光纤陀螺参数标定：

双光纤陀螺误差模型：

X轴陀螺：

Y轴陀螺：

上面两式中，W_bx，W_by分别是X轴和Y轴陀螺输出信息，W_0x，W_0y分别是X轴和Y轴陀螺输出零位，是陀螺对载体角速度的测量值，n＝x,y,z，K_xx，K_yy分别是X轴和Y轴陀螺刻度系数，k_xy，k_zx，k_xy，k_zy分别是陀螺交叉耦合系数；

在纬度已知，且水平静止条件下，有如下三式：

上式中，ω_ie是地球自转角速度，为常量；将(4)式带入(2)、(3)式，有

W_bx＝W_0x+K_xx(-ω_ie sinψcosL)+k_yx(ω_ie cosψcosL)+k_zx(ω_ie sinL) (5)

W_by＝W_0y+k_xy(-ω_ie sinψcosL)+K_yy(ω_ie cosψcosL)+k_zy(ω_ie sinL) (6)

标定陀螺过程中，始终保持水平静止，标定方位点依次选取i＝0、1…11，相应的X轴陀螺和Y轴陀螺输出为

构建如下两个矩阵，

X矩阵中的8个参数为双光纤陀螺误差模型(2)、(3)式的参数，所以在一个纬度已知，且标定的转台下，通过采集12水平位置的陀螺输出数据，可标定出以上8个参数；

2)加速度计标定

双加速度计误差模型如下：

上面两式中，V_ax，V_ay分别是X轴和Y轴加速度计输出信息，K_ax0，K_ay0分别是X轴和Y轴加速度计输出零位，f_i ^b是加速度计对载体比力的测量值，i＝x,y,z，K_axx，K_ayy分别是X轴和Y轴加速度计刻度系数，k_axy，k_azx，k_axy，k_azy分别是加速度计交叉耦合系数；

加速度计标定时候，静止条件下，依次选取X轴、Y轴、Z轴加速度计敏感正负重力加速，相应的X轴加速度计和Y轴加速度计输出为

构建如下两个矩阵，

X矩阵中的8个参数为双加速度计误差模型(8)式的参数，所以在转台下，通过敏感正负地球重力加速度信息，采集6种条件下，X轴Y轴加速度计输出数据，可标定出以上8个参数。

4.根据权利要求3所述的双陀螺抗扰动寻北方法，其特征在于：所述步骤3b中天向虚拟信息的具体构建方法如下：

1)天向陀螺虚拟信息构建

根据欧拉方程有：

考虑角速度动态约束条件，载体方位上角速度为零，

带入上式有，得到姿态角速度约束方程：

整理得到：

考虑到有下式成立

根据(2)、(3)中的误差模型知道，X轴陀螺和Y轴陀螺的输出为

将(14)带入(15)，得到：

其中

由于寻北时***在导航坐标系中是静止的，导航坐标系相对地球坐标系没有转动，所以为零，因此在东北天坐标系下

因此有：

2)天向加速度计虚拟信息构建

根据比力方程有：

考虑载体线运动的动态约束条件，垂直方向上的速度和加速度为零，以及水平方向上速度vⁿ为0，

带入上式有

整理得天向加速度信息为：

Z轴加速度计输出为：

根据(8)式中的误差模型知道，X加速度计和Y加速度计的输出为

将(24)带入(25)，得到：

因此有：

根据上面(19)和(27)构造的数据，就构成完整的三自由度惯性器件数据。

5.根据权利要求4所述的双陀螺抗扰动寻北方法，其特征在于：所述步骤4中利用惯性系对准计算方位角的方法如下：

姿态矩阵的四元数形式如下

其中

初始时刻，寻北过程中，载体处于相对静止，载体没有速度，此时 为(19)式中的陀螺数据，对(29)式采用四元数微分方程的毕卡求解法可以实时更新得到和

由于有

将(28)式带入(30)式得：

整理得到：

式中左核函数和右核函数分别为：

为求解转换成求矩阵最小特征值对应的特征矢量即可，将求解得到后的带入(28)式得到新的就可得到方位、俯仰、横滚信息；

方位：

俯仰：θ＝asin(2(q₂q₃+q₀q₁)) (34)

横滚：

其中

6.根据权利要求4所述的双陀螺抗扰动寻北方法，其特征在于：所述步骤3a中俯仰角与滚动角的初始装订的方法如下：

对照(27)式，不考虑天向相关参数，有：

此时利用水平加速计输出信息计算俯仰角和横滚角：