梁 哲, 周召發(fā),*, 徐志浩, 呂文亭, 段 輝

(1. 火箭軍工程大學(xué)導(dǎo)彈工程學(xué)院, 陜西 西安 710025; 2. 火箭軍工程大學(xué)兵器發(fā)射理論和技術(shù)國(guó)家重點(diǎn)學(xué)科實(shí)驗(yàn)室, 陜西 西安 710025; 3. 中國(guó)人民解放軍92281部隊(duì), 山東 青島 266000)

0 引 言

慣性導(dǎo)航與衛(wèi)星導(dǎo)航的方式相比,具有高自主性,高可靠性,高隱蔽性,長(zhǎng)時(shí)間導(dǎo)航等優(yōu)點(diǎn),姿態(tài)解算則是慣性導(dǎo)航過(guò)程中的核心環(huán)節(jié),傳統(tǒng)的捷聯(lián)慣導(dǎo)姿態(tài)算法主要包括歐拉角法、方向余弦法、四元數(shù)法和等效旋轉(zhuǎn)矢量法,鑒于上述各個(gè)方法的優(yōu)缺點(diǎn),目前主流的算法為后兩者。并且,利用方向余弦法和四元數(shù)法實(shí)現(xiàn)高精度計(jì)算結(jié)果的條件較為苛刻,而等效旋轉(zhuǎn)矢量法相較于四元數(shù)法有著計(jì)算量小,容易得到理想計(jì)算精度的特點(diǎn),成為目前最常用的姿態(tài)算法。

基于Bortz方程,學(xué)者們對(duì)旋轉(zhuǎn)矢量算法已經(jīng)進(jìn)行了大量研究,提出多種優(yōu)化和改進(jìn)算法,但這些算法大多都是在慣組輸出是角增量的條件下提出的。隨著光纖技術(shù)的發(fā)展,成本相對(duì)更低的中、高精度光纖捷聯(lián)慣導(dǎo)系統(tǒng)被越來(lái)越廣泛地應(yīng)用于軍事和民用領(lǐng)域,對(duì)于某些低成本的干涉式光纖捷聯(lián)慣導(dǎo),其陀螺的輸出量可視為角速率信息,導(dǎo)致傳統(tǒng)的角增量多子樣旋轉(zhuǎn)矢量算法不能直接對(duì)其應(yīng)用。若簡(jiǎn)單地對(duì)角速率進(jìn)行多項(xiàng)式擬合-積分得到角增量信息,再代入傳統(tǒng)算法進(jìn)行姿態(tài)更新,則會(huì)使傳統(tǒng)算法的性能大大降低。

針對(duì)上述問(wèn)題,國(guó)內(nèi)外多名學(xué)者先后提出了角速率表示的角速度多項(xiàng)式擬合方法和基于角速率的圓錐誤差補(bǔ)償結(jié)構(gòu),并在圓錐運(yùn)動(dòng)環(huán)境下優(yōu)化出了補(bǔ)償系數(shù);這類算法在原理上大致相同,都是將角速率子樣直接與角增量(通過(guò)對(duì)角速率擬合-積分提取)或者不同時(shí)刻的角速率進(jìn)行不同形式的叉乘運(yùn)算來(lái)構(gòu)造補(bǔ)償結(jié)構(gòu),然后在錐運(yùn)動(dòng)環(huán)境下進(jìn)行優(yōu)化得到補(bǔ)償系數(shù)。與傳統(tǒng)的角增量多子樣算法相比,改進(jìn)后的算法精度得到明顯提高,并且能夠滿足實(shí)際工程應(yīng)用。但部分算法提取角增量時(shí)需要大量的角速率信息,致使算法的姿態(tài)更新頻率降低。

本文將在慣組輸出為角速率的情況下,參考單子樣姿態(tài)算法的思路,在角速度多項(xiàng)式擬合過(guò)程中利用前周期的部分角速率子樣信息,通過(guò)對(duì)多個(gè)不同擬合區(qū)間的角速度多項(xiàng)式積分來(lái)提取對(duì)應(yīng)采樣周期的角增量信息,進(jìn)而利用角速率信息和多次修正后的角增量信息構(gòu)造圓錐誤差補(bǔ)償結(jié)構(gòu),以提高傳統(tǒng)角速率姿態(tài)算法的解算速度和姿態(tài)解算精度。文章最后在圓錐運(yùn)動(dòng)環(huán)境下對(duì)新方法與傳統(tǒng)角速率姿態(tài)算法的解算精度進(jìn)行比較。

1 圓錐運(yùn)動(dòng)及誤差描述

圓錐運(yùn)動(dòng)被認(rèn)為是最為苛刻的導(dǎo)航環(huán)境,在錐運(yùn)動(dòng)下優(yōu)化得到的不可交換誤差補(bǔ)償系數(shù)可用于一般角運(yùn)動(dòng)情況。圓錐運(yùn)動(dòng)角速度可描述為

(1)

式中:半錐角和圓錐運(yùn)動(dòng)頻率均為常數(shù),表明載體在軸上和軸上做同頻但相位差90°的正弦角運(yùn)動(dòng),導(dǎo)致在軸上輸出常值角運(yùn)動(dòng)。其對(duì)應(yīng)的姿態(tài)四元數(shù)為

(2)

由于不能直接得到單位姿態(tài)更新時(shí)間段[-1,]內(nèi)的等效旋轉(zhuǎn)矢量()(其中,=--1為姿態(tài)更新周期),通過(guò)比較對(duì)應(yīng)時(shí)間段內(nèi)的變換四元數(shù)(),可求得()的近似值(一般看作理想旋轉(zhuǎn)矢量):

(3)

根據(jù)式(1)積分可得單位姿態(tài)更新時(shí)間段[-1,]內(nèi)的角增量為

(4)

根據(jù)式(3)和式(4),學(xué)者基于角增量輸入情況提出的圓錐優(yōu)化方法通式為

(5)

(6)

(7)

將進(jìn)行泰勒級(jí)數(shù)展開(kāi),令低階項(xiàng)的系數(shù)為零,即可求出圓錐優(yōu)化補(bǔ)償系數(shù)矩陣,有

=[,,,…,-1]

(8)

綜上所述,圓錐優(yōu)化補(bǔ)償算法目的是在理想環(huán)境下,利用各姿態(tài)參數(shù)的理論值優(yōu)化出最優(yōu)補(bǔ)償系數(shù),再將所求系數(shù)應(yīng)用于一般角運(yùn)動(dòng)情況,從而提高傳統(tǒng)等效旋轉(zhuǎn)矢量算法的精度。

但針對(duì)角速率輸入的情況,若簡(jiǎn)單地對(duì)角速率進(jìn)行多項(xiàng)式擬合-積分得到角增量信息,再代入上述算法進(jìn)行姿態(tài)更新,則會(huì)使算法的性能大大降低。接下來(lái)將會(huì)對(duì)某一含有角速率信息的圓錐誤差補(bǔ)償結(jié)構(gòu)進(jìn)行具體描述,并分析其目前所存在的不足。

2 角速率輸入圓錐補(bǔ)償算法

針對(duì)上述問(wèn)題,有學(xué)者提出一種基于角速率輸入的圓錐補(bǔ)償算法,算法通式參考式(5),其中,

(9)

雖然該算法相較于傳統(tǒng)的角速率圓錐補(bǔ)償算法有著明顯的精度優(yōu)勢(shì),但因其在子樣計(jì)算條件下所需的角速率子樣數(shù)是傳統(tǒng)算法的倍,導(dǎo)致姿態(tài)更新周期至少延長(zhǎng)-1倍,失去了傳統(tǒng)算法的高實(shí)時(shí)性優(yōu)勢(shì)。

3 多區(qū)間角增量修正姿態(tài)算法

根據(jù)第2節(jié)中所提補(bǔ)償算法推導(dǎo)過(guò)程的分析結(jié)論和其存在的不足,本節(jié)探討一種角速率輸入條件下基于多區(qū)間角增量滯后修正的姿態(tài)解算方法。

3.1 類單子樣擬合方法原理

T=()=()=

=(-1),=1,2,…,

構(gòu)造圓錐補(bǔ)償項(xiàng):

(10)

圖1 類單子樣算法時(shí)間關(guān)系示意圖Fig.1 Schematic diagram of the time relationship of a class of monadic algorithms

(11)

(12)

(13)

經(jīng)過(guò)簡(jiǎn)單的推導(dǎo),式(10)可化簡(jiǎn)為

(14)

式中:、和為最終所求各化簡(jiǎn)叉乘項(xiàng)的圓錐誤差補(bǔ)償系數(shù)。其可進(jìn)一步表示為

(15)

最優(yōu)補(bǔ)償系數(shù)的求解過(guò)程為

(16)

其中,

(17)

值得指出的是,式(10)在理論值環(huán)境下化簡(jiǎn)到式(14)的過(guò)程也與第1節(jié)中所提圓錐優(yōu)化補(bǔ)償算法的基本思想相一致。

根據(jù)上述推導(dǎo)分析,現(xiàn)給出1～3子樣算法的補(bǔ)償系數(shù)如表1所示。

表1 1～3子樣算法的補(bǔ)償系數(shù)

3.2 角增量滯后修正方法

圖2 角增量滯后修正示意圖Fig.2 Schematic diagram of angular increment lag correction

分析圖2可知,每個(gè)角增量Δ可由-1個(gè)角速度多項(xiàng)式()在對(duì)應(yīng)的單位采樣周期內(nèi)積分求均值得到,即

(18)

(19)

相較于傳統(tǒng)算法使用單個(gè)多項(xiàng)式擬合的角增量提取方法,該方法所求的角增量包含更多的角速度變化信息,因而可在一定程度上提高角增量的計(jì)算精度。

4 姿態(tài)算法計(jì)算量分析

本節(jié)將從叉乘運(yùn)算次數(shù),擬合積分運(yùn)算次數(shù)和信息復(fù)用次數(shù)3個(gè)方面進(jìn)行計(jì)算量分析。為方便分析,記第2節(jié)中的算法為算法1(簡(jiǎn)記為Alg1),記第3節(jié)算法為算法2(簡(jiǎn)記為Alg2)。Alg1算法中擬合角速率數(shù)量記為1,角增量數(shù)量記為1;Alg2算法中擬合角速率數(shù)量記為2,角增量數(shù)量記為2。令導(dǎo)航周期為,且1=2,1=2。

對(duì)于Alg1算法,從式(9)可看出,在單位更新周期1=1內(nèi),誤差補(bǔ)償項(xiàng)要進(jìn)行(31-1)次叉乘運(yùn)算,進(jìn)行1次擬合積分運(yùn)算,信息復(fù)用次數(shù)為0。則Alg1算法在導(dǎo)航周期內(nèi)的叉乘運(yùn)算次數(shù)為

=(1)·(31-1)

(20)

積分?jǐn)M合運(yùn)算次數(shù)為

=(1)·1

(21)

信息復(fù)用次數(shù)為0。

對(duì)于Alg2算法,從式(10)、式(18)、式(19)可看出,在單位更新周期2=2內(nèi),誤差補(bǔ)償項(xiàng)要進(jìn)行(32-2)次叉乘運(yùn)算,進(jìn)行2·2次擬合積分運(yùn)算,分析圖2可知,信息復(fù)用次數(shù)為2·(2-2)。則Alg2算法在導(dǎo)航周期內(nèi)的叉乘運(yùn)算次數(shù)為

=(2)·(32-2)

(22)

積分?jǐn)M合運(yùn)算次數(shù)為

=(2)·2·2

(23)

信息復(fù)用次數(shù)為

=(2)·2·(2-2)

(24)

但是對(duì)于當(dāng)今計(jì)算機(jī)的計(jì)算速度來(lái)說(shuō),式(20)～式(24)的計(jì)算時(shí)間遠(yuǎn)小于姿態(tài)更新周期,更新周期是影響算法實(shí)時(shí)性的主要因素。則導(dǎo)航周期內(nèi)Alg2算法和Alg1算法的姿態(tài)更新的頻率比值可示為

12=-1

(25)

5 仿真與分析

圓錐運(yùn)動(dòng)環(huán)境仿真的參數(shù)設(shè)置為:圓錐頻率=1 Hz,圓錐半錐角的變化范圍為0.01″～90°,導(dǎo)航時(shí)長(zhǎng)設(shè)置為1 min,角速率采樣間隔分別設(shè)置為5 ms、1 ms,利用4階多項(xiàng)式對(duì)角速率進(jìn)行擬合。經(jīng)過(guò)仿真,導(dǎo)航周期內(nèi)在常值軸上的最大漂移誤差分別如圖3、圖4所示,實(shí)線為Alg1算法的常值漂移誤差,虛線為Alg2算法的常值漂移誤差,各圖例與子樣數(shù)的對(duì)應(yīng)關(guān)系依次為=2～5。

圖3 T=5 ms時(shí)常值漂移誤差Fig.3 Constant drift error when T=5 ms

圖4 T=1 ms時(shí)常值漂移誤差Fig.4 Constant drift error when T=1 ms

觀察分析圖3、圖4可以得出以下結(jié)論:

當(dāng)子樣數(shù)=2時(shí),Alg2算法和Alg1算法的計(jì)算精度均最高;且隨著子樣數(shù)的增加,兩種算法的計(jì)算精度均逐漸下降;隨著采樣頻率從200 Hz增加至1 000 Hz,兩種算法在不同子樣數(shù)下的計(jì)算精度整體提高1～3個(gè)數(shù)量級(jí)。

采樣頻率為200 Hz的情況下,子樣數(shù)=2～3時(shí),在<0.5°的小半錐角范圍內(nèi),Alg2算法的計(jì)算精度略低于Alg1算法;在=0.5°～90°的大半錐角范圍內(nèi),Alg2算法最高高于Alg1算法的計(jì)算精度3個(gè)數(shù)量級(jí);且在這兩種子樣數(shù)下Alg2算法的計(jì)算精度接近。子樣數(shù)=4時(shí),Alg2算法的計(jì)算精度整體高于Alg1算法1個(gè)數(shù)量級(jí)。子樣數(shù)=5時(shí),在半錐角<2°范圍內(nèi),Alg2算法的計(jì)算精度整體高于Alg1算法1個(gè)數(shù)量級(jí);在半錐角>2°范圍內(nèi),Alg2算法的計(jì)算精度與Alg1算法相當(dāng)。

采樣頻率為1 000 Hz的情況下,子樣數(shù)=2時(shí),在<0.08°的小半錐角范圍內(nèi),Alg2算法的計(jì)算精度略低于Alg1算法;在=0.08°～90°的大半錐角范圍內(nèi),Alg2算法最高高于Alg1算法的計(jì)算精度3個(gè)數(shù)量級(jí)。當(dāng)子樣數(shù)=3～4時(shí),Alg2算法的計(jì)算精度整體高于Alg1算法1個(gè)數(shù)量級(jí)。子樣數(shù)=5時(shí),在半錐角<10°范圍內(nèi),Alg2算法的計(jì)算精度整體高于Alg1算法1個(gè)數(shù)量級(jí);在半錐角>10°范圍內(nèi),Alg2算法的計(jì)算精度與Alg1算法相當(dāng)。

為了進(jìn)一步驗(yàn)證新算法的性能,在采樣周期為=04 ms,子樣數(shù)=2,利用4階多項(xiàng)式對(duì)角速率進(jìn)行擬合的條件下,將圓錐頻率的范圍設(shè)置為1～1 000 Hz,在常見(jiàn)的小半錐角=0.01°,0.1°,1°情況下分別進(jìn)行仿真實(shí)驗(yàn),比較兩種算法的性能,對(duì)比結(jié)果如圖5所示。

圖5 T=0.4 ms時(shí)不同小半錐角條件下算法性能對(duì)比Fig.5 Algorithm performance comparison with different smallhalf-cone-angles when T=0.4 ms

除上述在小半錐角條件下的仿真分析之外,在大半錐角=10°,30°,50°,70°,90°的情況下也分別進(jìn)行仿真實(shí)驗(yàn),比較兩種算法的性能,對(duì)比結(jié)果如表2所示。

表2 T=0.4 ms時(shí)不同大半錐角條件下算法性能對(duì)比

表2中，“Alg1優(yōu)勢(shì)程度”=(Alg1)(Alg2),“Alg2優(yōu)勢(shì)程度”=(Alg2)(Alg1)。

小半錐角條件下,分析圖5可以得到以下結(jié)論:

由圖5(a)可觀察到,在圓錐頻率=1～300 Hz范圍內(nèi),Alg2算法的計(jì)算精度整體略低于Alg1算法,在圓錐頻率=300～1 000 Hz范圍內(nèi),Alg2算法的計(jì)算精度整體高于Alg1算法0～3個(gè)數(shù)量級(jí),特別地,在圓錐頻率=885～905 Hz范圍內(nèi),Alg1算法的計(jì)算精度高于Alg2算法不到0.5個(gè)數(shù)量級(jí);由圖5(b)可觀察到,在圓錐頻率=1～9 Hz和Ω=300～1 000 Hz的范圍內(nèi),Alg2算法的計(jì)算精度整體高于Alg1算法0～3個(gè)數(shù)量級(jí),在圓錐頻率=9～300 Hz范圍內(nèi),Alg2算法的計(jì)算精度整體略低于Alg1算法,特別地,在圓錐頻率=885～905 Hz范圍內(nèi),Alg1算法的計(jì)算精度高于Alg2算法不到0.5個(gè)數(shù)量級(jí);觀察圖5(c)可知,在圓錐頻率=1～50 Hz和=70～1 000 Hz的范圍內(nèi),Alg2算法的計(jì)算精度整體高于Alg1算法0～3個(gè)數(shù)量級(jí),在圓錐頻率=50～70 Hz的小范圍內(nèi),Alg2算法的計(jì)算精度低于Alg1算法0～3個(gè)數(shù)量級(jí),特別地,在圓錐頻率=885～905 Hz范圍內(nèi),Alg1算法的計(jì)算精度高于Alg2算法不到0.5個(gè)數(shù)量級(jí);隨著半錐角的增大,在圓錐頻率=1～300 Hz的范圍內(nèi),Alg2算法的整體計(jì)算精度逐漸優(yōu)于Alg1算法計(jì)算精度,且在=300～1 000 Hz范圍內(nèi)Alg2算法的計(jì)算精度優(yōu)勢(shì)保持不變。

大半錐角條件下,分析表1可以得到以下結(jié)論:

(1) 在Alg1算法的精度優(yōu)勢(shì)頻率區(qū)間內(nèi),Alg2算法和Alg1算法的整體計(jì)算精度相差1個(gè)數(shù)量級(jí)以內(nèi),可認(rèn)為在該區(qū)間內(nèi)兩種算法的性能接近。

(2) 在Alg2算法的精度優(yōu)勢(shì)頻率區(qū)間內(nèi),除了相同精度的交點(diǎn)外,Alg1算法和Alg2算法的整體計(jì)算精度相差2～3個(gè)數(shù)量級(jí),可認(rèn)為該區(qū)間內(nèi)Alg2算法的性能優(yōu)于Alg1算法;且隨著半錐角的增大,Alg2算法的精度優(yōu)勢(shì)頻率區(qū)間逐漸縮短并向低頻范圍靠近。

6 結(jié) 論

本文在慣組輸出為角速率的情況下提出一種基于多區(qū)間角增量滯后修正方法來(lái)提取角增量信息,進(jìn)而構(gòu)造圓錐誤差補(bǔ)償結(jié)構(gòu)的旋轉(zhuǎn)矢量姿態(tài)算法。在上述的各種仿真條件下,該算法可在提高姿態(tài)更新頻率的同時(shí),使得其計(jì)算精度在一定的圓錐頻率范圍內(nèi),比傳統(tǒng)圓錐誤差補(bǔ)償算法的計(jì)算精度提高個(gè)2～3個(gè)數(shù)量級(jí)。仿真結(jié)果表明,新算法的整體性能優(yōu)于傳統(tǒng)算法。要想實(shí)現(xiàn)更高精度的導(dǎo)航,除了提高導(dǎo)航信息的提取精度之外,提高慣性器件的測(cè)量精度也越來(lái)越重要,現(xiàn)有捷聯(lián)慣導(dǎo)的器件噪聲也是限制高精度姿態(tài)算法(如基于畢卡迭代和多項(xiàng)式迭代的姿態(tài)算法)性能的主要因素。