梅春波，秦永元，楊鵬翔

基于羅德里格參數(shù)的線性最優(yōu)估計自對準

梅春波1，秦永元1，楊鵬翔2

(1. 西北工業(yè)大學 自動化學院，西安 710129；2. 西安現(xiàn)代控制技術(shù)研究所，西安710111)

針對車載捷聯(lián)慣導系統(tǒng)怠速條件下的初始對準問題，提出了一種基于羅德里格參數(shù)的線性最優(yōu)估計自對準算法。利用姿態(tài)陣分解和凱萊變換，將任意姿態(tài)下的無初值初始對準問題簡化為羅德里格參數(shù)的無約束線性最優(yōu)估計問題。討論了算法的有效性，推導了算法的對準誤差公式，并設計了一種簡潔的工程實現(xiàn)方案。利用車載捷聯(lián)慣導系統(tǒng)進行了四位置對準試驗，每個位置對準六次，結(jié)果表明，在發(fā)動機振動及外界隨機擾動下，新算法可以在5 min內(nèi)完成對準，統(tǒng)計方位均方差（1σ）不超過3′。

初始對準；羅德里格參數(shù)；凱萊變換；線性最優(yōu)估計

傳統(tǒng)的捷聯(lián)慣導系統(tǒng)初始對準算法基本可分為兩類：第一類，由粗對準和精對準構(gòu)成，粗對準估計姿態(tài)初值，對慣導系統(tǒng)誤差方程在姿態(tài)初值處進行線性化，精對準則采用線性卡爾曼濾波或線性最小二乘估計姿態(tài)初值誤差，進而完成對準[1-2]；第二類，以捷聯(lián)慣導系統(tǒng)非線性誤差模型為對象，采用非線性濾波[3-5]實現(xiàn)任意姿態(tài)角的初始對準。

上述兩類算法中，第一類算法計算量小，所采用的技術(shù)手段已有成熟的工程應用；第二類算法則統(tǒng)一了對準過程，無需姿態(tài)初值，但計算量大。

本文以慣性系多矢量定姿為基礎[6-9]，通過引入羅德里格參數(shù)，將任意姿態(tài)下的無初值對準問題簡化為一個無約束線性最優(yōu)估計問題，從而得到一種新的對準算法。新算法在結(jié)構(gòu)上與非線性濾波對準相同，無需姿態(tài)初值；在數(shù)值實現(xiàn)上則比分段對準算法更為簡潔，兼具兩種傳統(tǒng)對準算法的優(yōu)點。

1 慣性系多矢量定姿對準方案

慣性系基于矢量定姿對準方案以姿態(tài)陣鏈式分解為基礎，

式中：n為導航坐標系，取為東北天地理坐標系；ni為導航慣性系，與對準開始時刻的n系重合；b為載體坐標系；bi為載體慣性系，與對準開始時刻的b重合。

根據(jù)上述坐標系定義，可得

式中：cL=cos L，sL=sin L，cωt=cos(ωiet )，sωt=sin(ωiet )，s2L=sin(2L)；ωie為地球自轉(zhuǎn)角速率；L為對準點地理緯度信息；t為對準開始后持續(xù)時間；為陀螺儀測量角速度。

在車輛怠速、微幅晃動條件下，加計測量值可視為重力加速度在載體系內(nèi)投影，即有

記

則有，在對準過程中任意時刻kt，

慣性系多矢量定姿對準算法的核心，即是利用式(5)所有時刻積分矢量的等式關系，給定代價函數(shù)，完成常值姿態(tài)陣的最優(yōu)估計。然后，結(jié)合式(2)計算結(jié)果，利用式(1)關系得到對準過程中實時的姿態(tài)陣。

2 羅德里格參數(shù)最小二乘估計

將式(6)代入式(5)中，有

式(7)等號兩端左乘（I+（l×）），移項整理得

在上述推導中，并未對l有任何約束條件。依據(jù)(8)式，由全體離散積分結(jié)果設計代價函數(shù)：

通過最小化G，即可得到l的一種最優(yōu)估計。

進一步，考慮矢量用于定姿時，有效信息為其方向而非模值，故可對式(9)代價函數(shù)做等價變形，

對式(10)兩端同除以一個常數(shù)，不改變最優(yōu)估計結(jié)果，故式(10)代價函數(shù)可等價描述為

其中，ktξ可視為等效相對權(quán)重系數(shù)。

由式(11)可得l的加權(quán)最小二乘估計為

3 算法有效性及誤差分析

3.1算法有效性分析

新算法通過求解式(2)和式(12)來完成對準，易知式(2)計算結(jié)果有效性與器件正常工作狀態(tài)相關，而式(12)的有效求解則要求矩陣kS滿秩。

由式(12)易知，當存在至少兩個不同時刻的stk不共線時，Sk即為滿秩。若in系至ib系等效旋轉(zhuǎn)矢量為θ?r，θ為轉(zhuǎn)角，r為轉(zhuǎn)軸方向單位向量，則有

當θ=π時，有

此時，將式(13)代入stk計算式，整理可得

式(15)推導中，用到了恒等關系式I+（r×）2=rrT。

式(15)表明，當?shù)刃D(zhuǎn)矢量轉(zhuǎn)角為π時，任意時刻的kts均與r共線，為算法的奇異點。

此時，直接采用式(4)(12)無法完成對準，在算法數(shù)值實現(xiàn)中將給出對該奇異點的處理方式。

3.2對準算法誤差分析

考慮陀螺漂移εb及加速度計零偏?b影響，代入式(4)，易得比力積分誤差δ為

式中，A(tk)由式(17)給出。

為便于討論且不失一般性，假定對準開始時刻導航系和載體系重合，由式(2)(4)(17)(19)經(jīng)相應積分運算可得A(t )、B(t )、（t ）近似時域解析表達式：k k k

由式(21)可得?l的加權(quán)最小二乘估計解析值為

根據(jù)假定，ib系與in重合，羅德里格參數(shù)真值l為零。因此，式(21)得到的即是羅德里格參數(shù)估計誤差δ，即δ=。此時，由式(6)可得估計姿態(tài)陣：

4 考慮奇異點影響的算法實現(xiàn)

由3.1小節(jié)分析結(jié)果知，當in系至ib系等效旋轉(zhuǎn)矢量的轉(zhuǎn)角θ為π時，行列式=0，此時不存在；當θ位于π的小鄰域內(nèi)時，為解決零值的小量，此時l的求解為嚴重病態(tài)問題。據(jù)此，可通過對Sk行列式設定閾值來判斷算法是否位于奇異點附近。

若判斷位于奇異點附近，可通過對ib系進行一個虛擬轉(zhuǎn)動得ib1系，使in系至ib1系等效旋轉(zhuǎn)矢量的轉(zhuǎn)角遠離π。然后，利用式(12)求解，又虛擬轉(zhuǎn)動姿態(tài)陣準確已知，即可間接得到，完成對準。

進一步，對IMU系統(tǒng)近似為右前上安裝的陸用車載導航系統(tǒng)而言，奇異點僅出現(xiàn)在水平姿態(tài)角為小角，方位角位于π附近時。此時虛擬轉(zhuǎn)動可以唯一設定為：將bi繞著其z軸方向轉(zhuǎn)動π角得到b1i，此轉(zhuǎn)動姿態(tài)陣記為zC。

在上述討論的基礎上，經(jīng)過適當?shù)赝茖?，可以得到消除奇異點影響的羅德里格參數(shù)遞推整體最小二乘對準求解步驟為：

① 初始化其

中，,,ixyz=。

② 遞推更新

④ 對準實時姿態(tài)求解

5 試驗驗證

2013年12月在北京豐臺區(qū)（緯度為40.067°），利用某型號車載光纖捷聯(lián)慣導系統(tǒng)進行了四位置對準試驗。每個位置，對系統(tǒng)斷電重啟，重復對準六次，對準時長設為5 min。對準過程中，車輛發(fā)動機處于怠速狀態(tài)。對準算法中所用奇異點判別閾值設為1e-10。

由于對準是在室外導航車上進行，無姿態(tài)真值，為了評估算法的有效性，對同一組對準數(shù)據(jù)分別采用文中設計算法和傳統(tǒng)的分段初始對準算法進行處理，從而間接驗證所設計算法的有效性。分段對準算法中，采用慣性系粗對準和導航系卡爾曼濾波精對準，粗對準時長為60 s，精對準240 s。

考慮到對準的難點在于方位角估計，因此此處僅給出四位置上實時方位角估計曲線以及新算法與傳統(tǒng)算法方位角最終估值。

圖1～圖4分別給出了不同方位下，六組重復對準試驗實時方位角估計曲線。表1給出了對準結(jié)束時刻，新算法和分段對準算法各自得到的方位角估值及其統(tǒng)計結(jié)果。由圖1～圖4方位估計曲線及表1最終方位最終對準結(jié)果，可以得到如下結(jié)論：

第一，所設計算法無需姿態(tài)初值，即可實現(xiàn)任意方位角條件下的快速對準，且對準結(jié)果收斂速度與方位角的大小無關。

圖1 方位1時航向角估計結(jié)果Fig.1 Estimation of yaw at Ori.1

圖2 方位2時航向角估計結(jié)果Fig.2 Estimation of yaw at Ori.2

圖3 方位3時航向角估計結(jié)果Fig.3 Estimation of yaw at Ori.3

第二，由于最小二乘算法的平滑作用，收斂曲線非常光滑，常規(guī)的卡爾曼濾波對準則難以做到這一點。

第三，對比表1中對準結(jié)束時刻兩種算法方位角估值可知，新算法的對準精度與分段對準相當，而對準結(jié)果統(tǒng)計特性則略優(yōu)于傳統(tǒng)算法。

圖4 方位4時航向角估計結(jié)果Fig.4 Estimation of yaw at Ori.4

表1 對準結(jié)束時刻方位角對比Tab.1 Comparison of the final estimated yaw angles

6 結(jié) 論

針對車載捷聯(lián)慣導系統(tǒng)怠速條件下的快速自對準，利用姿態(tài)陣分解和凱萊變換，推導得到一種以無約束最小二乘估計為核心的對準算法。該算法適應任意姿態(tài)初值且無需進行粗對準，計算簡潔，便于工程實現(xiàn)。文中進一步采用試驗數(shù)據(jù)對算法進行了充分的驗證，表明了所設計算法無需任何初值及先驗信息，而對于對準過程中的噪聲和隨機擾動卻有更強的平滑能力，具有很好的工程應用價值。

（References）：

[1] Dai H D, Zhou S L, Chen M, et al. Quaternion based nonlinear error model for rapid transfer alignment[J]. Journal of Astronautics, 2010, 31(10): 2328-2334.

[2] 錢華明, 葛磊, 彭宇, 等. 多漸消因子卡爾曼濾波及其在SINS初始對準中的應用[J]. 中國慣性技術(shù)學報, 2012, 20(3): 287-291.

Qian Hua-ming, Ge Lei, Peng Yu, et al. Multiple fading factors Kalman filter and its application in SINS initial alignment[J]. Journal of Chinese Inertial Technology, 2012, 20(3): 287-291.

[3] 謝陽光, 伊國興, 王常虹, 等. 高斯-厄米特濾波器在捷聯(lián)慣導系統(tǒng)初始對準中的應用[J]. 航空學報, 2012, 33(3): 554-560.

Xie Yang-guang, Yi Guo-xing, Wang Chang-hong, et al. Application of Gauss-Hermite filter in SINS alignment[J]. Acta Aeronautica et Astronautica Sinica, 2012, 33(3): 554-560.

[4] 丁國強, 徐潔, 周衛(wèi)東, 等. EM-CDKF算法及其SINS初始對準應用[J]. 華中科技大學學報（自然科學版）, 2014, 42(10): 31-36.

Ding Guo-qiang, Xu Jie, Zhou Wei-dong, et al. EM-CDKF algorithm and its applications on SINS initial alignment [J]. Journal of Huazhong University of Science and Technology(Natural Science Edition), 2014, 42(10): 31-36.

[5] 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.

[6] Wu Y X, Pan X F. Velocity/position integration formula partⅠ: Application to in-flight coarse alignment[J]. IEEE Transaction on Aerospace and Electronic Systems, 2013, 49(2): 1006-1023.

[7] 王躍鋼, 楊家勝. 晃動基座下捷聯(lián)慣導的抗干擾自對準算法[J]. 控制與決策, 2014, 29(3): 546-550.

Wang Yue-gang, Yang Jia-sheng. SINS anti-interference self-alignment algorithm for the swaying base[J]. Control and Decision, 2014, 29(3): 546-550.

[8] Peter M G S. Coarse alignment of a ship’s strapdown inertial attitude reference system using velocity loci[J]. IEEE Transactions on Instrumentation and Measurement, 2011, 60(6): 1930-1941.

[9] Daniele M, Markley F L, Puneet S, et al. An optimal linear attitude estimator[J]. Journal of Guidance, Control, and Dynamics, 2007, 30(6): 1619-1627.

Linear optimized self-alignment for SINS using Rodrigues parameters

MEI Chun-bo1, QIN Yong-yuan1, YANG Peng-xiang2
(1. School of Automation, Northwestern Polytechnical University, Xi’an 710129, China; 2. No.203 Research Institute of China Ordnance Industries, Xi’an 710111, China)

A method for fast initial alignment of SINS using Rodrigues parameters was proposed for land vehicles under complex disturbances. Through attitude matrix decomposition and Cayley transform, the alignment is transformed into an unconstraint linear optimization of Rodrigues parameters. The effectiveness of the algorithm was discussed, the error equations were deduced, and a simple implementation scheme was proposed. A four-position alignment experiments with 6 times each position was carried out by using the vehicle SINS under the engine vibration and external random disturbances. The results show that the new algorithm can converge within 5 min, and the root mean square (1σ) of yaw error is less than 3′.

alignment; Rodrigues parameters; Cayley transform; linear optimization

V249.3

A

1005-6734(2015)03-0298-05

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

2015-01-16；

2015-05-05

國家自然科學基金（61273333）

梅春波（1985—），男，博士研究生，研究方向為慣性導航、組合導航。E-mail：meichunbo@126.com

聯(lián) 系 人：秦永元（1946—），男，教授，博士生導師。E-mail：qinyongyuan@nwpu.edu.cn