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

蔣郡祥,于飛

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

?

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

蔣郡祥1,于飛2

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

摘要：由于圓錐環(huán)境的運動形式過于簡單，使得基于其獲得的優(yōu)化系數(shù)在實際運動環(huán)境中應(yīng)用的適用性受到質(zhì)疑。針對上述問題，采用無窮項傅里葉級數(shù)展開的方法描述了載體的實際運動環(huán)境，基此對姿態(tài)算法相關(guān)公式進行了重新推導，并得出了相應(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)慣性導航系統(tǒng)；姿態(tài)算法；傅里葉級數(shù)；優(yōu)化系數(shù)

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

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

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

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

1傅里葉級數(shù)表達的載體運動環(huán)境

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

(1)

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

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

(2)

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

(3)

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

(4)

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

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

(5)

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

(6)

式中：θ=∫ωdt。

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

(7)

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

(8)

(9)

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

(10)

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

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

(11)

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

(12)

(13)

則旋轉(zhuǎn)補償項直流分量真值為

(14)

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

(15)

(16)

2個不同采樣周期角增量叉乘項的x軸分量如下：

(17)

通過式(17)可以發(fā)現(xiàn)，當i≠j時，其連加項為周期分量。當i=j時，其連加項為“直流”分量與周期分量的疊加，利用此直流分量對旋轉(zhuǎn)補償項真值中的直流分量加以補償?？梢缘玫疆攊=j時的旋轉(zhuǎn)補償項直流分量的計算值為

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

(18)

對式(14)、(18)進行泰勒級數(shù)展開

(19)

(20)

Miller姿態(tài)算法的相關(guān)系數(shù)優(yōu)化思路就是對直流分量加以補償，為使式(19)、(20)的直流分量盡可能接近，使泰勒展開后連加項中的前兩項對應(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)矢量真值與計算值各軸分量中同頻率直流分量具有相同的系數(shù)形式，而且不同頻率對應(yīng)的直流分量相互解耦。因此，不同頻率對應(yīng)直流分量具有相同的優(yōu)化系數(shù)且與相位無關(guān)，圓錐環(huán)境相當于相位固定的單一頻率對應(yīng)的載體運動環(huán)境。由此可以得出如下結(jié)論，對于那些與Miller姿態(tài)更新算法有相同思路的算法(各子樣姿態(tài)算法)，可以通過典型圓錐環(huán)境進行系數(shù)優(yōu)化。

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

3誤差分析

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

(22)

(23)

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

4仿真分析

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

(24)

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

fupdate=1H=1Lh=fsampleL

(25)

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

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

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

圖1　航向直流誤差項累積Fig.1　Cumulative DC error of yaw

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

圖3　縱搖直流誤差項累積Fig.3　Cumulative DC error of pitch

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

圖5　橫搖直流誤差項累積Fig.5　Cumulative DC error of roll

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

5結(jié)論

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

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

中圖分類號：U666.1

文獻標志碼：A

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

doi:10.11990/jheu.201411009

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

基金項目：國家自然科學基金資助項目 (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-), 男, 教授, 博士生導師.