一种晃动条件下重力仪的自对准方法与流程

本发明属于重力仪自对准领域，特别涉及一种晃动条件下重力仪的自对准方法。

背景技术：

基于惯性稳定平台的海空重力仪是利用地球自转角速率和重力完成自对准，然而在晃动条件下，重力仪敏感到的地球自转角速率和重力包含了由于载体运动带来的干扰，这些干扰直接影响重力仪自对准精度，甚至导致自对准失败。

技术实现要素：

根据上述问题，本发明提出了一种重力仪自对准方法，解决重力仪在晃动条件下无法完成高精度自对准的问题。晃动条件下自对准方法主要是利用调平回路和已有陀螺漂移完成重力仪的粗寻北，为精对准建立条件；在精对准阶段，利用重力仪误差方程和卡尔曼滤波器，完成精调平和精寻北，进而完成自对准。

为了实现这一目的，本发明采取的技术方案是：

一种晃动条件下重力仪的自对准方法，包括三个步骤，步骤一为初扶正，步骤二为第一位置对准，步骤三为第二位置对准，初扶正是将平台框架角归到零位，为启动平台为后续的对准提供良好的环境。

一种晃动条件下重力仪的自对准方法，所述步骤二、第一位置对准包括四个步骤，步骤如下；

步骤一、粗调平和粗寻北

(1)水平轴二阶调平回路；

(a)水平x轴二阶调平回路；

sinsvx＝sinsvx+(fa_x-mk1*sinsvx)*h

式中：

fa_x—所采集x轴加速度通道输出值；

sinsvx—平台坐标系x轴绝对速度；

h—计算周期10ms；

mk1—调平回路参数；

wkhx＝(sinsvx*(1+mk2))/r

式中：

wkhx—平台坐标系x轴校正角速率；

mk2—调平回路参数；

r—地球半径；

(b)水平y轴二阶调平回路

sinsvy＝sinsvy+(fa_y-mk1*sinsvy)*h

式中：

fa_y—所采集x轴加速度通道输出值；

sinsvy—平台坐标系x轴绝对速度；

h—计算周期10ms；

mk1—调平回路参数；

wkhy＝(sinsvy*(1+mk2))/r

式中：

wkhy—平台坐标系x轴校正角速率；

mk2—调平回路参数；

r—地球半径；

(c)方位粗锁定

vka1z＝vka1z+(d_az-kz1*vka1z)*h；

vka2z＝vka1z*kz2/r*g；

vdrz＝vdrz+kz3/r*g*vka1z*h；

vkaz＝vdrz+vka2z；

wkaz＝vkaz；

wdrz＝vdrz；

式中：

kz1、kz2、kz3—锁定回路参数；

vka1z、vka2z—计算中间变量；

r—地球半径；

d_az—锁定目标角；

wdrz—平台坐标系z轴漂移值；

wkaz、vkaz—平台坐标系z轴偏差角速率；

vdrz—平台坐标系z轴漂移值估算；

g—重力加速度；

(d)粗寻北

wx1＝wkhx+wbx；

wy1＝wkhy+wby；

sreal_hx0＝2.0*pi+atan2((-wy1+drift_y_old1),(wx1-drift_x_old1))；

式中：

wbx、wby为陀螺漂移的标定值；

wkhx、wkhy为平台坐标系x轴和y轴的偏差角速率；

wx1、wx1为平台坐标系x轴和y轴的修正角速率；

drift_x_old1、drift_y_old1为存在计算机中的x、y轴陀螺漂移；

sreal_hx0为方位角；

步骤二、快转

az_b＝trdcz-pi/2.+sreal_hx0；

式中：

trdcz为对准时从测试设备的装订值；

sreal_hx0为方位角；

az_b为快转目标角；

步骤三、粗对准

由于快转完成后，原来建立的调平和锁定都被打破，所以再进行一次粗调平和粗寻北步骤，所述粗对准过程和粗调平、粗寻北过程一致；

步骤四、精调平、精对准

(1)建立两位置对准误差模型

其中：

δvx、δvy为x、y方向速度误差；αx、αy、αz为x、y、z方向的水平失准角；εx、εy、εz为x、y、z方向的陀螺漂移；

(2)建立量测方程

zx＝δvx＝vx-0zy＝δvy＝vy-0

其中；

vx、vy：重力仪速度误差；

(3)使用kalman滤波算法

状态一步预测：

状态估计：

最优滤波增益：

或

一步预测均方误差阵：

估计均方误差阵：

或

pk＝(i-kkhk)pk/k-1

或

式中：

xk为n维状态向量；

φk,k-1为n×n维一步状态转移矩阵；

qk为系统噪声矩阵；

wk-1为r维系统噪声向量；

zk为m维量测向量；

hk为m×n维量测矩阵；

rk为m维量测噪声；

(4)进行实时反馈修正

(a)实时物理反馈修正

将kalman滤波实时估计出来的失准角转换成修正角速率，然后通过施矩进行实时反馈修正，同时为了保证修正过程中的振荡和发散，修正角速率根据失准角的大小采用线性区设置，具体设置如下：

注：

α：失准角；

ωα：修正角速率；

(b)状态数字修正

在进行完物理反馈修正后，为了保证状态变量和实际系统对应的变量一致，在进行下一次kalman滤波之前，需要对相应的状态变量进行数字修正，具体修正公式如下；

αxk+1＝αxk+t×kα×αxk

αyk+1＝αyk+t×kα×αyk

其中：

kv：速度反馈修正系数，暂取0.1；kα：失准角反馈修正系数，暂取1.；t：滤波周期，单位ms。

一种晃动条件下重力仪的自对准方法，：所述步骤三、第二位置对准，当第一位置对准时间到达设计时间时，通过快转将台体相对第一位置转动90度到达第二位置，第二位置也分为粗对准、精对准，步骤如下：

(1)、粗对准

粗对准过程和步骤一、第一位置对准中的粗调平、粗寻北中的调平和锁定算法一致；

(2)、精对准

精对准过程和步骤一、第一位置对准中的精对准的算法一致，在第二位置精对准时间内，通过不断的滤波估计和实时修正，使失准角减小到要求值以内，并估计北向陀螺漂移。

本发明的有益效果为：

传统方法在晃动条件下自对准时，由于地球自转角速率和重力受到干扰，影响了在晃动条件下重力仪的自对准精度；采用本方法后，在晃动条件下重力仪的自对准精度得到了提升，并满足技术指标要求。

附图说明

图1为自对准流程图

图2为水平x轴二阶调平回路结构图

图3为水平y轴二阶调平回路结构图

图4为方位锁定回路结构图

图5为kalman滤波解算框图

具体实施方式

本发明具体实施例如下：

本发明提出的方法包括三个步骤，步骤一为初扶正、步骤二为第一位置对准，步骤三为第二位置对准。所述初扶正是将平台框架角归到零位。

第一位置对准完成粗调平和粗寻北，根据粗寻北的结果将台体x轴快转到北向，之后完成粗对准、精对准、估计εy。

第一位置对准完成后，将台体y轴转动到北向上，进入第二位置对准。第二位置对准完成粗对准、精对准、估计εx。自对准的简单流程如图1所示。

每个位置均有粗对准和精对准，其中粗对准是传统的对准回路，精对准阶段有传统对准回路和kalman滤波回路。

本方法具体步骤如下：

步骤一、初扶正

初扶正是将平台框架角归到零位(参见专利《一种动力调谐陀螺稳定平台动基座启动方法》，其中将外部的姿态信息置为零)，目的是启动平台为后续的对准提供良好的环境。

步骤二、第一位置对准

1.粗调平和粗寻北

(1)水平轴二阶调平回路。

(a)水平x轴二阶调平回路。

水平x轴二阶调平回路结构图如图2所示。

sinsvx＝sinsvx+(fa_x-mk1*sinsvx)*h

式中：

fa_x—所采集x轴加速度通道输出值；

sinsvx—平台坐标系x轴绝对速度；

h—计算周期10ms

mk1—调平回路参数。

wkhx＝(sinsvx*(1+mk2))/r

式中：

wkhx—平台坐标系x轴校正角速率；

mk2—调平回路参数。

r—地球半径。

(b)水平y轴二阶调平回路

水平y轴二阶调平回路结构图如图3所示。

sinsvy＝sinsvy+(fa_y-mk1*sinsvy)*h

式中：

fa_y—所采集x轴加速度通道输出值；

sinsvy—平台坐标系x轴绝对速度；

h—计算周期10ms；

mk1—调平回路参数。

wkhy＝(sinsvy*(1+mk2))/r

式中：

wkhy—平台坐标系x轴校正角速率；

mk2—调平回路参数。

r—地球半径。

(c)方位粗锁定

方位锁定回路结构图如图4所示。

vka1z＝vka1z+(d_az-kz1*vka1z)*h；

vka2z＝vka1z*kz2/r*g；

vdrz＝vdrz+kz3/r*g*vka1z*h；

vkaz＝vdrz+vka2z；

wkaz＝vkaz；

wdrz＝vdrz；

式中：

kz1、kz2、kz3—锁定回路参数；

vka1z、vka2z—计算中间变量；

r—地球半径；

d_az—锁定目标角；

wdrz—平台坐标系z轴漂移值；

wkaz、vkaz—平台坐标系z轴偏差角速率；

vdrz—平台坐标系z轴漂移值估算；

g—重力加速度；

(d)粗寻北

wx1＝wkhx+wbx；

wy1＝wkhy+wby；

sreal_hx0＝2.0*pi+atan2((-wy1+drift_y_old1),(wx1-drift_x_old1))；

式中：

wbx、wby为陀螺漂移的标定值；

wkhx、wkhy为平台坐标系x轴和y轴的偏差角速率；

wx1、wx1为平台坐标系x轴和y轴的修正角速率；

drift_x_old1、drift_y_old1为存在计算机中的x、y轴陀螺漂移；

sreal_hx0为方位角。

2.快转

az_b＝trdcz-pi/2.+sreal_hx0；

式中：

trdcz为对准时从测试设备的装订值；

sreal_hx0为方位角；

az_b为快转目标角。

3.粗对准

由于快转完成后，原来建立的调平和锁定都被打破，所以再进行一次粗调平和粗寻北步骤，所述粗对准过程和粗调平、粗寻北过程一致。

4.精调平、精对准

(1)建立两位置对准误差模型

其中：

δvx、δvy为x、y方向速度误差；αx、αy、αz为x、y、z方向的水平失准角；εx、εy、εz为x、y、z方向的陀螺漂移。

(2)建立量测方程

zx＝δvx＝vx-0zy＝δvy＝vy-0

其中；

vx、vy：重力仪速度误差。

(3)使用kalman滤波算法

kalman滤波解算框图如图5所示。

状态一步预测：

状态估计：

最优滤波增益：

或

一步预测均方误差阵：

估计均方误差阵：

或

pk＝(i-kkhk)pk/k-1

或

式中：

xk为n维状态向量；

φk,k-1为n×n维一步状态转移矩阵；

qk为系统噪声矩阵；

wk-1为r维系统噪声向量；

zk为m维量测向量；

hk为m×n维量测矩阵；

rk为m维量测噪声。

注：在状态预测后，先进行状态修正，再进行状态估计。

(4)进行实时反馈修正

(a)实时物理反馈修正

将kalman滤波实时估计出来的失准角转换成修正角速率，然后通过施矩进行实时反馈修正，同时为了保证修正过程中的振荡和发散，修正角速率根据失准角的大小采用线性区设置，具体设置如下：

注：

α：失准角

ωα：修正角速率。

(b)状态数字修正

在进行完物理反馈修正后，为了保证状态变量和实际系统对应的变量一致，在进行下一次kalman滤波之前，需要对相应的状态变量进行数字修正，具体修正公式如下。

αxk+1＝αxk+t×kα×αxk

αyk+1＝αyk+t×kα×αyk

其中：

kv：速度反馈修正系数，暂取0.1；kα：失准角反馈修正系数，暂取1.；t：滤波周期，单位ms。

传统方法在晃动条件下自对准时，由于地球自转角速率和重力受到干扰，影响了在晃动条件下重力仪的自对准精度；采用本方法后，在晃动条件下重力仪的自对准精度得到了提升，并满足技术指标要求。

步骤三、第二位置对准

当第一位置对准时间到达设计时间时，通过快转将台体相对第一位置转动90度到达第二位置。第二位置也分为粗对准、精对准。

1.粗对准

粗对准过程和第一位置粗调平、粗寻北中的调平和锁定算法一致。

2.精对准

精对准过程和第一位置精对准的算法一致。在第二位置精对准时间内，通过不断的滤波估计和实时修正，可以使失准角减小到要求值以内，并估计北向陀螺漂移。