基于羅德里格參數(shù)的線(xiàn)性最優(yōu)估計(jì)自對(duì)準(zhǔn)

梅春波，秦永元，楊鵬翔

基于羅德里格參數(shù)的線(xiàn)性最優(yōu)估計(jì)自對(duì)準(zhǔn)

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

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

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

初始對(duì)準(zhǔn)；羅德里格參數(shù)；凱萊變換；線(xiàn)性最優(yōu)估計(jì)

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

上述兩類(lèi)算法中，第一類(lèi)算法計(jì)算量小，所采用的技術(shù)手段已有成熟的工程應(yīng)用；第二類(lèi)算法則統(tǒng)一了對(duì)準(zhǔn)過(guò)程，無(wú)需姿態(tài)初值，但計(jì)算量大。

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

1 慣性系多矢量定姿對(duì)準(zhǔn)方案

慣性系基于矢量定姿對(duì)準(zhǔn)方案以姿態(tài)陣鏈?zhǔn)椒纸鉃榛A(chǔ)，

式中：n為導(dǎo)航坐標(biāo)系，取為東北天地理坐標(biāo)系；ni為導(dǎo)航慣性系，與對(duì)準(zhǔn)開(kāi)始時(shí)刻的n系重合；b為載體坐標(biāo)系；bi為載體慣性系，與對(duì)準(zhǔn)開(kāi)始時(shí)刻的b重合。

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

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

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

記

則有，在對(duì)準(zhǔn)過(guò)程中任意時(shí)刻kt，

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

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

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

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

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

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

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

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

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

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

3 算法有效性及誤差分析

3.1算法有效性分析

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

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

當(dāng)θ=π時(shí)，有

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

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

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

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

3.2對(duì)準(zhǔn)算法誤差分析

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

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

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

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

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

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

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

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

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

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

① 初始化其

中，,,ixyz=。

② 遞推更新

④ 對(duì)準(zhǔn)實(shí)時(shí)姿態(tài)求解

5 試驗(yàn)驗(yàn)證

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

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

考慮到對(duì)準(zhǔn)的難點(diǎn)在于方位角估計(jì)，因此此處僅給出四位置上實(shí)時(shí)方位角估計(jì)曲線(xiàn)以及新算法與傳統(tǒng)算法方位角最終估值。

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

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

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

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

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

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

第三，對(duì)比表1中對(duì)準(zhǔn)結(jié)束時(shí)刻兩種算法方位角估值可知，新算法的對(duì)準(zhǔn)精度與分段對(duì)準(zhǔn)相當(dāng)，而對(duì)準(zhǔn)結(jié)果統(tǒng)計(jì)特性則略?xún)?yōu)于傳統(tǒng)算法。

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

表1 對(duì)準(zhǔn)結(jié)束時(shí)刻方位角對(duì)比Tab.1 Comparison of the final estimated yaw angles

6 結(jié) 論

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

（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] 錢(qián)華明, 葛磊, 彭宇, 等. 多漸消因子卡爾曼濾波及其在SINS初始對(duì)準(zhǔn)中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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] 謝陽(yáng)光, 伊國(guó)興, 王常虹, 等. 高斯-厄米特濾波器在捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)中的應(yīng)用[J]. 航空學(xué)報(bào), 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] 丁國(guó)強(qiáng), 徐潔, 周衛(wèi)東, 等. EM-CDKF算法及其SINS初始對(duì)準(zhǔn)應(yīng)用[J]. 華中科技大學(xué)學(xué)報(bào)（自然科學(xué)版）, 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] 王躍鋼, 楊家勝. 晃動(dòng)基座下捷聯(lián)慣導(dǎo)的抗干擾自對(duì)準(zhǔ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

國(guó)家自然科學(xué)基金（61273333）

梅春波（1985—），男，博士研究生，研究方向?yàn)閼T性導(dǎo)航、組合導(dǎo)航。E-mail：meichunbo@126.com

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