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

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

2、捷聯(lián)慣導(dǎo)；對(duì)準(zhǔn)；UKF；非線性誤差模型中圖分類號(hào)：U666.1文獻(xiàn)標(biāo)識(shí)碼：A國(guó)家標(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）可利用加速度計(jì)和陀螺分別對(duì)重力加速度和地球自轉(zhuǎn)角速度的量測(cè)值，粗略計(jì)算捷聯(lián)慣導(dǎo)的姿態(tài)矩陣以完成粗對(duì)準(zhǔn)，并在此基礎(chǔ)上進(jìn)行精對(duì)準(zhǔn)。但在使用低精度慣性器件的條件下，由于陀螺精度

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

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

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

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

10、18)和(21)就組成了非線性對(duì)準(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)和觀測(cè)方程，而使用真實(shí)非線性模型和隨機(jī)狀態(tài)變量的近似分布。在UKF中，狀態(tài)的分布為高斯分布，其特性由一組確定選擇的采樣點(diǎn)給出。這些采樣點(diǎn)能完全捕獲高斯分布變量的均值和方差，通過(guò)真實(shí)非線性系統(tǒng)的傳播后，其捕獲的均值和方差能精確到任意非線

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

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

13、特-卡洛仿真，每次仿真時(shí)間長(zhǎng)度為360 s。仿真結(jié)果如圖13所示。由圖12可知，對(duì)準(zhǔn)時(shí)，水平方向的失準(zhǔn)角收斂較快，在100 s內(nèi)平均誤差已收斂到0.01°，其均方差不大于0.01°；由圖3可知，方位角誤差收斂較慢，但在300 s左右平均誤差收斂到0.5°，其均方差不大于1°。仿真結(jié)果表明所設(shè)計(jì)的非線性對(duì)準(zhǔn)模型在大姿態(tài)誤差角的情況，其對(duì)準(zhǔn)精度能滿足低精度慣導(dǎo)的對(duì)準(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：為驗(yàn)證非線性對(duì)準(zhǔn)模型在極大初始姿態(tài)誤差條件下的對(duì)準(zhǔn)性能，將初始姿態(tài)誤差設(shè)置為45°、45°、 90°。圖4為此條件下傳統(tǒng)線性對(duì)準(zhǔn)方法和本文非線性對(duì)準(zhǔn)方法的方位對(duì)準(zhǔn)精度比較。圖4極大初始姿態(tài)誤差條件下的方位誤差比較Fig.4 The comparison of the heading errors underlarge initial attitude errors由圖4可看出，線性模型由于模型誤差，方位收斂速度比非線性模型慢，而且整個(gè)對(duì)準(zhǔn)過(guò)程中的精度比非線性模型差，360 s時(shí)其方位誤差大于20°，而非線性模型的誤差在10&

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

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

17、精度差，甚至最終濾波結(jié)果發(fā)散，而本文給出的非線性對(duì)準(zhǔn)模型能滿足低精度慣導(dǎo)的對(duì)準(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)角下的非線性對(duì)準(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空中快速對(duì)準(zhǔn)技術(shù)。航空學(xué)報(bào)，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 劉國(guó)海, 李沁雪, 施維, 等. 動(dòng)態(tài)卡爾曼濾波在導(dǎo)

21、航試驗(yàn)狀態(tài)估計(jì)中的應(yīng)用J. 儀器儀表學(xué)報(bào), 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