[發明專利]一種基于誤差四元數三維矢量分布的高斯粒子濾波數據處理方法

有效

• 申請號: 202010950221.0
• 申請日: 2020-09-10
• 公開（公告）號: CN112066993B
• 公開（公告）日: 2022-12-13
• 發明（設計）人: 周翟和;曾傳偉;鄒克臣;賴際舟;姚睿;曾慶喜
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/10;G01C21/00;G06F17/16
• 代理公司: 暫無信息
• 代理人: 暫無信息

本發明提出的一種基于誤差四元數三維矢量分布的高斯粒子濾波數據處理方法，屬于數字濾波和多傳感器數據融合技術領域，主要用于解決粒子濾波器在姿態估計中存在龐大的計算負擔問題，該方法以高斯粒子濾波為框架，融合陀螺儀、加速度計及磁力計數據，將誤差四元數三維矢量在三維歐式空間上的高斯分布作為姿態估計誤差的后驗分布。本發明將四元數作為全局姿態參數，乘性誤差四元數作為局部姿態誤差描述的一種間接濾波方法，既可以保證濾波器中四元數的歸一化，又可以實現濾波器的序貫估計，并且可實現并行計算加快計算速度，濾波精度穩定，適用于姿態估計、數據融合等應用場合。

技術領域

本發明為了解決粒子濾波算法在飛行器姿態估計中存在龐大的計算負擔問題，提出了一種以高斯粒子濾波為框架的飛行器間接姿態估計算法，屬于數據處理和多傳感器數據融合技術領域，適用于姿態估計、數據融合等應用場合。

背景技術

對于姿態估計，使用序貫矢量測量在微型飛行器導航系統中已經得到了深入的研究。目前姿態估計的成熟方案是基于陀螺儀、加速度計及磁力計的矢量觀測來確定飛機的姿態。四元數作為全局非奇異最小維數姿態描述形式，在姿態估計問題中得到了廣泛的應用。近幾十年來，人們已經進行了大量研究，使用各種新算法來提高估計精度。針對強非線性系統，借助于乘性姿態誤差描述的思想，J.L.Crassidis和Cheng分別提出了無跡卡爾曼濾波器(USQUE)和粒子濾波器(PF)，將四元數和修正羅德里格斯參數(MRPs)相結合，把誤差四元數的四參數轉化為修正羅德里格斯三參數，利用MRPs計算均值和協方差，保持了四元數規范化。但這種相互轉化增加了計算負擔和數值舍入誤差，降低了濾波速度；而且USQUE使用固定參數的高斯分布逼近真實分布，因此對隨機過程的統計分布非常敏感。當估計多個參數時，PF容易面臨維數災難而導致巨大的計算量。

四元數粒子濾波器(QPF)以四元數作為狀態量直接對姿態進行估計，無需狀態切換。雖然QPF對非線性、非高斯問題比其他算法更加逼近真實值，但是為克服粒子退化和樣本匱乏的缺陷，QPF需要對粒子進行重采樣進而導致隨著粒子數的增加而增大了計算負擔。近幾十年，各學者對粒子濾波的缺陷做了很多改進，盡管效果變好，但計算變得更加復雜。

發明內容

在本發明中，提出一種將誤差四元數的三維矢量定義為狀態量來描述局部姿態誤差，四元數作為全局姿態參數的間接高斯粒子濾波器(Indirect Gaussian particlefilter，IGPF)。所提出的濾波器以誤差四元數三維矢量在三維歐式空間上的高斯分布作為姿態估計誤差的后驗分布。將三維空間上的姿態估計問題轉化為其切空間上(歐里幾德空間)姿態誤差的估計問題。該濾波器通過并行計算和線性變換加快了計算速度，比擴展卡爾曼濾波(EKF)具有更快的收斂速度，并且比已知的四元數粒子濾波器(QPF)具有更低的復雜度和更少的計算時間。

本發明具體過程如下：

步驟1：將誤差四元數δq的三維矢量δρ定義為狀態量并利用陀螺儀輸出建立系統狀態方程；將加速度計及磁力計的輸出作為觀測量建立系統觀測方程；

步驟2：通過加速度計和磁力計的初始觀測量獲得初始四元數粒子集對系統進行初始化；其中，i表示粒子個數索引，M表示粒子數，上標+表示后驗，下標0表示初始時刻；

步驟3：已知k時刻狀態量的后驗分布，從中采樣獲得后驗誤差四元數三維矢量粒子集通過k時刻最優四元數估計值叉乘誤差四元數粒子集來生成后驗四元數粒子集其中，下標k表示k時刻；

步驟4：根據步驟3得到的后驗四元數粒子集利用步驟1中系統狀態方程得到更新后的四元數粒子集其中，上標-表示先驗含義，下標k+1表示k+1時刻；

步驟5：根據步驟1中系統觀測方程對步驟4得到的四元數粒子集進行量測更新得到k+1時刻權值

步驟6：計算k+1時刻最優四元數估計值并根據最優四元數估計值計算姿態角；其中，姿態角包括俯仰角θ、橫滾角γ、航向角ψ；