兩種SINS慣性系四元數(shù)粗對準算法等價性分析

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

(火箭軍工程大學兵器發(fā)射理論與技術國家重點學科實驗室, 陜西 西安 710025)

0 引 言

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

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

1 慣性系對準原理

1.1 坐標系定義

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

(2)慣性坐標系i: 地心慣性坐標系,原點位于地心,坐標軸指向與對準起始時刻的地球坐標系一致。

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

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

(5)凝固載體慣性系ib0: 原點為載體質(zhì)心,三坐標軸指向與對準起始時刻的載體坐標系保持一致。

(6)凝固導航慣性系in0: 原點為載體質(zhì)心,三坐標軸指向與對準起始時刻的導航坐標系保持一致。

1.2 慣性系粗對準基本原理

(1)

分析式(1)中各矩陣的計算方式。設載體所在地的緯度為L,對準起始時刻為零時刻,則

(2)

式中,ωie為地球自轉角速度。

(3)

(4)

式中,V為載體相對地球的速度;f為比力;ωen為n系相對e系的轉動角速度矢量;ωie為地球自轉角速度矢量;g為重力加速度矢量。

令

(5)

則式(4)可變形為

(6)

(7)

則初始對準問題可轉化為求

(8)

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

(9)

記

則有

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

(10)

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

(11)

式(7)中的目標函數(shù)l可等價變形為

(12)

?Q)?(P?Q)*=

(13)

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

‖P?Q‖2=‖P‖2

(14)

(15)

下面由式(15),分別推導出Davenport方法和Wu方法,進而證明二者的等價性。

2.1 Davenport方法

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

(16)

由于

(17)

(18)

令

則矩陣K中

于是有

(19)

則式(17)可等價變形為

(20)

同理可得

(21)

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

(22)

令

l′(Q)=QTKQ

(23)

s.t.QTQ=1

(24)

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

2.2 Wu方法

由式(15)可得

QTK1Q

(25)

式中

這樣式(8)表示的問題就轉化為求解式(26)。

s.t.QTQ=1

(26)

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

3 等價性證明

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

(27)

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

(28)

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

(29)

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

(30)

K1=ηI-2K

(31)

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

證明設與λi對應的K的特征值為xi,則有

Kxi=λixi

(32)

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

K1xi=(ηI-2K)xi=

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

(33)

證畢

(34)

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

C=

(35)

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

4 仿真與試驗分析

為驗證上述分析的正確性,進行了粗對準的仿真和試驗驗證。

(36)

(37)

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

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

圖1 試驗室和車載對準試驗設備Fig.1 Equipment for lab and vehicle alignment experiments

圖2 行進間對準車輛行駛軌跡Fig.2 Trajectory of vehicle in-motion alignment

表1 仿真和試驗結果

5 結 論

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

參考文獻：

[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] 秦永元. 慣性導航[M]. 2版. 北京:科學出版社,2014.

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

[5] 秦永元,嚴恭敏,顧冬晴,等. 搖擺基座上基于g信息的捷聯(lián)慣導粗對準研究[J]. 西北工業(yè)大學學報, 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] 吳楓,秦永元,周琪. 間接解析自對準算法誤差分析[J]. 系統(tǒng)工程與電子技術,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)慣導慣性系對準誤差分析[J]. 海軍工程大學學報,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] 嚴恭敏,白亮,翁浚,等. 基于頻域分離算子的SINS抗晃動干擾初始對準算法[J]. 宇航學報,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)化的抗線晃動慣性系粗對準算法[J]. 系統(tǒng)工程與電子技術, 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)慣導慣性系晃動基座自對準算法[J].上海交通大學學報,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)慣導晃動基座四元數(shù)估計對準算法[J]. 中國慣性技術學報,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] 高偉,奔粵陽,李倩. 捷聯(lián)慣性導航系統(tǒng)初始對準技術[M]. 北京:國防工業(yè)出版社,2014.

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

[22] 張賢達.矩陣分析與應用[M].2版.北京:清華大學出版社,2013.

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