捷聯(lián)慣導(dǎo)晃動(dòng)基座四元數(shù)估計(jì)對(duì)準(zhǔn)算法

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

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

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

捷聯(lián)慣導(dǎo)；晃動(dòng)基座；初始對(duì)準(zhǔn)；Wahba問(wèn)題；四元數(shù)估計(jì)算法

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

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

1 對(duì)準(zhǔn)方案

1.1 坐標(biāo)系說(shuō)明

b(t)系：t時(shí)刻的載體系，方向?yàn)橛仪吧稀?/p>

n(t)系：t時(shí)刻的導(dǎo)航系，采用東北天地理坐標(biāo)系。

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

in(0)系：由0時(shí)刻的導(dǎo)航系凝固而成的慣性坐標(biāo)系。

1.2 對(duì)準(zhǔn)基本原理

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

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

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

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

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

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

式中，

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

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

由式(11)可得：

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

式中，

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

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

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

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

3 搖擺基座抗干擾對(duì)準(zhǔn)算法

3.1 干擾加速度的抑制

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

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

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

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

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

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

圖1 晃動(dòng)基座四元數(shù)估計(jì)對(duì)準(zhǔn)算法流程圖Fig.1 Flowchart of quaternion estimation alignment algorithm on rocking base

4 實(shí)驗(yàn)驗(yàn)證

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

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

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

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

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

5 結(jié) 論

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

（References）：

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

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

[2] 秦永元，嚴(yán)恭敏，顧冬晴，等. 搖擺基座上基于重力信息的捷聯(lián)慣導(dǎo)粗對(duì)準(zhǔn)研究[J]. 西北工業(yè)大學(xué)學(xué)報(bào)，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)慣導(dǎo)系統(tǒng)粗對(duì)準(zhǔn)技術(shù)研究研究[J]. 儀器儀表學(xué)報(bào)，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)測(cè)定算法的艦載慣導(dǎo)粗對(duì)準(zhǔn)方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)，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] 嚴(yán)恭敏，白亮，翁浚，等. 基于頻域分離算子的 SINS抗晃動(dòng)干擾初始對(duì)準(zhǔn)算法[J]. 宇航學(xué)報(bào)，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)慣導(dǎo)晃動(dòng)基座四元數(shù)估計(jì)對(duì)準(zhǔn)算法

高 薪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—），男，博士研究生，研究方向?yàn)閷?dǎo)航、制導(dǎo)與控制。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

預(yù)研項(xiàng)目：航空科學(xué)基金資助項(xiàng)目（920120816001）

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