CN102169127B - 一种具有自适应能力的机抖激光陀螺实时去抖方法 - Google Patents

Abstract

一种具有自适应能力的机抖激光陀螺实时去抖方法，对陀螺信号和抖动信号进行低于两倍抖动频率的差频采样，得到陀螺角增量G_av和抖动角增量D_av，并分别累加得到陀螺角度G_a和抖动角度D_a，然后依次进行更新抖动频率f_d、更新相位差

及比例系数k_d以及数字移相去抖。本发明对常规去抖方法进行改进，对抖动的频率、振幅和相位的变化具有自适应能力，运算量小并具有实时性，能够更加有效减小机抖激光陀螺输出中的抖动角速度误差，有效避免因直接对陀螺角增量去抖而带来的随机游走。

Description

一种具有自适应能力的机抖激光陀螺实时去抖方法

技术领域

本发明涉及一种机抖激光陀螺的抖动去除的方法，特别是一种具有自适应能力的机抖激光陀螺实时去抖方法，用于激光陀螺中。

背景技术

机抖激光陀螺在捷联式惯导系统中已经得到了广泛应用，为了减小闭锁效应的影响，通过机械抖动人为引入了一个正弦交变角速度，使得陀螺的输出不仅包含了载体的运动角速度信息，还包含了抖动角速度，所以要对陀螺输出进行解调以去除抖动的影响。目前国内外有许多关于机抖激光陀螺去除抖动的文献，一般都采用相关滤波或者自适应滤波，也有将它们与陷波滤波器或者FIR滤波器相结合的方法，这些方法虽然有效但都需要高频采样(至少1kHz)，对硬件采样电路和数据处理速度的要求较高，增加了系统成本，还会降低系统的可靠性。

人们经常采用的一种简单的常规去抖方法为：同时对陀螺输出和抖动输出进行采样，然后将抖动输出乘以事先测定的比例系数再从陀螺输出中减去。该方法无需高频采样，可以在导航解算的同时进行实时去抖，但是它最大的缺点就在于陀螺测出的抖动角速度与抖动输出之间经常有相位差，并且随着环境因素的变化，抖动频率、相位差和比例系数可能发生变化，最终导致陀螺输出中还残留有较大的抖动角速度误差。

发明内容

本发明的技术解决问题：针对现有技术的不足，本发明提出了一种具有自适应能力的机抖激光陀螺实时去抖方法，该方法对抖动的频率、振幅和相位的变化具有自适应能力，能够有效减小激光陀螺输出中的抖动角速度误差并具有实时性。

本发明的技术解决方案：一种具有自适应能力的机抖激光陀螺实时去抖方法，实现如下：对陀螺信号和抖动信号进行差频采样，即陀螺和抖动信号的采样频率低于两倍的抖动信号的抖动频率，得到陀螺角增量G_av和抖动角增量D_av，分别对陀螺角增量G_av和抖动角增量D_av进行累加，得到陀螺角度G_a和抖动角度D_a；然后更新抖动频率f_d，再采用最小二乘法更新相位差

及比例系数k_d；最后采用数字移相滤波器根据f_d和

对抖动角度D_a进行数字移相，并由k_d以及数字移相滤波器增益系数K对陀螺角度G_a进行去抖，差分还原成角增量输出。

所述更新抖动频率f_d的方法为：当达到抖动频率f_d的更新周期时，判断抖动角度D_a与上次更新周期内计算出的D_a的平均值D_a0之差若大于阈值TH_d，则频率计数标志置位；若小于-TH_d，则频率计数标志清零，并且频率计数值加1；当到达更新周期时，频率计数值除以更新周期值即为f_d的更新值。

所述抖动频率f_d的更新周期为3秒钟。

所述阈值TH_d的大小为抖动角度D_a振幅的一半。

所述更新相位差

及比例系数k_d方法为：在达到

和k_d的更新周期时，认为抖动信号为正弦波，先对陀螺角增量G_av去除均值得到G_av1，然后利用最小二乘法，分别对G_av1的振幅A_g和相位

以及抖动角增量D_av的振幅A_d和相位

进行拟合，再按照(1)式得到更新后的

和k_d，

所述的更新

和k_d的周期是1分钟，计算

和k_d的周期为0.025秒，当达到更新周期时，将这一周期内计算的

和k_d的平均值作为更新结果。

所述数字移相器采用一阶数字滤波器，传递函数为

$F\left(s\right)=\frac{1+{\mathrm{\τ}}_{2}s}{1+{\mathrm{\τ}}_{1}s}---\left(2\right)$

根据f_d和

数字移相滤波器传递函数中分母的一阶参数τ₁和分子的一阶参数τ₂的计算公式为

再将(2)式按照双线性变换法转换成一阶数字滤波器。

所述对对陀螺角度G_a按照(4)式进行，去抖后得到G_aout，其中K是数字移相器的增益系数，最后再对G_aout进行差分得到陀螺角增量，

$G_{\mathrm{aout}}=G_a-\frac{k_d}{K}{D}_{\mathrm{az}}---\left(4\right).$

本发明与现有技术相比的有益效果在于：

(1)本发明对常规去抖方法进行改进，实时测算抖动频率、相位差及比例系数，具有自适应能力，而所需的陀螺和抖动信号的采样频率低于两倍的抖动频率，例如抖动频率为440Hz则采样频率可以是400Hz，这避免了高频采样所带来的问题；

(2)本发明中提出的更新抖动频率的方法是对抖动角增量累加后的抖动角度设阈值进行比较再计数，而抖动角度的振幅相对于角增量更加稳定，噪声更小，所以可以简单有效的计算出抖动频率，分辨率为0.33Hz；

(3)本发明中提出的一种更新相位差及比例系数的方法，采用0.025s的计算周期和1分钟的更新周期，这样既能够在计算相位差和比例系数的过程中有效去除陀螺角增量的均值，保证了计算结果的准确性，又能够通过对1分钟内的计算结果取平均来抑制更新结果中的噪声，并且计算周期中采用的最小二乘法也使得算法的运算量达到最小；

(4)本发明中提出的一种实时移相去抖方法，所用的移相器是一阶数字滤波器，结构简单，能够精确跟随测得的频率和相位差，对输入信号进行实时的数字移相，精确的去抖操作不仅可以有效减小陀螺输出中的抖动角速度误差，而且可以有效避免因直接对角增量去抖所引入的随机游走；

(5对某型抖动频率为443Hz，当量为0.4663″/pul的机抖激光陀螺使用本发明进行去抖，采样频率为400Hz，并与常规去抖的结果进行比较，实验结果表明，采用常规去抖后的角增量输出的标准差为4.1pul/0.025s，而实施本发明后的角增量输出的标准差减小为2.2pul/0.025s，去抖效果明显。

附图说明

图1为本发明的总流程图；

图2为本发明的更新抖动频率的程序流程图；

图3为本发明的更新相位差及比例系数的程序流程图；

图4为本发明的移相去抖的程序流程图；

图5为常规去抖和实施本发明后的陀螺角增量输出。

具体实施方式

如图1所示，在对各参数完成初始化后，对陀螺信号和抖动信号进行差频采样，抖动频率一般在400～500Hz之间，由于采样频率低于两倍的抖动频率，所以在采样过程中产生频率混叠，例如抖动频率为440Hz，采样频率为400Hz，则采样信号中的抖动频率为40Hz，以下针对400Hz的采样频率进行具体说明。采样得到陀螺角增量G_av和抖动角增量D_av，按照(5)式对其进行累加分别得到陀螺角度G_a和抖动角度D_a。

$\left\{\begin{array}{c}{G}_{\mathrm{ai}}=\underset{k=1}{\overset{i}{\mathrm{\Σ}}}{G}_{\mathrm{avk}}\\ D_{\mathrm{ai}}=\underset{k=1}{\overset{i}{\mathrm{\Σ}}}{D}_{\mathrm{avk}}\end{array}\right.---\left(5\right)$

然后更新抖动频率f_d，再采用最小二乘法更新相位差

及比例系数k_d，计算中认为抖动信号是正弦波并利用f_d上一次的更新结果，最后采用一阶数字移相滤波器根据f_d和

对抖动角度D_a进行数字移相，并由k_d以及滤波器增益系数对陀螺角度G_a进行去抖，最终差分还原成角增量输出。

图2是实施本发明的更新抖动频率的程序流程图。抖动频率f_d的更新周期为3秒钟，每次调用该程序时更新周期计数器k₁加1，然后将抖动角度D_a与上次更新周期内计算出的D_a的平均值D_a0相减，再与阈值TH_d进行比较，若D_a-D_a0大于TH_d则计数标志Flag置1；若D_a-D_a0小于-TH_d则计数标志Flag置0，并且抖动频率计数器Cnt加1，即又采到了一个抖动周期的正弦波。由于D_a的振幅相对稳定，所以选择TH_d为D_a振幅的一半。当计数器k₁的值为1200时，更新抖动频率f_d＝Cnt/3，单位是Hz，同时将Flag、Cnt和k₁清零。

图3是实施本发明的更新相位差及比例系数的程序流程图。在计算和k_d时要对陀螺角增量G_av去除均值，若计算周期太长会影响去除均值的效果，而太短又会使计算结果中的噪声过大，所以计算

和k_d的周期为0.025秒，再每1分钟对

和k_d的结果取平均作为最终的更新结果。每次调用该程序时更新周期计数器k₂加1，计算周期计数器k₃加1。若计算周期计数器k₃的值为10，将计数器k₃清零，此时得到一组陀螺角增量G_av和抖动角增量D_av，对这组G_av去除均值得到G_av1。计算中认为抖动信号为正弦波，则有

其中G_av1i和G_avi分别为第i时刻(i＝1，2…10)的G_av1和D_av，Δt＝0.0025s，A_g和分别为陀螺角增量中感测到的抖动振幅和相位，A_d和

为抖动角增量的振幅和相位，f_d是上一次的更新结果。令

$H=\left[\begin{array}{cc}\sin (2\mathrm{\π}{f}_d\·\mathrm{\Δt})& \cos (2\mathrm{\π}{f}_d\·\mathrm{\Δt})\\ \sin (2\mathrm{\π}{f}_d\·2\mathrm{\Δt})& \cos (2\mathrm{\π}{f}_d\·2\mathrm{\Δt})\\ .& .\\ .& .\\ .& .\\ \sin (2\mathrm{\π}{f}_d\·10\mathrm{\Δt})& \cos (2\mathrm{\π}{f}_d\·10\mathrm{\Δt})\end{array}\right]$

由最小二乘法则有

$\left\{\begin{array}{c}{X}_g={\left(H^{T}H\right)}^{-1}{H}^T{Z}_g\\ X_d={\left(H^{T}H\right)}^{-1}{H}^T{Z}_d\end{array}\right.---\left(7\right)$

其中(H^TH)^-1H^T只在更新f_d时计算。

从而有

最终得到

当更新周期计数器k₂的值为24000时，将这一分钟内计算出的

和k_d1的平均值作为

和k_d的更新结果，同时计数器k₂清零。

图4是实施本发明的移相去抖的程序流程图。数字移相器采用一阶数字滤波器，其传递函数为

$F\left(s\right)=\frac{1+{\mathrm{\τ}}_{2}s}{1+{\mathrm{\τ}}_{1}s}---\left(10\right)$

其幅频和相频特性函数为

$A\left(\mathrm{\ω}\right)=\left|\frac{1+{\mathrm{\τ}}_2\mathrm{j\ω}}{1+{\mathrm{\τ}}_1\mathrm{j\ω}}\right|=\sqrt{\frac{1+{{\mathrm{\τ}}_2}^2{\mathrm{\ω}}^2}{1+{{\mathrm{\τ}}_1}^2{\mathrm{\ω}}^2}}---\left(11\right)$

$B\left(\mathrm{\ω}\right)=-{\tan }^{-1}\frac{({\mathrm{\τ}}_1-{\mathrm{\τ}}_2)\mathrm{\ω}}{1+{\mathrm{\τ}}_1{\mathrm{\τ}}_2{\mathrm{\ω}}^2}---\left(12\right)$

要求在抖动频率处B′(ω_d)＝0，其中ω_d＝2πf_d，所以有

${\mathrm{\τ}}_1{\mathrm{\τ}}_2=\frac{1}{{{\mathrm{\ω}}_d}^2}---\left(13\right)$

根据f_d和

由(12)式和(13)式可得

同时可以得到数字移相器的增益系数K为

$K=\sqrt{\frac{1+{{\mathrm{\τ}}_2}^2{\left(2{\mathrm{\πf}}_d\right)}^2}{1+{{\mathrm{\τ}}_1}^2{\left(2{\mathrm{\πf}}_d\right)}^2}}---\left(15\right)$

然后再将(10)式按照双线性变换法转换成一阶数字滤波器，并对D_a进行数字移相得到D_az，再对陀螺角度G_a进行去抖，得到

$G_{\mathrm{aout}}=G_a-\frac{k_d}{K}{D}_{\mathrm{az}}---\left(16\right)$

最后再对G_aout进行差分得到陀螺角增量，即

G_avout(i)＝G_aout(i)-G_aout(i-1)                        (17)

其中G_aout(i)和G_aout(i-1)分别为当前时刻和上一时刻的去抖后的陀螺角度，G_avout(i)为当前时刻的去抖后的陀螺角增量。

图5是常规去抖和实施本发明后的陀螺角增量输出，图中的细线是常规去抖的结果，粗线是实施本发明后的结果。对某型抖动频率为443Hz，当量为0.4663″/pul的机抖激光陀螺使用本发明进行去抖，采样频率为400Hz，并与常规去抖的结果进行比较，实验结果表明，采用常规去抖后的角增量输出的标准差为4.1pul/0.025s，而实施本发明后的角增量输出的标准差减小为2.2pul/0.025s，可见本发明去抖效果明显。

本发明未详细阐述部分属于本领域公知技术。

Claims (5)

1.一种具有自适应能力的机抖激光陀螺实时去抖方法，其特征在于实现如下：对陀螺信号和抖动信号进行差频采样，即陀螺和抖动信号的采样频率低于两倍的抖动信号的抖动频率，得到陀螺角增量G_av和抖动角增量D_av，分别对陀螺角增量G_av和抖动角增量D_av进行累加，得到陀螺角度G_a和抖动角度D_a；然后更新抖动频率f_d，再采用最小二乘法更新相位差 

及比例系数k_d；最后采用数字移相滤波器根据f_d和 

对抖动角度D_a进行数字移相，并由k_d以及数字移相滤波器增益系数K对陀螺角度G_a进行去抖，差分还原成角增量输出；

所述更新抖动频率f_d的方法为：当达到抖动频率f_d的更新周期时，判断抖动角度D_a与上次更新周期内计算出的D_a的平均值D_a0之差若大于阈值TH_d，则频率计数标志置位；若小于-TH_d，则频率计数标志清零，并且频率计数值加1；当到达更新周期时，频率计数值除以更新周期值即为f_d的更新值；

所述更新相位差 

及比例系数k_d方法为：在达到 

和k_d的更新周期时，认为抖动信号为正弦波，先对陀螺角增量G_av去除均值得到G_av1，然后利用最小二乘法，分别对G_av1的振幅A_g和相位 以及抖动角增量D_av的振幅A_d和相位 

进行拟合，再按照(1)式得到更新后的 

和k_d，

2.根据权利要求1所述的具有自适应能力的机抖激光陀螺实时去抖方法，其特征在于：所述抖动频率f_d的更新周期为3秒钟。

3.根据权利要求1所述的具有自适应能力的机抖激光陀螺实时去抖方法，其特征在于：所述阈值TH_d的大小为抖动角度D_a振幅的一半。

4.根据权利要求1所述的具有自适应能力的机抖激光陀螺实时去抖方法，其特征在于：所述的更新 

和k_d的周期是1分钟，计算 

和k_d的周期为0.025秒，当达到更新周期时，将这一周期内计算的 

和k_d的平均值作为更新结果。

5.根据权利要求1所述的具有自适应能力的机抖激光陀螺实时去抖方法，其特征在于：所述数字移相滤波器采用一阶数字滤波器，传递函数为 

根据f_d和 数字移相滤波器传递函数中分母的一阶参数τ₁和分子的一阶参数τ₂的计算公式为

再将(2)式按照双线性变换法转换成一阶数字滤波器。 

Images