（1. 海軍工程大學 電氣工程學院，武漢 430033；2. 天津航海儀器研究所，天津 300131）

（1. 海軍工程大學 電氣工程學院，武漢 430033；2. 天津航海儀器研究所，天津 300131）

針對捷聯(lián)慣導晃動基座下的初始對準問題，提出了一種基于四元數(shù)估計(QUEST)的抗干擾對準算法。將慣性系對準方法中，求取初始姿態(tài)陣的問題轉(zhuǎn)化為基于觀測矢量確定載體姿態(tài)的Wahba問題，利用四元數(shù)估計算法得到最小二乘意義下載體初始姿態(tài)的最優(yōu)四元數(shù)解。闡述了四元數(shù)估計算法的基本原理，詳細給出了基于四元數(shù)估計算法的捷聯(lián)慣導晃動基座對準方案。進行了車載實驗，實驗結(jié)果表明：四元數(shù)估計對準算法姿態(tài)角誤差的收斂速度優(yōu)于雙矢量定姿對準算法，同時可進一步提高對準精度。經(jīng)120 s對準后，水平姿態(tài)誤差在5″以內(nèi)，航向誤差在1.3′以內(nèi)。

捷聯(lián)慣導；晃動基座；初始對準；Wahba問題；四元數(shù)估計算法

捷聯(lián)慣導的初始對準可分為粗對準和精對準兩個階段[1]。目前，捷聯(lián)慣導晃動基座粗對準方案大都采用慣性系方法解決，該方法將傳統(tǒng)解析式對準中載體角晃動干擾轉(zhuǎn)化為線晃動干擾，可有效解決搖擺基座下捷聯(lián)慣導快速粗對準的問題[2]。慣性系對準法[3]將載體姿態(tài)陣分解為三部分，以初始地理系和載體系凝固而成的慣性系作為過渡，運用雙矢量定姿原理中的TRAID算法[4]計算初始姿態(tài)陣。TRAID算法中的兩個矢量具有不對稱性，Bar-itzhack由此證明了由TRAID算法得到的姿態(tài)陣解不是最優(yōu)的，并提出了一種最優(yōu)TRAID 算法(Optimized TRAID)[5]。文獻[6]基于Optimized TRAID算法，提出了基于最優(yōu)三軸姿態(tài)測定算法的捷聯(lián)慣導粗對準方法。該算法分別以不同的觀測矢量作為基準，進行兩次姿態(tài)陣運算，并根據(jù)兩個觀測矢量的統(tǒng)計特性對兩個姿態(tài)陣進行加權(quán)處理，對準精度優(yōu)于傳統(tǒng)的 TRAID算法。上述基于雙矢量定姿原理的粗對準算法均過于簡單，只采用了對準期間兩個時刻的加速度計信息，而其余大部分觀測信息并未充分利用，所選時刻觀測矢量的測量精度對最終的對準結(jié)果影響較大。Wahba將利用觀測矢量確定姿態(tài)的問題描述為最小二乘意義下求解最優(yōu)正交姿態(tài)陣的問題[7]，Shuster提出的四元數(shù)估計算法(QUEST)[8]給出了Wahba問題的最優(yōu)四元數(shù)估計值。

本文受到文獻[7]的啟發(fā)，提出了基于四元數(shù)估計算法的捷聯(lián)慣導晃動基座對準方法。該算法可充分利用每一時刻的加速度計信息，與基于雙矢量定姿算法的對準方法相比，可進一步提高對準精度。

1 對準方案

1.1 坐標系說明

b(t)系：t時刻的載體系，方向為右前上。

n(t)系：t時刻的導航系，采用東北天地理坐標系。

ib(0)系：由0時刻的載體系凝固而成的慣性坐標系。

in(0)系：由0時刻的導航系凝固而成的慣性坐標系。

1.2 對準基本原理

晃動基座下載體的線速度 vn和線加速度都為零，則由捷聯(lián)慣導的比力方程可得：

式中， fb(t)為加速度計測得的比力，gn(t)為重力矢量。將式(1)帶入式(3)，并且等式兩邊同乘可得：

2 四元數(shù)估計算法

由上一節(jié)分析可知，捷聯(lián)慣導初始對準問題已經(jīng)轉(zhuǎn)化為根據(jù)觀測矢量求取姿態(tài)陣的問題。利用觀測矢量確定載體姿態(tài)的Wahba問題是指最優(yōu)姿態(tài)陣A的估計值應使如下?lián)p失函數(shù)達到最?。?/p>

式(6)可知損失函數(shù) L(A)最小即使增益函數(shù) g(A)取最大值。 g(A)可化為關(guān)于四元數(shù)q二次型函數(shù)：

式中，q為姿態(tài)陣A對應的四元數(shù)，K為4×4維矩陣：

式中，

考慮到約束條件 qTq= 1，根據(jù)拉格朗日數(shù)乘法，經(jīng)推導可得出：使 g(q)達到最大值的四元數(shù)就是矩陣K最大特征值 λmax所對應的特征向量，即最優(yōu)四元數(shù)qopt滿足

式中，qopt= [q0qv]T，q0為四元數(shù)的標量部分，qv為其矢量部分。式(12)進行矩陣運算可得如下兩式：

由式(11)可得：

則姿態(tài)陣對應的最優(yōu)四元數(shù)為：

式中，

由式(14)可知只要求得 λmax，便可以確定 qopt。將式(13)代入式(12)可得 λmax滿足如下四次方程：

直接解式(16)較為復雜，可使用牛頓迭代法求 λmax：

由文獻[9]分析可知， λmax的值近似等于非負權(quán)值的和 λ0，即有：

因此， λmax的迭代初值應設(shè)置為 1。在實際應用中，由于 λmax的值十分接近 λ0，所以只需要進行一次迭代就可以得到滿足精度要求的結(jié)果[10]。在得到 λmax后，根據(jù)式(13)便可以求出載體初始時刻姿態(tài)的最優(yōu)四元數(shù)解 qopt。

3 搖擺基座抗干擾對準算法

3.1 干擾加速度的抑制

根據(jù)第1節(jié)的對準方案，同時運用第2節(jié)中的四元數(shù)估計算法，理論上可以有效隔離載體角運動的干擾并獲得姿態(tài)陣的最優(yōu)估計值。但在實際環(huán)境中，由于載體的線振蕩、加速度計臂桿效應等干擾因素的影響，晃動基座下加速度計真實的輸出在ib(0)系的投影應為：

式中，▽b為加速度計零偏，δKA為刻度系數(shù)誤差陣，δA為安裝誤差陣，為臂桿干擾加速度，為船體線振蕩引起的干擾加速度。δKA、δA經(jīng)標定以后可以進行補償，但在實際應用中往往難以對進行精確建模并補償，造成式(4)不再保持嚴格成立。如果忽略式(19)中這些干擾加速度的影響，仍然按照式(4)進行對準，將會引起的計算誤差，最終影響對準精度。因此，保證對準精度的關(guān)鍵是如何抑制中的干擾加速度，從而提取較為純凈的重力信息重力矢量隨地球自轉(zhuǎn)在慣性系中圍繞自轉(zhuǎn)軸緩慢的轉(zhuǎn)動，由此可知的變化周期為24 h。在實際的艦船、車輛等環(huán)境中，外界振蕩干擾的頻率都遠大于地球自轉(zhuǎn)周期，因此根據(jù)實際使用環(huán)境對進行頻譜分析，并合理地設(shè)計數(shù)字低通濾波器就可以有效地抑制外界干擾的影響，從中提取較為真實的信息?？紤]到數(shù)字低通濾波器的群延遲效應（IIR型濾波器還存在非線性相位畸變），需對和同時進行低通濾波，這樣可保證式(4)仍然近似成立[10]。這里需要指出的是，慣性器件的常值誤差（加速度計零偏和陀螺常漂）是無法從頻域上與分離的，因此低通濾波器主要作用是抑制載體振蕩所引入的干擾加速度的影響，而慣性器件誤差對準精度的影響仍然存在。

3.2 非負權(quán)值的確定

四元數(shù)估計初始對準算法需對觀測矢量進行單位化處理，則觀測矢量選取為：

根據(jù)文獻[11]中的結(jié)論， ai的取值如下式所示：

式中，矩陣B的初始值設(shè)為零矩陣?；谒脑獢?shù)估計算法的搖晃動座對準法流程圖如圖1所示。

圖1 晃動基座四元數(shù)估計對準算法流程圖Fig.1 Flowchart of quaternion estimation alignment algorithm on rocking base

4 實驗驗證

為驗證本文算法的有效性，采用激光陀螺捷聯(lián)慣導的車載實驗數(shù)據(jù)進行了半物理仿真實驗，其中激光陀螺精度為0.01 (°)/h，加速度計零偏為5×10-5g，采樣頻率為125 Hz。在實驗過程中，待慣導系統(tǒng)靜態(tài)對準完畢后，開啟發(fā)動機、人員上下車等活動施加干擾，選取其中120 s的數(shù)據(jù)進行捷聯(lián)慣導抗干擾初始對準仿真實驗。由于無法取得載體真實的姿態(tài)信息，這里采用SINS/GPS組合導航系統(tǒng)的姿態(tài)輸出作為基準來檢驗對準算法的效果，如圖2所示。

圖2 車輛姿態(tài)角Fig.2 Attitude of the vehicle

圖3 X軸方向比力功率譜密度Fig.3 Power spectral density estimate of specific force along X axis direction

將以上選取的加速度計輸出量轉(zhuǎn)換到ib(0)系后，對其進行頻譜分析，其中X軸方向比力信息的功率譜如圖3所示，由局部放大圖可以看出外部干擾加速度的頻率主要集中在0.1 Hz以上的頻段，從Y軸和Z軸方向比力的譜分析可以得到相似的結(jié)果。為減少計算量，采用階數(shù)較低的IIR數(shù)字低通濾波器對三個軸向的比力信息進行濾波處理，濾波器的截止頻率設(shè)定為0.1 Hz，導通頻率設(shè)定為 0.005 Hz，則頻率為1.1574×10-5Hz的重力加速度可順利通過，同時可有效抑制頻率高于0.1 Hz的干擾加速度信息。

分別采用Optimized TRAID算法和本文提出的四元數(shù)估計法進行初始對準，兩種方法的對準誤差如圖4所示。隨著對準時間的增加，兩種對準算法的誤差都逐漸減??；兩者的水平姿態(tài)精度都較高，在 100 s以后達到5″以內(nèi)。然而，由于Optimized TRAID算法只利用兩個時刻的加速度計信息解算姿態(tài)角，而其余的觀測信息實質(zhì)上被丟棄，因而對準結(jié)果受到所選時刻觀測量誤差的影響較大。從圖4中可以明顯看出，與四元數(shù)估計法相比，Optimized TRAID法的結(jié)果具有較為明顯的波動。四元數(shù)估計法的航向誤差在第100 s后已經(jīng)收斂到1.3′以內(nèi)，而Optimized TRAID法的航向誤差波動較大，航向誤差約為 9.1′。試驗結(jié)果表明，四元數(shù)估計對準法姿態(tài)角的收斂速度優(yōu)于Optimized TRAID法，且航向角精度有較為明顯的提高。

5 結(jié) 論

四元數(shù)估計對準算法實質(zhì)上是根據(jù)觀測矢量確定最優(yōu)姿態(tài)。由于充分利用了對準期間所有的觀測信息，估計出載體姿態(tài)在最小二乘意義下的最優(yōu)姿態(tài)解。與基于雙矢量定姿算法對準方法相比，它具有更快的收斂速度和更高的對準精度。實驗結(jié)果表明，四元數(shù)估計對準算法可用于捷聯(lián)慣導在晃動基座下的快速初始對準。

（References）：

[1] White D, Psota F. A precision navigation system for

autonomous undersea vehicles[C]//IEEE. 1996: 262-267.

[2] 秦永元，嚴恭敏，顧冬晴，等. 搖擺基座上基于重力信息的捷聯(lián)慣導粗對準研究[J]. 西北工業(yè)大學學報，2005，23(5)：681-684.

QIN Yong-yuan, YAN Gong-min, GU Dong-qing. A clever way of SINS coarse alignment despite rocking ship[J]. Journal of Northwestern Polytechnical University, 2005, 23(5): 681-684.

[3] 孫楓，孫偉. 搖擺基座下旋轉(zhuǎn)捷聯(lián)慣導系統(tǒng)粗對準技術(shù)研究研究[J]. 儀器儀表學報，2010，31(4)：929-935.

SUN Feng, SUN Wei. Research on coarse alignment of rotary SINS on swing base[J]. Chinese Journal of Scientific Instrument, 2010, 31(4): 929-935.

[4] Lee Seon-Ho, Ahn Hyo-Sung, Yong Ki-Lyuk. Three-axis attitude determination using incomplete vector observations[J]. IEEE Transactions Industrial Electronics, 2012, 59(11): 4465-4474.

[5] Bar-itzhack I Y, Harman R R. Optimized TRAID algorithm for attitude determination[J]. Journal of Guidance, Control and Dynamics, 1997, 20(1): 208-221.

[6] 王勇軍，徐景碩，盛飛，等. 基于最優(yōu)三軸姿態(tài)測定算法的艦載慣導粗對準方法[J]. 中國慣性技術(shù)學報，2013，21(2)：294-297.

WANG Yong-Jun, XU Jing-Shuo, SHENG Fei, et al. Coarse alignment method based on optimized three-axis attitude determination algorithm for shipboard SINS[J]. Journal of Chinese inertial Technology, 2013, 21(2): 294-295.

[7] Wahba G. A least squares estimate of spacecraft attitude[J]. SIAM Review(S0036-1445), 1965, 7(3): 409.

[8] Shster M D, Oh S D. Three-axis attitude determination from vector observations[J]. J. Guidance and Control, 1981, 4(1): 70-77.

[9] Markley F L, Mortari D. How to estimate attitude from vector observations[J]. Advances in the Astronautical Sciences, 2000, 103(3): 1979-1996.

[10] 嚴恭敏，白亮，翁浚，等. 基于頻域分離算子的 SINS抗晃動干擾初始對準算法[J]. 宇航學報，2011，32(7)：1486-1490.

YAN Gong-min, BAI Liang, WENG Jun, et al. SINS anti-Rocking disturbance initial alignment based on frequency domain isolation operator[J]. Journal of Astronautics, 2011, 32(7): 1486-1490.

[11] Markley F L, Mortari D. Quaternion attitude estimation using vector measurements[J]. Journal of the Astronautical Sciences, 2000, 48(2): 359-380.

捷聯(lián)慣導晃動基座四元數(shù)估計對準算法

高 薪1，卞鴻巍1，傅中澤2，張禮偉2

Alignment algorithm based on quaternion estimator for SINS on rocking base

GAO Xin1，BIAN Hong-wei1，F(xiàn)U Zhong-ze2，ZHANG Li-wei2
（1. College of Electricity Engineering, Naval University of engineering, Wuhan 430033, China；2. Tianjin Navigation Instruments Research Institute, Tianjin 300131, China）

An alignment algorithm based on quaternion estimation (QUEST) is introduced to solve the alignment problem of SINS on rocking base. The solution of initial attitude matrix is transferred into a Wahba problem, i.e. determining the attitude from vector observations. The quaternion estimation algorithm is employed to estimate the optimized initial attitude-quaternion in the least-squares sense. The basic principle of quaternion estimation is given, and the solution of a quaternion estimation alignment algorithm for SINS on rocking base is presented. The results of vehicle tests show that the quaternion estimation algorithm can achieve faster convergence speed, compared with the alignment algorithm based on the double-vector attitude determination theory. Furthermore, the attitude accuracy is further improved, and the errors of horizontal attitude and the heading error are within 5″ and 1.3′ respectively in 120 s by using the quaternion estimation algorithm.

SINS; rocking base; initial alignment; Wahba problem; quaternion estimation algorithm

高薪（1982—），男，博士研究生，研究方向為導航、制導與控制。Email：xiaomaolv82@126.com

1005-6734(2014)06-0724-04

10.13695/j.cnki.12-1222/o3.2014.06.005

U666.1

A

2014-07-16；

2014-11-27

預研項目：航空科學基金資助項目（920120816001）

圖4 兩種對準算法姿態(tài)誤差對比Fig.4 Comparison of attitude errors between the two alignment algorithms