本發(fā)明屬于慣性導(dǎo)航技術(shù)領(lǐng)域，具體涉及一種雙軸光纖慣導(dǎo)系統(tǒng)快速自標(biāo)定自對準(zhǔn)方法。

背景技術(shù)：

國內(nèi)外雙軸激光旋轉(zhuǎn)慣導(dǎo)系統(tǒng)的研究相對較為成熟，激光陀螺標(biāo)度因數(shù)穩(wěn)定，受溫度磁場影響較小，所以系統(tǒng)設(shè)計相對簡單。而光纖陀螺具有可靠性高，無機(jī)抖裝置利于旋轉(zhuǎn)機(jī)構(gòu)的控制，且陀螺隨機(jī)游走誤差相對較小利于慣導(dǎo)系統(tǒng)對準(zhǔn)的快速收斂等優(yōu)點(diǎn)。所以，雙軸光纖慣導(dǎo)系統(tǒng)引起越來越多人的注意，但光纖陀螺具有自身的缺陷：對溫度和磁場較為敏感，同時標(biāo)度因數(shù)和零偏穩(wěn)定性較差。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明需要解決的技術(shù)問題為：針對光纖陀螺的特性，提供一種適用于雙軸光纖慣導(dǎo)系統(tǒng)的快速自標(biāo)定自對準(zhǔn)方法。

本發(fā)明是這樣實(shí)現(xiàn)的：一種雙軸光纖慣導(dǎo)系統(tǒng)快速自標(biāo)定自對準(zhǔn)方法，包括以下步驟：

步驟1、自標(biāo)定自對準(zhǔn)旋轉(zhuǎn)流程

30min系統(tǒng)自標(biāo)定自對準(zhǔn)的旋轉(zhuǎn)流程如下所示：

X軸向旋轉(zhuǎn)流程

X軸以30°/s正向旋轉(zhuǎn)450°，旋轉(zhuǎn)完成后停留1s，用時16s；

Z軸向旋轉(zhuǎn)流程，依次進(jìn)行下述旋轉(zhuǎn)

Z軸以30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留38s，用時44s；

Z軸以30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留38s，用時44s；

Z軸以30°/s正向旋轉(zhuǎn)90°，旋轉(zhuǎn)完成后停留2s，用時5s；

地向旋轉(zhuǎn)流程

X軸以30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以-30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以-30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以30°/s正向旋轉(zhuǎn)90°，旋轉(zhuǎn)完成后停留1s，用時4s；

Y軸向旋轉(zhuǎn)流程

Y軸以30°/s正向旋轉(zhuǎn)540°，旋轉(zhuǎn)完成后停留1s，用時19s；

天向旋轉(zhuǎn)流程

X軸以30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以-30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以-30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

以上步驟中每個旋轉(zhuǎn)流程循環(huán)5次，即X軸向旋轉(zhuǎn)循環(huán)5次，地向旋轉(zhuǎn)循環(huán)5次，天向旋轉(zhuǎn)循環(huán)5次，

將以上流程循環(huán)6次，即整個步驟1循環(huán)6次，

采集三個陀螺和三個加速度計的輸出數(shù)據(jù)，內(nèi)環(huán)和外環(huán)的旋轉(zhuǎn)角度，

步驟2、自標(biāo)定自對準(zhǔn)誤差模型建立

系統(tǒng)狀態(tài)方程和量測方程如下：

狀態(tài)方程：

觀測方程：Z＝HX+V

H＝[I_5×5 0_5×24]，

為的第一列，

其中，

δV_n、δV_u、δV_e為慣導(dǎo)北、天、東速度誤差；

δL、δλ為慣導(dǎo)緯度和經(jīng)度誤差；

φ_n、φ_u、φ_e為慣導(dǎo)北、天、東失準(zhǔn)角；

ε_x為慣導(dǎo)X軸(天向)陀螺儀零偏；

為慣導(dǎo)X、Y、Z軸加速度計零偏；

δK_gx、δK_gy、δK_gz為陀螺儀標(biāo)度因數(shù)誤差；

δG_xy、δG_yx、δG_xz、δG_zx、δG_yz、δG_zy為陀螺儀安裝誤差；

為X軸朝上和朝下時，等效Y、Z加速度計零偏的不對稱性；

以上23個參數(shù)為待求解參數(shù)，

A_out為外環(huán)角度，該參數(shù)由外部給出，

R為地球半徑，該參數(shù)為常數(shù)，由外部給出；

ω_ie為地球自轉(zhuǎn)角速率，該參數(shù)為常數(shù)，由外部給出；

f_N、f_U、f_E為慣導(dǎo)北、天、東向加速度，該參數(shù)由外部給出；

表示的第一列和第二列；和sign是取內(nèi)部數(shù)據(jù)的符號，若內(nèi)部函數(shù)大于等于0取正，小于0取負(fù)

V_U天向速度、V_N北向速度、V_E動向速度，該三個參數(shù)的數(shù)值由導(dǎo)航系統(tǒng)給出，L為緯度、外部給定

請解釋C₁₁～C₃₃是矩陣中的元素，f_x、f_y是慣導(dǎo)x和y向的加速度，其數(shù)值由加速度計輸出，經(jīng)過誤差補(bǔ)償后獲得，

ω包含ω_x、ω_y、ω_z，其中ω_x、ω_y、ω_z分別為x、y、z方向的角速度，其數(shù)值由陀螺輸出經(jīng)誤差補(bǔ)償后獲得，

W＝[w₁ w₂ w₃ 0 0 w₄ w₅ w₆ 0 … 0]^T為23維系統(tǒng)噪聲列向量，w₁、w₂、w₃、w₄、w₅、w₆均為零均值隨機(jī)白噪聲；

V＝[v₁ v₂ v₃ v₄ v₅]^T為5維量測噪聲列向量，v₁、v₂、v₃、v₄、v₅均為零均值隨機(jī)白噪聲，

步驟3、粗對準(zhǔn)算法

由陀螺采集數(shù)據(jù)經(jīng)誤差補(bǔ)償后得到的角增量更新四元數(shù)：

其中：k初值為0，表示當(dāng)前時刻；k-1表示上一時刻；I為4×4階單位矩陣；

由計算姿態(tài)矩陣的公式如下：

其中q₀～q₃用本步驟最開始的公式進(jìn)行計算，計算得到中的四個元素就為q₀～q₃，

將加速度計采集的速度增量Δv(k)＝[v_xk v_yk v_xk]^T，利用姿態(tài)矩陣可求得i_b0系下速度：

與互為轉(zhuǎn)置

用下述公式計算速度

T_n采樣間隔，外部給定、g重力加速度、L是緯度，

計算：

取中間時刻和該中間時刻指粗對準(zhǔn)時間的一半，以及粗對準(zhǔn)結(jié)束時刻和可構(gòu)造速度矩陣如下：

利用最后得到的計算

步驟4、精對準(zhǔn)導(dǎo)航濾波

粗對準(zhǔn)完成后，系統(tǒng)旋轉(zhuǎn)機(jī)構(gòu)開始執(zhí)行自標(biāo)定自對準(zhǔn)旋轉(zhuǎn)流程，系統(tǒng)開始進(jìn)行導(dǎo)航解算，其中陀螺標(biāo)度因數(shù)誤差初始值設(shè)為1，其余各個誤差系數(shù)的初始值都設(shè)定為0；同時進(jìn)行“速度+位置”匹配閉環(huán)卡爾曼濾波估計，主要涉及以下幾個誤差的修正，

姿態(tài)誤差的修正

濾波估計得到的姿態(tài)誤差為則姿態(tài)修正方程為：

Tn,是采樣間隔、的意義可以統(tǒng)一表示如下，以為例，其表示的是e坐標(biāo)系相對于n坐標(biāo)系的角速度的分量表示，

速度誤差的修正

濾波估計得到的速度誤差為δV_n,δV_u,δV_e，則速度修正方程為：

V_i＝V_i-δV_i (i＝n、u、e)

位置誤差的修正

濾波估計得到的位置誤差為δL和δλ，則位置修正方程為：

h＝h-δh

λ＝λ-δλ

誤差參數(shù)的修正

首先更新待辨識的各誤差參數(shù)，如下所示：

ε_x＝ε_x+X(9)

δK_gx＝δK_gx+X(13)

δK_gy＝δK_gy+X(14)

δK_gz＝δK_gz+X(15)

δG_xy＝δG_xy+X(16)

δG_yx＝δG_yx+X(17)

δG_xz＝δG_xz+X(18)

δG_zx＝δG_zx+X(19)

δG_yz＝δG_yz+X(20)

δG_zy＝δG_zy+X(21)

X(9)～X(23)表示X中第9～23個參數(shù)的數(shù)值，

由此修正各誤差參數(shù)：

D_x＝D_x+ε_x

K_Gx＝K_Gx·δK_Gx

K_Gy＝K_Gy0·δK_Gy

K_Gz＝K_Gz0·δK_Gz

G_xy＝G_xy+δG_xy

G_yx＝G_yx+δG_yx

G_xz＝G_xz+δG_xz

G_zx＝G_zx+δG_zx

G_yz＝G_yz+δG_yz

G_zy＝G_zy+δG_zy

上述計算迭代進(jìn)行，因此使用相同符號表示，

誤差進(jìn)行閉環(huán)修正后，各狀態(tài)變量清零，

X(i)＝0，i＝1,2…23

每一個循環(huán)結(jié)束時，要對中間變量進(jìn)行處理：

δG_xy＝δG_xy-δA_yx

δG_xz＝δG_xz-δA_zx

A_yx＝A_yx+δA_yx

A_zx＝A_zx+δA_zx

修正后，兩狀態(tài)清零，

其中，

A_yx、A_zx為慣導(dǎo)系統(tǒng)加速度計安裝誤差角；

B_x、B_y、B_z為慣導(dǎo)系統(tǒng)加速度計零偏值；

K_gx、K_gy、K_gz為慣導(dǎo)系統(tǒng)陀螺儀標(biāo)度因數(shù)；

G_xy、G_yx、G_xz、G_zx、G_yz、G_zy為慣導(dǎo)系統(tǒng)陀螺儀安裝誤差角；

D_x、D_y、D_z為慣導(dǎo)系統(tǒng)陀螺儀零偏值，

在慣導(dǎo)系統(tǒng)導(dǎo)航計算時，利用上述參數(shù)標(biāo)定值進(jìn)行補(bǔ)償，補(bǔ)償公式如下：

其中，

N_Ax、N_Ay、N_Az為一個導(dǎo)航周期內(nèi)加速度計輸出脈沖數(shù)；

N_Gx、N_Gy、N_Gz為一個導(dǎo)航周期內(nèi)陀螺儀輸出脈沖數(shù)；

為慣導(dǎo)系統(tǒng)標(biāo)定補(bǔ)償后加速度值；

為慣導(dǎo)系統(tǒng)標(biāo)定補(bǔ)償后角速度值。

本發(fā)明的顯著效果是：本發(fā)明的一種雙軸光纖慣導(dǎo)系統(tǒng)快速自標(biāo)定自對準(zhǔn)方法，針對光纖陀螺標(biāo)度因數(shù)和零偏等穩(wěn)定性差的缺陷，在雙軸光纖慣導(dǎo)系統(tǒng)啟動準(zhǔn)備過程中，實(shí)現(xiàn)了快速自標(biāo)定和自對準(zhǔn)，確保系統(tǒng)在30min內(nèi)能有效估計系統(tǒng)的主要誤差參數(shù)，同時獲得較高的對準(zhǔn)精度。

具體實(shí)施方式

一種雙軸光纖慣導(dǎo)系統(tǒng)快速自標(biāo)定自對準(zhǔn)方法，包括以下步驟：

步驟1、自標(biāo)定自對準(zhǔn)旋轉(zhuǎn)流程

30min系統(tǒng)自標(biāo)定自對準(zhǔn)的旋轉(zhuǎn)流程如下所示：

X軸向旋轉(zhuǎn)流程

X軸以30°/s正向旋轉(zhuǎn)450°，旋轉(zhuǎn)完成后停留1s，用時16s；

Z軸向旋轉(zhuǎn)流程，依次進(jìn)行下述旋轉(zhuǎn)

Z軸以30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留38s，用時44s；

Z軸以30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留38s，用時44s；

Z軸以30°/s正向旋轉(zhuǎn)90°，旋轉(zhuǎn)完成后停留2s，用時5s；

地向旋轉(zhuǎn)流程

X軸以30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以-30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以-30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以30°/s正向旋轉(zhuǎn)90°，旋轉(zhuǎn)完成后停留1s，用時4s；

Y軸向旋轉(zhuǎn)流程

Y軸以30°/s正向旋轉(zhuǎn)540°，旋轉(zhuǎn)完成后停留1s，用時19s；

天向旋轉(zhuǎn)流程

X軸以30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以-30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以-30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

X軸以30°/s正向旋轉(zhuǎn)180°，旋轉(zhuǎn)完成后停留1s，用時7s；

以上步驟中每個旋轉(zhuǎn)流程循環(huán)5次，即X軸向旋轉(zhuǎn)循環(huán)5次，地向旋轉(zhuǎn)循環(huán)5次，天向旋轉(zhuǎn)循環(huán)5次。

將以上流程循環(huán)6次，即整個步驟1循環(huán)6次。

采集三個陀螺和三個加速度計的輸出數(shù)據(jù)，內(nèi)環(huán)和外環(huán)的旋轉(zhuǎn)角度。

步驟2、自標(biāo)定自對準(zhǔn)誤差模型建立

系統(tǒng)狀態(tài)方程和量測方程如下：

狀態(tài)方程：

觀測方程：Z＝HX+V

H＝[I_5×5 0_5×24]，

為的第一列。

其中，

δV_n、δV_u、δV_e為慣導(dǎo)北、天、東速度誤差；

δL、δλ為慣導(dǎo)緯度和經(jīng)度誤差；

φ_n、φ_u、φ_e為慣導(dǎo)北、天、東失準(zhǔn)角；

ε_x為慣導(dǎo)X軸(天向)陀螺儀零偏；

為慣導(dǎo)X、Y、Z軸加速度計零偏；

δK_gx、δK_gy、δK_gz為陀螺儀標(biāo)度因數(shù)誤差；

δG_xy、δG_yx、δG_xz、δG_zx、δG_yz、δG_zy為陀螺儀安裝誤差；

為X軸朝上和朝下時，等效Y、Z加速度計零偏的不對稱性；

以上23個參數(shù)為待求解參數(shù)，

A_out為外環(huán)角度，該參數(shù)由外部給出。

R為地球半徑，該參數(shù)為常數(shù)，由外部給出；

ω_ie為地球自轉(zhuǎn)角速率，該參數(shù)為常數(shù)，由外部給出；

f_N、f_U、f_E為慣導(dǎo)北、天、東向加速度，該參數(shù)由外部給出；

表示的第一列和第二列；和sign是取內(nèi)部數(shù)據(jù)的符號，若內(nèi)部函數(shù)大于等于0取正，小于0取負(fù)

V_U天向速度、V_N北向速度、V_E動向速度，該三個參數(shù)的數(shù)值由導(dǎo)航系統(tǒng)給出，L為緯度、外部給定

請解釋C₁₁～C₃₃是矩陣中的元素，f_x、f_y是慣導(dǎo)x和y向的加速度，其數(shù)值由加速度計輸出，經(jīng)過誤差補(bǔ)償后獲得。

ω包含ω_x、ω_y、ω_z，其中ω_x、ω_y、ω_z分別為x、y、z方向的角速度，其數(shù)值由陀螺輸出經(jīng)誤差補(bǔ)償后獲得。

W＝[w₁ w₂ w₃ 0 0 w₄ w₅ w₆ 0 … 0]^T為23維系統(tǒng)噪聲列向量，w₁、w₂、w₃、w₄、w₅、w₆均為零均值隨機(jī)白噪聲；

V＝[v₁ v₂ v₃ v₄ v₅]^T為5維量測噪聲列向量，v₁、v₂、v₃、v₄、v₅均為零均值隨機(jī)白噪聲。

步驟3、粗對準(zhǔn)算法

由陀螺采集數(shù)據(jù)經(jīng)誤差補(bǔ)償后得到的角增量更新四元數(shù)：

其中：k初值為0，表示當(dāng)前時刻；k-1表示上一時刻；I為4×4階單位矩陣；

由計算姿態(tài)矩陣的公式如下：

其中q₀～q₃用本步驟最開始的公式進(jìn)行計算，計算得到中的四個元素就為q₀～q₃。

將加速度計采集的速度增量Δv(k)＝[v_xk v_yk v_xk]^T，利用姿態(tài)矩陣可求得i_b0系下速度：

與互為轉(zhuǎn)置

用下述公式計算速度

T_n采樣間隔，外部給定、g重力加速度、L是緯度。

計算：

取中間時刻和該中間時刻指粗對準(zhǔn)時間的一半，以及粗對準(zhǔn)結(jié)束時刻和可構(gòu)造速度矩陣如下：

利用最后得到的計算

步驟4、精對準(zhǔn)導(dǎo)航濾波

粗對準(zhǔn)完成后，系統(tǒng)旋轉(zhuǎn)機(jī)構(gòu)開始執(zhí)行自標(biāo)定自對準(zhǔn)旋轉(zhuǎn)流程，系統(tǒng)開始進(jìn)行導(dǎo)航解算，其中陀螺標(biāo)度因數(shù)誤差初始值設(shè)為1，其余各個誤差系數(shù)的初始值都設(shè)定為0；同時進(jìn)行“速度+位置”匹配閉環(huán)卡爾曼濾波估計，主要涉及以下幾個誤差的修正。

姿態(tài)誤差的修正

濾波估計得到的姿態(tài)誤差為則姿態(tài)修正方程為：

Tn,是采樣間隔、的意義可以統(tǒng)一表示如下，以為例，其表示的是e坐標(biāo)系相對于n坐標(biāo)系的角速度的分量表示。

速度誤差的修正

濾波估計得到的速度誤差為δV_n,δV_u,δV_e，則速度修正方程為：

V_i＝V_i-δV_i (i＝n、u、e)

位置誤差的修正

濾波估計得到的位置誤差為δL和δλ，則位置修正方程為：

h＝h-δh

λ＝λ-δλ

誤差參數(shù)的修正

首先更新待辨識的各誤差參數(shù)，如下所示：

ε_x＝ε_x+X(9)

δK_gx＝δK_gx+X(13)

δK_gy＝δK_gy+X(14)

δK_gz＝δK_gz+X(15)

δG_xy＝δG_xy+X(16)

δG_yx＝δG_yx+X(17)

δG_xz＝δG_xz+X(18)

δG_zx＝δG_zx+X(19)

δG_yz＝δG_yz+X(20)

δG_zy＝δG_zy+X(21)

X(9)～X(23)表示X中第9～23個參數(shù)的數(shù)值。

由此修正各誤差參數(shù)：

D_x＝D_x+ε_x

K_Gx＝K_Gx·δK_Gx

K_Gy＝K_Gy0·δK_Gy

K_Gz＝K_Gz0·δK_Gz

G_xy＝G_xy+δG_xy

G_yx＝G_yx+δG_yx

G_xz＝G_xz+δG_xz

G_zx＝G_zx+δG_zx

G_yz＝G_yz+δG_yz

G_zy＝G_zy+δG_zy

上述計算迭代進(jìn)行，因此使用相同符號表示，

誤差進(jìn)行閉環(huán)修正后，各狀態(tài)變量清零。

X(i)＝0，i＝1,2…23

每一個循環(huán)結(jié)束時，要對中間變量進(jìn)行處理：

δG_xy＝δG_xy-δA_yx

δG_xz＝δG_xz-δA_zx

A_yx＝A_yx+δA_yx

A_zx＝A_zx+δA_zx

修正后，兩狀態(tài)清零。

其中，

A_yx、A_zx為慣導(dǎo)系統(tǒng)加速度計安裝誤差角；

B_x、B_y、B_z為慣導(dǎo)系統(tǒng)加速度計零偏值；

K_gx、K_gy、K_gz為慣導(dǎo)系統(tǒng)陀螺儀標(biāo)度因數(shù)；

G_xy、G_yx、G_xz、G_zx、G_yz、G_zy為慣導(dǎo)系統(tǒng)陀螺儀安裝誤差角；

D_x、D_y、D_z為慣導(dǎo)系統(tǒng)陀螺儀零偏值。

在慣導(dǎo)系統(tǒng)導(dǎo)航計算時，利用上述參數(shù)標(biāo)定值進(jìn)行補(bǔ)償，補(bǔ)償公式如下：

其中，

N_Ax、N_Ay、N_Az為一個導(dǎo)航周期內(nèi)加速度計輸出脈沖數(shù)；

N_Gx、N_Gy、N_Gz為一個導(dǎo)航周期內(nèi)陀螺儀輸出脈沖數(shù)；

為慣導(dǎo)系統(tǒng)標(biāo)定補(bǔ)償后加速度值；

為慣導(dǎo)系統(tǒng)標(biāo)定補(bǔ)償后角速度值。