[發明專利]一種基于優化的UKF算法的雙通道組合定姿算法的實現方法

有效

• 申請號: 201810256124.4
• 申請日: 2018-03-27
• 公開（公告）號: CN108519090B
• 公開（公告）日: 2021-08-20
• 發明（設計）人: 劉昊;黃成
• 申請（專利權）人: 東南大學—無錫集成電路技術研究所
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/00;G01C21/18;G01C21/16;G01C21/08
• 代理公司: 南京瑞弘專利商標事務所(普通合伙) 32249
• 代理人: 唐紹焜

1.一種基于優化的UKF算法的雙通道組合定姿算法的實現方法，其特征在于：包括步驟：

步驟1：用加速度計與磁力計分別與陀螺儀組成濾波系統；通過MEMS九軸傳感器采集九軸數據，并傳輸至主機端；

步驟2：主機端通過UKF算法分別對陀螺儀/加速度計通道及陀螺儀/磁力計通道進行姿態解算，分別得到定姿系統的橫滾角和俯仰角以及航向角；

具體流程為：

1)讀取k時刻陀螺儀、加速度計及磁力計數據；并計算得到k-1時刻的最優狀態量和誤差協方差矩陣；

2)根據陀螺儀/加速度計當前時刻的數值計算一步轉移矩陣，根據計算得到的一步轉移矩陣、步驟1)中得到的k-1時刻的最優狀態量計算k時刻的一步預測狀態量；

3)由步驟2)得到的一步轉移矩陣、步驟1)得到的k-1時刻的誤差協方差矩陣和系統狀態噪聲矩陣計算k時刻的預測誤差協方差矩陣；

4)對步驟3)得到的預測誤差協方差矩陣進行正定化檢測與處理；

5)對步驟2)得到的一步預測狀態量進行UT分解得到預測狀態量的Sigma點集和相應權重；

6)將步驟5)得到的預測狀態量的Sigma點集代入觀測方程解算出觀測量預測點集；其中，觀測方程根據具體的傳感器采用相應方程；

7)對步驟6)得到的觀測量預測點集按照權重求和得到觀測量預測點集的均值作為觀測量預測值；

8)由步驟6)得到的觀測量預測點集、步驟7)得到的觀測量預測值、由預測狀態量的Sigma點集、一步預測狀態量和系統觀測噪聲矩陣計算觀測預測值協方差矩陣和狀態預測值協方差矩陣；

9)由觀測預測值協方差矩陣和狀態預測值協方差矩陣計算卡爾曼增益矩陣；

10)由一步預測狀態量、步驟9)得到的卡爾曼增益矩陣、觀測量預測值和加速度計/磁力計表示的觀測量測量值計算k時刻最優狀態量；

11)根據系統預測誤差協方差矩陣、卡爾曼增益矩陣和觀測預測值協方差矩陣計算出k時刻的系統誤差協方差矩陣；

12)將k時刻的最優狀態量轉化為歐拉角作為系統k時刻的輸出；

步驟3：將步驟2得到的定姿系統的橫滾角和俯仰角以及航向角進行數據融合得到最終姿態。

2.根據權利要求1所述的實現方法，其特征在于：所述正定化處理為：對誤差協方差矩陣非負定性判定，如果負定，則改變UT變化中的參數κ。

3.根據權利要求1所述的實現方法，其特征在于：所述陀螺儀的姿態解算的狀態方程計算如下：

四元數與旋轉的關系表示為：

其中，Q是表示姿態的四元數，μ^R表示參考坐標系下的旋轉軸，θ表示是旋轉的角度，符號服從右手定則；

上式兩邊對時間求導，再展開成矩陣形式為：

其中是四元數對時間求導的結果，ω_x、ω_y、ω_z為載體在載體坐標系的三個軸上k時刻的角速度，是k時刻四元數符號；

采用龍哥庫塔算法解此微分方程得：

其中，表示t時刻的姿態，表示t+h時刻的姿態，h是采樣周期；

將上式作為UKF的狀態方程。

4.根據權利要求1所述的實現方法，其特征在于：對于加速度計和磁力計，計算姿態的模型表示為：

其中u^b是載體坐標系下的測量值，u^R是參考坐標系下的測量值，由于本系統的參考坐標系是固定的地理坐標系，所以u^R是常量；是由四元數組成的旋轉矩陣；

加速度姿態解算公式如下：

其中，a_x、a_y、a_z是加速度計在載體坐標系上的加速度歸一化的結果，V(k)是觀測噪聲，滿足E[v_k]＝0，Cov(v_kv_j)＝R_gδ_kj，該參數根據經驗和實際情況，初始化為一極小常量；

磁力計姿態解算公式如下：

其中，m_x、m_y、m_z是磁力計在載體坐標系上的磁場強度歸一化的結果；M_y、M_z是地理磁場在地理坐標系的y和z軸分量，由當地地理磁場分布決定。