基于多區間參數修正的角速率姿態算法

梁 哲, 周召發,*, 徐志浩, 呂文亭, 段 輝

(1. 火箭軍工程大學導彈工程學院, 陜西 西安 710025; 2. 火箭軍工程大學兵器發射理論和技術國家重點學科實驗室, 陜西 西安 710025; 3. 中國人民解放軍92281部隊, 山東 青島 266000)

0 引 言

慣性導航與衛星導航的方式相比,具有高自主性,高可靠性,高隱蔽性,長時間導航等優點,姿態解算則是慣性導航過程中的核心環節,傳統的捷聯慣導姿態算法主要包括歐拉角法、方向余弦法、四元數法和等效旋轉矢量法,鑒于上述各個方法的優缺點,目前主流的算法為后兩者。并且,利用方向余弦法和四元數法實現高精度計算結果的條件較為苛刻,而等效旋轉矢量法相較于四元數法有著計算量小,容易得到理想計算精度的特點,成為目前最常用的姿態算法。

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

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

本文將在慣組輸出為角速率的情況下,參考單子樣姿態算法的思路,在角速度多項式擬合過程中利用前周期的部分角速率子樣信息,通過對多個不同擬合區間的角速度多項式積分來提取對應采樣周期的角增量信息,進而利用角速率信息和多次修正后的角增量信息構造圓錐誤差補償結構,以提高傳統角速率姿態算法的解算速度和姿態解算精度。文章最后在圓錐運動環境下對新方法與傳統角速率姿態算法的解算精度進行比較。

1 圓錐運動及誤差描述

圓錐運動被認為是最為苛刻的導航環境,在錐運動下優化得到的不可交換誤差補償系數可用于一般角運動情況。圓錐運動角速度可描述為

(1)

式中:半錐角和圓錐運動頻率均為常數,表明載體在軸上和軸上做同頻但相位差90°的正弦角運動,導致在軸上輸出常值角運動。其對應的姿態四元數為

(2)

由于不能直接得到單位姿態更新時間段[-1,]內的等效旋轉矢量()(其中,=--1為姿態更新周期),通過比較對應時間段內的變換四元數(),可求得()的近似值(一般看作理想旋轉矢量):

(3)

根據式(1)積分可得單位姿態更新時間段[-1,]內的角增量為

(4)

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

(5)

(6)

(7)

將進行泰勒級數展開,令低階項的系數為零,即可求出圓錐優化補償系數矩陣,有

=[,,,…,-1]

(8)

綜上所述,圓錐優化補償算法目的是在理想環境下,利用各姿態參數的理論值優化出最優補償系數,再將所求系數應用于一般角運動情況,從而提高傳統等效旋轉矢量算法的精度。

但針對角速率輸入的情況,若簡單地對角速率進行多項式擬合-積分得到角增量信息,再代入上述算法進行姿態更新,則會使算法的性能大大降低。接下來將會對某一含有角速率信息的圓錐誤差補償結構進行具體描述,并分析其目前所存在的不足。

2 角速率輸入圓錐補償算法

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

(9)

雖然該算法相較于傳統的角速率圓錐補償算法有著明顯的精度優勢,但因其在子樣計算條件下所需的角速率子樣數是傳統算法的倍,導致姿態更新周期至少延長-1倍,失去了傳統算法的高實時性優勢。

3 多區間角增量修正姿態算法

根據第2節中所提補償算法推導過程的分析結論和其存在的不足,本節探討一種角速率輸入條件下基于多區間角增量滯后修正的姿態解算方法。

3.1 類單子樣擬合方法原理

T=()=()=

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

構造圓錐補償項:

(10)

圖1 類單子樣算法時間關系示意圖Fig.1 Schematic diagram of the time relationship of a class of monadic algorithms

(11)

(12)

(13)

經過簡單的推導,式(10)可化簡為

(14)

式中:、和為最終所求各化簡叉乘項的圓錐誤差補償系數。其可進一步表示為

(15)

最優補償系數的求解過程為

(16)

其中,

(17)

值得指出的是,式(10)在理論值環境下化簡到式(14)的過程也與第1節中所提圓錐優化補償算法的基本思想相一致。

根據上述推導分析,現給出1～3子樣算法的補償系數如表1所示。

表1 1～3子樣算法的補償系數

3.2 角增量滯后修正方法

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

分析圖2可知,每個角增量Δ可由-1個角速度多項式()在對應的單位采樣周期內積分求均值得到,即

(18)

(19)

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

4 姿態算法計算量分析

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

對于Alg1算法,從式(9)可看出,在單位更新周期1=1內,誤差補償項要進行(31-1)次叉乘運算,進行1次擬合積分運算,信息復用次數為0。則Alg1算法在導航周期內的叉乘運算次數為

=(1)·(31-1)

(20)

積分擬合運算次數為

=(1)·1

(21)

信息復用次數為0。

對于Alg2算法,從式(10)、式(18)、式(19)可看出,在單位更新周期2=2內,誤差補償項要進行(32-2)次叉乘運算,進行2·2次擬合積分運算,分析圖2可知,信息復用次數為2·(2-2)。則Alg2算法在導航周期內的叉乘運算次數為

=(2)·(32-2)

(22)

積分擬合運算次數為

=(2)·2·2

(23)

信息復用次數為

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

(24)

但是對于當今計算機的計算速度來說,式(20)～式(24)的計算時間遠小于姿態更新周期,更新周期是影響算法實時性的主要因素。則導航周期內Alg2算法和Alg1算法的姿態更新的頻率比值可示為

12=-1

(25)

5 仿真與分析

圓錐運動環境仿真的參數設置為:圓錐頻率=1 Hz,圓錐半錐角的變化范圍為0.01″～90°,導航時長設置為1 min,角速率采樣間隔分別設置為5 ms、1 ms,利用4階多項式對角速率進行擬合。經過仿真,導航周期內在常值軸上的最大漂移誤差分別如圖3、圖4所示,實線為Alg1算法的常值漂移誤差,虛線為Alg2算法的常值漂移誤差,各圖例與子樣數的對應關系依次為=2～5。

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

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

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

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

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

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

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

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

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

表2 T=0.4 ms時不同大半錐角條件下算法性能對比

表2中，“Alg1優勢程度”=(Alg1)(Alg2),“Alg2優勢程度”=(Alg2)(Alg1)。

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

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

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

(1) 在Alg1算法的精度優勢頻率區間內,Alg2算法和Alg1算法的整體計算精度相差1個數量級以內,可認為在該區間內兩種算法的性能接近。

(2) 在Alg2算法的精度優勢頻率區間內,除了相同精度的交點外,Alg1算法和Alg2算法的整體計算精度相差2～3個數量級,可認為該區間內Alg2算法的性能優于Alg1算法;且隨著半錐角的增大,Alg2算法的精度優勢頻率區間逐漸縮短并向低頻范圍靠近。

6 結 論

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