一種基于mems慣性傳感器組的多旋翼飛行器姿態(tài)檢測方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及的是飛行器控制過程中，通過純慣性傳感器來檢測多旋翼飛行器飛行 姿態(tài)的算法。屬于慣性導(dǎo)航技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 飛行器的姿態(tài)檢測對于飛行器的控制是極其關(guān)鍵的，也是影響慣性導(dǎo)航系統(tǒng)精度 的最主要因素。而由于MEMS工藝的慣性傳感器精度較低，使得傳統(tǒng)的姿態(tài)檢測方法無法直 接嫁接在其之上，需結(jié)合MEMS傳感器的自身特性，以及飛行器的工作狀態(tài)研宄相應(yīng)的姿態(tài) 檢測方法。
[0003] 目前，對于基于MEMS慣性傳感器組的姿態(tài)檢測方法，主要是通過卡爾曼濾波算法 對陀螺儀傳感器和加速度傳感器進(jìn)行數(shù)據(jù)融合，從而檢測出物體姿態(tài)信息。然后對于多旋 翼飛行器，該方法并沒有考慮到慣性傳感器組在飛行器工作過程中所引入的干擾誤差，從 而導(dǎo)致該方法在飛行器加速減速過程中，姿態(tài)檢測精度大幅下降。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明所要解決的技術(shù)問題是針對上述缺陷，提出了一種對干擾誤差進(jìn)行處理從 而提高飛行器姿態(tài)檢測精度的基于MEMS慣性傳感器組的多旋翼飛行器姿態(tài)檢測方法。
[0005] 為解決上述技術(shù)問題，本發(fā)明采用的技術(shù)方案是：
[0006] 一種基于MEMS慣性傳感器組的多旋翼飛行器姿態(tài)檢測方法，其特征在于：通過陀 螺儀傳感器獲取先驗估計姿態(tài)四元數(shù)；通過加速度傳感器的數(shù)據(jù)估計噪聲強(qiáng)度并優(yōu)化卡爾 曼濾波器的增益系數(shù)；通過優(yōu)化后的卡爾曼濾波器融合先驗估計姿態(tài)四元數(shù)和加速度傳感 器數(shù)據(jù)計算后驗姿態(tài)四元數(shù)。
[0007] 在通過陀螺儀傳感器獲取先驗估計姿態(tài)四元數(shù)的過程中，采用如下方法進(jìn)行計 算：
【主權(quán)項】
1. 一種基于MEMS慣性傳感器組的多旋翼飛行器姿態(tài)檢測方法，其特征在于：通過陀螺 儀傳感器獲取先驗估計姿態(tài)四元數(shù)；通過加速度傳感器的數(shù)據(jù)估計噪聲強(qiáng)度并優(yōu)化卡爾曼 濾波器的增益系數(shù)；通過優(yōu)化后的卡爾曼濾波器融合先驗估計姿態(tài)四元數(shù)和加速度傳感器 數(shù)據(jù)計算后驗姿態(tài)四元數(shù)。
2. 如權(quán)利要求1所述的基于慣性傳感器組的多旋翼飛行器姿態(tài)檢測方法，其特征在 于：在通過陀螺儀傳感器獲取先驗估計姿態(tài)四元數(shù)的過程中，采用如下方法進(jìn)行計算：
其中I為單位矩陣，A 0為陀螺儀在[tg，tk]采樣時間間隔內(nèi)的角增量 陣，A 0 x、A 0 y、A 0 2為x、y、z陀螺儀在[th，tk]采樣時間間隔內(nèi)的角增量， AM =A0X2+Ae彡+A0Z2, Q(I1)為上一時刻的后驗估計姿態(tài)四元數(shù)，Q(tk)為當(dāng)前時刻的 先驗估計姿態(tài)四元數(shù)。
3. 如權(quán)利要求2所述的基于MEMS慣性傳感器組的多旋翼飛行器姿態(tài)檢測方法，其特征 在于：在通過加速度傳感器估計噪聲強(qiáng)度并優(yōu)化卡爾曼濾波器的增益系數(shù)的過程中，采用 如下步驟： (1) 將加速度傳感器的觀測方程線性化，得到雅可比行列式Hk:
其中qp q2,。3為先驗估計姿態(tài)四元數(shù)Q(t k)的元素； (2) 計算卡爾曼濾波器參數(shù)：狀態(tài)量Xk，量測量Zk，系統(tǒng)方差陣Qk， 量測量方差陣Rk，一步姿態(tài)預(yù)測矩陣
其中qk為先驗估計姿態(tài)四元數(shù)，a k為加速度傳感器數(shù)據(jù)，Q "為 陀螺儀傳感器的測量誤差方差陣，&為加速度傳感器的測量誤差方 差陣； (3) 計算卡爾曼濾波算法中的一步預(yù)測均方誤差Pk/k_1:
其中Ph為k-1時刻的估計均方誤差； (4) 通過UDUt分解卡爾曼濾波器中的一步均方誤差P k/k_1:
其中Uk/k_$ UDU T分解后的上三角矩陣，D k/k_$ UDU T分 解后的對角矩陣，為UDUt分解后的下三角矩陣，F(xiàn) k， Gk，Wk均為后續(xù)計算需要用到的中間變量； (5) 計算有害加速度的方差矩陣<^^: (51)計算加速度計的估計誤差矩陣AZk: AZk=Zk-CkXk 其中Ck為k時刻的先驗姿態(tài)四元數(shù)轉(zhuǎn)換成的方向余弦矩陣：
其中ra，k_$ A Zk矩陣中的第i行列向量，M為列向量的個數(shù)； (53)求取外界有害加速度的方差矩陣
其中X i k為矩陣Uk的特征值，u i;k為對應(yīng)的特征向量， Ma + (6)通過步驟（5)中的有害加速 度方差矩陣優(yōu)化卡爾曼濾波的增益陣Kk: Kk =Gk(Wk +Qabji)-1 其中Gk，Wk為一步預(yù)測均方誤差P k/H進(jìn)行UDU T分解時的中間 變量。
4. 如權(quán)利要求3所述的基于MEMS慣性傳感器組的多旋翼飛行器姿態(tài)檢測方法，其特征
y 取 〇? 1〇
5. 如權(quán)利要求3所述的基于MEMS慣性傳感器組的多旋翼飛行器姿態(tài)檢測方法，其特征 在于：在通過優(yōu)化后的卡爾曼濾波器融合先驗估計姿態(tài)四元數(shù)和加速度傳感器數(shù)據(jù)計算后 驗姿態(tài)四元數(shù)過程中，采用如下步驟： (1)通過優(yōu)化后的增益矩陣Kk和加速度傳感器的數(shù)據(jù)Zk計算后 驗估計姿態(tài)四元數(shù)戈fc:
其中為Affc估計誤差四元數(shù)^即為后驗姿態(tài)四元數(shù)； (2)根據(jù)后驗估計姿態(tài)四元數(shù)估計均方誤差Pk: Pk - P k/k-l_KkHkPk/k-1 Pk作為k+1時刻的估計均方誤差P k+l/k繼續(xù)姿態(tài)檢測計算。
【專利摘要】本發(fā)明公開了一種基于MEMS慣性傳感器組的多旋翼飛行器姿態(tài)檢測方法。本方法根據(jù)多旋翼飛行器工作過程中，慣性傳感器組所引入的噪聲干擾，動態(tài)調(diào)節(jié)陀螺儀與加速度計的增益系數(shù)，彌補(bǔ)了傳統(tǒng)的姿態(tài)檢測方法在加速度計受到干擾時精度下降的缺陷。步驟為：先驗估計姿態(tài)四元數(shù)的獲取，干擾噪聲強(qiáng)度估計，改變卡爾曼濾波算法中的增益系數(shù)，計算后驗估計姿態(tài)四元數(shù)。優(yōu)點(diǎn)：根據(jù)噪聲改變卡爾曼濾波中的增益系數(shù)能夠很好的抑制慣性傳感器組所引入的噪聲干擾，提高多旋翼飛行器工作時的姿態(tài)檢測精度。
【IPC分類】G01C21-16, G01C21-20
【公開號】CN104819717
【申請?zhí)枴緾N201510259903
【發(fā)明人】彭輝波, 姜天宇
【申請人】蘇州聯(lián)芯威電子有限公司
【公開日】2015年8月5日
【申請日】2015年5月20日