兩種SINS慣性系四元數(shù)粗對(duì)準(zhǔn)算法等價(jià)性分析

陳 河, 張志利, 周召發(fā), 劉朋朋, 趙軍陽(yáng)

(火箭軍工程大學(xué)兵器發(fā)射理論與技術(shù)國(guó)家重點(diǎn)學(xué)科實(shí)驗(yàn)室, 陜西 西安 710025)

0 引 言

作為一種航位推算系統(tǒng),捷聯(lián)慣導(dǎo)系統(tǒng)(strapdown inertial navigation system,SINS)正常工作前需要進(jìn)行初始對(duì)準(zhǔn)獲取載體的初始姿態(tài)[1]。初始對(duì)準(zhǔn)一般分為粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)兩個(gè)過(guò)程[2]。粗對(duì)準(zhǔn)傳統(tǒng)上采用解析法,即根據(jù)重力加速度矢量和地球自轉(zhuǎn)角速度矢量,采用雙矢量定姿法解算載體的姿態(tài)矩陣。解析法的缺點(diǎn)是抗干擾能力差,僅適用于干擾較小時(shí)的靜基座初始對(duì)準(zhǔn)[3-4]。為了克服解析法的不足,文獻(xiàn)[5]提出了一種基于凝固慣性系的粗對(duì)準(zhǔn)方案,即通過(guò)姿態(tài)矩陣分解,將初始對(duì)準(zhǔn)轉(zhuǎn)化為求取初始地球慣性系和初始載體坐標(biāo)系之間的轉(zhuǎn)換矩陣,然后選取不同時(shí)刻的重力矢量進(jìn)行雙矢量定姿;為了抑制高頻干擾的影響,通常將重力加速度積分得到速度矢量,然后選取兩個(gè)不同時(shí)刻的速度矢量進(jìn)行雙矢量定姿[6]。該方法能夠有效抑制載體晃動(dòng)對(duì)初始對(duì)準(zhǔn)的影響,在載體劇烈晃動(dòng)時(shí)也能取得很好的對(duì)準(zhǔn)效果,已成為SINS粗對(duì)準(zhǔn)的研究熱點(diǎn)。文獻(xiàn)[7-8]分析了該算法的誤差特性,指出其在靜基座條件下的誤差與傳統(tǒng)解析法相同;文獻(xiàn)[9-12]針對(duì)該方法不能抑制線振動(dòng)干擾影響的問(wèn)題,研究了不同的線振動(dòng)干擾抑制方案。

由于地球自轉(zhuǎn),短時(shí)間內(nèi)(小于24 h)每一時(shí)刻的重力加速度矢量均不共線,因而均可作為參考矢量用于定姿。基于上述思想,文獻(xiàn)[13]以重力矢量分段積分得到的一系列速度增量為參考矢量,將初始對(duì)準(zhǔn)轉(zhuǎn)化為多矢量定姿的Wahba問(wèn)題[14]。針對(duì)Wahba問(wèn)題,文獻(xiàn)[15]采用奇異值分解法直接求出對(duì)應(yīng)的姿態(tài)變換矩陣;Davenport把目標(biāo)函數(shù)轉(zhuǎn)化為四元數(shù)表示的形式,并將求解最優(yōu)四元數(shù)轉(zhuǎn)化為計(jì)算矩陣最大特征值對(duì)應(yīng)的特征向量[16-17]。文獻(xiàn)[18-20]在四元數(shù)表示的基礎(chǔ)上,將最優(yōu)四元數(shù)的求解轉(zhuǎn)化為計(jì)算矩陣最小特征值對(duì)應(yīng)的特征向量問(wèn)題。Davenport方法和文獻(xiàn)[18-20]的方法求取的是同一姿態(tài)矩陣對(duì)應(yīng)的姿態(tài)四元數(shù),因而必然是等價(jià)的,但相關(guān)文獻(xiàn)并未給出兩者等價(jià)性的直接證明。本文針對(duì)二者的等價(jià)性展開(kāi)討論,首先闡述了基于Wahba問(wèn)題的慣性系粗對(duì)準(zhǔn)基本原理,然后證明了上述兩種形式的等價(jià)性,最后進(jìn)行了仿真和試驗(yàn)驗(yàn)證。為便于敘述,下文分別稱兩種方法為Davenport方法和Wu方法。

1 慣性系對(duì)準(zhǔn)原理

1.1 坐標(biāo)系定義

(1) 地球坐標(biāo)系e: 原點(diǎn)位于地心,ze指向地球自轉(zhuǎn)方向,xe軸位于赤道平面內(nèi)且從地心指向載體所在點(diǎn)的子午線方向,ye軸與xe、ze軸構(gòu)成右手坐標(biāo)系。

(2)慣性坐標(biāo)系i: 地心慣性坐標(biāo)系,原點(diǎn)位于地心,坐標(biāo)軸指向與對(duì)準(zhǔn)起始時(shí)刻的地球坐標(biāo)系一致。

(3)導(dǎo)航坐標(biāo)系n: 取地理坐標(biāo)系為導(dǎo)航坐標(biāo)系,原點(diǎn)位于載體質(zhì)心,xn、yn、zn軸分別指向東向、北向和天向。

(4)載體坐標(biāo)系b: 原點(diǎn)位于載體質(zhì)心,xb、yb、zb分別指向載體的右向、前向和上方。

(5)凝固載體慣性系ib0: 原點(diǎn)為載體質(zhì)心,三坐標(biāo)軸指向與對(duì)準(zhǔn)起始時(shí)刻的載體坐標(biāo)系保持一致。

(6)凝固導(dǎo)航慣性系in0: 原點(diǎn)為載體質(zhì)心,三坐標(biāo)軸指向與對(duì)準(zhǔn)起始時(shí)刻的導(dǎo)航坐標(biāo)系保持一致。

1.2 慣性系粗對(duì)準(zhǔn)基本原理

(1)

分析式(1)中各矩陣的計(jì)算方式。設(shè)載體所在地的緯度為L(zhǎng),對(duì)準(zhǔn)起始時(shí)刻為零時(shí)刻,則

(2)

式中,ωie為地球自轉(zhuǎn)角速度。

(3)

(4)

式中,V為載體相對(duì)地球的速度;f為比力;ωen為n系相對(duì)e系的轉(zhuǎn)動(dòng)角速度矢量;ωie為地球自轉(zhuǎn)角速度矢量;g為重力加速度矢量。

令

(5)

則式(4)可變形為

(6)

(7)

則初始對(duì)準(zhǔn)問(wèn)題可轉(zhuǎn)化為求

(8)

2 Wahba問(wèn)題的四元數(shù)形式

(9)

記

則有

P?Q=(P?)Q=Q⊕P=(Q⊕)P

(10)

四元數(shù)乘法不滿足交換律但滿足結(jié)合律[4]。

(11)

式(7)中的目標(biāo)函數(shù)l可等價(jià)變形為

(12)

?Q)?(P?Q)*=

(13)

根據(jù)式(13)可得

‖P?Q‖2=‖P‖2

(14)

(15)

下面由式(15),分別推導(dǎo)出Davenport方法和Wu方法,進(jìn)而證明二者的等價(jià)性。

2.1 Davenport方法

將式(15)的被積函數(shù)展開(kāi)可得

(16)

由于

(17)

(18)

令

則矩陣K中

于是有

(19)

則式(17)可等價(jià)變形為

(20)

同理可得

(21)

將式(20)、式(21)代入式(16)可得

(22)

令

l′(Q)=QTKQ

(23)

s.t.QTQ=1

(24)

可以證明,矩陣K最大特征值對(duì)應(yīng)的特征向量即式(24)的最優(yōu)解[15-16]。

2.2 Wu方法

由式(15)可得

QTK1Q

(25)

式中

這樣式(8)表示的問(wèn)題就轉(zhuǎn)化為求解式(26)。

s.t.QTQ=1

(26)

可以證明,矩陣K1最小特征值對(duì)應(yīng)的特征向量即式(24)的最優(yōu)解。

3 等價(jià)性證明

證明式(24)和式(26)的等價(jià)性即證明K最大特征值對(duì)應(yīng)的特征向量與K1最小特征值對(duì)應(yīng)的特征向量相等。根據(jù)K1的定義可得

(27)

根據(jù)四元數(shù)運(yùn)算的定義,可得

(28)

由式(17)、式(20)、式(21)可知

(29)

將式(28)、式(29)代入式(27),則有

(30)

K1=ηI-2K

(31)

設(shè)K的特征值為λi,則根據(jù)矩陣特征值和特征向量的性質(zhì)可知[22]:K1的特征值為η-2λi,是λi的線性函數(shù),且η-2λi和λi對(duì)應(yīng)的特征向量相同。

證明設(shè)與λi對(duì)應(yīng)的K的特征值為xi,則有

Kxi=λixi

(32)

綜合運(yùn)用式(31)、式(32)可得

K1xi=(ηI-2K)xi=

ηxi-2Kxi=(η-2λi)xi

(33)

證畢

(34)

式(34)的幾何意義是,坐標(biāo)系ib0可以看作in0系通過(guò)一次旋轉(zhuǎn)得到,u為表示轉(zhuǎn)動(dòng)方向的單位向量,φ表示轉(zhuǎn)過(guò)的角度。若對(duì)四元數(shù)Q取反,相當(dāng)于in0系沿-u方向轉(zhuǎn)動(dòng)2π-φ角度,這與沿u轉(zhuǎn)過(guò)角度φ的效果一致。實(shí)際上,由四元數(shù)計(jì)算姿態(tài)矩陣的公式為

C=

(35)

由式(35)可知,Q和-Q對(duì)應(yīng)同一姿態(tài)矩陣。這從另一個(gè)角度說(shuō)明了互為負(fù)向量的兩個(gè)姿態(tài)四元數(shù)表示的姿態(tài)相同。

4 仿真與試驗(yàn)分析

為驗(yàn)證上述分析的正確性,進(jìn)行了粗對(duì)準(zhǔn)的仿真和試驗(yàn)驗(yàn)證。

(36)

(37)

式中,φE、φN、φU均為[0, 2π]上服從均勻分布的隨機(jī)相位。

采用實(shí)驗(yàn)室內(nèi)的激光陀螺SINS進(jìn)行試驗(yàn)驗(yàn)證。其中,激光陀螺的零偏穩(wěn)定性為0.005 °/h,加速度計(jì)的零偏穩(wěn)定性為50 μg,系統(tǒng)采樣頻率200 Hz。進(jìn)行試驗(yàn)室靜基座對(duì)準(zhǔn)、車載晃動(dòng)基座對(duì)準(zhǔn)和車載行進(jìn)間對(duì)準(zhǔn)3類試驗(yàn)。室內(nèi)靜基座對(duì)準(zhǔn)試驗(yàn)在圖1(a)所示的轉(zhuǎn)臺(tái)上進(jìn)行,對(duì)準(zhǔn)時(shí)間60 s。車載晃動(dòng)基座和行進(jìn)間對(duì)準(zhǔn)試驗(yàn)均在圖1(b)所示的試驗(yàn)用車上進(jìn)行?；蝿?dòng)基座對(duì)準(zhǔn)時(shí)車輛不運(yùn)動(dòng),利用發(fā)動(dòng)機(jī)振動(dòng)、開(kāi)關(guān)車門(mén)和人員上下車施加晃動(dòng)干擾,對(duì)準(zhǔn)時(shí)間60 s。行進(jìn)間對(duì)準(zhǔn)時(shí)采用全球定位系統(tǒng)測(cè)速輔助對(duì)準(zhǔn),車輛行駛軌跡如圖2所示,對(duì)準(zhǔn)時(shí)間60 s。

圖1 試驗(yàn)室和車載對(duì)準(zhǔn)試驗(yàn)設(shè)備Fig.1 Equipment for lab and vehicle alignment experiments

圖2 行進(jìn)間對(duì)準(zhǔn)車輛行駛軌跡Fig.2 Trajectory of vehicle in-motion alignment

表1 仿真和試驗(yàn)結(jié)果

5 結(jié) 論

本文討論了兩種基于四元數(shù)的SINS慣性系粗對(duì)準(zhǔn)算法的等價(jià)性。建立了以姿態(tài)四元數(shù)為優(yōu)化變量的多矢量定姿目標(biāo)函數(shù),根據(jù)四元數(shù)的運(yùn)算性質(zhì)導(dǎo)出了兩種不同的最優(yōu)四元數(shù)求解算法(Davenport方法和Wu方法),即將四元數(shù)求解問(wèn)題轉(zhuǎn)化為求解其各自構(gòu)建的四階實(shí)對(duì)稱矩陣的最大和最小特征值對(duì)應(yīng)的特征向量。理論分析、仿真和試驗(yàn)結(jié)果均表明,兩種算法構(gòu)建的四階實(shí)對(duì)稱矩陣之間存在線性關(guān)系,且該線性關(guān)系的斜率為負(fù)數(shù),因此Davenport方法中的矩陣最大特征值對(duì)應(yīng)Wu方法中的矩陣最小特征值,而兩者對(duì)應(yīng)的特征向量相同。

參考文獻(xiàn)：

[1] FELIPE O S, ELDER M H, WALDEMAR C F. Error analysis of analytical coarse alignment formulations for stationary SINS[J].IEEE Trans.on Aerospace and Electronic Systems,2016,52(4): 1777-1796.

[2] FELIPE O S, ELDER M H, WALDEMAR C F. On the error state selection for stationary SINS alignment and calibration Kalman filters-part I: estimation algorithms[J]. Aerospace Science and Technology, 2016,61:45-56.

[3] KENNETH R B. Inertial navigation systems analysis[M]. New York: Wiley, 1971.

[4] 秦永元. 慣性導(dǎo)航[M]. 2版. 北京:科學(xué)出版社,2014.

QIN Y Y. Inertial navigation[M]. 2nd ed. Beijing: Science Press, 2014.

[5] 秦永元,嚴(yán)恭敏,顧冬晴,等. 搖擺基座上基于g信息的捷聯(lián)慣導(dǎo)粗對(duì)準(zhǔn)研究[J]. 西北工業(yè)大學(xué)學(xué)報(bào), 2005, 23(5): 681-684.

QIN Y Y, YAN G M, GU D Q, et al. A clever way of SINS coarse alignment despite rocking ship[J]. Journal of Northwestern Polytechnical University, 2005, 23(5): 681-684.

[6] LIU X J, LI Y T. Fast alignment algorithm of inertial fixed frame in quasi-static environment[J]. Journal of Chinese Inertial Technology, 2014, 22(4): 459-462.

[7] 吳楓,秦永元,周琪. 間接解析自對(duì)準(zhǔn)算法誤差分析[J]. 系統(tǒng)工程與電子技術(shù),2013, 35(3): 586-590.

WU F, QIN Y Y, ZHOU Q. Error analysis of indirect analytic alignment algorithm[J]. Systems Engineering and Electronics, 2013, 35(3): 586-590.

[8] 高薪,卞鴻巍,王榮穎,等. 捷聯(lián)慣導(dǎo)慣性系對(duì)準(zhǔn)誤差分析[J]. 海軍工程大學(xué)學(xué)報(bào),2014, 26(6): 43-46.

GAO X, BIAN H W, WANG R Y, et al. Error analysis of coarse alignment for SINS inertial frame[J]. Journal of Naval University of Engineering, 2014, 26(6): 43-46.

[9] LIAN J X, HU D W, WU Y X, et al. Research on SINS alignment algorithm based on FIR filters[J]. Journal of Beijing Institute of Technology, 2007, 16(4): 437-442.

[10] 嚴(yán)恭敏,白亮,翁浚,等. 基于頻域分離算子的SINS抗晃動(dòng)干擾初始對(duì)準(zhǔn)算法[J]. 宇航學(xué)報(bào),2011, 32(7): 1486-1490.

YAN G M, BAI L, WENG J, et al. SINS anti-rocking disturbance initial alignment based on frequency domain isolation operator[J].Journal of Astronautics,2011,32(7):1486-1490.

[11] 譚彩銘,王宇,蘇巖,等. 優(yōu)化的抗線晃動(dòng)慣性系粗對(duì)準(zhǔn)算法[J]. 系統(tǒng)工程與電子技術(shù), 2016, 38(1): 142-146.

TAN C M, WANG Y, SU Y, et al. Improved inertial coarse alignment algorithm with suppression of linear vibration[J]. Systems Engineering and Electronics, 2016, 38(1): 142-146.

[12] 薛海建,郭曉松,張東方,等.基于四元數(shù)的捷聯(lián)慣導(dǎo)慣性系晃動(dòng)基座自對(duì)準(zhǔn)算法[J].上海交通大學(xué)學(xué)報(bào),2016,50(3): 419-424.

XUE H J, GUO X S, ZHANG D F, et al. SINS self-alignment algorithm with inertial frame for swaying base based on quaternion[J].Journal of Shanghai Jiaotong University,2016,50(3): 419-424.

[13] SILSON P. Coarse alignment of a ship’s strapdown inertial attitude reference system using velocity loci[J]. IEEE Trans.on Aerospace and Electronic Systems, 2011, 60(6): 1930-1941.

[14] WAHBA G. A least-squares estimate of satellite attitude[J]. SIAM Review, 1965, 7(3) : 409-411

[15] MARKLEY F L, CRASSIDIS J L. Fundamentals of spacecraft attitude determination and control[M].New York:Springer,2014.

[16] KEAT J. Analysis of least-squares attitude determination routine DOAOP[R]. Technical Report CSC/TM-77/6304 Computer Sciences Corporation, 1977.

[17] 高薪,卞鴻巍,傅中澤,等. 捷聯(lián)慣導(dǎo)晃動(dòng)基座四元數(shù)估計(jì)對(duì)準(zhǔn)算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào),2014,22(6): 724-727.

GAO X, BIAN H W, FU Z Z, et al. Alignment algorithm based on quaternion estimator for SINS on rocking base[J]. Journal of Chinese Inertial Technology,2014,22(6):724-727.

[18] WU M P, WU Y X, HU X P, et al. Optimization-based alignment for inertial navigation systems: theory and algorithm[J]. Aerospace Science and Technology, 2011, 15(1): 1-17.

[19] KANG T Z, FANG J C, WANG W. Quaternion-optimization-based in-flight alignment approach for airborne POS[J]. IEEE Trans.on Aerospace and Electronic Systems, 2012, 61(11): 2916-2923.

[20] WU Y X, PAN X F. Velocity/position integration formula part I: application to in-flight coarse alignment[J]. IEEE Trans.on Aerospace and Electronic Systems, 2013, 49(2): 1006-1023.

[21] 高偉,奔粵陽(yáng),李倩. 捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)技術(shù)[M]. 北京:國(guó)防工業(yè)出版社,2014.

GAO W, BEN Y Y, LI Q. Initial alignment for strapdown inertial navigation system[M]. Beijing: National Defense Industry Press, 2014.

[22] 張賢達(dá).矩陣分析與應(yīng)用[M].2版.北京:清華大學(xué)出版社,2013.

ZHANG X D. Matrix analysis and applications[M]. 2nd ed. Beijing: Tsinghua University Press, 2013.