一種基于mems慣性傳感器組的多旋翼飛行器姿態檢測方法
【技術領域】
[0001] 本發明涉及的是飛行器控制過程中，通過純慣性傳感器來檢測多旋翼飛行器飛行 姿態的算法。屬于慣性導航技術領域。
【背景技術】
[0002] 飛行器的姿態檢測對于飛行器的控制是極其關鍵的，也是影響慣性導航系統精度 的最主要因素。而由于MEMS工藝的慣性傳感器精度較低，使得傳統的姿態檢測方法無法直 接嫁接在其之上，需結合MEMS傳感器的自身特性，以及飛行器的工作狀態研宄相應的姿態 檢測方法。
[0003] 目前，對于基于MEMS慣性傳感器組的姿態檢測方法，主要是通過卡爾曼濾波算法 對陀螺儀傳感器和加速度傳感器進行數據融合，從而檢測出物體姿態信息。然后對于多旋 翼飛行器，該方法并沒有考慮到慣性傳感器組在飛行器工作過程中所引入的干擾誤差，從 而導致該方法在飛行器加速減速過程中，姿態檢測精度大幅下降。
【發明內容】
[0004] 本發明所要解決的技術問題是針對上述缺陷，提出了一種對干擾誤差進行處理從 而提高飛行器姿態檢測精度的基于MEMS慣性傳感器組的多旋翼飛行器姿態檢測方法。
[0005] 為解決上述技術問題，本發明采用的技術方案是：
[0006] 一種基于MEMS慣性傳感器組的多旋翼飛行器姿態檢測方法，其特征在于：通過陀 螺儀傳感器獲取先驗估計姿態四元數；通過加速度傳感器的數據估計噪聲強度并優化卡爾 曼濾波器的增益系數；通過優化后的卡爾曼濾波器融合先驗估計姿態四元數和加速度傳感 器數據計算后驗姿態四元數。
[0007] 在通過陀螺儀傳感器獲取先驗估計姿態四元數的過程中，采用如下方法進行計 算：
【主權項】
1. 一種基于MEMS慣性傳感器組的多旋翼飛行器姿態檢測方法，其特征在于：通過陀螺 儀傳感器獲取先驗估計姿態四元數；通過加速度傳感器的數據估計噪聲強度并優化卡爾曼 濾波器的增益系數；通過優化后的卡爾曼濾波器融合先驗估計姿態四元數和加速度傳感器 數據計算后驗姿態四元數。
2. 如權利要求1所述的基于慣性傳感器組的多旋翼飛行器姿態檢測方法，其特征在 于：在通過陀螺儀傳感器獲取先驗估計姿態四元數的過程中，采用如下方法進行計算：
其中I為單位矩陣，A 0為陀螺儀在[tg，tk]采樣時間間隔內的角增量 陣，A 0 x、A 0 y、A 0 2為x、y、z陀螺儀在[th，tk]采樣時間間隔內的角增量， AM =A0X2+Ae彡+A0Z2, Q(I1)為上一時刻的后驗估計姿態四元數，Q(tk)為當前時刻的 先驗估計姿態四元數。
3. 如權利要求2所述的基于MEMS慣性傳感器組的多旋翼飛行器姿態檢測方法，其特征 在于：在通過加速度傳感器估計噪聲強度并優化卡爾曼濾波器的增益系數的過程中，采用 如下步驟： (1) 將加速度傳感器的觀測方程線性化，得到雅可比行列式Hk:
其中qp q2,。3為先驗估計姿態四元數Q(t k)的元素； (2) 計算卡爾曼濾波器參數：狀態量Xk，量測量Zk，系統方差陣Qk， 量測量方差陣Rk，一步姿態預測矩陣
其中qk為先驗估計姿態四元數，a k為加速度傳感器數據，Q "為 陀螺儀傳感器的測量誤差方差陣，&為加速度傳感器的測量誤差方 差陣； (3) 計算卡爾曼濾波算法中的一步預測均方誤差Pk/k_1:
其中Ph為k-1時刻的估計均方誤差； (4) 通過UDUt分解卡爾曼濾波器中的一步均方誤差P k/k_1:
其中Uk/k_$ UDU T分解后的上三角矩陣，D k/k_$ UDU T分 解后的對角矩陣，為UDUt分解后的下三角矩陣，F k， Gk，Wk均為后續計算需要用到的中間變量； (5) 計算有害加速度的方差矩陣<^^: (51)計算加速度計的估計誤差矩陣AZk: AZk=Zk-CkXk 其中Ck為k時刻的先驗姿態四元數轉換成的方向余弦矩陣：
其中ra，k_$ A Zk矩陣中的第i行列向量，M為列向量的個數； (53)求取外界有害加速度的方差矩陣
其中X i k為矩陣Uk的特征值，u i;k為對應的特征向量， Ma + (6)通過步驟（5)中的有害加速 度方差矩陣優化卡爾曼濾波的增益陣Kk: Kk =Gk(Wk +Qabji)-1 其中Gk，Wk為一步預測均方誤差P k/H進行UDU T分解時的中間 變量。
4. 如權利要求3所述的基于MEMS慣性傳感器組的多旋翼飛行器姿態檢測方法，其特征
y 取 〇? 1〇
5. 如權利要求3所述的基于MEMS慣性傳感器組的多旋翼飛行器姿態檢測方法，其特征 在于：在通過優化后的卡爾曼濾波器融合先驗估計姿態四元數和加速度傳感器數據計算后 驗姿態四元數過程中，采用如下步驟： (1)通過優化后的增益矩陣Kk和加速度傳感器的數據Zk計算后 驗估計姿態四元數戈fc:
其中為Affc估計誤差四元數^即為后驗姿態四元數； (2)根據后驗估計姿態四元數估計均方誤差Pk: Pk - P k/k-l_KkHkPk/k-1 Pk作為k+1時刻的估計均方誤差P k+l/k繼續姿態檢測計算。
【專利摘要】本發明公開了一種基于MEMS慣性傳感器組的多旋翼飛行器姿態檢測方法。本方法根據多旋翼飛行器工作過程中，慣性傳感器組所引入的噪聲干擾，動態調節陀螺儀與加速度計的增益系數，彌補了傳統的姿態檢測方法在加速度計受到干擾時精度下降的缺陷。步驟為：先驗估計姿態四元數的獲取，干擾噪聲強度估計，改變卡爾曼濾波算法中的增益系數，計算后驗估計姿態四元數。優點：根據噪聲改變卡爾曼濾波中的增益系數能夠很好的抑制慣性傳感器組所引入的噪聲干擾，提高多旋翼飛行器工作時的姿態檢測精度。
【IPC分類】G01C21-16, G01C21-20
【公開號】CN104819717
【申請號】CN201510259903
【發明人】彭輝波, 姜天宇
【申請人】蘇州聯芯威電子有限公司
【公開日】2015年8月5日
【申請日】2015年5月20日