邢麗, 熊智, 劉建業(yè), 杭義軍

(南京航空航天大學(xué) 導(dǎo)航研究中心， 江蘇 南京 211100)

基于對偶四元數(shù)的圓錐及劃船誤差補(bǔ)償改進(jìn)算法

邢麗, 熊智, 劉建業(yè), 杭義軍

(南京航空航天大學(xué) 導(dǎo)航研究中心， 江蘇 南京 211100)

為提高高動態(tài)環(huán)境下的對偶四元數(shù)捷聯(lián)慣性導(dǎo)航算法解算精度，將梯形數(shù)字積分算法應(yīng)用于圓錐和劃船誤差補(bǔ)償算法中，改進(jìn)了姿態(tài)和速度解算算法，提高了對偶四元數(shù)捷聯(lián)慣性導(dǎo)航算法的解算精度。在單個(gè)采樣周期內(nèi)，利用前一時(shí)刻采集的陀螺角速率信號和當(dāng)前時(shí)刻采集的陀螺角速率信號，通過梯形積分方式計(jì)算角增量進(jìn)行圓錐誤差補(bǔ)償；利用前一時(shí)刻采集的加速度計(jì)信號和當(dāng)前時(shí)刻的加速度計(jì)信號，通過梯形積分方式計(jì)算速度增量并結(jié)合同一時(shí)刻的角增量進(jìn)行劃船誤差補(bǔ)償。通過設(shè)計(jì)的多組動態(tài)模擬仿真航跡驗(yàn)證表明，當(dāng)角速率和比力作為圓錐和劃船誤差補(bǔ)償算法輸入時(shí)，梯形積分算法的精度高于傳統(tǒng)的矩形積分算法，且航跡的動態(tài)性越高，改進(jìn)算法的性能優(yōu)勢越顯著。同時(shí)，通過動態(tài)跑車實(shí)驗(yàn)結(jié)果的分析對比，進(jìn)一步驗(yàn)證了該改進(jìn)算法的實(shí)用性。

控制科學(xué)與技術(shù)； 對偶四元數(shù)； 捷聯(lián)慣性導(dǎo)航算法； 圓錐誤差補(bǔ)償算法； 劃船誤差補(bǔ)償算法； 梯形積分方法

0 引言

捷聯(lián)慣性導(dǎo)航(簡稱慣導(dǎo))的計(jì)算任務(wù)包含姿態(tài)解算和速度、位置解算。在大機(jī)動環(huán)境中，高精度捷聯(lián)慣導(dǎo)算法性能的關(guān)鍵要素是姿態(tài)的更新是否能夠準(zhǔn)確實(shí)時(shí)跟蹤載體姿態(tài)的變化[1]。姿態(tài)更新過程中旋轉(zhuǎn)不可交換性誤差是主要的誤差來源，為了補(bǔ)償旋轉(zhuǎn)不可交換性誤差，現(xiàn)有的文獻(xiàn)集中在圓錐誤差補(bǔ)償算法研究中，且大部分采用截?cái)嗵├占墧?shù)展開進(jìn)行旋轉(zhuǎn)矢量的近似計(jì)算，實(shí)現(xiàn)圓錐誤差的補(bǔ)償[2-13]?，F(xiàn)有圓錐誤差補(bǔ)償算法的研究大致可以分為3個(gè)方面：多子樣圓錐誤差補(bǔ)償算法的系數(shù)設(shè)計(jì)[3-6]，不同輸入方式(角速率或角增量)的圓錐誤差補(bǔ)償算法研究[7-11]以及圓錐誤差補(bǔ)償算法的結(jié)構(gòu)設(shè)計(jì)[12]。在多子樣圓錐誤差補(bǔ)償算法的系數(shù)設(shè)計(jì)方面，主要采用了基于時(shí)域的泰勒展開方法[4-6]和基于圓錐運(yùn)動頻域的泰勒展開方法[7]。在文獻(xiàn)[12]中，Savage進(jìn)一步擴(kuò)展了Miller的方法，并且提出了一種顯式頻率成形方法用于圓錐誤差補(bǔ)償算法設(shè)計(jì)中，即圓錐誤差補(bǔ)償系數(shù)的設(shè)計(jì)是通過最小二乘法而不是頻域展開方式獲取最優(yōu)圓錐誤差補(bǔ)償性能。根據(jù)不同陀螺儀的輸出信號，圓錐誤差補(bǔ)償算法中旋轉(zhuǎn)矢量的計(jì)算分別采用了角增量和角速率作為輸入。而在圓錐算法結(jié)構(gòu)設(shè)計(jì)方面目前的研究也分為兩類，一類是雙速結(jié)構(gòu)，另一類是單速結(jié)構(gòu)。

與姿態(tài)解算相似，速度更新過程中存在積分誤差，對于積分誤差的補(bǔ)償算法稱為劃船誤差補(bǔ)償算法。雖然圓錐和劃船誤差補(bǔ)償具有對偶性[13]，但是很少有參考文獻(xiàn)針對性研究劃船誤差補(bǔ)償算法。在文獻(xiàn)[14-15]中，武元新采用了對偶四元數(shù)算法，由于對偶四元數(shù)可以同時(shí)描述剛體的轉(zhuǎn)動和平移，因此可以同時(shí)進(jìn)行姿態(tài)和速度更新，實(shí)現(xiàn)圓錐和劃船誤差的同步補(bǔ)償[16-18]，簡化了圓錐和劃船誤差補(bǔ)償算法。雖然對偶四元數(shù)捷聯(lián)慣導(dǎo)算法可以簡化圓錐和劃船誤差補(bǔ)償算法，但是目前給出的補(bǔ)償算法是基于角增量和速度增量輸入的圓錐和劃船誤差補(bǔ)償算法，如果慣性傳感器輸出為角速率和比力時(shí)，使用基于角增量和速度增量輸入的圓錐和劃船誤差補(bǔ)償算法，就涉及到角速率和比力積分為角增量和速度增量的過程。若采用傳統(tǒng)矩形積分方法，積分過程中會避免不了引入積分計(jì)算誤差。另外，雖然目前有算法提出采用角速率輸入的圓錐誤差補(bǔ)償算法，但是基于角增量和角速率輸入的圓錐誤差補(bǔ)償算法在一定意義上是具有等效性的[19]，且基于角速率的圓錐誤差算法的補(bǔ)償通式相對較為復(fù)雜。因此，在角速率和比力輸入條件下，需要對傳統(tǒng)基于增量形式的對偶四元數(shù)圓錐及劃船誤差補(bǔ)償算法進(jìn)行改進(jìn)，從而提高其補(bǔ)償精度。

為此，本文提出一種對偶四元數(shù)圓錐及劃船誤差補(bǔ)償?shù)母倪M(jìn)算法，即在圓錐和劃船誤差補(bǔ)償算法中采用梯形數(shù)字積分算法，將角速率和比力輸入積分為角增量和速度增量，減少了傳統(tǒng)方法中采用矩形數(shù)字積分方法帶來的積分誤差。改進(jìn)的算法性能通過多組不同參數(shù)設(shè)置的動態(tài)模擬航跡進(jìn)行了仿真驗(yàn)證，仿真結(jié)果表明當(dāng)陀螺儀角速率和加速度計(jì)比力作為輸入時(shí)，梯形積分算法的精度高于傳統(tǒng)的矩形積分算法，且航跡的動態(tài)性越高，改進(jìn)算法的性能優(yōu)勢越顯著。同時(shí)，為了更充分地驗(yàn)證改進(jìn)算法性能，設(shè)計(jì)了動態(tài)跑車實(shí)驗(yàn)，通過采集的實(shí)際系統(tǒng)慣性器件輸出，分別采用改進(jìn)算法和傳統(tǒng)算法進(jìn)行捷聯(lián)慣導(dǎo)解算，實(shí)際數(shù)據(jù)驗(yàn)證結(jié)果也同樣證明了改進(jìn)算法能夠進(jìn)一步提高對偶四元數(shù)捷聯(lián)慣導(dǎo)算法的解算精度。

1 基于對偶四元數(shù)的傳統(tǒng)圓錐和劃船誤差補(bǔ)償算法

(1)

式中：qIt、q′It都是普通四元數(shù)；ε為對偶算子；下標(biāo)I代表慣性坐標(biāo)系，下標(biāo)t代表推力速度坐標(biāo)系。

(2)

(3)

(4)

(5)

(6)

(7)

(8)

0≤τ≤ΔT, ΔT為更新周期。

與等效旋轉(zhuǎn)矢量類似，微分方程(6)式在實(shí)際計(jì)算過程中可以簡化為

(9)

(10)

(11)

(12)

(13)

(14)

(15)

(16)

(aω+εaf)=(bω+εbf),

(17)

(18)

(19)

(20)

(21)

(22)

將(21)式和(22)式代入(20)式中得

(23)

由(23)式可以看出當(dāng)采用二子樣算法對等效螺旋矢量進(jìn)行更新時(shí)，其表達(dá)式與等效旋轉(zhuǎn)矢量的二子樣算法的求解式子相似，由此類比推導(dǎo)得到文獻(xiàn)[14]中等效螺旋矢量多子樣算法：

(24)

式中：螺旋誤差補(bǔ)償?shù)膬?yōu)化系數(shù)Kij的數(shù)值與圓錐誤差補(bǔ)償?shù)膬?yōu)化系數(shù)數(shù)值完全相同。

(25)

2)傳統(tǒng)方法Ⅱ，為了減少傳統(tǒng)方法I的積分計(jì)算誤差，在圓錐誤差補(bǔ)償算法中，提出了基于角速率輸出條件下的圓錐誤差補(bǔ)償算法，推導(dǎo)了基于角速率的等效旋轉(zhuǎn)矢量計(jì)算方法，但其推導(dǎo)過程和補(bǔ)償通式較為復(fù)雜。

因此，借鑒角速率輸入條件下的等效旋轉(zhuǎn)矢量計(jì)算方法的推導(dǎo)理論，將其進(jìn)一步擴(kuò)展得到角速率和比力輸入條件下的等效螺旋矢量更新過程中，改進(jìn)對偶四元數(shù)算法中傳統(tǒng)的圓錐和劃船誤差補(bǔ)償算法。

2 基于對偶四元數(shù)圓錐和劃船誤差補(bǔ)償改進(jìn)算法

(26)

式中：

(27)

圖和等效螺旋矢量關(guān)系圖Fig.1 Relationship among , and

(28)

對比(26)式和(28)式可以看出，采用傳統(tǒng)方法Ⅰ推導(dǎo)得到的(26)式，由于積分誤差而帶來了計(jì)算精度的損失；而(28)式的補(bǔ)償通式和計(jì)算過程又相對復(fù)雜。因此，如果將(26)式的計(jì)算過程進(jìn)行改進(jìn)，用梯形數(shù)字積分方法計(jì)算螺旋增量，如(29)式和(30)式所示：

(29)

(30)

將(29)式和(30)式代入(24)式，則可得

(31)

(32)

(33)

圖2 基于對偶四元數(shù)的改進(jìn)及傳統(tǒng)圓錐和劃船誤差補(bǔ)償算法的對比Fig.2 Comparison of improved coning and sculling algorithms based on dual quaternions and conventional coning and sculling algorithms

3 仿真驗(yàn)證

3.1 仿真參數(shù)設(shè)置

為了驗(yàn)證本文所提出的基于對偶四元數(shù)的圓錐及劃船誤差補(bǔ)償改進(jìn)算法的優(yōu)勢及有效性，設(shè)計(jì)了多組不同參數(shù)條件下的數(shù)字仿真航跡，進(jìn)行捷聯(lián)慣導(dǎo)解算，并且將其與傳統(tǒng)方法Ⅰ進(jìn)行對比分析。為了模擬圓錐和劃船效應(yīng)，仿真航跡中將角運(yùn)動和線運(yùn)動設(shè)置為周期性變化的正余弦形式，并且為了模擬高動態(tài)航跡，按照文獻(xiàn)[20]中給出的高動態(tài)航跡定義，將運(yùn)動加速度設(shè)置為50g，正余弦變化頻率設(shè)置為5～12 Hz.

數(shù)字仿真航跡中設(shè)計(jì)的姿態(tài)角角速率和載體運(yùn)動加速度為

(34)

ax=ay=az=nsin (2πf1t)，

(35)

仿真總時(shí)間為300 s，導(dǎo)航解算周期為0.01 s，慣性測量單元數(shù)據(jù)生成頻率為200 Hz.

為了充分驗(yàn)證不同動態(tài)航跡下提出的改進(jìn)算法的有效性，將(34)式和(35)式中的相關(guān)參數(shù)m、n和f1進(jìn)行多組設(shè)計(jì)，如表1所示。這3組仿真參數(shù)的設(shè)置主要從以下兩個(gè)方面進(jìn)行考慮：1)第1組和第2組的參數(shù)設(shè)置目的是驗(yàn)證同一運(yùn)動頻率下，不同的姿態(tài)角波動幅值和不同的運(yùn)動速度幅值條件下，改進(jìn)算法與傳統(tǒng)算法相比的精度改善情況；2)第2組和第3組的參數(shù)設(shè)置目的是驗(yàn)證姿態(tài)角和運(yùn)動速度幅值相同，而運(yùn)動頻率不同條件下改進(jìn)算法與傳統(tǒng)算法相比的精度改善情況。

表1 航跡參數(shù)m、n和f1的多組設(shè)計(jì)

3.2 仿真結(jié)果分析

圖3 第1組航跡的導(dǎo)航誤差對比Fig.3 Navigation error comparison of Track 1

圖4 第2組航跡的導(dǎo)航誤差對比Fig.4 Navigation error comparison of Track 2

從第1組和第2組的仿真對比曲線中可以看出，在正余弦形式變化的仿真航跡中，無論是動態(tài)性相對較小的第1組航跡，還是高動態(tài)情況下的第2組航跡，其姿態(tài)、速度和位置解算精度對比結(jié)果表明，改進(jìn)算法優(yōu)于傳統(tǒng)算法。

為了更加清晰和明確地說明改進(jìn)算法與傳統(tǒng)算法相比較的優(yōu)勢，表2和表3中統(tǒng)計(jì)了兩種算法在不同航跡參數(shù)設(shè)置條件下，姿態(tài)、速度和位置解算誤差的最大值和均方根(RMS)對比結(jié)果。從表2和表3中可以看出，不同組參數(shù)設(shè)置的動態(tài)航跡在采用了改進(jìn)算法后，導(dǎo)航解算誤差的最大值和RMS均小于傳統(tǒng)算法，即姿態(tài)、速度和位置的解算精度均得到了提升。

表2 導(dǎo)航解算誤差的最大值對比分析

表3 導(dǎo)航解算誤差RMS對比分析

圖5 第1組和第2組航跡的改進(jìn)算法導(dǎo)航精度提升對比Fig.5 Navigation precision improvements of Tracks 1 and 2

圖6 第2組和第3組航跡的改進(jìn)算法導(dǎo)航精度提升對比Fig.6 Navigation precision improvements of Tracks 2 and 3

同時(shí)圖5和圖6分別將表2和表3中統(tǒng)計(jì)的第1組和第2組航跡、第2組和第3組航跡的導(dǎo)航解算誤差最大值的精度提升幅度以直方圖的形式進(jìn)行了對比。圖5中寬條矩形框代表第1組航跡中改進(jìn)算法相比傳統(tǒng)算法在解算姿態(tài)、速度和位置時(shí)精度提升幅度，箭頭直線代表第2組航跡中精度提升幅度。從圖5中明顯看出采用改進(jìn)算法后，第2組航跡導(dǎo)航解算精度得到了更大的提升。

圖6中窄條矩形框代表第3組航跡改進(jìn)算法相比傳統(tǒng)算法在解算姿態(tài)、速度和位置時(shí)精度提升幅度。從圖6中看出采用改進(jìn)算法后，第3組航跡導(dǎo)航解算精度得到了更大的提升。

從圖5和圖6中給出的對比圖線，可以得到如下的結(jié)論：

1)圖5中第1組和第2組航跡相對比，當(dāng)同一運(yùn)動頻率下，載體的運(yùn)動幅值變大時(shí)，改進(jìn)算法的導(dǎo)航解算精度更優(yōu)于傳統(tǒng)算法；

2)圖6中第2組和第3組航跡相對比，當(dāng)在同一運(yùn)動幅值下，載體的運(yùn)動頻率變高時(shí)，改進(jìn)算法的導(dǎo)航解算精度也更優(yōu)于傳統(tǒng)算法。

綜合結(jié)論1和結(jié)論2可以充分地證明當(dāng)應(yīng)用于高動態(tài)環(huán)境中時(shí)，本文提出的改進(jìn)算法在進(jìn)行了圓錐和劃船誤差修正后，導(dǎo)航解算精度優(yōu)于傳統(tǒng)算法。

4 動態(tài)跑車實(shí)驗(yàn)驗(yàn)證

為了更充分的驗(yàn)證本文提出的基于對偶四元數(shù)的圓錐和劃船改進(jìn)算法，采用了加拿大NovAtel公司的產(chǎn)品SPAN-CPT光纖/全球定位系統(tǒng)(GPS)組合導(dǎo)航系統(tǒng)輸出的實(shí)際數(shù)據(jù)，設(shè)計(jì)了動態(tài)跑車實(shí)驗(yàn)對本文提出的改進(jìn)算法進(jìn)行實(shí)際數(shù)據(jù)驗(yàn)證。

圖7 跑車實(shí)驗(yàn)設(shè)備示意圖Fig.7 Kinematic vehicle experimental equipment

圖8 跑車實(shí)驗(yàn)的數(shù)據(jù)實(shí)時(shí)采集Fig.8 Real-time data acquisition process of kinematic vehicle experiment

圖9 跑車實(shí)驗(yàn)的軌跡運(yùn)行曲線Fig.9 Dynamic track curve of kinematic vehicle experiment

圖10 光纖/GPS組合導(dǎo)航系統(tǒng)輸出的跑車位置Fig.10 Vehicle position outputs of FOG SINS/GPS integrated navigation system

圖11 光纖/GPS組合導(dǎo)航系統(tǒng)輸出的跑車速度Fig.11 Vehicle velocity outputs of FOG SINS/GPS integrated navigation system

跑車實(shí)驗(yàn)的場地及設(shè)備示意圖如圖7所示，實(shí)時(shí)數(shù)據(jù)的采集如圖8所示。跑車實(shí)驗(yàn)軌跡運(yùn)行曲線如圖9所示，圖中曲線上每點(diǎn)的位置是光纖/GPS組合導(dǎo)航系統(tǒng)輸出的相對精確的位置信息。為了對改進(jìn)算法進(jìn)行驗(yàn)證，選取了運(yùn)行曲線中開始的前300 s數(shù)據(jù)，如圖9中紅色曲線標(biāo)注部分。這300 s的數(shù)據(jù)中光纖/GPS組合導(dǎo)航系統(tǒng)輸出的跑車位置、速度和姿態(tài)如圖10～圖12所示，將其作為改進(jìn)算法和傳統(tǒng)算法對比時(shí)的導(dǎo)航參數(shù)基準(zhǔn)值，圖中λ、L、H分別表示位置的經(jīng)度，緯度和高度，γ、θ、φ分別為橫滾角、俯仰角和航向角。輸出數(shù)據(jù)中陀螺儀和加速度計(jì)頻率為100 Hz，基準(zhǔn)導(dǎo)航參數(shù)輸出頻率為1 Hz.

圖12 光纖/GPS組合導(dǎo)航系統(tǒng)輸出的跑車姿態(tài)Fig.12 Vehicle attitude outputs of FOG SINS/GPS integrated navigation system

使用跑車實(shí)驗(yàn)采集的陀螺儀角速率和加速度計(jì)數(shù)據(jù)，在同等初始條件下分別采用兩種算法進(jìn)行慣導(dǎo)解算。對比兩種算法解算得到的姿態(tài)、速度、位置誤差，即用傳統(tǒng)算法解算得到的姿態(tài)、速度和位置誤差減去改進(jìn)算法得到的相應(yīng)導(dǎo)航誤差，得到的誤差對比曲線如圖13～圖15所示。同樣統(tǒng)計(jì)這兩種算法解算得到的姿態(tài)、速度和位置誤差最大值以及RMS，統(tǒng)計(jì)結(jié)果如表4和表5所示。

圖13 傳統(tǒng)算法與改進(jìn)算法的姿態(tài)角誤差對比曲線Fig.13 Attitude error comparison of traditional algorithm and improved algorithm

從圖13～圖15以及表4和表5的統(tǒng)計(jì)結(jié)果可以看出，改進(jìn)算法的解算精度整體優(yōu)于傳統(tǒng)算法，解算得到的姿態(tài)、速度和位置精度均得到了提升。但鑒于實(shí)際驗(yàn)證數(shù)據(jù)中慣性器件誤差的存在，以及跑車實(shí)驗(yàn)所能模擬的動態(tài)性有限，所以兩類算法在實(shí)際跑車實(shí)驗(yàn)對比下的性能提升相對仿真數(shù)據(jù)較小。

5 結(jié)論

通過多組不同動態(tài)特性的數(shù)字仿真航跡的設(shè)計(jì)，對比分析了傳統(tǒng)算法和改進(jìn)算法的導(dǎo)航解算精度，仿真實(shí)驗(yàn)結(jié)果充分表明當(dāng)應(yīng)用于高動態(tài)環(huán)境中時(shí)，改進(jìn)算法將優(yōu)于傳統(tǒng)算法，并且隨著動態(tài)性的提高，更能體現(xiàn)出改進(jìn)算法的優(yōu)勢。與此同時(shí)，通過設(shè)計(jì)動態(tài)跑車實(shí)驗(yàn)，進(jìn)一步驗(yàn)證了本文所提出的改進(jìn)算法性能優(yōu)勢。

圖14 傳統(tǒng)算法與改進(jìn)算法的速度誤差對比曲線Fig.14 Velocity error comparison of traditional algorithm and improved algorithm

圖15 傳統(tǒng)算法與改進(jìn)算法的位置誤差對比曲線Fig.15 Position error comparison of traditional algorithm and improved algorithm

表4 跑車實(shí)驗(yàn)數(shù)據(jù)導(dǎo)航解算誤差的最大值

表5 跑車實(shí)驗(yàn)數(shù)據(jù)導(dǎo)航解算誤差RMS

References)

[1] 邢麗, 熊智, 劉建業(yè), 等. 針對高動態(tài)載體應(yīng)用的高精度捷聯(lián)慣導(dǎo)姿態(tài)算法優(yōu)化方法[J]. 中國慣性技術(shù)學(xué)報(bào), 2014, 22(6):701-706. XING Li, XIONG Zhi, LIU Jian-ye, et al. Optimized method of high-precision attitude algorithm for high dynamic vehicles[J]. Journal of Chinese Inertial Technology, 2014, 22(6):701-706. (in Chinese)

[2] 蔣郡祥, 于飛. 圓錐環(huán)境作為姿態(tài)算法優(yōu)化環(huán)境的適用性分析[J]. 哈爾濱工程大學(xué)學(xué)報(bào), 2016, 37(2):231-235. JIANG Jun-xiang, YU Fei. Applicability of coning environment used as the environment for attitude algorithm design[J]. Journal of Harbin Engineering University, 2016, 37(2): 231-235. (in Chinese)

[3] Ignagni M B. Efficient class of optimized coning compensation algorithms[J]. Journal of Guidance Control & Dynamics, 2015, 19(2):424-429.

[4] Song M, Wu W, Pan X. Approach to recovering maneuver accuracy in classical coning algorithms[J]. Journal of Guidance Control & Dynamics, 2013, 36(6):1872-1881.

[5] 黃磊, 劉建業(yè), 曾慶化. 基于高階補(bǔ)償模型的新圓錐算法[J]. 中國慣性技術(shù)學(xué)報(bào), 2013, 21(1): 37-41. HUANG Lei, LIU Jian-ye, ZENG Qing-hua. New coning algorithm based on high-order error compensation model[J]. Journal of Chinese Inertial Technology, 2013, 21(1):37-41. (in Chinese)

[6] Huang L, Liu J Y, Zeng Q H. Optimized strapdown coning correction algorithm[J]. Transactions of Nanjing University of Aeronautics & Astronautics, 2013, 30(4):343-349.

[7] Titterton D, Weston J. Strapdown inertial navigation technology[M]. Stevenage, Hertfordshire, UK: the Institution of Electrical Engineers, 2004.

[8] 王立冬, 劉軍, 魯軍. 捷聯(lián)慣導(dǎo)搖擺基座自對準(zhǔn)中圓錐誤差補(bǔ)償算法[J]. 兵工學(xué)報(bào), 2012, 33(7): 826-830. WANG Li-dong, LIU Jun, LU Jun. Coning error compensation algorithm of SINS self-alignment on swaying base[J]. Acta Armamentarii, 2012, 33(7): 826-830. (in Chinese)

[9] 湯傳業(yè), 陳熙源, 李建利. 一種角速率輸入的圓錐算法設(shè)計(jì)[J]. 中國慣性技術(shù)學(xué)報(bào), 2013, 21(4): 456-461. TANG Chuan-ye, CHEN Xi-yuan, LI Jian-li. Coning algorithm design for angular rate inputs[J]. Journal of Chinese Inertial Technology, 2013,21(4): 456-461. (in Chinese)

[10] Tang C, Chen X. A generalized coning correction structure for attitude algorithms[J]. Mathematical Problems in Engineering, 2014: 614378.

[11] Ben Y Y, Sun F, Gao W, et al. Generalized method for improved coning algorithms using angular rate[J]. IEEE Transactions on Aerospace & Electronic Systems, 2009, 45(4):1565-1572.

[12] Savage P G. Coning algorithm design by explicit frequency shaping[J]. Journal of Guidance Control & Dynamics, 2010, 33(4):1123-1132.

[13] Roscoe K M. Equivalency between strapdown inertial navigation coning and sculling integrals/algorithms[J]. Journal of Guidance Control & Dynamics, 2001, 24(2):201-205.

[14] Wu Y X, Hu X P, Hu D W, et al. Strapdown inertial navigation system algorithms based on dual quaternions[J]. IEEE Transactions on Aerospace & Electronic Systems, 2005, 41(1):110-132.

[15] Wu Y X, Hu X P, Wu M P, et al. Strapdown inertial navigation using dual quaternion algebra: error analysis[J]. IEEE Transactions on Aerospace & Electronic Systems, 2006, 42(1):259-266.

[16] Wang Z H, Chen X J, Zeng Q S. Comparison of strapdown inertial navigation algorithm based on rotation vector and dual quaternion[J]. Chinese Journal of Aeronautics, 2013, 26(2):442-448.

[17] Li K Z, Yuan J P, Yue X K. Autonomous navigation algorithm for spacecrafts based on dual quaternion[C]∥ 2nd International Conference on Space Information Technology. Wuhan, Hubei:SPIE, 2007.

[18] 夏琳琳, 趙琳, 劉繁明, 等. 基于對偶四元數(shù)的航姿系統(tǒng)姿態(tài)更新算法研究[J]. 系統(tǒng)仿真學(xué)報(bào), 2008, 20(2): 276-280. XIA Lin-lin, ZHAO Lin, LIU Fan-ming, et al. Research on attitude updating algorithm based on dual quaternion for attitude and heading reference system[J]. Journal of System Simulation, 2008, 20(2): 276-280.(in Chinese)

[19] 王立冬, 孟亞峰, 高慶. 基于角增量和角速率的旋轉(zhuǎn)矢量算法的等效性[J]. 宇航學(xué)報(bào), 2014, 35(3): 340-344. WANG Li-dong, MENG Ya-feng, GAO Qing. Equivalence analysis of rotation vector algorithms based on angle increment and angular velocity[J]. Journal of Astronautics, 2014, 35(3): 340-344. (in Chinese)

[20] Hinedi S, Statman J I. High dynamic GPS Tracking, NASA-CR-184868[R]. Washington, DC, US: NASA, 1988.

Improved Coning and Sculling Error Compensation Algorithms Based on Dual Quaternion for Strapdown Inertial Navigation System

XING Li, XIONG Zhi, LIU Jian-ye, HANG Yi-jun

(Navigation Research Center, Nanjing University of Aeronautics and Astronautics, Nanjing 211100, Jiangsu, China)

In order to improve the precision of the strapdown inertial navigation algorithm based on dual quaternion in the high dynamic environment, a trapezoid digital integral algorithm is applied to compensate the coning and sculling errors, which optimizes the attitude and velocity calculating algorithm and improves the calculating precision of the strapdown inertial navigation algorithm. During a sampling period of gyroscope and accelerometer, the angular rates of gyroscope outputs at previous and current times are both integrated by using the trapezoid digital integral algorithm, and the integral angle increment is used for the coning error compensation. The accelerometer outputs at previous and current times are integated by using the trapezoid digital integral algorithm, and the integral velocity increment and the integral angular increment are used for the sculling error compensation. Through the simulation of multi-group dynamic tracks, it is shown that the improved coning and sculling error compensation algorithms adopting the trapezoid digital integration have higher navigation precision than the traditional rectangular digital integration method when the inputs are angular rate and acceleration. The dynamics of the track is higher, and the performance advantage of the improved algorithm is more obvious. The analysis and comparison of kinematic vehicle experimental results further verify the performance advantage of the proposed improved algorithm.

control science and technology; dual quaternion; strapdown inertial navigation algorithm; coning error compensation algorithm; sculling error compensation algorithm; trapezoid digital integral algorithm

2016-10-29

國家自然科學(xué)基金項(xiàng)目(61533008、61374115、61533009、61673208)；江蘇省研究生培養(yǎng)創(chuàng)新工程項(xiàng)目(CXLX13_156)；南京航空航天大學(xué)研究生創(chuàng)新基地(實(shí)驗(yàn)室)開放基金項(xiàng)目(KFJJ20160320)；江蘇省“333工程”科研資助立項(xiàng)項(xiàng)目(BRA2016405)；留學(xué)人員科技活動項(xiàng)目擇優(yōu)資助項(xiàng)目(2016年)；航空科學(xué)基金項(xiàng)目(20165552043)

邢麗(1987—)，女，博士研究生。E-mail:nuaaxl@nuaa.edu.cn

熊智(1976—)，男，研究員，博士生導(dǎo)師。E-mail:xznuaa@nuaa.edu.cn

V249.32+2

A

1000-1093(2017)07-1336-12

10.3969/j.issn.1000-1093.2017.07.012