[發明專利]一種基于二階量測更新的低成本姿態估計方法

有效

• 申請號: 201410047878.0
• 申請日: 2014-02-11
• 公開（公告）號: CN103822633A
• 公開（公告）日: 2014-05-28
• 發明（設計）人: 周廣濤;孫艷濤;姜鑫;梁宏;時貴敏;趙博;郝勤順;李佳璇;林萌萌;張麗麗
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

技術領域

本發明涉及的是一種適用于利用低成本的傳感器，對車輛、船舶、無人機載體等進行姿態估計地方法。具體地說是一種基于二階量測更新的低成本姿態估計方法。

背景技術

在很多工程應用領域中都需要進行姿態估計，例如，機動追蹤、無人機和生物應用等方面。如果給定載體初始狀態，則可以通過積分陀螺儀計算出估計姿態值。它的估計誤差主要由陀螺漂移和算法誤差組成。此外，也可以通過加速度計和磁力計的輸出計算出姿態值。在這種情況下，由于外部加速度和磁干擾，估計誤差可能會變大。因此每種姿態估計算法的基本問題就是如何將陀螺儀、加速度計和磁力計結合起來使用。因此，用微慣性傳感器和磁傳感器來估計姿態是近年來的流行方法之一。

目前，大多數的姿態估計中觀測量都會選取地球磁場，利用磁強計或羅經等設備輸出磁力信息。其中磁強計的輸出不僅能提供航向信息同時也包括縱搖和橫搖的一些信息。但它有一個嚴重的缺點就是縱搖角和橫搖角受磁干擾影響，由于室內存在較強的磁干擾，因此在室內應用該種算法時縱搖誤差和橫搖誤差都有較大的變化。所以較為明智的選擇就是利用磁強計輸出數據只計算航向角，因此這不適用于傳統的四元數間接卡爾曼濾波。

發明內容

本發明的目的在于提供一種通過測量微慣性測量單元和磁強計輸出的載體三個軸的角速率、線加速度和磁力數據，利用明改進的四元數卡爾曼濾波處理，得到載體的精確姿態信息的基于二階量測更新的低成本姿態估計方法。

本發明的目的是這樣實現的：

(1)在k=0時刻，即初始時刻由初始對準得到初始姿態φ₀，所述初始姿態包括橫搖、縱搖和航向；

(2)在k≥1時刻，利用微機械陀螺輸出的載體三軸的角速率數據，計算出姿態角的變化值Δφ，由式計算出濾波開始前的粗略姿態角；

(3)采集微機械加速度計和磁力計輸出的載體三軸線的加速度和磁力數據；

(4)設濾波的狀態變量為x=[q_e?b_g?b_a]^T∈R^9×1；式中：q_e表示四元數誤差矢量部分，b_g表示陀螺漂移，b_a表示加速度計零偏；假設四元數誤差q~e=1qeT,]]>根據四元數相關性質建立狀態方程：