一種基于蟻群pf算法的原子自旋陀螺儀和磁強(qiáng)計(jì)緊組合定姿方法
【專(zhuān)利摘要】一種基于蟻群PF算法的原子自旋陀螺儀／磁強(qiáng)計(jì)緊組合定姿方法，本發(fā)明涉及一種SERF原子自旋陀螺儀／磁強(qiáng)計(jì)組合定姿方法。該方法首先利用原子自旋陀螺儀獲取慣性角速度信息，然后進(jìn)行陀螺誤差補(bǔ)償，通過(guò)姿態(tài)解算獲取載體的姿態(tài)信息；其次利用原子磁強(qiáng)計(jì)獲得地磁量測(cè)信息，并對(duì)其進(jìn)行地磁匹配，獲取地磁矢量信息；最后利用蟻群(Ant?Colony)粒子濾波(Particle?Filter)算法采用緊組合方式將地磁矢量信息和載體姿態(tài)信息相融合，解決系統(tǒng)非線性和噪聲非高斯問(wèn)題，求解高精度載體姿態(tài)信息，估計(jì)陀螺漂移，并反饋校正載體姿態(tài)和補(bǔ)償陀螺漂移；最終實(shí)現(xiàn)基于SERF原子自旋陀螺儀／磁強(qiáng)計(jì)緊組合定姿系統(tǒng)陀螺隨機(jī)誤差的在線修正，完成對(duì)運(yùn)動(dòng)載體的長(zhǎng)時(shí)間、高精度組合定姿。
【專(zhuān)利說(shuō)明】一種基于蟻群PF算法的原子自旋陀螺儀和磁強(qiáng)計(jì)緊組合定姿方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種基于蟻群PF算法的原子自旋陀螺儀和磁強(qiáng)計(jì)緊組合定姿方法，可實(shí)現(xiàn)全地域、全天時(shí)、全天候、長(zhǎng)時(shí)間、高精度的自主定姿，且自主性強(qiáng)、隱蔽性好、成本低，可廣泛用于水下探測(cè)器、飛機(jī)、導(dǎo)彈、航天器等多個(gè)領(lǐng)域。
【背景技術(shù)】
[0002]為滿足天基對(duì)地觀測(cè)、武器精確打擊以及空間、水下探索開(kāi)發(fā)的迫切需求，各類(lèi)航天器、水下探測(cè)器必須具備自主運(yùn)行和管理能力，而高精度的自主定姿是航天器自主運(yùn)行和管理的核心技術(shù)瓶頸。由于在無(wú)白旋交換弛豫(SERF)狀態(tài)下利用原子自旋效應(yīng)可同時(shí)實(shí)現(xiàn)超高精度慣性與磁場(chǎng)測(cè)量，使得SERF原子陀螺儀/磁強(qiáng)計(jì)組合定姿成為實(shí)現(xiàn)長(zhǎng)時(shí)間高精度自主定姿的重要發(fā)展方向之一。陀螺儀/磁強(qiáng)計(jì)組合定姿具有能夠自主、實(shí)時(shí)提供連續(xù)、全面的定姿信息，且不受氣候、地域、時(shí)間等影響的特點(diǎn)。
[0003]目前，航天器的高精度自主定姿，無(wú)法依靠任何一種定姿手段獨(dú)立實(shí)現(xiàn)。純慣性定姿系統(tǒng)，短時(shí)精度高，但其誤差隨工作時(shí)間積累，難以滿足航天器的長(zhǎng)時(shí)間高精度定姿要求；地磁定姿可實(shí)時(shí)測(cè)量地磁場(chǎng)強(qiáng)度實(shí)現(xiàn)載體的定位，且誤差不隨時(shí)間積累，但受限于地磁匹配精度以及磁場(chǎng)測(cè)量精度等，使得目前地磁定姿精度仍然相對(duì)較低；將這兩者相結(jié)合、優(yōu)勢(shì)互補(bǔ)，構(gòu)成陀螺儀/磁強(qiáng)計(jì)組合定姿系統(tǒng)，是實(shí)現(xiàn)航天器長(zhǎng)時(shí)間、高精度定姿的最為有效的手段。
[0004]在陀螺儀/磁強(qiáng)計(jì)組合定姿技術(shù)方面，以往都采用擴(kuò)展卡爾曼濾波EKF (ExtendedKalman Filter)方法，但是EKF僅適用于濾波誤差和預(yù)測(cè)誤差很小的情況。近年來(lái)提出的Unscented卡爾曼濾波UKF(Unscented Kalman Filter)是一種EKF的改進(jìn)算法,有效的解決了系統(tǒng)的非線性問(wèn)題，但其不足是不適用于噪聲非高斯分布的系統(tǒng)。粒子濾波(ParticleFilter, PF)由于采用蒙特卡洛采樣(Monte Carlo sampling)結(jié)構(gòu)而在非線性、非高斯系統(tǒng)狀態(tài)跟蹤上體現(xiàn)出越來(lái)越大的優(yōu)越性，但其缺點(diǎn)是存在退化現(xiàn)象，消除退化現(xiàn)象主要依賴于兩個(gè)關(guān)鍵技術(shù):適當(dāng)選取重要密度函數(shù)和進(jìn)行重采樣。對(duì)于前者的改進(jìn)方法，可使用EKPF(Extented Kalman Particle Filter)、無(wú)跡粒子濾波(Unscented Particle Filter,UPF)來(lái)進(jìn)行重要密度函數(shù)的選擇。對(duì)于后者的改進(jìn)方法，常用的重采樣算法有累積分布重米樣(Binary search)、系統(tǒng)重米樣(Systematic resampling)、剩余重米樣(Residualresampling)等，這些算法通過(guò)增加粒子的有效性解決了粒子的退化問(wèn)題，但是在實(shí)際應(yīng)用中會(huì)影響系統(tǒng)的魯棒性，重采樣完成后，重要度高的粒子通過(guò)重采樣被多次選取，這在一定程度上丟失了粒子的多樣性，由此造成的后果是一旦目標(biāo)丟失或跟蹤精度不夠，系統(tǒng)自動(dòng)收斂的可能性很小，為此，很多學(xué)者提出了遺傳粒子濾波(GPF)算法，GPF算法雖然在保證粒子有效性的同時(shí)又增加了粒子的多樣性，仍然存在濾波速度慢和魯棒性差的問(wèn)題。

【發(fā)明內(nèi)容】
[0005]本發(fā)明的技術(shù)解決問(wèn)題是:克服現(xiàn)有技術(shù)的不足，提出一種基于蟻群PF算法的原子白旋陀螺儀和磁強(qiáng)計(jì)緊組合定姿方法，解決系統(tǒng)非線性和噪聲非高斯問(wèn)題，以快速獲得高精度的姿態(tài)信息，并能夠準(zhǔn)確地估計(jì)陀螺漂移，實(shí)現(xiàn)各種類(lèi)型航天器長(zhǎng)時(shí)間、高精度的組合定姿。
[0006]本發(fā)明的技術(shù)解決方案為:一種基于原子自旋效應(yīng)的陀螺儀和磁強(qiáng)計(jì)組合定姿方法，特別是一種基于蟻群PF的濾波算法的組合定姿方法，其特點(diǎn)在于:利用慣性量測(cè)信息和地磁量測(cè)信息，通過(guò)蟻群(Ant Colony Algorithm) PF (粒子濾波)方法,實(shí)現(xiàn)航天器長(zhǎng)時(shí)間、高精度的快速組合定姿，其實(shí)現(xiàn)步驟如下:
[0007](I)利用原子白旋陀螺儀獲取慣性角速度信息，然后進(jìn)行陀螺誤差補(bǔ)償，通過(guò)姿態(tài)解算獲取載體的姿態(tài)信息；
[0008](2)利用原子磁強(qiáng)計(jì)獲得地磁量測(cè)信息，并對(duì)其進(jìn)行地磁匹配，獲取地磁矢量信息；
[0009](3)利用地磁數(shù)據(jù)庫(kù)給出的地磁匹配數(shù)據(jù)和由步驟(I)獲得的姿態(tài)信息來(lái)匹配出相應(yīng)的地磁矢量信息，然后與步驟(2)獲取的地磁矢量信息(I)獲得的姿態(tài)信息進(jìn)行比較，比較結(jié)果作為測(cè)量值；
[0010](4)利用蟻群(Ant Colony)粒子濾波(Particle Filter)算法將步驟(3)得到的匹配出的相應(yīng)地磁矢量信息和步驟(2)原子磁強(qiáng)計(jì)給出的地磁矢量信息進(jìn)行比較濾波，得到誤差的最優(yōu)估計(jì)，利用此估計(jì)分別對(duì)原子白旋陀螺儀和磁強(qiáng)計(jì)的數(shù)據(jù)進(jìn)行校正。
[0011](5)原子自旋陀螺儀和原子磁強(qiáng)計(jì)采用緊組合方式:利用原子磁強(qiáng)計(jì)匹配出的地磁信息去修正陀螺的漂移，具體表現(xiàn)為:用原子磁強(qiáng)計(jì)測(cè)得的磁偏角和磁傾角信息去修正濾波器輸出的載體姿態(tài)信息和陀螺漂移；用原子自旋陀螺儀輸出的姿態(tài)信息去修正原子磁強(qiáng)計(jì)的磁場(chǎng)信息，具體表現(xiàn)為:用濾波器輸出的載體姿態(tài)角信息去修正地磁匹配得到的原子磁強(qiáng)計(jì)的相應(yīng)磁偏角和磁傾角，使原子陀螺儀和原子磁強(qiáng)計(jì)的測(cè)量信息相互修正，實(shí)現(xiàn)原子陀螺儀和原子磁強(qiáng)計(jì)的緊組合定姿。
[0012]本發(fā)明的原理是:首先利用原子白旋陀螺儀獲取慣性角速度信息，然后進(jìn)行陀螺誤差補(bǔ)償，通過(guò)姿態(tài)解算獲取載體的姿態(tài)信息；其次利用原子磁強(qiáng)計(jì)獲得地磁量測(cè)信息，并對(duì)其采用基于“地磁熵和一種改進(jìn)的基于標(biāo)準(zhǔn)方差的Hausdorff距離”的匹配方法進(jìn)行地磁矢量匹配，獲得載體所在位置的地磁矢量信息；再次利用蟻群(Ant Colony)粒子濾波(Particle Filter)算法采用緊組合方式將地磁信息和載體姿態(tài)信息相融合，解決系統(tǒng)非線性和噪聲非高斯問(wèn)題，求解高精度載體姿態(tài)信息，估計(jì)陀螺漂移，并反饋校正載體姿態(tài)、補(bǔ)償陀螺漂移并校正原子磁強(qiáng)計(jì)測(cè)得的磁偏角和磁傾角；最終實(shí)現(xiàn)原子白旋陀螺儀/磁強(qiáng)計(jì)緊組合定姿系統(tǒng)陀螺隨機(jī)誤差的在線修正，完成對(duì)航天器的長(zhǎng)時(shí)間、高精度組合定姿。
[0013]本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于:本發(fā)明克服了傳統(tǒng)陀螺儀/磁強(qiáng)計(jì)組合定姿方法定姿精度低及濾波性能差的不足，利用蟻群(Ant Colony Algorithm)PF(粒子濾波)算法有效解決了系統(tǒng)非線性和噪聲非高斯的問(wèn)題，利用蟻群算法對(duì)PF(粒子濾波)進(jìn)行優(yōu)化，有效的提高了組合定姿的精度；采用基于地磁熵和一種改進(jìn)的基于標(biāo)準(zhǔn)方差的HausdorfT距離的匹配方法進(jìn)行地磁矢量匹配；采用原子陀螺儀/原子磁強(qiáng)計(jì)緊組合方式，將慣性量測(cè)信息和地磁量測(cè)信息相融合，進(jìn)一步提高了組合定姿的精度，實(shí)現(xiàn)了對(duì)陀螺漂移的精確估計(jì)，滿足了航天器長(zhǎng)時(shí)間、高精度組合定姿的要求?！緦?zhuān)利附圖】

【附圖說(shuō)明】
[0014]圖1為本發(fā)明一種基于蟻群PF的濾波算法的組合定姿方法原理圖；
[0015]圖2為本發(fā)明中用的基于地磁熵和一種改進(jìn)的基于標(biāo)準(zhǔn)方差Hausdorff距離的地磁匹配算法流程圖；
[0016]圖3為本發(fā)明中用蟻群算法優(yōu)化粒子的流程圖。
【具體實(shí)施方式】
[0017]如圖1所示，本發(fā)明的具體實(shí)施步驟如下:
[0018]步驟1、首先對(duì)原子陀螺儀測(cè)得的慣性量測(cè)信息進(jìn)行補(bǔ)償陀螺輸出數(shù)據(jù)后，通過(guò)姿態(tài)解算，得到載體姿態(tài)信息，流程如下:
[0019]步驟1、慣性測(cè)量信息，即利用原子陀螺儀測(cè)量得到而輸出的數(shù)據(jù)，利用信息融合算法估計(jì)得到的陀螺漂移f來(lái)校正補(bǔ)償陀螺輸出數(shù)據(jù)ω=ΚΧν+ε+ζ中的ε，之后通過(guò)姿態(tài)解算，得到載體姿態(tài)信息；所述步驟I具體包括以下步驟:
[0020]步驟1.1.設(shè)定初始姿態(tài)為θ0 η)，計(jì)算得出初始姿態(tài)四元數(shù)q(O)陣:
【權(quán)利要求】
1.一種基于蟻群PF算法的原子自旋陀螺儀和磁強(qiáng)計(jì)緊組合定姿方法，其特征在于包括以下步驟: (1)利用原子自旋陀螺儀獲取慣性角速度信息，然后進(jìn)行陀螺誤差補(bǔ)償，通過(guò)姿態(tài)解算獲取載體的姿態(tài)信息； (2)利用原子磁強(qiáng)計(jì)獲得地磁量測(cè)信息，并對(duì)其進(jìn)行地磁匹配，獲取地磁矢量信息； (3)利用地磁數(shù)據(jù)庫(kù)給出的地磁匹配數(shù)據(jù)和由步驟(I)獲得的姿態(tài)信息來(lái)匹配出相應(yīng)的地磁矢量信息，然后與步驟(2)獲取的地磁矢量信息(I)獲得的姿態(tài)信息進(jìn)行比較，比較結(jié)果作為測(cè)量值； (4)利用蟻群(AntColony)粒子濾波(Particle Filter)算法將步驟(3)得到的匹配出的相應(yīng)地磁矢量信息和步驟(2)原子磁強(qiáng)計(jì)給出的地磁矢量信息進(jìn)行比較濾波，得到誤差的最優(yōu)估計(jì)，利用此估計(jì)分別對(duì)原子自旋陀螺儀和磁強(qiáng)計(jì)的數(shù)據(jù)進(jìn)行校正； (5)原子自旋陀螺儀和原子磁強(qiáng)計(jì)采用緊組合方式:利用原子磁強(qiáng)計(jì)匹配出的地磁信息去修正陀螺的漂移，具體表現(xiàn)為:用原子磁強(qiáng)計(jì)測(cè)得的磁偏角和磁傾角信息去修正濾波器輸出的載體姿態(tài)信息和陀螺漂移；用原子自旋陀螺儀輸出的姿態(tài)信息去修正原子磁強(qiáng)計(jì)的磁場(chǎng)信息，具體表現(xiàn)為:用濾波器輸出的載體姿態(tài)角信息去修正地磁匹配得到的原子磁強(qiáng)計(jì)的相應(yīng)磁偏角和磁傾角，使原子陀螺儀和原子磁強(qiáng)計(jì)的測(cè)量信息相互修正，實(shí)現(xiàn)原子陀螺儀和原子磁強(qiáng)計(jì)的緊組合定姿。
2.根據(jù)權(quán)利要求1所述的一種基于蟻群PF算法的原子自旋陀螺儀和磁強(qiáng)計(jì)緊組合定姿方法，其特征在于:所述步驟(2)中利用基于地磁熵和一種改進(jìn)的基于標(biāo)準(zhǔn)方差的Hausdorff距離的方法進(jìn)行地磁匹配,具體步驟為: (21)根據(jù)載體當(dāng)前位置用原子磁強(qiáng)計(jì)測(cè)得實(shí)時(shí)地磁圖； (22)將一段時(shí)間內(nèi)測(cè)得的地磁強(qiáng)度數(shù)據(jù)按照獲得時(shí)間的先后排成一個(gè)數(shù)組，按照以下公式計(jì)算實(shí)時(shí)地磁圖的地磁熵:
3.根據(jù)權(quán)利要求1所述的一種基于蟻群PF算法的原子自旋陀螺儀和磁強(qiáng)計(jì)緊組合定姿方法，其特征在于:所述步驟(2)中原子自旋陀螺儀和原子磁強(qiáng)計(jì)采用緊組合方式: (A)利用原子磁強(qiáng)計(jì)匹配出的地磁信息去修正陀螺的漂移，具體表現(xiàn)為:用原子磁強(qiáng)計(jì)測(cè)得的磁偏角和磁傾角信息去修正濾波器輸出的載體姿態(tài)信息和陀螺漂移； (B)用原子自旋陀螺儀輸出的姿態(tài)信息去修正原子磁強(qiáng)計(jì)的磁場(chǎng)信息，具體表現(xiàn)為:用濾波器輸出的載體姿態(tài)角信息去修正地磁匹配得到的原子磁強(qiáng)計(jì)的相應(yīng)磁偏角和磁傾角； 使原子陀螺儀和原子磁強(qiáng)計(jì)的測(cè)量信息相互修正，實(shí)現(xiàn)原子陀螺儀和原子磁強(qiáng)計(jì)的緊組合定姿。
4.根據(jù)權(quán)利要求1所述的一種基于蟻群PF算法的原子自旋陀螺儀和磁強(qiáng)計(jì)緊組合定姿方法，其特征在于:所述步驟(3)中利用蟻群粒子濾波算法實(shí)現(xiàn)步驟為: (31)當(dāng)初始采樣時(shí)刻t=0時(shí)，初始化: 對(duì)初始的先驗(yàn)概率密度P GO進(jìn)行采樣，生成N個(gè)服從P (?)分布的粒子i = Ir..，N，其均值和方差/f滿足:


5.根據(jù)權(quán)利要求4所述的一種基于蟻群PF算法的原子自旋陀螺儀和磁強(qiáng)計(jì)緊組合定姿方法，其特征在于:所述步驟④通過(guò)蟻群算法從得到的這組新的粒子中選取優(yōu)等粒子的步驟如下:a初始化 令時(shí)間t=0,迭代次數(shù)N=O,信息素τ U(O)=GC為正常數(shù)，
【文檔編號(hào)】G01C25/00GK103438879SQ201310390181
【公開(kāi)日】2013年12月11日 申請(qǐng)日期:2013年9月2日 優(yōu)先權(quán)日:2013年9月2日
【發(fā)明者】全偉, 呂琳, 房建成, 龍華保, 陳熙, 劉翔, 吳雙卿 申請(qǐng)人:北京航空航天大學(xué), 上海航天控制工程研究所