基于四元數(shù)粒子濾波的飛行器姿態(tài)估計算法研究

喬相偉，周衛(wèi)東，吉宇人

(哈爾濱工程大學 自動化學院，黑龍江 哈爾濱150001)

0 引言

飛行器姿態(tài)確定系統(tǒng)是飛行器導航及姿態(tài)控制系統(tǒng)的重要組成部分，其精度直接影響到載體的姿態(tài)控制精度及導航定位精度。常用的姿態(tài)描述參數(shù)主要有方向余弦矩陣、歐拉角、姿態(tài)四元數(shù)(AQ)、旋轉矢量、羅德里格參數(shù)(RPs)、修正羅德里格參數(shù)(MRPs)、凱萊-哈密爾頓參數(shù)等［1］。其中姿態(tài)四元數(shù)因為計算量小、非奇異性、可全姿態(tài)工作而廣泛應用于實際系統(tǒng)當中。但由于四元數(shù)本質屬于旋轉矢量，其4 個變量不獨立，使得它必須滿足規(guī)范化的限制。如何克服四元數(shù)規(guī)范化限制及旋轉矢量方差計算問題，讓它與先進的濾波算法相結合成為國內外研究的熱點問題［2-3］。

作為一個典型的非線性系統(tǒng)，飛行器姿態(tài)確定的精度不僅取決于姿態(tài)測量系統(tǒng)硬件配置的性能與精度，還與所采用的姿態(tài)估計算法密切相關。在四元數(shù)和非線性濾波算法的結合中，最常用的算法就是基于四元數(shù)的擴展卡爾曼濾波(EKF)算法。其中文獻［4 -6］提出的乘性四元數(shù)EKF 算法因為利用乘性誤差四元數(shù)來表示估計四元數(shù)點與真實點之間的距離，解決了姿態(tài)四元數(shù)協(xié)方差計算問題，而得到了廣泛的應用。然而當系統(tǒng)非線性程度較強時，EKF 算法較高的截斷誤差將大大地降低濾波精度甚至會導致濾波發(fā)散。針對此，Vathsal 等［7］又給出了基于四元數(shù)的二階EKF 算法，使得濾波精度達到了泰勒展開的二階水平，但卻涉及到Hessian 矩陣的計算，使得計算負擔顯著增加。之后，Crassidis 等提出了基于MRPs 和AQ 相互轉換的飛行器姿態(tài)確定無跡卡爾曼濾波(UKF)的算法［8］，通過參數(shù)之間的相互轉換，避免了四元數(shù)歸一化問題;有效提高了姿態(tài)估計的精度。但是和EKF 一樣，UKF 算法也是基于高斯逼近的濾波算法，采用均值和方差來表征狀態(tài)的后驗分布。對于實際中的強非線性、非高斯問題，這種簡單的采用以均值和方差為特征的高斯逼近方法已不足以精確表示狀態(tài)的真實分布。而采用樣本形式對狀態(tài)概率密度進行描述的粒子濾波(PF)算法則完全克服了系統(tǒng)線性、高斯的假設，成為目前最適合于強非線性系統(tǒng)的濾波方法［9-10］?；诖?，Cheng 等［9］給出了基于MRPs 的姿態(tài)確定粒子濾波算法。事實上，影響四元數(shù)直接與粒子濾波結合主要存在2 個問題:1)粒子濾波中涉及到四元數(shù)的加權和運算，而四元數(shù)本質是旋轉矢量無法像普通向量那樣直接進行數(shù)學意義上的四則運算;2)正則化過程中，擾動粒子點的選取涉及到四元數(shù)協(xié)方差陣均方根的計算問題，即如何保證計算出來的擾動四元數(shù)點為規(guī)范化四元數(shù)。針對問題1)，文獻［11 -12］給出了構造姿態(tài)矩陣代價函數(shù)法將四元數(shù)均值問題轉化為代價函數(shù)最小時的四元數(shù)向量求解問題。在此基礎上，Barfoot 等［13］又提出了四元數(shù)加權求和的拉格朗日代價函數(shù)法，進一步降低了計算的復雜度，簡化了計算量。針對問題2)，本文通過對四元數(shù)協(xié)方差矩陣的跡進行研究，給出一種通過協(xié)方差陣的對角線元素開平方得到擾動四元數(shù)的方法，保證了擾動四元數(shù)的規(guī)范化。另外，為減少粒子濾波的計算量，文章將狀態(tài)向量分為非線性部分和線性部分，對非線性部分采用粒子濾波，線性部分采用卡爾曼濾波算法，進一步減小了計算量，一定程度地提高了系統(tǒng)的實時性。

綜上，本文給出了四元數(shù)粒子濾波的具體流程，針對大初始姿態(tài)誤差角的飛行器姿態(tài)確定仿真實驗表明，與基于四元數(shù)的EKF 算法和文獻［8］的UKF算法相比，該算法具有更高的估計精度和穩(wěn)定性。

1 飛行器姿態(tài)空間模型建立

1.1 姿態(tài)四元數(shù)微分方程

設四元數(shù)q 為一四維向量，定義如下:

式中:ρ≡［q1q2q3］T為向量部分;q0為實數(shù)部分。則四元數(shù)滿足如下的規(guī)范化限制

由導航坐標系(n 系)到載體坐標系(b 系)的姿態(tài)矩陣用四元數(shù)表示如下:

式中:I3×3為3 ×3 的單位矩陣;〈ρ ×〉為斜對稱矩陣，有

姿態(tài)四元數(shù)滿足如下微分方程［13］:

1.2 傳感器模型

本文采用光纖陀螺來提取角速度，其輸出模型可表示為

1.3 CCD 星敏感器量測模型建立

直接采用CCD 星敏感器的四元數(shù)輸出來測量陀螺的姿態(tài)，其模型如下:

式中:δq 對應的是星跟蹤器的四元數(shù)形式的量測噪聲，用歐拉角形式可表示為ηs，它是均值為零方差為σ2s的高斯白噪聲。

2 粒子濾波

假設非線性動態(tài)離散系統(tǒng)為

式中:xk∈Rn為k 時刻的n 維狀態(tài)向量;zk∈Rm為m 維觀測向量;wk和vk分別為過程噪聲和觀測噪聲。

粒子濾波算法步驟如下:

1)初始化:k =0 時，產生服從初始概率密度p(x0)的N 個粒子點

2)時間更新:從重要性密度函數(shù)q(xk|z1:k)進行重要性采樣，并且設置得到采樣粒子xik.一般重要性函數(shù)選取為q(xk|x0:k-1，y1:k)=p(xk|xk-1).

3)測量更新:在得到新的量測值zk后，計算粒子的權值

歸一化粒子權值為

4)狀態(tài)估計及經驗協(xié)方差計算:

5)重采樣:序貫粒子濾波中一個不可避免的問題就是粒子的退化問題，為降低粒子退化的影響，一個有效的方法就是引入重采樣。重采樣的基本思想就是刪除權值小的粒子，復制權值大的粒子。在重采樣開始之前首先要判斷粒子退化的程度，它可由下面方法來度量:

式中有效粒子數(shù)定義為

當Neff小于預定的門限值Nth時，進行重采樣。重采樣的過程只取決于歸一化后的權值，而與粒子的維數(shù)、大小無關。

6)正則化。重采樣雖然消除了小權值粒子在估計中的影響，但隨之而來帶來了粒子多樣性減小的問題，即粒子的貧化問題，為此需要引入粗化［15］或正則化重采樣步驟［16］。

3 四元數(shù)粒子濾波

3.1 四元數(shù)粒子加權求和計算問題

由文獻［12］可知，四元數(shù)加權求和計算問題最終都歸結為求代價函數(shù)的極值問題。由文獻［13］，當取四元數(shù)的向量部分為狀態(tài)變量時，其加權求和問題即為如下代價函數(shù)極值問題:

式中:qi為時間更新得到的四元數(shù);q 為待求的加權和四元數(shù)。

將誤差四元數(shù)的向量部分δρi用qi和q 表示為

從而(14)式的最小化問題可轉為

又待求的四元數(shù)滿足規(guī)范化限制，所以在這里采用拉格朗日乘法構造如下的代價函數(shù)為

式中λ 為拉格朗日乘性因子。

解(18)式可得

(19)式表示對應矩陣的特征值問題，其中要求的四元數(shù)就轉化為使(q)最小時的矩陣N 對應的最小特征向量。

3.2 四元數(shù)粒子濾波

濾波時取狀態(tài)變量為x =［xqxb］T，將狀態(tài)量分為四元數(shù)部分和陀螺漂移部分。四元數(shù)部分涉及到非線性運算采用粒子濾波方法，而陀螺漂移部分為線性運算，采用卡爾曼濾波算法。

3.2.1 時間更新

四元數(shù)部分時間更新:

當采用的重要性函數(shù)為q(xk|x0:(k-1)，y1:k)=p(xk|xk-1)時，由時間更新得到的四元數(shù)粒子點為

非四元數(shù)部分主要考慮陀螺漂移，其時間更新計算為

3.2.2 測量更新

當?shù)玫絢 時刻的測量輸出值qs，k及四元數(shù)粒子的量測預測輸出值qi，s，k|(k-1)時，k 時刻四元數(shù)權值更新為粒子后驗更新滿足

歸一化四元數(shù)權值

3.2.3 狀態(tài)更新

由于四元數(shù)為旋轉矢量，則四元數(shù)加權狀態(tài)更新用(19)式計算。對應地，利用乘性誤差四元數(shù)表示真實四元數(shù)點與估計點之間的距離。定義乘性四元數(shù)如下:

則對應的四元數(shù)誤差協(xié)方差矩陣為

由于真實四元數(shù)點無法得到，在這里用四元數(shù)重采樣點與估計四元數(shù)來表示四元數(shù)的誤差方差矩陣:

3.2.4 重采樣

四元數(shù)再采樣采用基本粒子濾波的方法進行，如(13)式所述。

3.2.5 正則化

重采樣雖然一定程度地避免了粒子的退化現(xiàn)象，但卻帶來了粒子貧化問題，針對此，本節(jié)主要討論旋轉四元數(shù)的正則化問題。在正則化過程中涉及到四元數(shù)方差的均方根計算問題。

四元數(shù)均方根可由(29)式求得

式中，qvari的4 個元素分別由()ii陣的4 個對角線元素開平方得到。

一般地，估計四元數(shù)與采樣四元數(shù)之間的距離誤差為小誤差，此時誤差四元數(shù)δq 可表示為

此時誤差四元數(shù)的方差矩陣可簡化為

這樣，其均方根就可以表示為

(36)式產生了一組對稱的Nk(i)個向量{δξ(j)}Nk(i)i=1.那么第i 個四元數(shù)粒子將被估計利用(37)式:

式中δq(j)=［δξT(j)1］T.

正則采樣獲得后代粒子之后，重新加權可以通過前面的(8)式進行，但為了使得到的估計粒子更加準確，在這里重新計算后驗粒子的似然度。即

式中c=pyk|qk(Yk|(k)).

從而，得到的更新四元數(shù)值用(20)式計算為

由于陀螺漂移沒有量測信息，所以陀螺漂移更新為

從而k 時刻用于計算的陀螺輸出為

4 仿真實驗

本文以軟件SINS/CCD 姿態(tài)估計系統(tǒng)為平臺，實驗初始條件設為:光纖陀螺測量白噪聲為0.02°/h，驅動白噪聲為0.002°/h，陀螺輸出采樣頻率為100 Hz，星敏感器采用2 個光軸垂直安裝，其輸出頻率為1 Hz，飛行器初始姿態(tài)誤差設定:橫滾角、俯仰角、偏航角分別為1°、5°、-30°.初始陀螺漂移在3 軸上分別為1°/h、1°/h、1°/h.星敏感器運動速率為0.05°/s，測量白噪聲標準差為20″.卡爾曼濾波器中的初始姿態(tài)和陀螺漂移估計值均設定為0，初始粒子數(shù)選為2 000 個。

根據(jù)以上條件分別采用EKF，UKF 和PF 算法進行仿真實驗。如圖1所示，當姿態(tài)誤差角為小角度時，3 種算法估計精度相當;如圖2～圖3所示，特別是圖3可看出，當初始姿態(tài)誤差角很大，非線性程度很強時，EKF 算法濾波精度明顯下降且有發(fā)散趨勢。而PF 算法因為對強非線性系統(tǒng)具有較好地逼近效果，濾波精度最高;從收斂速度看，PF 算法的收斂速度也和UKF 算法相當，這是由于本文采用的PF 算法中陀螺漂移部分進行了線性化處理，從而減少了粒子濾波的維數(shù)，使得計算量降低。

圖1 橫滾角姿態(tài)誤差對比Fig.1 Comparison of roll angle errors

圖2 俯仰角姿態(tài)誤差對比Fig.2 Comparison of pitch angle errors

圖3 偏航角姿態(tài)誤差對比Fig.3 Comparison of yaw angle errors

5 結論

針對飛行器非線性姿態(tài)確定問題，本文將四元數(shù)與粒子濾波相結合，給出了一種基于四元數(shù)的粒子濾波算法。針對四元數(shù)在粒子濾波應用中的規(guī)范化及協(xié)方差奇異性問題，考慮四元數(shù)為一旋轉矢量，解決了四元數(shù)的加權和計算、協(xié)方差奇異性及正則化擾動四元數(shù)求取等問題，擴展了粒子濾波的應用范圍。最后，利用SINS/CCD 組合姿態(tài)估計系統(tǒng)為平臺，進行了仿真實驗，實驗結果表明:當初始姿態(tài)誤差角為大角度時，本文的算法具有更好的估計精度和穩(wěn)定性。

References)

［1］ Markley F L.Attitude error representations for Kalman filtering［J］.Journal of Guidance，Control，and Dynamics，2003，26(2):311 -317.

［2］ Shuster M D.Constraint in attitude estimation part I:constrained estimation［J］.Journal of the Astronautical Sciences，2003，51(1):51 -74.

［3］ Choukroun D，Bar-Itzhack I Y，Oshman Y.A novel quaternion filter［C］∥AIAA Guidance，Navigation，and Control Conference.Monterey:CSA，2002，42(1):174 -190.

［4］ Murrell W.Precision attitude determination for multimission spacecraft［C］∥AIAA Guidance and Control Conference.New York:American Institute of Aeronautics and Astronautics，1978:70 -87.

［5］ Pittelkau M E.Rotation vector attitude estimation［J］.Journal of Guidance，Control，and Dynamics，2003，26(6):855 -860.

［6］ Psiaki M L.The super-iterated extended Kalman filter［J］.Journal of Guidance，Control，and Dynamics，2004，21(8):342 -347.

［7］ Vathsal S.Spacecraft attitude determination using a second-order nonlinear filter［J］.Journal of Guidance，Control，and Dynamics，1987，10(5):559 -566.

［8］ Crassidis J L，Markley F L.Unscented filtering for spacecraft attitude estimation［J］.Journal of Guidance，Control，and Dynamics，2007，26(4):536 -542.

［9］ Cheng Y，Crassidis J L.Particle filtering for sequential spacecraft attitude estimation［C］∥AIAA Guidance，Control，and Control Conference and Exhibit.Providence:American Institute of Aeronautics and Astronautics，2004:1 -18.

［10］ 梁軍.粒子濾波算法及其應用研究［D］.哈爾濱:哈爾濱工業(yè)大學，2009.LIANG Jun.Research on particle filter algorithm and its application［D］.Harbin:Harbin Institute of Technology，2009.(in Chinese)

［11］ Moakher M.Means and averaging in the group of rotations［J］.SIAM Journal on Matrix Analysis and Applications，2002，24(1):1 -16.

［12］ Markley F L，Cheng Y，Crassidis J L，et al.Averaging quaternions［J］.Journal of Guidance，Control，and Dynamics，2007，30(4):1193 -1196.

［13］ Barfoot T，F(xiàn)orbes J R，F(xiàn)urgale P T.Pose estimation using linearized rotations and quaternion algebra ［J］.Acta Astronautica，2010，49(6):1 -12.

［14］ 秦永元.慣性導航［M］.北京:科學出版社，2006:358-361.QIN Yong-yuan.Inertial navigation ［M］.Beijing:Science Press，2006:358 -361.(in Chinese)

［15］ Gordon N J，Salmond D J.Novel approach to nonlinear/non-Gaussian Bayesian state estimation ［J］.IEEE Proceedings on Radar and Signal Processing，1993，140(2):107 -113.

［16］ Doucet A，F(xiàn)reitas N D，Gordon N.Sequential Monte Carlo methods in practice［M］.New York:Springer，2001.