專利名稱：一種基于蟻群Unscented粒子濾波算法的組合定姿方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種慣性/天文組合定姿方法，特別是一種基于蟻群Unscented粒子濾波 (無跡粒子濾波)算法的組合定姿方法，可用于各種航天器的高精度組合定姿。
背景技術(shù)：
為滿足天基對地觀測、武器精確打擊以及空間探索開發(fā)的迫切需求，各類地球衛(wèi)星、深空探測器、載人飛船、彈道導(dǎo)彈和運載火箭等航天器必須具備自主運行和管理能力， 而高精度的自主定姿是航天器自主運行和管理的核心技術(shù)瓶頸。目前，航天器的高精度自主定姿，無法依靠任何一種導(dǎo)航手段獨立實現(xiàn)。純慣性導(dǎo)航系統(tǒng)能夠自主、實時提供連續(xù)、 全面的導(dǎo)航信息，短時精度高，但其誤差隨工作時間積累，難以滿足航天器的長時間高精度定姿要求；天文導(dǎo)航能夠提供高精度姿態(tài)信息，誤差不隨時間積累，但易受氣候條件限制， 且輸出信息不連續(xù)；將這兩者相結(jié)合、優(yōu)勢互補，構(gòu)成慣性/天文組合定姿系統(tǒng)，是實現(xiàn)航天器長時間、高精度定姿的最為有效的手段。在慣性/天文組合定姿技術(shù)方面，以往都采用擴展卡爾曼濾波EKF(Extended Kalman Filter)方法，但是EKF僅適用于濾波誤差和預(yù)測誤差很小的情況。近年來提出的 Unscented卡爾曼濾波UKF(Unscented Kalman Filter)是一種EKF的改進算法，有效的解決了系統(tǒng)的非線性問題，但其不足是不適用于噪聲非高斯分布的系統(tǒng)。粒子濾波PF由于采用蒙特卡洛采樣(Monte Carlo sampling)結(jié)構(gòu)而在非線性、非高斯系統(tǒng)狀態(tài)跟蹤上體現(xiàn)出越來越大的優(yōu)越性，但其缺點是存在退化現(xiàn)象，消除退化現(xiàn)象主要依賴于兩個關(guān)鍵技術(shù)適當選取重要密度函數(shù)和進行重采樣。對于前者的改進方法，可使用EKPF(Extented Kalman Particle Filter)、UPF(Unscented Particle Filter)來進行重要密度函數(shù)的選擇，其中 UPF算法是利用UKF來得到粒子重要性概率密度函數(shù)的一種粒子濾波方法，由于該重要密度函數(shù)中包含了最新量測信息，因此具有更好的性能。對于后者的改進方法，常用的重采樣算法有累積分布重采樣(Binary search)、系統(tǒng)重采樣(Systematic resampling)、剩余重采樣(Residual resampling)等，這些算法通過增加粒子的有效性解決了粒子的退化問題， 但是在實際應(yīng)用中會影響系統(tǒng)的魯棒性，重采樣完成后，重要度高的粒子通過重采樣被多次選取，這在一定程度上丟失了粒子的多樣性，由此造成的后果是一旦目標丟失或跟蹤精度不夠，系統(tǒng)自動收斂的可能性很小，為此，很多學(xué)者提出了遺傳粒子濾波(GPF)算法，GPF 算法雖然在保證粒子有效性的同時又增加了粒子的多樣性，仍然存在濾波速度慢和魯棒性差的問題。

發(fā)明內(nèi)容
本發(fā)明的技術(shù)解決問題是克服現(xiàn)有技術(shù)的不足，提出一種基于蟻群UPF的組合定姿方法，解決系統(tǒng)非線性和噪聲非高斯問題，以快速獲得高精度的姿態(tài)信息，并能夠準確地估計陀螺漂移，實現(xiàn)各種類型航天器長時間、高精度的組合定姿。本發(fā)明的技術(shù)解決方案為一種基于蟻群UPF組合定姿方法，其特點在于利用慣性量測信息和天文量測信息，通過蟻群(Ant Colony Algorithm)UPF(無跡粒子濾波)方法，實現(xiàn)航天器長時間、高精度的快速組合定姿，其實現(xiàn)步驟如下(1)利用慣性量測信息補償陀螺輸出數(shù)據(jù)，通過姿態(tài)解算，得到載體姿態(tài)信息；(2)利用天文量測信息，通過確定性算法，獲得所需的天文姿態(tài)信息；

(3)禾l」ML (Ant Colony Algorithm)Unscented|4^iit^ (Unscented Particle Filter)算法將天文姿態(tài)信息和載體姿態(tài)信息相融合，求解高精度的載體姿態(tài)信息，估計陀螺漂移，并反饋校正載體姿態(tài)和補償陀螺漂移補償；最終實現(xiàn)基于天文量測信息實時消除慣性/天文組合導(dǎo)航系統(tǒng)陀螺隨機誤差的在線修正，完成對航天器的高精度組合定姿；利用蟻群UPF算法進行信息融合的步驟為(3. 1)采樣時間t = 0時，初始化對初始的先驗概率密度ρ (Xtl)進行采樣，生成N個服從ρ (Xtl)分布的粒子= 1，…，N，生成的粒子xf的均值和方差滿足J。(')=五[4')]，々')=五[(4')-野))(4')-對〒]’其中，茍K的均值，W為<的方差，Ε[·]為求取口內(nèi)元素的期望，將ρ (Xtl)分布取為均值為4，方差為Ptl正態(tài)分布；(3. 2)采樣時間t彡1時，步驟如下①采樣利用(3.1)中生成的服從P(Xtl)分布的粒子<進行下一時刻的采樣，用 Unscented卡爾曼濾波對粒子IKDj進行估計，得到丨，采樣#
^k |、卜1’ )=N^k_UKF'Pk_UKF^ ’得到更新的粒子{4')，f)} 4-1 = PkJJKF ’ i = 1，…，N，其中，^和^f1分別為k-Ι時刻狀態(tài)對應(yīng)的第i個粒子和粒子的誤差方差陣， 和Piukf分別為根據(jù)k-Ι時刻的粒子估計的第k時刻狀態(tài)估計值和估計誤差方差陣，
X0 :k-!為第0 k-Ι時刻的狀態(tài)估計值，y1:k為第1 k時刻的狀態(tài)觀測值，q(xkI Xtl Md1J 為重要性概率密度，此處選為—M^I-MW^為均值為、，方差為 PkMF的正態(tài)分布；②利用①中UKF更新的粒子Rlifl，計算粒子丨j^^fl的權(quán)重 < =-‘——,歸一化權(quán)重(,_) .(0 , N
N 廣、其中，為k時刻第i個粒子的權(quán)值，w 為歸一化后的權(quán)重，g為所有粒子的
vvkk/=I
權(quán)值的和，為對應(yīng)于觀測模型的系統(tǒng)狀態(tài)的觀測似然概率密度，樹χΓ|4'」,)為對應(yīng)
于系統(tǒng)的模型的系統(tǒng)狀態(tài)轉(zhuǎn)移概率密度，擬I^tl,m)為重要性概率密度；③利用①中得出的粒子和②中得出的粒子的權(quán)重使用蟻群算法進行重采樣，選取優(yōu)等粒子(權(quán)值較大的粒子)，剔出低等(權(quán)值較小的粒子)的粒子，以解決粒子枯竭問題，利用蟻群算法進行優(yōu)化的步驟如下首先引入如下記號

m——蟻群中螞蟻的數(shù)量；

Clij——兩城市i和j之間的距離；n.ja)——邊(i，j)的能見度，反映由城市i轉(zhuǎn)移到城市j的啟發(fā)程度，這個量在螞蟻系統(tǒng)的運行中不改變；τ u.a) — t時刻邊(i，j)上的信息素軌跡強度；Δ τ u—螞蟻k在邊(i，j)上的留下的單位長度軌跡信息素量；p\——螞蟻k的轉(zhuǎn)移概率，j為未訪問的城市。每只螞蟻都是具有如下特征的簡單主體I從城市i到城市j的運動過程中或是在完成一次循環(huán)后，螞蟻在邊(i，j)上釋放的一種物質(zhì)，稱為信息素軌跡；II螞蟻概率的選擇下一個將要訪問的城市，這個概率是兩城市間距離和連接兩城市的路徑上存有軌跡量的函數(shù)；III為了滿足問題的約束條件，在完成一次循環(huán)之前，不允許螞蟻選擇已經(jīng)訪問過的城市。a.初始化令時間t = 0，迭代次數(shù)Nc = 0，信息素τ u (0) = C，C為正常數(shù)，根據(jù)具體應(yīng)用進行設(shè)置，此處隨意設(shè)置為C = 1，Tij(O) ^Jt = O時邊(i，j)上的信息素軌跡強度，(i，j) 為某時刻螞蟻所處的位置；b.對N個粒子的權(quán)值進行一次排序選擇權(quán)值最大的點作為起點，將m只螞蟻置于起點，各只螞蟻，按照下列轉(zhuǎn)移概率公式，采用賭輪選擇方式移動，
權(quán)利要求
1. 一種基于蟻群Unscented粒子濾波算法的組合定姿方法，其實現(xiàn)步驟如下(1)利用慣性量測信息補償陀螺輸出數(shù)據(jù)，通過姿態(tài)解算，得到載體姿態(tài)信息；(2)利用天文量測信息，通過確定性算法，獲得所需天文姿態(tài)信息；(3)利用蟻群Unscented粒子濾波(UnscentedParticle Filter)算法將天文姿態(tài)信息和載體姿態(tài)信息相融合，求解高精度的載體姿態(tài)信息，估計陀螺漂移，并反饋校正載體姿態(tài)和補償陀螺漂移補償；最終實現(xiàn)基于天文量測信息實時消除慣性/天文組合導(dǎo)航系統(tǒng)陀螺隨機誤差的在線修正，完成對航天器的長時間、高精度組合定姿；所述步驟(3)中利用蟻群UPF算法步驟為 (3. 1)采樣時間t = 0時，初始化對初始的先驗概率密度P OO進行采樣，生成N個服從ρ ( )分布的粒子<，i = 1，···， N，生成的粒子xf的均值和方差滿足
2.根據(jù)要求1所述的基于蟻群Unscented粒子濾波算法的組合定姿方法，其特征在于 所述利用蟻群算法實現(xiàn)步驟如下 首先引入如下符號 m——蟻群中螞蟻的數(shù)量； Clij——兩城市i和j之間的距離；η^-ω—邊(i，j)的能見度，反映由城市i轉(zhuǎn)移到城市j的啟發(fā)程度，這個量在螞蟻系統(tǒng)的運行中不改變；τ ^t)——t時刻邊(i，j)上的信息素軌跡強度； Δ τ u—螞蟻k在邊(i，j)上的留下的單位長度軌跡信息素量； Py——螞蟻k的轉(zhuǎn)移概率，j為未訪問的城市； 每只螞蟻都是具有如下特征的簡單主體I從城市i到城市j的運動過程中或是在完成一次循環(huán)后，螞蟻在邊(i，j)上釋放的一種物質(zhì)，稱為信息素軌跡；II螞蟻概率的選擇下一個將要訪問的城市，這個概率是兩城市間距離和連接兩城市的路徑上存有軌跡量的函數(shù)；III為了滿足問題的約束條件，在完成一次循環(huán)之前，不允許螞蟻選擇已經(jīng)訪問過的城市；具體實現(xiàn)過程為a.初始化令時間t = 0，迭代次數(shù)Nc = 0，信息素τ。_(0) = C，C為正常數(shù)，根據(jù)具體應(yīng)用進行設(shè)置，此處隨意設(shè)置為C = 1，Tij(O) ^t = O時邊(i，j)上的信息素軌跡強度，(i，j)為某時刻螞蟻所處的位置；b.對N個粒子的權(quán)值進行一次排序選擇權(quán)值最大的點作為起點，將m只螞蟻置于起點， 各只螞蟻，按照下列轉(zhuǎn)移概率公式，采用賭輪選擇方式移動，
3.根據(jù)要求1所述的基于蟻群Unscented粒子濾波算法的組合定姿方法，其特征在于所述確定性算法實現(xiàn)如下A.定義3X3的矩陣w，v，B和S，3X1的列向量z，a，標量σ，4X1的列向量q; 其中，w= [Wi W2 w3]為k時刻觀測的三顆星的星光在星敏感器坐標系中的坐標矢量，ν = [V1 V2 V3]為k時刻該三顆星的星光在地心慣性坐標系中的參考矢量
全文摘要
一種基于蟻群Unscented粒子濾波算法的組合定姿方法，本發(fā)明涉及一種慣性/天文組合定姿方法。該方法首先利用慣性量測信息進行補償陀螺輸出數(shù)據(jù)，通過姿態(tài)解算，得到載體姿態(tài)信息；然后利用天文量測信息，通過確定性算法，獲得所需天文姿態(tài)信息；最后利用蟻群Unscented粒子濾波(Unscented Particle Filter)算法將天文姿態(tài)信息和載體姿態(tài)信息相融合，解決系統(tǒng)非線性和噪聲非高斯問題，求解高精度載體姿態(tài)信息，估計陀螺漂移，并反饋校正載體姿態(tài)和補償陀螺漂移；最終實現(xiàn)基于天文量測信息實時消除慣性/天文組合導(dǎo)航系統(tǒng)陀螺隨機誤差的在線修正，完成對航天器的長時間、高精度組合定姿。
文檔編號G05D1/08GK102156478SQ20101062252
公開日2011年8月17日 申請日期2010年12月28日 優(yōu)先權(quán)日2010年12月28日
發(fā)明者全偉, 劉翠翠, 崔培玲, 房建成, 楊照華, 郭雷 申請人:北京航空航天大學(xué)