一種基于四元數(shù)ckf的低精度慣導(dǎo)初始對(duì)準(zhǔn)方法
【專利摘要】本發(fā)明提供的是一種基于四元數(shù)CKF的低精度慣導(dǎo)初始對(duì)準(zhǔn)方法。首先利用加速度計(jì)和磁強(qiáng)計(jì)的測(cè)量值進(jìn)行粗對(duì)準(zhǔn)，得到粗略的姿態(tài)矩陣。然后利用GPS提供的準(zhǔn)確外界信息，取位置和速度誤差作為量測(cè)量，通過四元數(shù)CKF進(jìn)行非線性濾波，得到姿態(tài)誤差矩陣。最后利用姿態(tài)誤差矩陣進(jìn)行校正，得到精確的姿態(tài)矩陣。本發(fā)明利用加速度計(jì)和磁強(qiáng)計(jì)分別對(duì)重力加速度和磁力的量測(cè)值，進(jìn)行粗對(duì)準(zhǔn)。解決了在低精度慣性器件的條件下，由于陀螺性能低，無法正確敏感地球自轉(zhuǎn)角速率，不能應(yīng)用傳統(tǒng)粗對(duì)準(zhǔn)方法的問題；另一方面，本方法中提出的四元數(shù)CKF能適用于大角度誤差的非線性模型，可以在大方位失準(zhǔn)角的條件下依然很好的完成對(duì)準(zhǔn)，且對(duì)準(zhǔn)精度高。
【專利說明】一種基于四元數(shù)CKF的低精度慣導(dǎo)初始對(duì)準(zhǔn)方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及的是一種捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)方法，尤其涉及一種基于四元數(shù)CKF的低精度捷聯(lián)慣導(dǎo)的非線性初始對(duì)準(zhǔn)方法。
【背景技術(shù)】
[0002]靜基座條件下，捷聯(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)。但在使用低精度慣性器件的條件下，由于陀螺精度低(其精度都在每小時(shí)零點(diǎn)幾度到上百度)，無法正確敏感地球自轉(zhuǎn)角速率，以及其他各種環(huán)境因素的干擾，很難根據(jù)陀螺和加速度計(jì)的輸出完成具有一定精度的粗對(duì)準(zhǔn)。此時(shí)精對(duì)準(zhǔn)往往需要在大姿態(tài)誤差角的情況進(jìn)行，特別是方位角誤差可能達(dá)到幾十度。傳統(tǒng)捷聯(lián)慣導(dǎo)的線性誤差方程是在姿態(tài)誤差為小角度的基礎(chǔ)上推導(dǎo)得到的，當(dāng)姿態(tài)誤差角較大時(shí)無法準(zhǔn)確刻畫捷聯(lián)慣導(dǎo)的誤差傳播特性。因此，需要研究能適用于大角度誤差的非線性模型及相應(yīng)的非線性濾波算法。
[0003]初始對(duì)準(zhǔn)是捷聯(lián)慣導(dǎo)系統(tǒng)中的關(guān)鍵技術(shù)之一，初始對(duì)準(zhǔn)的時(shí)間和精度直接影響捷聯(lián)慣導(dǎo)系統(tǒng)的工作性能。捷聯(lián)慣導(dǎo)誤差模型和濾波算法在初始對(duì)準(zhǔn)中扮演著重要的角色。常用的SINS誤差模型主要有Φ-angle誤差模型、Ψ-angle誤差模型、旋轉(zhuǎn)矢量誤差模型(RVE)、四元數(shù)誤差模型(QE)等。其中四元數(shù)法因?yàn)槠溆?jì)算量小、精度高、非奇異性和可全姿態(tài)工作而得到廣泛的應(yīng)用。
[0004]傳統(tǒng)的非線性濾波EKF在處理非線性較強(qiáng)的系統(tǒng)時(shí)，由于高階項(xiàng)截?cái)嗾`差，會(huì)導(dǎo)致濾波數(shù)值不穩(wěn)定性且有可能發(fā)散。而最近由加拿大學(xué)者Arasaratnam提出的基于三階spherical-radial cubature 準(zhǔn)則的 cubature kalman filter 則克服了 EKF 缺點(diǎn)。CKF 類似于UKF都避免了對(duì)非線性模型的線性化處理，不依賴于具體系統(tǒng)模型的非線性方程，算法相對(duì)獨(dú)立，適用于任何形式的非線性模型。

【發(fā)明內(nèi)容】

[0005]本發(fā)明的目的在于提供一種能夠解決低精度的慣導(dǎo)大方位失準(zhǔn)角初始對(duì)準(zhǔn)問題，并且估計(jì)精度高、所需對(duì)準(zhǔn)時(shí)間短，可靠性高的基于四元數(shù)CKF的低精度慣導(dǎo)初始對(duì)準(zhǔn)方法。
[0006]本發(fā)明的目的是這樣實(shí)現(xiàn)的:
[0007]1、包括以下步驟:
[0008](I)利用GPS確定載體的初始精度、緯度參數(shù)；
[0009](2)采集加速度計(jì)和磁強(qiáng)計(jì)輸出的載體三軸加速度和磁力信息；
[0010](3)利用采集的載體三軸加速度信息，根據(jù)下式確定初始縱搖角a C1和初始橫搖角
β O，
【權(quán)利要求】
1.一種基于四元數(shù)CKF的低精度慣導(dǎo)初始對(duì)準(zhǔn)方法，其特征在于包括以下步驟: (1)利用GPS確定載體的初始精度、緯度參數(shù)； (2)采集加速度計(jì)和磁強(qiáng)計(jì)輸出的載體三軸加速度和磁力信息； (3)利用采集的載體三軸加速度信息，根據(jù)下式確定初始縱搖角Citl和初始橫搖角 ' fb
【文檔編號(hào)】G01C25/00GK103900608SQ201410105384
【公開日】2014年7月2日 申請(qǐng)日期:2014年3月21日 優(yōu)先權(quán)日:2014年3月21日
【發(fā)明者】周廣濤, 孫艷濤, 姜鑫, 郝勤順, 趙博, 李佳璇, 林萌萌, 梁宏, 張麗麗, 于春陽 申請(qǐng)人:哈爾濱工程大學(xué)