蔣郡祥,于飛

(1.哈爾濱工程大學(xué) 自動(dòng)化學(xué)院, 黑龍江 哈爾濱 150001; 2.哈爾濱工程大學(xué) 理學(xué)院, 黑龍江 哈爾濱 150001)

?

圓錐環(huán)境作為姿態(tài)算法優(yōu)化環(huán)境的適用性分析

蔣郡祥1,于飛2

(1.哈爾濱工程大學(xué) 自動(dòng)化學(xué)院, 黑龍江 哈爾濱 150001; 2.哈爾濱工程大學(xué) 理學(xué)院, 黑龍江 哈爾濱 150001)

摘要：由于圓錐環(huán)境的運(yùn)動(dòng)形式過于簡(jiǎn)單，使得基于其獲得的優(yōu)化系數(shù)在實(shí)際運(yùn)動(dòng)環(huán)境中應(yīng)用的適用性受到質(zhì)疑。針對(duì)上述問題，采用無窮項(xiàng)傅里葉級(jí)數(shù)展開的方法描述了載體的實(shí)際運(yùn)動(dòng)環(huán)境，基此對(duì)姿態(tài)算法相關(guān)公式進(jìn)行了重新推導(dǎo)，并得出了相應(yīng)的姿態(tài)算法優(yōu)化系數(shù)，在理論上證明了圓錐環(huán)境可以作為姿態(tài)算法優(yōu)化環(huán)境。證明了增加姿態(tài)算法的子樣數(shù)，提高陀螺儀的采樣頻率可以有效抑制姿態(tài)算法解算誤差。仿真結(jié)果表明了相關(guān)結(jié)論的正確性。

關(guān)鍵詞：圓錐環(huán)境；捷聯(lián)慣性導(dǎo)航系統(tǒng)；姿態(tài)算法；傅里葉級(jí)數(shù)；優(yōu)化系數(shù)

捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system, SINS)的慣性器件量測(cè)單元?jiǎng)傂怨搪?lián)在載體上。陀螺儀量測(cè)的角運(yùn)動(dòng)的動(dòng)態(tài)范圍較大，轉(zhuǎn)動(dòng)的不可交換性體現(xiàn)明顯，導(dǎo)航計(jì)算機(jī)需要通過合適的方式處理量測(cè)數(shù)據(jù)，獲得捷聯(lián)姿態(tài)矩陣。姿態(tài)算法的目的即在于優(yōu)化捷聯(lián)姿態(tài)矩陣計(jì)算過程中的相關(guān)系數(shù)，以期得到更加準(zhǔn)確的捷聯(lián)姿態(tài)矩陣。

Bortz[1]首先推導(dǎo)得到了旋轉(zhuǎn)矢量的微分方程，同時(shí)提出了一種旋轉(zhuǎn)矢量已知的周期性的載體運(yùn)動(dòng)形式，即圓錐環(huán)境。Bortz提出這種圓錐環(huán)境的最初目的是圓錐環(huán)境的旋轉(zhuǎn)矢量已知，可以在姿態(tài)算法計(jì)算機(jī)仿真時(shí)用于算法的精度分析，并沒有說明這種運(yùn)動(dòng)環(huán)境可以用于姿態(tài)算法的系數(shù)優(yōu)化。Miller[2]在Bortz研究的基礎(chǔ)上，基于旋轉(zhuǎn)矢量微分方程設(shè)計(jì)出二子樣、三子樣姿態(tài)算法，并利用圓錐環(huán)境對(duì)算法的系數(shù)進(jìn)行了優(yōu)化，這也是首次有學(xué)者利用圓錐環(huán)境作為姿態(tài)算法相關(guān)系數(shù)的優(yōu)化環(huán)境，類似的算法也得到了進(jìn)一步的發(fā)展[3-8]。但是，這些文章都沒有對(duì)圓錐環(huán)境作為優(yōu)化環(huán)境的適用性進(jìn)行合理的分析說明。而且圓錐環(huán)境相比于載體的實(shí)際運(yùn)動(dòng)環(huán)境，其運(yùn)動(dòng)形式過于簡(jiǎn)單，基于圓錐環(huán)境優(yōu)化的系數(shù)在實(shí)際環(huán)境中的表現(xiàn)受到質(zhì)疑。

針對(duì)這一問題，本文假設(shè)慣性器件為理想器件，即量測(cè)無誤差且傳遞函數(shù)幅頻與相頻特性不計(jì)[9-10]，利用無窮項(xiàng)傅里葉級(jí)數(shù)描述載體實(shí)際運(yùn)動(dòng)環(huán)境，在這種環(huán)境下對(duì)姿態(tài)算法的相關(guān)公式進(jìn)行重新推導(dǎo)，并對(duì)相關(guān)系數(shù)進(jìn)行了優(yōu)化，推導(dǎo)更具有普適性的優(yōu)化系數(shù)。

通過對(duì)比2種環(huán)境下的姿態(tài)算法相關(guān)公式和優(yōu)化系數(shù)，以分析圓錐環(huán)境作為姿態(tài)算法優(yōu)化環(huán)境的適用性，如此便可以通過運(yùn)動(dòng)形式非常簡(jiǎn)單的圓錐環(huán)境完成各子樣姿態(tài)算法的系數(shù)優(yōu)化。通過實(shí)際運(yùn)動(dòng)環(huán)境對(duì)應(yīng)的姿態(tài)算法相關(guān)公式，可以更加直觀地對(duì)解算誤差進(jìn)行分析，得出陀螺儀的量測(cè)精度與采樣速率越高解算誤差越小這一結(jié)論。最后，通過計(jì)算機(jī)仿真驗(yàn)證了相關(guān)結(jié)論的正確性。

1傅里葉級(jí)數(shù)表達(dá)的載體運(yùn)動(dòng)環(huán)境

對(duì)于圓錐環(huán)境，其形式相對(duì)于載體的實(shí)際運(yùn)動(dòng)情況過于理想化，基于典型圓錐環(huán)境的相關(guān)系數(shù)優(yōu)化過程難以使人信服。以下即是人為設(shè)定的載體運(yùn)動(dòng)圓錐環(huán)境的旋轉(zhuǎn)矢量以及相應(yīng)的載體運(yùn)動(dòng)角速度：

(1)

對(duì)應(yīng)的在環(huán)境進(jìn)行的算法設(shè)計(jì)及其系數(shù)優(yōu)化過程[2-8]，本文將不再重述。

本文通過傅里葉級(jí)數(shù)形式表達(dá)了載體的實(shí)際運(yùn)動(dòng)形式。設(shè)捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system，SINS)完成初始對(duì)準(zhǔn)后，進(jìn)入導(dǎo)航狀態(tài)的時(shí)刻為ton，SINS退出導(dǎo)航狀態(tài)的時(shí)刻為toff。在此期間內(nèi)，載體相對(duì)于慣性空間的角運(yùn)動(dòng)數(shù)據(jù)用以下函數(shù)形式表示:

(2)

以ω(t)的形式在整個(gè)時(shí)間域內(nèi)進(jìn)行周期性延拓，得其延拓形式為

(3)

將式(2)通過傅里葉級(jí)數(shù)的形式展開為

(4)

2姿態(tài)算法相關(guān)公式的重新推導(dǎo)

對(duì)于基于旋轉(zhuǎn)矢量的姿態(tài)算法而言，其數(shù)學(xué)本質(zhì)就是利用陀螺儀量測(cè)數(shù)據(jù)更加精確地求解旋轉(zhuǎn)矢量微分方程。其中旋轉(zhuǎn)矢量的微分方程[1]如下:

(5)

由于SISN的姿態(tài)信息更新頻率較快，單個(gè)更新周期內(nèi)的旋轉(zhuǎn)矢量的幅值并不是很大，其第三項(xiàng)一般可以忽略。在等式右側(cè)用θ代替φ得到旋轉(zhuǎn)矢量微分方程的一階近似表達(dá)式[9]為

(6)

式中：θ=∫ωdt。

對(duì)式(6)在更新區(qū)間t∈(tm-1,tm)內(nèi)做積分，獲得本更新區(qū)間的旋轉(zhuǎn)矢量為

(7)

式中：θ(tm,tm-1)被稱為旋轉(zhuǎn)項(xiàng)，數(shù)值等于在一個(gè)更新周期內(nèi)激光陀螺儀輸出的角增量相加，精度取決于采樣精度與A/D的字長(zhǎng)，偏差一般可以忽略不計(jì)[11]。β(tm,tm-1)被稱為旋轉(zhuǎn)補(bǔ)償項(xiàng)，其數(shù)值包含了ω方向改變而引入的誤差，在算法設(shè)計(jì)過程中要重點(diǎn)補(bǔ)償，表達(dá)式為[11]

(8)

(9)

將式(4)與式(9)代入式(8)中，可以得到旋轉(zhuǎn)補(bǔ)償項(xiàng)在本文所提用無窮項(xiàng)傅里葉級(jí)數(shù)描述的載體實(shí)際運(yùn)動(dòng)環(huán)境下的x軸分量真值為

(10)

其中,H=tm-tm-1為更新周期。

通過式(10)可以發(fā)現(xiàn)，當(dāng)i≠j時(shí)，其連加項(xiàng)為周期分量，誤差不會(huì)隨時(shí)間“累積”。當(dāng)i=j時(shí)，其連加項(xiàng)為“直流”分量與周期分量的疊加形式，若不對(duì)直流分量加以補(bǔ)償，由此引起的誤差會(huì)隨時(shí)間累積，導(dǎo)致偏差愈來愈大。當(dāng)i=j時(shí)，其旋轉(zhuǎn)補(bǔ)償項(xiàng)x軸向直流分量如下:

(11)

同理，可以對(duì)應(yīng)寫出其y, z軸向分量為

(12)

(13)

則旋轉(zhuǎn)補(bǔ)償項(xiàng)直流分量真值為

(14)

旋轉(zhuǎn)補(bǔ)償項(xiàng)的計(jì)算值是通過角增量叉乘，然后相加的形式[3]近似獲取，以三子樣Miller姿態(tài)算法[2]為例:

(15)

(16)

2個(gè)不同采樣周期角增量叉乘項(xiàng)的x軸分量如下：

(17)

通過式(17)可以發(fā)現(xiàn)，當(dāng)i≠j時(shí)，其連加項(xiàng)為周期分量。當(dāng)i=j時(shí)，其連加項(xiàng)為“直流”分量與周期分量的疊加，利用此直流分量對(duì)旋轉(zhuǎn)補(bǔ)償項(xiàng)真值中的直流分量加以補(bǔ)償。可以得到當(dāng)i=j時(shí)的旋轉(zhuǎn)補(bǔ)償項(xiàng)直流分量的計(jì)算值為

(2b-4a)sin(2Ωih)+2asin(3Ωih))

(18)

對(duì)式(14)、(18)進(jìn)行泰勒級(jí)數(shù)展開

(19)

(20)

Miller姿態(tài)算法的相關(guān)系數(shù)優(yōu)化思路就是對(duì)直流分量加以補(bǔ)償，為使式(19)、(20)的直流分量盡可能接近，使泰勒展開后連加項(xiàng)中的前兩項(xiàng)對(duì)應(yīng)相等，得到

(21)

解得a=0.450 0,b=1.350 0，這與Miller的三子樣姿態(tài)算法[2]優(yōu)化后的系數(shù)一致。觀察式(14)與式(18)可以發(fā)現(xiàn)，旋轉(zhuǎn)矢量真值與計(jì)算值各軸分量中同頻率直流分量具有相同的系數(shù)形式，而且不同頻率對(duì)應(yīng)的直流分量相互解耦。因此，不同頻率對(duì)應(yīng)直流分量具有相同的優(yōu)化系數(shù)且與相位無關(guān)，圓錐環(huán)境相當(dāng)于相位固定的單一頻率對(duì)應(yīng)的載體運(yùn)動(dòng)環(huán)境。由此可以得出如下結(jié)論，對(duì)于那些與Miller姿態(tài)更新算法有相同思路的算法(各子樣姿態(tài)算法)，可以通過典型圓錐環(huán)境進(jìn)行系數(shù)優(yōu)化。

HPLC-MS/MS法測(cè)定人血漿中地佐辛的濃度及其應(yīng)用……………………… 崔志敏，王逸雅，焦菲菲，等（1·14）

3誤差分析

通過式(19)、(20)可以發(fā)現(xiàn)，利用泰勒級(jí)數(shù)展開式進(jìn)行旋轉(zhuǎn)補(bǔ)償項(xiàng)直流分量補(bǔ)償時(shí)，只做到了前兩項(xiàng)相等(L=3時(shí))，對(duì)于其后的無窮多項(xiàng)，根據(jù)傅里葉級(jí)數(shù)的收斂性可知，一定存在某一正整數(shù)Num，當(dāng)i,j≥Num時(shí)其幅值工程上可以忽略不計(jì)[12]，但數(shù)值上依然會(huì)隨時(shí)間累計(jì)。此類姿態(tài)算法在設(shè)計(jì)之初并沒有考慮周期分量的補(bǔ)償問題。雖然誤差的周期性分量不會(huì)隨時(shí)間累積，但是某一時(shí)刻周期性分量的絕對(duì)誤差依然存在。同樣應(yīng)該注意到泰勒級(jí)數(shù)的基頻周期為toff-ton，時(shí)間很長(zhǎng)，誤差的周期性分量需要極長(zhǎng)的時(shí)間才能做到累積為零。因此需要想辦法對(duì)其加以抑制，削弱周期分量的幅值。旋轉(zhuǎn)補(bǔ)償項(xiàng)解算誤差可以表示為

(22)

(23)

因此，提高陀螺儀采樣頻率可以有效地遏制誤差的周期性分量幅值。

4仿真分析

為了更好地體現(xiàn)載體運(yùn)動(dòng)環(huán)境對(duì)旋轉(zhuǎn)補(bǔ)償項(xiàng)周期分量幅值及直流分量補(bǔ)償精度的影響，仿真環(huán)境保留式(4)中傅里葉級(jí)數(shù)的常數(shù)項(xiàng)及前三個(gè)周期項(xiàng)，幅值和相位各異。根據(jù)式(16)得出起始時(shí)刻為tm-1的更新周期內(nèi)第n個(gè)陀螺儀采樣值：

(24)

式中：常數(shù)項(xiàng)在區(qū)間(0.1°～0.2°)內(nèi)任意取值。周期項(xiàng)幅值(Pi,Qi,Ri,i=1,2,3)在區(qū)間(0.01°～0.05°)內(nèi)任意取值。為使載體運(yùn)動(dòng)的周期項(xiàng)影響更加明顯，取基頻fbase=1Hz，基頻的選取不會(huì)影響旋轉(zhuǎn)補(bǔ)償項(xiàng)周期性誤差分量幅值及直流分量補(bǔ)償誤差隨fbase/fupdate的變化趨勢(shì)。更新頻率:

fupdate=1H=1Lh=fsampleL

(25)

式中：fsample=1/h為采樣頻率。

通過計(jì)算機(jī)仿真得到6組關(guān)于載體橫搖(roll)、縱搖(pitch)，及航向(yaw)誤差的周期分量均方根和60s直流分量補(bǔ)償誤差累積與fbase/fupdate的關(guān)系圖，如圖1～6所示。仿真結(jié)果縱軸數(shù)值與仿真環(huán)境數(shù)值的選取有關(guān)，沒有參考價(jià)值，但誤差的變化趨勢(shì)與實(shí)際相符。

可以發(fā)現(xiàn)，各軸向誤差的變化趨勢(shì)一致，即誤差的絕對(duì)值隨fbase/fupdate的增大而單調(diào)增加。當(dāng)更新頻率fupdate相同時(shí)，四子樣算法(L=4)的誤差要比三子樣算法(L=3)的誤差小。此時(shí)，四子樣算法對(duì)應(yīng)的陀螺儀采樣頻率fsample高于三子樣算法。當(dāng)子樣數(shù)L相同時(shí)，誤差隨fbase/fupdate減小而減小。即，誤差隨著陀螺儀采樣頻率fsample的增加而減小。綜上所述，若想切實(shí)提高SISN的姿態(tài)解算精度，要在保證陀螺儀采樣精度的前提下，提高采樣頻率。

圖1　航向直流誤差項(xiàng)累積Fig.1　Cumulative DC error of yaw

圖2　航向周期誤差項(xiàng)均方根Fig.2　Periodic error′s RMS of yaw

圖3　縱搖直流誤差項(xiàng)累積Fig.3　Cumulative DC error of pitch

圖4　縱搖周期誤差項(xiàng)均方根Fig.4　Periodic error′s RMS of pitch

圖5　橫搖直流誤差項(xiàng)累積Fig.5　Cumulative DC error of roll

圖6　橫搖周期誤差項(xiàng)均方根Fig.6　Periodic error′s RMS of roll

5結(jié)論

1)本文的系數(shù)優(yōu)化可以認(rèn)為是基于實(shí)際環(huán)境進(jìn)行的。通過這種方式證明了圓錐環(huán)境在姿態(tài)算法系數(shù)優(yōu)化中的適用性，為圓錐環(huán)境在各子樣姿態(tài)算法中的應(yīng)用提供了理論基礎(chǔ)。但是，這種方法也無法通過算法上的改進(jìn)對(duì)姿態(tài)解算中的周期性誤差進(jìn)行抑制，這是一個(gè)值得深入思考的問題。

2)通過研究發(fā)現(xiàn)，在保持陀螺儀采樣精度的前提下，提高陀螺儀的采樣頻率可以在一定程度上抑制周期性誤差項(xiàng)。因此，高性能的陀螺儀在捷聯(lián)慣性導(dǎo)航系統(tǒng)的姿態(tài)解算精度方面起到至關(guān)重要的作用。

參考文獻(xiàn)：

[1]BORTZ J E. A new mathematical formulation for strapdown inertial navigation[J]. IEEE transactions on aerospace and electronic systems, 1971, 7(1): 61-66.

[2]MILLER R B. A new strapdown attitude algorithm[J]. Journal of guidance, control, and dynamics, 1983, 6(4): 287-291.

[3]IGNAGNI M B. Optimal strapdown attitude integration algorithms[J]. Journal of guidance, control, and dynamics, 1990, 13(2): 363-369.

[4]MUSOFF H, MURPHY J H. Study of strapdown navigation attitude algorithms[J]. Journal of guidance, control, and dynamics, 1995, 18(2): 287-290.

[5]BEN Yueyang, SUN Feng, GAO Wei, et al. Generalized method for improved coning algorithms using angular rate[J]. IEEE transactions on aerospace and electronics systems, 2009, 45(4): 1565-1572.

[6]LI Zongtao, WU Tiejun, MA Longhua. A coning compensation algorithm for SINS in high dynamic motion[J]. Journal of control engineering and applied informatics, 2011, 13(3): 32-40.

[7]IGNAGNI M. Optimal sculling and coning algorithms for analog-sensor systems[J]. Journal of guidance, control, and dynamics, 2012, 35(3): 851-860.

[8]FU Li, WANG Lingling, HU Jianghai. Coning algorithm based on singular perturbation[J]. Aircraft engineering and aerospace technology, 2013, 85(3): 178-185.

[9]SAVAGE P G. Strapdown sculling algorithm design for sensor dynamic amplitude and phase-shift error[J]. Journal of Guidance, Control, and Dynamics, 2012, 35(6): 1718-1729.

[10]MARK J G, TAZARTES D A. Tuning of coning algorithms to gyro data frequency response characteristics[J]. Journal of guidance, control, and dynamics, 2001, 24(4): 641-647.

[11]SAVAGE P G. Strapdown inertial navigation integration algorithm design part 1: Attitude algorithms[J]. Journal of guidance, control, and dynamics, 1998, 21(1): 19-28.

[12]BOGGESS A, NARCOWICH F J. A first course in wavelets with Fourier analysis[M]. 2nd ed. Hoboken: John Wiley and Sons, 2009: 129-133.

[13] SAVAGE P G. Explicit frequency-shaped coning algorithms for pseudo coning environments[J]. Journal of guidance, control, and dynamics, 2011, 34(3): 774-782.

[14]PATERA R P. Attitude propagation for a slewing angular rate vector[J]. Journal of guidance, control, and dynamics, 2010, 33(6): 1847-1855.

Analysis of applicability of cone environment as optimization environment of attitude algorithm

JIANG Junxiang1，YU Fei2

(1.College of Automation, Harbin Engineering University, Harbin 150001, China; 2. College of Science, Harbin Engineering University, Harbin 150001, China)

Abstract：Since the movement form in a cone environment is too simplistic, the applicability of an optimization coefficient based on this movement is questionable for a real movement environment. In this regard, we describe the real movement environment of a carrier using a method based on an infinite Fourier series. We carry out a new derivation for formulas related to the attitude algorithm, and obtain the corresponding optimization coefficient of the attitude coefficient, which theoretically proves that the cone environment can be used as the optimization environment of the attitude algorithm. We prove that increasing the sample number of the attitude algorithm and increasing the sampling frequency of the gyroscope can effectively reduce the calculation error of the attitude algorithm. Lastly, our simulation results validate the above conclusion.

Keywords：coning environment; strapdown inertial navigation system; attitude algorithm; Fourier series; optimized coefficient

中圖分類號(hào)：U666.1

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1006-7043(2016)02-0231-06

doi:10.11990/jheu.201411009

作者簡(jiǎn)介：蔣郡祥(1991-), 男, 碩士研究生;通信作者：蔣郡祥，E-mail:1505920735@qq.com.

基金項(xiàng)目：國(guó)家自然科學(xué)基金資助項(xiàng)目 (51379047，51379042).

收稿日期：2014-11-03.網(wǎng)絡(luò)出版日期：2015-12-15.

網(wǎng)絡(luò)出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20151215.1517.034.html

于飛(1974-), 男, 教授, 博士生導(dǎo)師.