適用于低精度慣導(dǎo)的非線性對準(zhǔn)方法研究

1、適用于低精度慣導(dǎo)的非線性對準(zhǔn)方法研究夏家和，秦永元，趙長山（西北工業(yè)大學(xué)自動化學(xué)院西安710072）摘要：給出了一種適用于低精度慣導(dǎo)的非線性對準(zhǔn)模型。用乘性四元數(shù)形式定義捷聯(lián)慣導(dǎo)的姿態(tài)誤差，推導(dǎo)了捷聯(lián)慣導(dǎo)的非線性速度誤差方程和姿態(tài)誤差方程?；谒俣攘繙y信息，給出了大失準(zhǔn)角條件下的非線性對準(zhǔn)模型，通過UKF算法估計失準(zhǔn)角完成精對準(zhǔn)。仿真結(jié)果表明，在陀螺精度為0.1 °/h的情況下，在360 s對準(zhǔn)時間內(nèi)達(dá)到水平0.03°，方位1.5°的精度（1）。即使當(dāng)方位誤差達(dá)到90°，非線性模型仍能正常收斂。最后通過轉(zhuǎn)臺搖擺試驗進(jìn)一步驗證了非線性模型的有效性。關(guān)鍵詞：

2、捷聯(lián)慣導(dǎo)；對準(zhǔn)；UKF；非線性誤差模型中圖分類號：U666.1文獻(xiàn)標(biāo)識碼：A國家標(biāo)準(zhǔn)學(xué)科分類代碼：590.35Study on nonlinear alignment method for low precision INSXia Jiahe, Qin Yongyuan, Zhao Changshan（School of Automation, Northwestern Polytechnical University, Xian 710072, China）Abstract：A nonlinear error model based alignment method for low prec

3、ision strapdown inertial navigation system is studied. The quaternion is employed to describe the attitude error. The nonlinear velocity error model and attitude error model are deduced. Based on the velocity measurements, a nonlinear alignment model under large attitude error is made. After that th

4、e UKF is employed to estimate the misalignment. Simulation results show that the level attitude error quickly decreases to 0.03° and the heading error decreases to 1.5°（1）in 360 s alignment time. Even when the heading error reaches 90°, the nonlinear alignment model still can converge

5、 normally. Finally, the nonlinear alignment model is validated by turntable tests.Key words：strapdown inertial navigation; alignment; UKF; nonlinear error model1引言收稿日期：2008-12Received Date：2008-12靜基座條件下，捷聯(lián)慣導(dǎo)系統(tǒng)（SINS）可利用加速度計和陀螺分別對重力加速度和地球自轉(zhuǎn)角速度的量測值，粗略計算捷聯(lián)慣導(dǎo)的姿態(tài)矩陣以完成粗對準(zhǔn)，并在此基礎(chǔ)上進(jìn)行精對準(zhǔn)。但在使用低精度慣性器件的條件下，由于陀螺精度

6、低（其精度都在每小時零點(diǎn)幾度到上百度），以及其他各種環(huán)境因素的干擾，很難根據(jù)陀螺和加速度計的輸出完成具有一定精度的粗對準(zhǔn)。此時精對準(zhǔn)往往需要在大姿態(tài)誤差角的情況進(jìn)行，特別是方位角誤差可能達(dá)到幾十度1。傳統(tǒng)捷聯(lián)慣導(dǎo)的線性誤差方程是在姿態(tài)誤差為小角度的基礎(chǔ)上推導(dǎo)得到的，當(dāng)姿態(tài)誤差角較大時無法準(zhǔn)確刻畫捷聯(lián)慣導(dǎo)的誤差傳播特性。為此，眾多學(xué)者研究了能適用于大角度誤差的非線性模型及相應(yīng)的非線性濾波算法1-3。文獻(xiàn)1給出了一種允許三個失準(zhǔn)角都是大角度的非線性模型。但文獻(xiàn)2指出這種模型還是應(yīng)用了水平小角度的條件，得到的模型僅適用于大方位誤差角的情況。文獻(xiàn)3給出了一種加性四元數(shù)誤差模型，允許大的姿態(tài)誤差，但在推

7、導(dǎo)速度誤差方程時需要作線性化處理，影響了模型的精度。本文重新推導(dǎo)一種非線性誤差方程，推導(dǎo)時用乘性四元數(shù)定義姿態(tài)誤差，不作任何線性化處理來保證模型的精度。在此基礎(chǔ)上，采用速度量測信息給出了適用于低精度慣導(dǎo)對準(zhǔn)的非線性對準(zhǔn)模型，用UKF對所建立的非線性系統(tǒng)進(jìn)行濾波獲得失準(zhǔn)角完成精對準(zhǔn)。最后通過仿真和轉(zhuǎn)臺搖擺驗證了該方法用于大失準(zhǔn)角對準(zhǔn)的有效性。2捷聯(lián)慣導(dǎo)非線性誤差方程記慣性坐標(biāo)系為i系，地球坐標(biāo)系為e系，載體坐標(biāo)系為b系，選東北天地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，記作n系，計算平臺坐標(biāo)系為p系，即。用表示p系與n系之間的變換矩陣對應(yīng)的四元數(shù)，則有： (1)2.1速度誤差方程當(dāng)不考慮任何誤差時，速度的理想值由

8、下式確定： (2)而捷聯(lián)慣導(dǎo)解算的速度由下式確定： (3)式中： (4)另外有： (5)將式(5)代入式(3)并減去式(2)，并認(rèn)為，可得： (6)式(6)即為大失準(zhǔn)角時捷聯(lián)慣導(dǎo)的速度誤差方程，式(6)中由式(1)確定。2.2姿態(tài)誤差方程當(dāng)不考慮任何誤差時，姿態(tài)的理想值由下式確定： (7)而捷聯(lián)慣導(dǎo)解算的姿態(tài)由下式確定： (8)具體計算時， (9) (10)這里為陀螺的測量誤差。定義乘性四元數(shù)誤差： (11)式中：為的共軛四元數(shù)，所以， (12)式(12)即為大失準(zhǔn)角時捷聯(lián)慣導(dǎo)用乘性四元數(shù)表示的姿態(tài)誤差方程。若將寫成三角形式： (13)式中：為對應(yīng)的旋轉(zhuǎn)矢量。為的模，。當(dāng)為小角度時，可寫成：

9、(14)式（14）兩邊對t求導(dǎo)后得： (15)將式(14)和式(15)代入式(12)，略去二階小量后容易得到傳統(tǒng)線性姿態(tài)誤差方程： (16)3非線性濾波模型與UKF濾波算法3.1非線性對準(zhǔn)濾波模型對于慣性器件誤差僅考慮其隨機(jī)常值項和白噪聲項，并將加速度計的常值零偏和陀螺的常值漂移擴(kuò)充為系統(tǒng)狀態(tài)，狀態(tài)向量選為： (17)式中：為東向、北向和天向速度誤差；為陀螺隨機(jī)常值漂移；為加速度計隨機(jī)常值偏置。結(jié)合捷聯(lián)慣導(dǎo)的非線性誤差方程，可得到大失準(zhǔn)角對準(zhǔn)的系統(tǒng)方程為： (18)式中： (19)是陀螺和加速度計的量測白噪聲；為噪聲驅(qū)動陣， (20)以速度誤差作為量測量，量測方程為： (21)為量測噪聲。式(

10、18)和(21)就組成了非線性對準(zhǔn)的濾波模型。3.2UKF算法式(18)和(21)構(gòu)成一加性噪聲非線性系統(tǒng)?；诒平S機(jī)變量的條件分布比逼近其非線性函數(shù)更容易的思想，S J Juliear和J K Uhlman提出了基于UT（unscented transformation）的采樣卡爾曼濾波方法（UKF）8。與擴(kuò)展卡爾曼濾波（EKF）不同，UKF不去近似非線性系統(tǒng)和觀測方程，而使用真實(shí)非線性模型和隨機(jī)狀態(tài)變量的近似分布。在UKF中，狀態(tài)的分布為高斯分布，其特性由一組確定選擇的采樣點(diǎn)給出。這些采樣點(diǎn)能完全捕獲高斯分布變量的均值和方差，通過真實(shí)非線性系統(tǒng)的傳播后，其捕獲的均值和方差能精確到任意非線

11、性系統(tǒng)的Taylor展開的二次項。UKF算法避開了Jacobian矩陣的繁瑣計算，也不需要對系統(tǒng)方程和量測方程線性化，其實(shí)現(xiàn)比EKF更方便。這里采用UKF算法對式(18)和(21)構(gòu)成非線性模型進(jìn)行濾波。對稱采樣的UKF算法具體如下：1）濾波初始選擇與權(quán)值計算：， (22)2）計算sigma點(diǎn)： (23)式中：為方差陣平方根的第i行或第i列（時，取A的第i列）；n為狀態(tài)向量維數(shù)；為調(diào)節(jié)因子。3）時間更新： (24) (25) (26) (27) (28)4）量測更新： (29) (30) (31) (32) (33)式中:、分別為系統(tǒng)噪聲方差陣和量測噪聲方差陣。4算法仿真分析與試驗驗證4.1仿

12、真分析為驗證算法的有效性，進(jìn)行了如下蒙特-卡洛仿真。仿真條件如下：1）載體靜止，載體所處緯度為；2）慣性器件精度如下：陀螺隨機(jī)常值漂移為0.1(°)/h，陀螺白噪聲為0.3(°)/h,加速度計隨機(jī)常值零偏為，加速度計量測白噪聲為。速度量測噪聲為0.05 m/s。濾波時間步長取為0.5 s，并對慣導(dǎo)的速度、姿態(tài)進(jìn)行反饋校正。對準(zhǔn)精度用慣導(dǎo)反饋校正后的姿態(tài)與真實(shí)姿態(tài)的誤差進(jìn)行評估。仿真1：驗證所建立的非線性對準(zhǔn)模型在各種不同誤差角大小情況下的對準(zhǔn)性能。仿真中，假設(shè)粗對準(zhǔn)完成后姿態(tài)誤差角的分布如下：方位角誤差角服從上的均勻分布，俯仰、橫滾角誤差角服從上的均勻分布。總共進(jìn)行20次蒙

13、特-卡洛仿真，每次仿真時間長度為360 s。仿真結(jié)果如圖13所示。由圖12可知，對準(zhǔn)時，水平方向的失準(zhǔn)角收斂較快，在100 s內(nèi)平均誤差已收斂到0.01°，其均方差不大于0.01°；由圖3可知，方位角誤差收斂較慢，但在300 s左右平均誤差收斂到0.5°，其均方差不大于1°。仿真結(jié)果表明所設(shè)計的非線性對準(zhǔn)模型在大姿態(tài)誤差角的情況，其對準(zhǔn)精度能滿足低精度慣導(dǎo)的對準(zhǔn)要求。圖1俯仰角誤差Fig.1 The error of pitch angle圖2橫滾角誤差Fig.2 The error of roll angle圖3方位角誤差Fig.3 The error

14、 of heading仿真2：為驗證非線性對準(zhǔn)模型在極大初始姿態(tài)誤差條件下的對準(zhǔn)性能，將初始姿態(tài)誤差設(shè)置為45°、45°、 90°。圖4為此條件下傳統(tǒng)線性對準(zhǔn)方法和本文非線性對準(zhǔn)方法的方位對準(zhǔn)精度比較。圖4極大初始姿態(tài)誤差條件下的方位誤差比較Fig.4 The comparison of the heading errors underlarge initial attitude errors由圖4可看出，線性模型由于模型誤差，方位收斂速度比非線性模型慢，而且整個對準(zhǔn)過程中的精度比非線性模型差，360 s時其方位誤差大于20°，而非線性模型的誤差在10&

15、#176;以內(nèi)。4.2轉(zhuǎn)臺搖擺試驗為進(jìn)一步驗證所建立的非線性模型在大失準(zhǔn)角情況下的有效性，進(jìn)行了轉(zhuǎn)臺搖擺試驗。試驗主要設(shè)備有：激光捷聯(lián)慣組、三軸搖擺臺、采樣計算機(jī)以及相關(guān)電源等。捷聯(lián)慣組中激光陀螺的隨機(jī)常值漂移為0.008°/h，加速度計常值偏置誤差為3×10-5。試驗所用搖擺臺的搖擺幅度不大于6°，搖擺頻率不大于1 Hz。試驗時俯仰軸、橫滾軸和方位軸的搖擺幅度分別為4°、6°和4°，搖擺頻率均為0.1 Hz。初始水平姿態(tài)角誤差不小于10°，初始方位角誤差不小于25°。速度量測噪聲取為1 m/s。對準(zhǔn)結(jié)果如圖5所示

16、。誤差曲線在600 s左右已基本收斂，最終水平姿態(tài)角誤差為0.008°，方位角誤差0.03°。由試驗結(jié)果可知，所建立的非線性模型用于大失準(zhǔn)角情況下的對準(zhǔn)是有效的。圖5轉(zhuǎn)臺搖擺試驗對準(zhǔn)誤差Fig.5 The alignment errors in the turntable tests5結(jié)論給出了一種適用于低精度慣導(dǎo)的非線性對準(zhǔn)模型，并對其進(jìn)行了仿真分析和轉(zhuǎn)臺搖擺試驗。采用非線性模型進(jìn)行對準(zhǔn)時，僅需要根據(jù)慣組量測信息粗略確定姿態(tài)初始值即可，其初始姿態(tài)誤差可允許達(dá)到90°。低精度慣導(dǎo)的陀螺精度很低，根據(jù)解析方法計算得到的初始姿態(tài)陣誤差較大，此時用線性對準(zhǔn)模型進(jìn)行對準(zhǔn)時

17、精度差，甚至最終濾波結(jié)果發(fā)散，而本文給出的非線性對準(zhǔn)模型能滿足低精度慣導(dǎo)的對準(zhǔn)要求。參考文獻(xiàn)1 KONG X Y, NEBOT E M, D WHYTE H. Development of a nonlinear psi-angle model for large misalignment errors and its application in ins alignment and calibration C. Proceedings of the 1999 IEEE Int. Conf. on Robotics & Automation, Detroit, Michigan, 19

18、99:1430-1435.2 魏春嶺，張洪鉞，郝曙光. 捷聯(lián)慣導(dǎo)系統(tǒng)大方位失準(zhǔn)角下的非線性對準(zhǔn)J. 航天控制，2003:21(4):25-35. WEI CH L, ZHANG H Y, HAO SH G. SINS nonlinear alignment with large azimuth misalignment anglesJ. Aerospace Control, 2003,21(4):25-35.3 YU M J, LEE J G, PARK H W. Comparison of SDINS in flight alignment using equivalent error mo

19、dels J. IEEE Transaction on Aerospace and Electronic Systems, 1999, 35(3):1046-1054.4 YU M J, PARK H W, JEON C B. Equivalent nonlinear error models of strapdown inertial navigation systemsR. AIAA-9723563, 1997:581-587.5 WEI C L, ZHANG H Y. SINS in flight alignment using quaternion error modelsJ. Chi

20、nese Journal of eronautics, 2001,14(3):166-170.6 曹娟娟，房建成，盛蔚. 大失準(zhǔn)角下MIMU空中快速對準(zhǔn)技術(shù)。航空學(xué)報，2007,28(6):1395-1400.CAO J J, FANG J CH, SHENG W. A fast in-flight alignment method for MIMU under large attitude errorsJ. Acta Aeronautica et Astronautica Sinica, 2007, 28(6): 1395- 1400.7 劉國海, 李沁雪, 施維, 等. 動態(tài)卡爾曼濾波在導(dǎo)

21、航試驗狀態(tài)估計中的應(yīng)用J. 儀器儀表學(xué)報, 2009, 30(2): 396-400. LIU G H, LI Q X, SHI W, et al. Application of dynamic Kalman filtering in state estimation of navigation testJ. Chinese Journal of Scientific Instrument, 2009,30(2):396- 400.8 JULIER S J, UHLMANN J K. A new extension of the Kalman filter to nonlinear systemC. Proceedings of AeroSense, The 11th International Symposium on Aerospace/Defense Sensing, Simulation and Controls, 1997.9 JULIER S J, UHLMANN J K. The scaled unscented transformationC. Proceedings o