彈載SINS/GPS組合自對(duì)準(zhǔn)方法

屈新芬,李世玲,祝 剛,王啟明

(中國工程物理研究院電子工程研究所,四川 綿陽 621900)

0 引言

中低精度SINS/GPS組合系統(tǒng)在無參考角度、無轉(zhuǎn)位機(jī)構(gòu)或速度信息輔助的地面靜態(tài)或動(dòng)態(tài)發(fā)射場景下,方位角自對(duì)準(zhǔn)誤差將達(dá)到數(shù)度至數(shù)十度甚至更大,大失準(zhǔn)角下后續(xù)必須采用非線性濾波算法在載體飛行動(dòng)態(tài)下對(duì)姿態(tài)角誤差進(jìn)行估計(jì)與修正[1-2],而非線性濾波計(jì)算量大,不能滿足實(shí)時(shí)性要求較高的場景,過大的失準(zhǔn)角還將導(dǎo)致非線性濾波算法發(fā)散。空中飛行載體在SINS/GPS組合系統(tǒng)發(fā)生故障、發(fā)射段高過載沖擊等都需SINS/GPS組合在空中動(dòng)態(tài)下加電啟動(dòng)并完成飛行中的滿足一定精度要求的初始自對(duì)準(zhǔn),以滿足后續(xù)SINS/GPS組合線性濾波甚至較長時(shí)間丟星下純慣性工作精度的要求。

針對(duì)上述問題,國內(nèi)外學(xué)者開展了多種動(dòng)態(tài)對(duì)準(zhǔn)方法。文獻(xiàn)[3-5]為基于載體小側(cè)滑角和小攻角方法。文獻(xiàn)[6-7]分別利用單矢量迭代求解方法于雙矢量實(shí)現(xiàn)粗對(duì)準(zhǔn),單/雙矢量迭代算法對(duì)準(zhǔn)精度受載體動(dòng)態(tài)和沖擊影響大。文獻(xiàn)[8] 通過構(gòu)建積分多矢量并采用Request 算法,在積分多矢量構(gòu)建中反復(fù)使用測量數(shù)據(jù)。近年來基于慣性系慣導(dǎo)自對(duì)準(zhǔn)方法在艦載、車載、機(jī)載等動(dòng)態(tài)場景或晃動(dòng)場景得到了一定的工程應(yīng)用[9-17],但其在彈載等大動(dòng)態(tài)場景的應(yīng)用尚少。本文將基于慣性系動(dòng)態(tài)初始對(duì)準(zhǔn)方法引入彈載SINS/GPS初始對(duì)準(zhǔn)中,剖析大動(dòng)態(tài)場景下影響姿態(tài)角對(duì)準(zhǔn)精度的主要因素,提出用大動(dòng)態(tài)特征值實(shí)現(xiàn)對(duì)準(zhǔn)開始時(shí)間與結(jié)束時(shí)間的自動(dòng)輔助判斷策略。

1 慣性系初始對(duì)準(zhǔn)算法理論基礎(chǔ)

1.1 慣性系初始對(duì)準(zhǔn)原理

(1)

(2)

(3)

式中,

其中,ωie為地球自轉(zhuǎn)角速度,上標(biāo)T表示轉(zhuǎn)置。

1.2 Wahba問題與求解方法

(4)

(5)

式(5)中,ak為最小二乘系數(shù),工程上常取為1;tr( )表示對(duì)矩陣求跡。

Wahba問題常用經(jīng)典求解方法是SVD(singular value decomposition)算法與QUEST(quaternion estimator)算法。兩種算法理論上等價(jià),都充分利用盡可能多的信息,計(jì)算量相當(dāng),在本文給定動(dòng)態(tài)下仿真效果相同,算法可參考文獻(xiàn)[14-17]。

2 SINS/GPS組合自對(duì)準(zhǔn)算法

2.1 SINS/GPS組合動(dòng)態(tài)自對(duì)準(zhǔn)算法

圖1 INS/GPS初始對(duì)準(zhǔn)算法結(jié)構(gòu)圖Fig.1 INS/GPS self-alignment algorithm configuration

(6)

(7)

2.2 誤差分析與對(duì)準(zhǔn)啟動(dòng)與結(jié)束時(shí)間點(diǎn)的自動(dòng)判斷

(8)

(9)

(10)

(11)

綜上,載體速度、加速度大小及測量精度、角速度大小及測量精度、載體測量速度的時(shí)間微分誤差及速度測量精度是影響初始對(duì)準(zhǔn)精度的主要因素。較之于低動(dòng)態(tài),大動(dòng)態(tài)環(huán)境下SINS內(nèi)各傳感器標(biāo)度因數(shù)誤差和安裝誤差引起的角速度和加速度測量誤差大很多。因此設(shè)置加速度和角速度門限和持續(xù)時(shí)間,當(dāng)加速度和角速度絕對(duì)值及變化率小于設(shè)置門限且持續(xù)時(shí)間大于設(shè)定時(shí)間時(shí),即式(12)條件同時(shí)滿足才啟動(dòng)初始對(duì)準(zhǔn)并記錄啟動(dòng)時(shí)間點(diǎn)tq;同理在初始對(duì)準(zhǔn)時(shí)長大于設(shè)定最短對(duì)準(zhǔn)時(shí)間長度且加速度和角速度及變化率絕對(duì)值小于設(shè)置門限值即式(13)條件同時(shí)滿足才結(jié)束初始對(duì)準(zhǔn)。

(12)

式(12)中,fset1,ωset1,Δfset1,Δωset1分別為初始對(duì)準(zhǔn)啟動(dòng)加速度與角速度及變化率(由相鄰采樣值之差除以采樣間隔時(shí)間得到)門限;t0,tk,tset1分別為載體起飛時(shí)間點(diǎn)、當(dāng)前時(shí)間點(diǎn)和預(yù)設(shè)的初始對(duì)準(zhǔn)啟動(dòng)點(diǎn)距起飛點(diǎn)時(shí)間長度;Δfset1,Δωset1,tset1根據(jù)具體的設(shè)計(jì)軌跡動(dòng)態(tài)進(jìn)行粗略設(shè)計(jì)。

(13)

式(13)中,fset2,ωset2,Δfset2,Δωset2分別為初始對(duì)準(zhǔn)結(jié)束加速度與角速度及變化率門限,tset2為最小初始對(duì)準(zhǔn)時(shí)長設(shè)定值,nset2為初始對(duì)準(zhǔn)結(jié)束前加速度和角速度及變化率小于設(shè)定值的持續(xù)時(shí)間最小采樣次數(shù)設(shè)定值,這些門限值可以根據(jù)具體的設(shè)計(jì)軌跡動(dòng)態(tài)進(jìn)行粗略設(shè)計(jì)。n為初始對(duì)準(zhǔn)啟動(dòng)后連續(xù)滿足式(13)前5個(gè)方程的角速度加速度連續(xù)采樣次數(shù)。

3 試驗(yàn)與測試驗(yàn)證

為驗(yàn)證本文算法對(duì)大動(dòng)態(tài)環(huán)境的有效性與適應(yīng)性,設(shè)計(jì)搭載試驗(yàn)進(jìn)行考核。該飛行前段即起飛點(diǎn)至飛行30 s之內(nèi),最大加速度與加加速度達(dá)到196 m/s2,980 m/s2以上,最大角速度與角加速度達(dá)到220 (°)/s和1 000 (°)/s2以上,試驗(yàn)飛行80 s附近設(shè)計(jì)約8 s時(shí)長幅值大于40 (°)/s的滾轉(zhuǎn)角速度運(yùn)動(dòng),試驗(yàn)飛行110～130 s時(shí)間段內(nèi)滾轉(zhuǎn)角速度最大值達(dá)60 (°)/s以上。

飛行試驗(yàn)配置光纖陀螺慣組和GPS接收機(jī),陀螺和加速度計(jì)零偏穩(wěn)定性、重復(fù)性為1 (°)/h,0.001g水平,標(biāo)度因數(shù)誤差都為0.01%水平,SINS,GPS輸出數(shù)據(jù)頻率分別為200 Hz和10 Hz。試驗(yàn)搭載DSP+FPGA嵌入式計(jì)算機(jī)內(nèi)采用大動(dòng)態(tài)特征值決策輔助的SVD算法,即當(dāng)加速度和角速度絕對(duì)值小于設(shè)置門限且持續(xù)時(shí)間大于設(shè)定值時(shí)(fset1,Δfset1,ωset1,Δωset1,tset1分別取12 m/s2,100 m/s3,5 (°)/s,100 (°)/ s2,40 s),才啟動(dòng)初始對(duì)準(zhǔn);在啟動(dòng)初始對(duì)準(zhǔn)后,加速度和角速度絕對(duì)值小于設(shè)置門限且持續(xù)時(shí)間大于設(shè)定值時(shí)(fset2,Δfset2,ωset2,Δωset2,tset2分別取12 m/s2,100 m/s3,5 (°)/s,100 (°)/ s2,200 s)時(shí)輸出姿態(tài)角,本文為方便查看,將不同時(shí)間下初始對(duì)準(zhǔn)精度將tset2設(shè)置為0。姿態(tài)角對(duì)準(zhǔn)精度的評(píng)判基準(zhǔn)為飛行全程工作的慣性/GPS組合系統(tǒng),用對(duì)準(zhǔn)輸出姿態(tài)角減去基于地面方位角裝訂的全程工作慣性/GPS組合系統(tǒng)的姿態(tài)角所得結(jié)果如圖2(a)。對(duì)飛行試驗(yàn)回收SINS數(shù)據(jù)和GPS數(shù)據(jù)分別采用TRIAD積分算法和SVD算法進(jìn)行解算,TRIAD積分算法的初始對(duì)準(zhǔn)開始時(shí)間依次從載體起飛時(shí)間點(diǎn)開始每間隔1 s往后移動(dòng),且每組對(duì)準(zhǔn)時(shí)長都設(shè)為100 s,姿態(tài)角對(duì)準(zhǔn)誤差見圖2(b);SVD算法初始對(duì)準(zhǔn)啟動(dòng)開始時(shí)間為載體起飛點(diǎn),啟動(dòng)后姿態(tài)角對(duì)準(zhǔn)誤差時(shí)間關(guān)系見圖2(c)和圖2(d)。

圖2 飛行全程下對(duì)準(zhǔn)姿態(tài)角誤差Fig.2 Alignment attitude error in flight

在不采用大動(dòng)態(tài)特征值決策輔助的圖2(c)、圖2(d)中,在起飛至飛行30 s、試驗(yàn)飛行80 s附近和試驗(yàn)飛行110～130 s時(shí)間段,因存在大角速度、大加速度、大角加速度或大加加速度及其耦合影響,在大動(dòng)態(tài)時(shí)間段內(nèi)SVD算法實(shí)時(shí)對(duì)準(zhǔn)姿態(tài)角誤差很大甚至達(dá)到上百度,且在這些大動(dòng)態(tài)之外的平穩(wěn)飛行段的三個(gè)姿態(tài)角實(shí)時(shí)對(duì)準(zhǔn)誤差最大值也達(dá)到-2.3°甚至更大誤差值;而采用大動(dòng)態(tài)特征值決策輔助的圖2(a)結(jié)果中,SVD算法輸出的姿態(tài)角對(duì)準(zhǔn)誤差較小,在飛行55 s后姿態(tài)角對(duì)準(zhǔn)誤差最大值小于0.2°,滿足SINS/GPS組合線性濾波與較短時(shí)間工作的純慣性初始對(duì)準(zhǔn)精度的要求。采用TRIAD積分算法的圖2(b)結(jié)果中,姿態(tài)角對(duì)準(zhǔn)誤差較大,除受對(duì)準(zhǔn)時(shí)長的影響外,還與對(duì)準(zhǔn)起始點(diǎn)的選擇有關(guān),不方便彈載飛行環(huán)境的工程應(yīng)用。

為考察不同精度慣組下SVD算法的對(duì)準(zhǔn)精度,對(duì)飛行試驗(yàn)所設(shè)計(jì)的標(biāo)稱六自由度數(shù)據(jù)采用大動(dòng)態(tài)特征值決策輔助SVD算法進(jìn)行仿真,并在此基礎(chǔ)上分別疊加慣性測量誤差(第1組設(shè)置陀螺與加速度計(jì)零偏穩(wěn)定性(1σ)和重復(fù)性(1σ)分別為0.01 (°)/h,0.000 01 m/s2,非線性都為0.001%;第2組為1 (°)/h,0.001 m/s2,0.01%;第3組為10 (°)/h,0.005 m/s2,0.03%)進(jìn)行50條蒙特卡洛仿真,對(duì)準(zhǔn)時(shí)長取100 s,仿真統(tǒng)計(jì)結(jié)果見表1。表1中δφ,δγ,δθ分別表示對(duì)準(zhǔn)滾轉(zhuǎn)角誤差、方位角誤差和俯仰角誤差。

表1 標(biāo)稱數(shù)據(jù)與三種精度慣導(dǎo)下動(dòng)態(tài)對(duì)準(zhǔn)誤差Tab.1 Standard data and alignment errors with the three different precision IMU

由表1知,試驗(yàn)動(dòng)態(tài)下,當(dāng)SINS的陀螺與加速度計(jì)零偏穩(wěn)定性、重復(fù)性、標(biāo)度因數(shù)誤差分別為1 (°)/h,0.01 m/s2,0.01%或更高精度時(shí),姿態(tài)角動(dòng)態(tài)對(duì)準(zhǔn)精度優(yōu)于0.35°(1σ),與圖2(a)解算結(jié)果精度相吻合;陀螺與加速度計(jì)相應(yīng)誤差分別為10 (°)/h,0.05 m/s2,0.03%下,姿態(tài)角動(dòng)態(tài)對(duì)準(zhǔn)精度優(yōu)于1.8°(1σ)。

4 結(jié)論

本文將慣性系的最優(yōu)初始對(duì)準(zhǔn)SVD和TRIAD算法引入大動(dòng)態(tài)場景,在分析姿態(tài)角動(dòng)態(tài)對(duì)準(zhǔn)誤差影響因素基礎(chǔ)上,通過飛行試驗(yàn)、回收數(shù)據(jù)離線測試和標(biāo)稱理想數(shù)據(jù)仿真,得到以下結(jié)論:

1) 本文提出的大動(dòng)態(tài)特征值決策輔助SVD姿態(tài)角對(duì)準(zhǔn)算法根據(jù)載體動(dòng)態(tài)對(duì)對(duì)準(zhǔn)啟動(dòng)點(diǎn)和對(duì)準(zhǔn)結(jié)束點(diǎn)進(jìn)行在線調(diào)整,提高了過程機(jī)動(dòng)下姿態(tài)角對(duì)準(zhǔn)精度;

2) 大動(dòng)態(tài)場景,基于TRIAD算法的姿態(tài)角對(duì)準(zhǔn)誤差除受對(duì)準(zhǔn)時(shí)長的影響外,還與對(duì)準(zhǔn)起始點(diǎn)的選擇有關(guān),過程機(jī)動(dòng)下姿態(tài)角對(duì)準(zhǔn)精度難保證;

3) 慣組精度直接影響動(dòng)態(tài)姿態(tài)角對(duì)準(zhǔn)精度,慣組精度越高,姿態(tài)角對(duì)準(zhǔn)精度越高。