CN109084761A - 一种基于惯性寻北装置的抗冲击寻北解算方法 - Google Patents

Abstract

本发明公开了一种基于惯性寻北装置的抗冲击寻北解算方法，首先进行数据采集，得到陀螺测量数据G1和加速度计测量数据A1，然后基于卡尔曼滤波器对陀螺数据进行滤波，再将单个位置的测试数据进行分组，并对每个分组数据的发散程度进行量化，之后基于格拉布斯滤波算法，剔除数值偏差异常的分组，最后再依次进行数据统计、转位控制、姿态解算和圆周补偿，得到惯性寻北装置航向角和姿态角的解算结果。本发明采用多种滤波方式，对测量数据进行滤波处理，减少了冲击振动对寻北结果的影响，并采用了倾斜状态无纬度寻北解算方法，从而得到精确的解算结果。

Description

一种基于惯性寻北装置的抗冲击寻北解算方法

技术领域

本发明涉及惯性导航技术领域，具体是一种基于惯性寻北装置的抗冲击寻北解算方法。

背景技术

惯性寻北装置是一种利用陀螺和加速度计测量地球自转角速度和重力加速度，解算出载体航向角和姿态角的设备，测量精度高，不受磁场影响。现有惯性寻北装置的寻北解算普遍采用静态两位置或静态四位置方法实现，其具体过程为：将陀螺仪1和加速度计2测量方向平行，安装在转位锁紧机构3上，通过转位锁紧机构3带动在两个180°对称位置或四个90°对称位置上测量地球自转角速度和重力加速度的水平分量，将对称位置上陀螺仪1和加速度计2的数据相减消除各自零偏，得到等效测量值，然后信号处理板4通过反正切关系解算出载体航向角和姿态角。其基本结构如图1所示。惯性寻北装置在实际使用中难免会受到外界的冲击和振动干扰，因寻北时间较短，这些干扰运动叠加到陀螺和加速度计的测量数据中，会对航向角和姿态角的解算结果造成影响，降低惯性寻北装置的测量精度。

发明内容

本发明要解决的技术问题是提供一种基于惯性寻北装置的抗冲击寻北解算方法，减少冲击振动对惯性寻北装置解算结果的影响。

本发明的技术方案为：

一种基于惯性寻北装置的抗冲击寻北解算方法，具体包括有以下步骤：

(1)、数据采集：连接固定惯性寻北装置，上电稳定后开始测量；在单个位置测试过程中，惯性寻北装置以固定周期时钟脉冲为采样同步信号，接收陀螺测量数据G1和加速度计测量数据A1，并存储在系统内存中；

(2)、数据滤波：基于卡尔曼滤波器对陀螺数据进行滤波，利用惯性寻北装置工作状态位准静态的已知前提，构建卡尔曼滤波器并进行参数设置，其中状态变量X为输入角速率值G2和陀螺测量噪声ε，观测量Z为陀螺实际测量数据G1和零值，状态转移矩阵A为2×2单位阵，依据G1＝G2+ε关系设计观测矩阵H，对滤波参数中的一步预测方差阵P_k/k-1乘上滤波修正系数P_nav以增强滤波效果，最终以状态变量中的输入角速率值G2为滤波结果；

(3)、数据分组：将步骤(2)得到的输入角速率值G2和加速度计测量数据A1各按50个数据一组，分为若干组，计算每组数据的均值G3、A3和各自的方差VG3、VA3；

(4)、数据野值剔除：基于格拉布斯滤波算法，设定格拉布斯滤波的设定阈值，对于陀螺数据，对多组数据的均值G3计算均值M和标准差S，并计算出偏差系数将偏差系数大于设定阈值的数据即点序列k判定为野值数据，对野值数据即点序列k及其对应的方差数据从原数据中删除，得到新的G3和VG3数组，按以上算法进行若干轮迭代，直至无野值数据即原数据中的点序列k为空集或数据长度小于预设下限，最终得到野值剔除后数组G4和VG4；按照上述步骤，对于加速度计数据A3也进行同样处理，得到A4和VA4；

(5)、数据统计：采用分组方差加权法，将步骤(4)得到的VG4的倒数作为加权系数TG，将步骤(4)得到的VA4的倒数作为加权系数TA，根据加权系数TG对G4数组分别进行加权平均得G5，根据加权系数TA对A4数组分别进行加权平均得A5；

(6)、转位控制：经步骤(1)至(5)完成一个测试位置的数据测量和计算后，通过转位控制程序控制转位转动到下一个测试位置，继续进行测试；对四位置测试方法的惯性寻北装置，转位控制顺序是0°位置、180°位置、270°位置、90°位置，经过三次转动得到四个位置下的陀螺计算均值G5₁、G5₂、G5₃、G5₄和加速度计计算均值A5₁、A5₂、A5₃、A5₄；

(7)、姿态解算：采用倾斜四位置无纬度寻北姿态解算方法或倾斜两位置寻北姿态解算方法，

所述的倾斜四位置无纬度寻北姿态解算方法中，令ψ、θ、γ分别为寻北装置的航向、俯仰和横滚角，解算公式如下：

其中，

其中，K_ω为光纤陀螺标度因数，K_a为加速度计标度因数，g为地球重力加速度；

所述的倾斜两位置寻北姿态解算方法中，令两个加速度计Ax、Ay以90°正交水平安装在转位锁紧机构上，一个陀螺仪测量方向与Ay平行安装在转位锁紧机构上，转位锁紧机构在0°和90°位置测量，按以上(1)-(5)步骤处理得到陀螺计算均值G5₁'、G5₂'和A5_X1、A5_X2、A5_Y1、A5_Y2，采用倾斜两位置无纬度寻北姿态解算方法，令ψ、θ、γ分别为寻北装置的航向、俯仰和横滚角，解算公式如下：

其中，

其中，ω₀为光纤陀螺零偏，K_ω为光纤陀螺标度因数，a0_X、a0_Y分别为加速度X和Y的零偏，K_ax、K_ay分别为加速度X和Y的标度因数，g为地球重力加速度；

(8)、圆周补偿：采用正弦函数拟合的方法消除航向角圆周误差，补拟合模型为：

其中，ψ为步骤(7)解算得到的航向角，为圆周补偿后的航向角，a_i、b_i、c_i为圆周补偿模型参数。

所述的步骤(2)中，卡尔曼滤波器的参数设置和滤波公式如下：

参数设置：

状态变量观测量状态转移矩阵观测矩阵观测噪声阵

滤波公式：

状态一步预测：

状态估计：

一步预测方差阵：P_k/k-1＝A×P_k-1×A^T×P_nav (10),

滤波增益：

估计方差阵：P_k＝(I-K×H)×P_k/k-1×(I-K×H)^T+K×R×K^T (12)；

基于卡尔曼滤波器对陀螺数据进行滤波的具体步骤为：先进行一步预测方差阵的滤波公式计算，然后再依次进行滤波增益的计算、估计方差阵的计算、状态一步预测的计算，最后将陀螺实际测量数据G1代入状态估计公式，得到输入角速率值G2。

所述的步骤(4)中，格拉布斯滤波算法的迭代循环次数小于等于10次，数据长度预设下限为输入数据长度的40％。

所述的步骤(5)中，所述的G5根据公式(13)得出，所述的A5根据公式(14)得到：

其中，

所述的步骤(8)中，正弦函数叠加个数小于等于5个，所述的圆周补偿模型参数的标定和计算方法如下：

(a)、将惯性寻北装置置于回转工作台上，转台归零；

(b)、从0°至350°依次间隔10°测量36个点的圆周，转台定位误差＜0.001°，记录航向测量数据；

(c)、通过MATLAB软件曲线拟合工具，导入测量数据，并按照正弦函数叠加模型拟合误差曲线，计算补偿参数，具体包括有以下步骤：首先选择拟合模型为单正弦函数a1*sin(b1*x+c1)，若拟合结果RMSE≤0.03，记录参数，拟合完成；若拟合结果RMSE＞0.03，需对测试数据进行评估，评估方法为计算36个角度增量偏差值，若计算的角增量偏差值大于0.4°，表明该位置测试数据异常，将此位置数据复测6次取均值后重新进行拟合；若计算的角增量偏差值小于等于0.4°，评估结果正常，则将正弦函数叠加模型的叠加数量加1，再次进行拟合和评估，重复多次直至得到满足要求的参数，如果叠加个数超过设定次数，则判定拟合失败，对惯性寻北装置进行重新装配调试；

(d)、将拟合得到的圆周补偿参数的值烧录到惯性寻北装置内部的信号处理板上的内存中即可。

本发明的优点：

(1)、本发明将准静态下的卡尔曼滤波引入惯性寻北装置，可降低陀螺测量噪声的幅度，以便分辨出冲击振动造成的输入角速率变化波形；

(2)、本发明通过对卡尔曼滤波、格拉布斯滤波、分组方差加权滤波三种方式进行组合设计，有效降低冲击振动干扰对寻北精度的影响，适用性强，可推广到其他；

(3)、本发明的姿态解算采用了倾斜状态无纬度寻北解算方法，通过建立虚拟的天向陀螺轴，消除常规寻北解算公式中的ω_N分量，装置上电后可直接测量，使用时无需设置参数，无需装订纬度信息，满足不同纬度下的使用需求，提高了惯性寻北装置的适用性和易用性。

附图说明

图1是惯性寻北装置的结构框图。

图2是本发明的流程图。

图3是本发明数据滤波的流程图。

图4是本发明数据滤波前后的陀螺数据对比图一。

图5是本发明数据滤波前后的陀螺数据对比图二。

图6是本发明数据野值剔除的流程图。

图7是本发明格拉布斯六次滤波后的效果图。

图8是本发明圆周补偿模型参数标定和计算的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

见图2，一种基于惯性寻北装置的抗冲击寻北解算方法，具体包括有以下步骤：

(1)、数据采集：连接固定惯性寻北装置，上电稳定后开始测量；在单个位置测试过程中，惯性寻北装置以固定周期时钟脉冲为采样同步信号，接收陀螺测量数据G1和加速度计测量数据A1，并存储在系统内存中；

(2)、见图3，数据滤波，此步骤的作用是降低陀螺测量噪声的幅度，以分辨出冲击振动造成的输入角速率变化波形：具体方式是基于卡尔曼滤波器对陀螺数据进行滤波，利用惯性寻北装置工作状态位准静态的已知前提，构建卡尔曼滤波器并进行参数设置，其中状态变量X为输入角速率值G2和陀螺测量噪声ε，观测量Z为陀螺实际测量数据G1和零值，状态转移矩阵A为2×2单位阵，依据G1＝G2+ε关系设计观测矩阵H，对滤波参数中的一步预测方差阵P_k/k-1乘上滤波修正系数Pnav以增强滤波效果，最终以状态变量中的输入角速率值G2为滤波结果；

其中，卡尔曼滤波器的参数设置和滤波公式如下：

参数设置：

状态变量观测量状态转移矩阵观测矩阵观测噪声阵

滤波公式：

状态一步预测：

状态估计：

一步预测方差阵：P_k/k-1＝A×P_k-1×A^T×P_nav (10),

滤波增益：

估计方差阵：P_k＝(I-K×H)×P_k/k-1×(I-K×H)^T+K×R×K^T (12)；

基于卡尔曼滤波器对陀螺数据进行滤波的具体步骤为：先进行一步预测方差阵的滤波公式计算，然后再依次进行滤波增益的计算、估计方差阵的计算、状态一步预测的计算，最后将陀螺实际测量数据G1代入状态估计公式，得到输入角速率值G2；对冲击和振动干扰的滤波前后效果如图4和图5所示；

(3)、数据分组，此步骤的作用是将单个位置的测试数据进行分割，减少后续解算的数据量，并对每个分组数据的发散程度进行量化：具体方式是将步骤(2)得到的输入角速率值G2和加速度计测量数据A1各按50个数据一组，分为若干组，计算每组数据的均值G3、A3和各自的方差VG3、VA3；储存3600个数据后，完成此位置的数据采集；

(4)、见图6，数据野值剔除，此步骤的作用是剔除数值偏差异常的分组：具体方式是基于格拉布斯滤波算法，设定格拉布斯滤波的设定阈值，对于陀螺数据，对多组数据的均值G3计算均值M和标准差S，并计算出偏差系数将偏差系数大于设定阈值的数据即点序列k判定为野值数据，对野值数据即点序列k及其对应的方差数据从原数据中删除，得到新的G3和VG3数组，按以上算法进行六轮迭代后，点序列k为空集，最终得到野值剔除后数组G4和VG4；按照上述步骤，对于加速度计数据A3也进行同样处理，得到A4和VA4；格拉布斯滤波效果如图7所示；

(5)、数据统计，此步骤的作用是根据各分组数据的发散程度进行加权统计，降低受冲击振动干扰大的分组数据的权重，从而计算出单个位置下的测量均值：具体方式是采用分组方差加权法，将步骤(4)得到的VG4的倒数作为加权系数TG，将步骤(4)得到的VA4的倒数作为加权系数TA，根据加权系数TG对G4数组分别进行加权平均得G5，根据加权系数TA对A4数组分别进行加权平均得A5；

其中，G5根据公式(13)得出，A5根据公式(14)得到：

其中，

(6)、转位控制：经步骤(1)至(5)完成一个测试位置的数据测量和计算后，通过转位控制程序控制转位转动到下一个测试位置，继续进行测试；对四位置测试方法的惯性寻北装置，转位控制顺序是0°位置、180°位置、270°位置、90°位置，经过三次转动得到四个位置下的陀螺计算均值G5₁、G5₂、G5₃、G5₄和加速度计计算均值A5₁、A5₂、A5₃、A5₄；

(7)、姿态解算：采用倾斜四位置无纬度寻北姿态解算方法或倾斜两位置寻北姿态解算方法，

倾斜四位置无纬度寻北姿态解算方法中，令ψ、θ、γ分别为寻北装置的航向、俯仰和横滚角，解算公式如下：

其中，

其中，K_ω为光纤陀螺标度因数，K_a为加速度计标度因数，g为地球重力加速度；

倾斜两位置寻北姿态解算方法中，令两个加速度计Ax、Ay以90°正交水平安装在转位锁紧机构上，一个陀螺仪测量方向与Ay平行安装在转位锁紧机构上，转位锁紧机构在0°和90°位置测量，按以上(1)-(5)步骤处理得到陀螺计算均值G5₁'、G5₂'和A5_X1、A5_X2、A5_Y1、A5_Y2，采用倾斜两位置无纬度寻北姿态解算方法，令ψ、θ、γ分别为寻北装置的航向、俯仰和横滚角，解算公式如下：

其中，

其中，ω₀为光纤陀螺零偏，K_ω为光纤陀螺标度因数，a0_X、a0_Y分别为加速度X和Y的零偏，K_ax、K_ay分别为加速度X和Y的标度因数，g为地球重力加速度；

(8)、圆周补偿：采用正弦函数拟合的方法消除航向角圆周误差，补拟合模型为：

其中，ψ为步骤(7)解算得到的航向角，为圆周补偿后的航向角，a_i、b_i、c_i为圆周补偿模型参数。

其中，见图8，圆周补偿模型参数的标定和计算方法如下：

(a)、将惯性寻北装置置于回转工作台上，转台归零；

(b)、从0°至350°依次间隔10°测量36个点的圆周，转台定位误差＜0.001°，记录航向测量数据；

(c)、通过MATLAB软件曲线拟合工具，导入测量数据，并按照正弦函数叠加模型拟合误差曲线，计算补偿参数，具体包括有以下步骤：首先选择拟合模型为单正弦函数a1*sin(b1*x+c1)，若拟合结果RMSE≤0.03，记录参数，拟合完成；若拟合结果RMSE＞0.03，需对测试数据进行评估，评估方法为计算36个角度增量偏差值，若计算的角增量偏差值大于0.4°，表明该位置测试数据异常，将此位置数据复测6次取均值后重新进行拟合；若计算的角增量偏差值小于等于0.4°，评估结果正常，则将正弦函数叠加模型的叠加数量加1，再次进行拟合和评估，重复多次直至得到满足要求的参数，如果叠加个数大于5个，则判定拟合失败，对惯性寻北装置进行重新装配调试；

(d)、将拟合得到的圆周补偿参数的值烧录到惯性寻北装置内部的信号处理板上的内存中即可。

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

Claims (5)

1.一种基于惯性寻北装置的抗冲击寻北解算方法，其特征在于：具体包括有以下步骤：

(1)、数据采集：连接固定惯性寻北装置，上电稳定后开始测量；在单个位置测试过程中，惯性寻北装置以固定周期时钟脉冲为采样同步信号，接收陀螺测量数据G1和加速度计测量数据A1，并存储在系统内存中；

(2)、数据滤波：基于卡尔曼滤波器对陀螺数据进行滤波，利用惯性寻北装置工作状态位准静态的已知前提，构建卡尔曼滤波器并进行参数设置，其中状态变量X为输入角速率值G2和陀螺测量噪声ε，观测量Z为陀螺实际测量数据G1和零值，状态转移矩阵A为2×2单位阵，依据G1＝G2+ε关系设计观测矩阵H，对滤波参数中的一步预测方差阵P_k/k-1乘上滤波修正系数P_nav以增强滤波效果，最终以状态变量中的输入角速率值G2为滤波结果；

(3)、数据分组：将步骤(2)得到的输入角速率值G2和加速度计测量数据A1各按50个数据一组，分为若干组，计算每组数据的均值G3、A3和各自的方差VG3、VA3；

(4)、数据野值剔除：基于格拉布斯滤波算法，设定格拉布斯滤波的设定阈值，对于陀螺数据，对多组数据的均值G3计算均值M和标准差S，并计算出偏差系数将偏差系数大于设定阈值的数据即点序列k判定为野值数据，对野值数据即点序列k及其对应的方差数据从原数据中删除，得到新的G3和VG3数组，按以上算法进行若干轮迭代，直至无野值数据即原数据中的点序列k为空集或数据长度小于预设下限，最终得到野值剔除后数组G4和VG4；按照上述步骤，对于加速度计数据A3也进行同样处理，得到A4和VA4；

(5)、数据统计：采用分组方差加权法，将步骤(4)得到的VG4的倒数作为加权系数TG，将步骤(4)得到的VA4的倒数作为加权系数TA，根据加权系数TG对G4数组分别进行加权平均得G5，根据加权系数TA对A4数组分别进行加权平均得A5；

(6)、转位控制：经步骤(1)至(5)完成一个测试位置的数据测量和计算后，通过转位控制程序控制转位转动到下一个测试位置，继续进行测试；对四位置测试方法的惯性寻北装置，转位控制顺序是0°位置、180°位置、270°位置、90°位置，经过三次转动得到四个位置下的陀螺计算均值G5₁、G5₂、G5₃、G5₄和加速度计计算均值A5₁、A5₂、A5₃、A5₄；

(7)、姿态解算：采用倾斜四位置无纬度寻北姿态解算方法或倾斜两位置寻北姿态解算方法，

所述的倾斜四位置无纬度寻北姿态解算方法中，令ψ、θ、γ分别为寻北装置的航向、俯仰和横滚角，解算公式如下：

其中，

其中，K_ω为光纤陀螺标度因数，K_a为加速度计标度因数，g为地球重力加速度；

所述的倾斜两位置寻北姿态解算方法中，令两个加速度计Ax、Ay以90°正交水平安装在转位锁紧机构上，一个陀螺仪测量方向与Ay平行安装在转位锁紧机构上，转位锁紧机构在0°和90°位置测量，按以上(1)-(5)步骤处理得到陀螺计算均值G5₁'、G5₂'和A5_X1、A5_X2、A5_Y1、A5_Y2，采用倾斜两位置无纬度寻北姿态解算方法，令ψ、θ、γ分别为寻北装置的航向、俯仰和横滚角，解算公式如下：

其中，

其中，ω₀为光纤陀螺零偏，K_ω为光纤陀螺标度因数，a0_X、a0_Y分别为加速度X和Y的零偏，K_ax、K_ay分别为加速度X和Y的标度因数，g为地球重力加速度；

(8)、圆周补偿：采用正弦函数拟合的方法消除航向角圆周误差，补拟合模型为：

其中，ψ为步骤(7)解算得到的航向角，为圆周补偿后的航向角，a_i、b_i、c_i为圆周补偿模型参数。

2.根据权利要求1所述的一种基于惯性寻北装置的抗冲击寻北解算方法，其特征在于：所述的步骤(2)中，卡尔曼滤波器的参数设置和滤波公式如下：

参数设置：

状态变量观测量状态转移矩阵观测矩阵观测噪声阵

滤波公式：

状态一步预测：

状态估计：

一步预测方差阵：P_k/k-1＝A×P_k-1×A^T×P_nav (10),

滤波增益：

估计方差阵：P_k＝(I-K×H)×P_k/k-1×(I-K×H)^T+K×R×K^T (12)；

基于卡尔曼滤波器对陀螺数据进行滤波的具体步骤为：先进行一步预测方差阵的滤波公式计算，然后再依次进行滤波增益的计算、估计方差阵的计算、状态一步预测的计算，最后将陀螺实际测量数据G1代入状态估计公式，得到输入角速率值G2。

3.根据权利要求1所述的一种基于惯性寻北装置的抗冲击寻北解算方法，其特征在于：所述的步骤(4)中，格拉布斯滤波算法的迭代循环次数小于等于10次，数据长度预设下限为输入数据长度的40％。

4.根据权利要求1所述的一种基于惯性寻北装置的抗冲击寻北解算方法，其特征在于：所述的步骤(5)中，所述的G5根据公式(13)得出，所述的A5根据公式(14)得到：

其中，

5.根据权利要求1所述的一种基于惯性寻北装置的抗冲击寻北解算方法，其特征在于：所述的步骤(8)中，正弦函数叠加个数小于等于5个，所述的圆周补偿模型参数的标定和计算方法如下：

(a)、将惯性寻北装置置于回转工作台上，转台归零；

(b)、从0°至350°依次间隔10°测量36个点的圆周，转台定位误差＜0.001°，记录航向测量数据；

(c)、通过MATLAB软件曲线拟合工具，导入测量数据，并按照正弦函数叠加模型拟合误差曲线，计算补偿参数，具体包括有以下步骤：首先选择拟合模型为单正弦函数a1*sin(b1*x+c1)，若拟合结果RMSE≤0.03，记录参数，拟合完成；若拟合结果RMSE＞0.03，需对测试数据进行评估，评估方法为计算36个角度增量偏差值，若计算的角增量偏差值大于0.4°，表明该位置测试数据异常，将此位置数据复测6次取均值后重新进行拟合；若计算的角增量偏差值小于等于0.4°，评估结果正常，则将正弦函数叠加模型的叠加数量加1，再次进行拟合和评估，重复多次直至得到满足要求的参数，如果叠加个数超过设定次数，则判定拟合失败，对惯性寻北装置进行重新装配调试；

(d)、将拟合得到的圆周补偿参数的值烧录到惯性寻北装置内部的信号处理板上的内存中即可。