本發(fā)明屬于慣性導(dǎo)航技術(shù)領(lǐng)域,具體涉及一種雙軸光纖慣導(dǎo)系統(tǒng)快速自標(biāo)定自對(duì)準(zhǔn)方法。
背景技術(shù):
國(guó)內(nèi)外雙軸激光旋轉(zhuǎn)慣導(dǎo)系統(tǒng)的研究相對(duì)較為成熟,激光陀螺標(biāo)度因數(shù)穩(wěn)定,受溫度磁場(chǎng)影響較小,所以系統(tǒng)設(shè)計(jì)相對(duì)簡(jiǎn)單。而光纖陀螺具有可靠性高,無(wú)機(jī)抖裝置利于旋轉(zhuǎn)機(jī)構(gòu)的控制,且陀螺隨機(jī)游走誤差相對(duì)較小利于慣導(dǎo)系統(tǒng)對(duì)準(zhǔn)的快速收斂等優(yōu)點(diǎn)。所以,雙軸光纖慣導(dǎo)系統(tǒng)引起越來(lái)越多人的注意,但光纖陀螺具有自身的缺陷:對(duì)溫度和磁場(chǎng)較為敏感,同時(shí)標(biāo)度因數(shù)和零偏穩(wěn)定性較差。
技術(shù)實(shí)現(xiàn)要素:
本發(fā)明需要解決的技術(shù)問(wèn)題為:針對(duì)光纖陀螺的特性,提供一種適用于雙軸光纖慣導(dǎo)系統(tǒng)的快速自標(biāo)定自對(duì)準(zhǔn)方法。
本發(fā)明是這樣實(shí)現(xiàn)的:一種雙軸光纖慣導(dǎo)系統(tǒng)快速自標(biāo)定自對(duì)準(zhǔn)方法,包括以下步驟:
步驟1、自標(biāo)定自對(duì)準(zhǔn)旋轉(zhuǎn)流程
30min系統(tǒng)自標(biāo)定自對(duì)準(zhǔn)的旋轉(zhuǎn)流程如下所示:
X軸向旋轉(zhuǎn)流程
X軸以30°/s正向旋轉(zhuǎn)450°,旋轉(zhuǎn)完成后停留1s,用時(shí)16s;
Z軸向旋轉(zhuǎn)流程,依次進(jìn)行下述旋轉(zhuǎn)
Z軸以30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留38s,用時(shí)44s;
Z軸以30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留38s,用時(shí)44s;
Z軸以30°/s正向旋轉(zhuǎn)90°,旋轉(zhuǎn)完成后停留2s,用時(shí)5s;
地向旋轉(zhuǎn)流程
X軸以30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以-30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以-30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以30°/s正向旋轉(zhuǎn)90°,旋轉(zhuǎn)完成后停留1s,用時(shí)4s;
Y軸向旋轉(zhuǎn)流程
Y軸以30°/s正向旋轉(zhuǎn)540°,旋轉(zhuǎn)完成后停留1s,用時(shí)19s;
天向旋轉(zhuǎn)流程
X軸以30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以-30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以-30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
以上步驟中每個(gè)旋轉(zhuǎn)流程循環(huán)5次,即X軸向旋轉(zhuǎn)循環(huán)5次,地向旋轉(zhuǎn)循環(huán)5次,天向旋轉(zhuǎn)循環(huán)5次,
將以上流程循環(huán)6次,即整個(gè)步驟1循環(huán)6次,
采集三個(gè)陀螺和三個(gè)加速度計(jì)的輸出數(shù)據(jù),內(nèi)環(huán)和外環(huán)的旋轉(zhuǎn)角度,
步驟2、自標(biāo)定自對(duì)準(zhǔn)誤差模型建立
系統(tǒng)狀態(tài)方程和量測(cè)方程如下:
狀態(tài)方程:
觀測(cè)方程:Z=HX+V
H=[I5×5 05×24],
為的第一列,
其中,
δVn、δVu、δVe為慣導(dǎo)北、天、東速度誤差;
δL、δλ為慣導(dǎo)緯度和經(jīng)度誤差;
φn、φu、φe為慣導(dǎo)北、天、東失準(zhǔn)角;
εx為慣導(dǎo)X軸(天向)陀螺儀零偏;
為慣導(dǎo)X、Y、Z軸加速度計(jì)零偏;
δKgx、δKgy、δKgz為陀螺儀標(biāo)度因數(shù)誤差;
δGxy、δGyx、δGxz、δGzx、δGyz、δGzy為陀螺儀安裝誤差;
為X軸朝上和朝下時(shí),等效Y、Z加速度計(jì)零偏的不對(duì)稱性;
以上23個(gè)參數(shù)為待求解參數(shù),
Aout為外環(huán)角度,該參數(shù)由外部給出,
R為地球半徑,該參數(shù)為常數(shù),由外部給出;
ωie為地球自轉(zhuǎn)角速率,該參數(shù)為常數(shù),由外部給出;
fN、fU、fE為慣導(dǎo)北、天、東向加速度,該參數(shù)由外部給出;
表示的第一列和第二列;和sign是取內(nèi)部數(shù)據(jù)的符號(hào),若內(nèi)部函數(shù)大于等于0取正,小于0取負(fù)
VU天向速度、VN北向速度、VE動(dòng)向速度,該三個(gè)參數(shù)的數(shù)值由導(dǎo)航系統(tǒng)給出,L為緯度、外部給定
請(qǐng)解釋C11~C33是矩陣中的元素,fx、fy是慣導(dǎo)x和y向的加速度,其數(shù)值由加速度計(jì)輸出,經(jīng)過(guò)誤差補(bǔ)償后獲得,
ω包含ωx、ωy、ωz,其中ωx、ωy、ωz分別為x、y、z方向的角速度,其數(shù)值由陀螺輸出經(jīng)誤差補(bǔ)償后獲得,
W=[w1 w2 w3 0 0 w4 w5 w6 0 … 0]T為23維系統(tǒng)噪聲列向量,w1、w2、w3、w4、w5、w6均為零均值隨機(jī)白噪聲;
V=[v1 v2 v3 v4 v5]T為5維量測(cè)噪聲列向量,v1、v2、v3、v4、v5均為零均值隨機(jī)白噪聲,
步驟3、粗對(duì)準(zhǔn)算法
由陀螺采集數(shù)據(jù)經(jīng)誤差補(bǔ)償后得到的角增量更新四元數(shù):
其中:k初值為0,表示當(dāng)前時(shí)刻;k-1表示上一時(shí)刻;I為4×4階單位矩陣;
由計(jì)算姿態(tài)矩陣的公式如下:
其中q0~q3用本步驟最開(kāi)始的公式進(jìn)行計(jì)算,計(jì)算得到中的四個(gè)元素就為q0~q3,
將加速度計(jì)采集的速度增量Δv(k)=[vxk vyk vxk]T,利用姿態(tài)矩陣可求得ib0系下速度:
與互為轉(zhuǎn)置
用下述公式計(jì)算速度
Tn采樣間隔,外部給定、g重力加速度、L是緯度,
計(jì)算:
取中間時(shí)刻和該中間時(shí)刻指粗對(duì)準(zhǔn)時(shí)間的一半,以及粗對(duì)準(zhǔn)結(jié)束時(shí)刻和可構(gòu)造速度矩陣如下:
利用最后得到的計(jì)算
步驟4、精對(duì)準(zhǔn)導(dǎo)航濾波
粗對(duì)準(zhǔn)完成后,系統(tǒng)旋轉(zhuǎn)機(jī)構(gòu)開(kāi)始執(zhí)行自標(biāo)定自對(duì)準(zhǔn)旋轉(zhuǎn)流程,系統(tǒng)開(kāi)始進(jìn)行導(dǎo)航解算,其中陀螺標(biāo)度因數(shù)誤差初始值設(shè)為1,其余各個(gè)誤差系數(shù)的初始值都設(shè)定為0;同時(shí)進(jìn)行“速度+位置”匹配閉環(huán)卡爾曼濾波估計(jì),主要涉及以下幾個(gè)誤差的修正,
姿態(tài)誤差的修正
濾波估計(jì)得到的姿態(tài)誤差為則姿態(tài)修正方程為:
Tn,是采樣間隔、的意義可以統(tǒng)一表示如下,以為例,其表示的是e坐標(biāo)系相對(duì)于n坐標(biāo)系的角速度的分量表示,
速度誤差的修正
濾波估計(jì)得到的速度誤差為δVn,δVu,δVe,則速度修正方程為:
Vi=Vi-δVi (i=n、u、e)
位置誤差的修正
濾波估計(jì)得到的位置誤差為δL和δλ,則位置修正方程為:
h=h-δh
λ=λ-δλ
誤差參數(shù)的修正
首先更新待辨識(shí)的各誤差參數(shù),如下所示:
εx=εx+X(9)
δKgx=δKgx+X(13)
δKgy=δKgy+X(14)
δKgz=δKgz+X(15)
δGxy=δGxy+X(16)
δGyx=δGyx+X(17)
δGxz=δGxz+X(18)
δGzx=δGzx+X(19)
δGyz=δGyz+X(20)
δGzy=δGzy+X(21)
X(9)~X(23)表示X中第9~23個(gè)參數(shù)的數(shù)值,
由此修正各誤差參數(shù):
Dx=Dx+εx
KGx=KGx·δKGx
KGy=KGy0·δKGy
KGz=KGz0·δKGz
Gxy=Gxy+δGxy
Gyx=Gyx+δGyx
Gxz=Gxz+δGxz
Gzx=Gzx+δGzx
Gyz=Gyz+δGyz
Gzy=Gzy+δGzy
上述計(jì)算迭代進(jìn)行,因此使用相同符號(hào)表示,
誤差進(jìn)行閉環(huán)修正后,各狀態(tài)變量清零,
X(i)=0,i=1,2…23
每一個(gè)循環(huán)結(jié)束時(shí),要對(duì)中間變量進(jìn)行處理:
δGxy=δGxy-δAyx
δGxz=δGxz-δAzx
Ayx=Ayx+δAyx
Azx=Azx+δAzx
修正后,兩狀態(tài)清零,
其中,
Ayx、Azx為慣導(dǎo)系統(tǒng)加速度計(jì)安裝誤差角;
Bx、By、Bz為慣導(dǎo)系統(tǒng)加速度計(jì)零偏值;
Kgx、Kgy、Kgz為慣導(dǎo)系統(tǒng)陀螺儀標(biāo)度因數(shù);
Gxy、Gyx、Gxz、Gzx、Gyz、Gzy為慣導(dǎo)系統(tǒng)陀螺儀安裝誤差角;
Dx、Dy、Dz為慣導(dǎo)系統(tǒng)陀螺儀零偏值,
在慣導(dǎo)系統(tǒng)導(dǎo)航計(jì)算時(shí),利用上述參數(shù)標(biāo)定值進(jìn)行補(bǔ)償,補(bǔ)償公式如下:
其中,
NAx、NAy、NAz為一個(gè)導(dǎo)航周期內(nèi)加速度計(jì)輸出脈沖數(shù);
NGx、NGy、NGz為一個(gè)導(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)定自對(duì)準(zhǔn)方法,針對(duì)光纖陀螺標(biāo)度因數(shù)和零偏等穩(wěn)定性差的缺陷,在雙軸光纖慣導(dǎo)系統(tǒng)啟動(dòng)準(zhǔn)備過(guò)程中,實(shí)現(xiàn)了快速自標(biāo)定和自對(duì)準(zhǔn),確保系統(tǒng)在30min內(nèi)能有效估計(jì)系統(tǒng)的主要誤差參數(shù),同時(shí)獲得較高的對(duì)準(zhǔn)精度。
具體實(shí)施方式
一種雙軸光纖慣導(dǎo)系統(tǒng)快速自標(biāo)定自對(duì)準(zhǔn)方法,包括以下步驟:
步驟1、自標(biāo)定自對(duì)準(zhǔn)旋轉(zhuǎn)流程
30min系統(tǒng)自標(biāo)定自對(duì)準(zhǔn)的旋轉(zhuǎn)流程如下所示:
X軸向旋轉(zhuǎn)流程
X軸以30°/s正向旋轉(zhuǎn)450°,旋轉(zhuǎn)完成后停留1s,用時(shí)16s;
Z軸向旋轉(zhuǎn)流程,依次進(jìn)行下述旋轉(zhuǎn)
Z軸以30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留38s,用時(shí)44s;
Z軸以30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留38s,用時(shí)44s;
Z軸以30°/s正向旋轉(zhuǎn)90°,旋轉(zhuǎn)完成后停留2s,用時(shí)5s;
地向旋轉(zhuǎn)流程
X軸以30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以-30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以-30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以30°/s正向旋轉(zhuǎn)90°,旋轉(zhuǎn)完成后停留1s,用時(shí)4s;
Y軸向旋轉(zhuǎn)流程
Y軸以30°/s正向旋轉(zhuǎn)540°,旋轉(zhuǎn)完成后停留1s,用時(shí)19s;
天向旋轉(zhuǎn)流程
X軸以30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以-30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以-30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
X軸以30°/s正向旋轉(zhuǎn)180°,旋轉(zhuǎn)完成后停留1s,用時(shí)7s;
以上步驟中每個(gè)旋轉(zhuǎn)流程循環(huán)5次,即X軸向旋轉(zhuǎn)循環(huán)5次,地向旋轉(zhuǎn)循環(huán)5次,天向旋轉(zhuǎn)循環(huán)5次。
將以上流程循環(huán)6次,即整個(gè)步驟1循環(huán)6次。
采集三個(gè)陀螺和三個(gè)加速度計(jì)的輸出數(shù)據(jù),內(nèi)環(huán)和外環(huán)的旋轉(zhuǎn)角度。
步驟2、自標(biāo)定自對(duì)準(zhǔn)誤差模型建立
系統(tǒng)狀態(tài)方程和量測(cè)方程如下:
狀態(tài)方程:
觀測(cè)方程:Z=HX+V
H=[I5×5 05×24],
為的第一列。
其中,
δVn、δVu、δVe為慣導(dǎo)北、天、東速度誤差;
δL、δλ為慣導(dǎo)緯度和經(jīng)度誤差;
φn、φu、φe為慣導(dǎo)北、天、東失準(zhǔn)角;
εx為慣導(dǎo)X軸(天向)陀螺儀零偏;
為慣導(dǎo)X、Y、Z軸加速度計(jì)零偏;
δKgx、δKgy、δKgz為陀螺儀標(biāo)度因數(shù)誤差;
δGxy、δGyx、δGxz、δGzx、δGyz、δGzy為陀螺儀安裝誤差;
為X軸朝上和朝下時(shí),等效Y、Z加速度計(jì)零偏的不對(duì)稱性;
以上23個(gè)參數(shù)為待求解參數(shù),
Aout為外環(huán)角度,該參數(shù)由外部給出。
R為地球半徑,該參數(shù)為常數(shù),由外部給出;
ωie為地球自轉(zhuǎn)角速率,該參數(shù)為常數(shù),由外部給出;
fN、fU、fE為慣導(dǎo)北、天、東向加速度,該參數(shù)由外部給出;
表示的第一列和第二列;和sign是取內(nèi)部數(shù)據(jù)的符號(hào),若內(nèi)部函數(shù)大于等于0取正,小于0取負(fù)
VU天向速度、VN北向速度、VE動(dòng)向速度,該三個(gè)參數(shù)的數(shù)值由導(dǎo)航系統(tǒng)給出,L為緯度、外部給定
請(qǐng)解釋C11~C33是矩陣中的元素,fx、fy是慣導(dǎo)x和y向的加速度,其數(shù)值由加速度計(jì)輸出,經(jīng)過(guò)誤差補(bǔ)償后獲得。
ω包含ωx、ωy、ωz,其中ωx、ωy、ωz分別為x、y、z方向的角速度,其數(shù)值由陀螺輸出經(jīng)誤差補(bǔ)償后獲得。
W=[w1 w2 w3 0 0 w4 w5 w6 0 … 0]T為23維系統(tǒng)噪聲列向量,w1、w2、w3、w4、w5、w6均為零均值隨機(jī)白噪聲;
V=[v1 v2 v3 v4 v5]T為5維量測(cè)噪聲列向量,v1、v2、v3、v4、v5均為零均值隨機(jī)白噪聲。
步驟3、粗對(duì)準(zhǔn)算法
由陀螺采集數(shù)據(jù)經(jīng)誤差補(bǔ)償后得到的角增量更新四元數(shù):
其中:k初值為0,表示當(dāng)前時(shí)刻;k-1表示上一時(shí)刻;I為4×4階單位矩陣;
由計(jì)算姿態(tài)矩陣的公式如下:
其中q0~q3用本步驟最開(kāi)始的公式進(jìn)行計(jì)算,計(jì)算得到中的四個(gè)元素就為q0~q3。
將加速度計(jì)采集的速度增量Δv(k)=[vxk vyk vxk]T,利用姿態(tài)矩陣可求得ib0系下速度:
與互為轉(zhuǎn)置
用下述公式計(jì)算速度
Tn采樣間隔,外部給定、g重力加速度、L是緯度。
計(jì)算:
取中間時(shí)刻和該中間時(shí)刻指粗對(duì)準(zhǔn)時(shí)間的一半,以及粗對(duì)準(zhǔn)結(jié)束時(shí)刻和可構(gòu)造速度矩陣如下:
利用最后得到的計(jì)算
步驟4、精對(duì)準(zhǔn)導(dǎo)航濾波
粗對(duì)準(zhǔn)完成后,系統(tǒng)旋轉(zhuǎn)機(jī)構(gòu)開(kāi)始執(zhí)行自標(biāo)定自對(duì)準(zhǔn)旋轉(zhuǎn)流程,系統(tǒng)開(kāi)始進(jìn)行導(dǎo)航解算,其中陀螺標(biāo)度因數(shù)誤差初始值設(shè)為1,其余各個(gè)誤差系數(shù)的初始值都設(shè)定為0;同時(shí)進(jìn)行“速度+位置”匹配閉環(huán)卡爾曼濾波估計(jì),主要涉及以下幾個(gè)誤差的修正。
姿態(tài)誤差的修正
濾波估計(jì)得到的姿態(tài)誤差為則姿態(tài)修正方程為:
Tn,是采樣間隔、的意義可以統(tǒng)一表示如下,以為例,其表示的是e坐標(biāo)系相對(duì)于n坐標(biāo)系的角速度的分量表示。
速度誤差的修正
濾波估計(jì)得到的速度誤差為δVn,δVu,δVe,則速度修正方程為:
Vi=Vi-δVi (i=n、u、e)
位置誤差的修正
濾波估計(jì)得到的位置誤差為δL和δλ,則位置修正方程為:
h=h-δh
λ=λ-δλ
誤差參數(shù)的修正
首先更新待辨識(shí)的各誤差參數(shù),如下所示:
εx=εx+X(9)
δKgx=δKgx+X(13)
δKgy=δKgy+X(14)
δKgz=δKgz+X(15)
δGxy=δGxy+X(16)
δGyx=δGyx+X(17)
δGxz=δGxz+X(18)
δGzx=δGzx+X(19)
δGyz=δGyz+X(20)
δGzy=δGzy+X(21)
X(9)~X(23)表示X中第9~23個(gè)參數(shù)的數(shù)值。
由此修正各誤差參數(shù):
Dx=Dx+εx
KGx=KGx·δKGx
KGy=KGy0·δKGy
KGz=KGz0·δKGz
Gxy=Gxy+δGxy
Gyx=Gyx+δGyx
Gxz=Gxz+δGxz
Gzx=Gzx+δGzx
Gyz=Gyz+δGyz
Gzy=Gzy+δGzy
上述計(jì)算迭代進(jìn)行,因此使用相同符號(hào)表示,
誤差進(jìn)行閉環(huán)修正后,各狀態(tài)變量清零。
X(i)=0,i=1,2…23
每一個(gè)循環(huán)結(jié)束時(shí),要對(duì)中間變量進(jìn)行處理:
δGxy=δGxy-δAyx
δGxz=δGxz-δAzx
Ayx=Ayx+δAyx
Azx=Azx+δAzx
修正后,兩狀態(tài)清零。
其中,
Ayx、Azx為慣導(dǎo)系統(tǒng)加速度計(jì)安裝誤差角;
Bx、By、Bz為慣導(dǎo)系統(tǒng)加速度計(jì)零偏值;
Kgx、Kgy、Kgz為慣導(dǎo)系統(tǒng)陀螺儀標(biāo)度因數(shù);
Gxy、Gyx、Gxz、Gzx、Gyz、Gzy為慣導(dǎo)系統(tǒng)陀螺儀安裝誤差角;
Dx、Dy、Dz為慣導(dǎo)系統(tǒng)陀螺儀零偏值。
在慣導(dǎo)系統(tǒng)導(dǎo)航計(jì)算時(shí),利用上述參數(shù)標(biāo)定值進(jìn)行補(bǔ)償,補(bǔ)償公式如下:
其中,
NAx、NAy、NAz為一個(gè)導(dǎo)航周期內(nèi)加速度計(jì)輸出脈沖數(shù);
NGx、NGy、NGz為一個(gè)導(dǎo)航周期內(nèi)陀螺儀輸出脈沖數(shù);
為慣導(dǎo)系統(tǒng)標(biāo)定補(bǔ)償后加速度值;
為慣導(dǎo)系統(tǒng)標(biāo)定補(bǔ)償后角速度值。