確定采樣型濾波器優(yōu)化算法及其在編隊(duì)導(dǎo)航中的應(yīng)用

叢源材　吳青坡　周紹磊

摘 要： 針對基于視覺的編隊(duì)導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)估計(jì)問題，開展了確定采樣濾波器對導(dǎo)航參數(shù)的估計(jì)研究。根據(jù)僚機(jī)體坐標(biāo)系下的編隊(duì)運(yùn)動方程和視覺量測方程，建立了適用于確定采樣濾波器的系統(tǒng)狀態(tài)方程和量測方程。針對采樣點(diǎn)不能滿足姿態(tài)四元數(shù)的歸一化要求，采用無約束的羅德里格參數(shù)代替四元數(shù)進(jìn)行采樣，選擇的采樣點(diǎn)能夠滿足編隊(duì)導(dǎo)航系統(tǒng)的要求。采用3階采樣點(diǎn)的確定采樣型濾波器對編隊(duì)導(dǎo)航狀態(tài)進(jìn)行估計(jì)。UKF和CKF由于分別存在中心采樣點(diǎn)權(quán)值過大和非局部效應(yīng)而導(dǎo)致濾波發(fā)散，因此通過正交變換的方法對非局部效應(yīng)進(jìn)行修正，實(shí)現(xiàn)了估計(jì)誤差的穩(wěn)定收斂。通過仿真驗(yàn)證表明，提出的改進(jìn)方法能夠滿足編隊(duì)導(dǎo)航狀態(tài)估計(jì)的要求。

關(guān)鍵詞： 編隊(duì)導(dǎo)航； 采樣點(diǎn)； 狀態(tài)估計(jì)； 正交變換

中圖分類號： TN96?34 文獻(xiàn)標(biāo)識碼： A 文章編號： 1004?373X（2015）05?0033?06

Optimization algorithm of determination sampling type filter

and its application in formation navigation

CONG Yuan?cai， WU Qing?po， ZHOU Shao?lei

（Department of Control Engineering， Naval Aeronautical and Astronautical University， Yantai 264001， China）

Abstract： To solve the problem existing in navigation parameter estimation for formation navigation system based on vision， the estimation of navigation parameter by the determine sampling type filter is studied. According to the formation motion equation and vision measuration equation in body frame of wing plane， the systematical state equation and measuration equation suitable for the determination sampling type filter are established. In consideration that the sampling point can not meet the normalization demand of attitude quaternion， the unrestraint Rodrigues parameter is used to replace quaternion. The sampling point selected by it can meet the demand of formation navigation system. The determination sampling type filter with third?order sampling point is used for the estimation of formation navigation state. Since the centre sampling point weight of UKF is oversize， and CKF has non?local effect， which may lead to filter divergence， a method of orthogonal conversion is used to correct non?local effect to realize stable convergence of evaluated error. Simulation results are given to prove the technique.

Keywords： formation navigation； sampling point； state estimation； orthogonal conversion

0 引 言

無人機(jī)編隊(duì)飛行需要編隊(duì)導(dǎo)航系統(tǒng)提供飛行器之間的編隊(duì)位置、編隊(duì)速度和編隊(duì)姿態(tài)信息。目前通常采用GPS與慣性測量裝置的組合來獲得位置和姿態(tài)信息。由于GPS信號容易受干擾，因此對不依賴GPS信號的編隊(duì)導(dǎo)航系統(tǒng)成為近年來的研究熱點(diǎn)。基于視覺的導(dǎo)航系統(tǒng)可以取代GPS，提供編隊(duì)導(dǎo)航以及絕對導(dǎo)航信息。本文以視覺編隊(duì)導(dǎo)航為研究背景，對確定采樣型濾波器在編隊(duì)導(dǎo)航中的應(yīng)用展開研究。

編隊(duì)導(dǎo)航系統(tǒng)有如下幾個特點(diǎn)，對濾波器提出了不同的要求：

（1） 系統(tǒng)方程中的狀態(tài)包括編隊(duì)姿態(tài)、編隊(duì)位置、編隊(duì)速度和慣性器件誤差，系統(tǒng)狀態(tài)維數(shù)為16，會出現(xiàn)權(quán)值為負(fù)或非局部效應(yīng)。例如，采用UKF則會出現(xiàn)很大的負(fù)權(quán)值[ω0=-133；]而采用CKF，雖然可以避免這一情況的發(fā)生，但是由于采樣點(diǎn)離采樣中心過遠(yuǎn)，可能會存在非局部效應(yīng)的問題；

（2） 狀態(tài)向量中，各分量之間的數(shù)值大小差異較大，在采樣時應(yīng)當(dāng)避免對數(shù)值較小分量的忽視；

（3） 采用四元數(shù)描述姿態(tài)，在采樣時會遇到問題，由于四元數(shù)要求[q=q20+q21+q22+q23=1，]而得到的采樣點(diǎn)中，這一條件無法滿足。因此需要采用無約束的狀態(tài)變量來替代四元數(shù)進(jìn)行采樣。

1993年，Shuster提出了一種由3個變量組成的姿態(tài)誤差向量來表示姿態(tài)四元數(shù)的誤差向量[1]，這3個變量可有多種表示方法，包括Gibbs向量（180°出現(xiàn)奇點(diǎn)）和修正Rodrigues參數(shù)（360°出現(xiàn)奇點(diǎn)），雖然存在奇點(diǎn)，但是由于是用來表示誤差的，所以不會發(fā)生奇點(diǎn)出現(xiàn)的情況。本文采用無約束的Rodrigues參數(shù)來描述編隊(duì)姿態(tài)[2?3]。

1 確定采樣濾波器對編隊(duì)導(dǎo)航參數(shù)的估計(jì)

1.1 編隊(duì)導(dǎo)航運(yùn)動方程

1.1.1 編隊(duì)姿態(tài)運(yùn)動方程

對于姿態(tài)的描述有多種描述方式，如旋轉(zhuǎn)矢量、方向余弦矩陣、四元數(shù)和修正羅德里格參數(shù)。通常采用方向余弦矩陣和四元數(shù)來表示坐標(biāo)系之間的轉(zhuǎn)動關(guān)系，由于方向余弦矩陣有9個未知參數(shù)，而四元數(shù)只有4個，因此在姿態(tài)更新算法中采用四元數(shù)的計(jì)算量更小、精度更高[4]。當(dāng)向量在不同坐標(biāo)系下變換時，習(xí)慣上采用方向余弦矩陣，而方向余弦矩陣和姿態(tài)四元數(shù)之間可以相互轉(zhuǎn)換，因此文中采用四元數(shù)法進(jìn)行姿態(tài)更新，在坐標(biāo)變換時仍然采用方向余弦矩陣來表示。

解算坐標(biāo)系（b系）是指進(jìn)行編隊(duì)位置解算的坐標(biāo)系。編隊(duì)飛行中，通常選用導(dǎo)航坐標(biāo)系或體坐標(biāo)系。解算坐標(biāo)系相對慣性空間的轉(zhuǎn)動引起的姿態(tài)變化通過姿態(tài)運(yùn)動方程來描述，可以用方向余弦矩陣或四元數(shù)來表示。

1.1.2 編隊(duì)質(zhì)心運(yùn)動方程

編隊(duì)飛行示意圖如圖1所示。

其中，[ηfgu]和[ηfau]為高斯白噪聲。

1.3.2 量測方程

式中[Pj]為第[j]個光標(biāo)的安裝位置。

1.3.3 編隊(duì)導(dǎo)航參數(shù)估計(jì)

通過式（17）得到每個采樣點(diǎn)對應(yīng)的誤差Rodrigues參數(shù)，將采樣點(diǎn)中的編隊(duì)位置、編隊(duì)速度和慣性器件誤差部分代入系統(tǒng)狀態(tài)方程從而得到預(yù)測更新后的采樣點(diǎn)[χi。]

在整個濾波過程中，姿態(tài)四元數(shù)用于編隊(duì)導(dǎo)航狀態(tài)的更新，而Rodrigues參數(shù)用于獲得采樣點(diǎn)和計(jì)算均值及協(xié)方差。

2 應(yīng)用舉例

選取一個固定的地理坐標(biāo)系[n，]給出初始經(jīng)度[λ=38°，]緯度[?=-77°，]在該地理坐標(biāo)系下長機(jī)和僚機(jī)的飛行軌跡為[5]：

長機(jī)位置軌跡：

3 結(jié) 論

通過構(gòu)建僚機(jī)體坐標(biāo)系下基于視覺量測的無人機(jī)編隊(duì)導(dǎo)航系統(tǒng)模型，采用確定采樣型濾波器對編隊(duì)導(dǎo)航狀態(tài)進(jìn)行估計(jì)。采用姿態(tài)四元數(shù)來完成編隊(duì)姿態(tài)的更新，由于四元數(shù)有歸一化要求，而在濾波過程中，獲得的采樣點(diǎn)中的姿態(tài)部分并不能滿足這一要求，因此需要采用無約束的參數(shù)來表示姿態(tài)。文中在獲取采樣點(diǎn)時以廣義羅德里格參數(shù)來代替四元數(shù)，通過仿真對不同的采樣策略下的編隊(duì)導(dǎo)航參數(shù)的估計(jì)效果進(jìn)行了分析。仿真結(jié)果表明，本文提出的改進(jìn)方法能夠滿足編隊(duì)導(dǎo)航狀態(tài)估計(jì)的要求。

參考文獻(xiàn)

[1] SHUSTER M D. Survey of attitude representations [J]. Journal of Astronautical Sciences， 1993， 41： 439?517.

[2] CHEON Y J， KIM J H. Unscented filtering in a unit quaternion space for spacecraft attitude estimation [C]// IEEE International Symposium on Industrial Electronics. Vigo， Spain： IEEE， 2007： 66?71.

[3] MARKLEY F L， CRASSIDIS J， CHENG Y. Nonlinear attitude filtering methods， AIAA 2005?5927 [R]. San Francisco， California， USA： AIAA， 2005.

[4] MORTENSEN R E. Strapdown guidance error analysis [J]. IEEE Transactions on Aerospace and Electroic Systems， 1974， 10（4）： 451?457.

[5] FOSBURY A M. Control and Kalman filtering for relative dynamics of a formation of uninhabited autonomous vehicles [D]. Buffalo， USA： State University of New York， 2006.

[6] 酒銳波，王彪，曹云峰.基于DSP的無人機(jī)編隊(duì)視頻跟蹤技術(shù)[J].現(xiàn)代電子技術(shù)，2012，35（6）：99?101.