一種陀螺儀數(shù)據(jù)融合方法
【專利摘要】本發(fā)明公開了一種陀螺儀數(shù)據(jù)融合方法，包括以下步驟：A、在陀螺儀設(shè)備開啟時(shí)，利用靜止?fàn)顟B(tài)下加速度計(jì)輸出的三個(gè)軸矢量，計(jì)算三個(gè)軸矢量和的模值；B、在運(yùn)動(dòng)狀態(tài)下加速度計(jì)輸出的三個(gè)軸矢量，計(jì)算三個(gè)軸矢量和的模值；C、利用上一時(shí)刻的姿態(tài)角、根據(jù)加速度計(jì)的輸出得到的姿態(tài)角、根據(jù)角速度計(jì)得到的特定軸的角速度、上一時(shí)刻與當(dāng)前時(shí)刻的間隔時(shí)間以及數(shù)據(jù)融合參考系數(shù)，計(jì)算當(dāng)前的姿態(tài)角。本發(fā)明通過在數(shù)據(jù)融合過程中引入數(shù)據(jù)融合參考系數(shù)，在不增加計(jì)算復(fù)雜度的情況下實(shí)現(xiàn)高精度數(shù)據(jù)融合，同時(shí)解決了現(xiàn)有技術(shù)在設(shè)備存在加速度的情況下不能進(jìn)行姿態(tài)定位的問題。本發(fā)明作為一種陀螺儀數(shù)據(jù)融合方法可廣泛應(yīng)用于陀螺儀【技術(shù)領(lǐng)域】。
【專利說明】一種陀螺儀數(shù)據(jù)融合方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及陀螺儀【技術(shù)領(lǐng)域】，尤其是一種陀螺儀數(shù)據(jù)融合方法。

【背景技術(shù)】
[0002] 現(xiàn)有設(shè)備中采用陀螺儀來做智能控制，大多設(shè)備采用雙軸或三軸陀螺儀來控制設(shè) 備的姿態(tài)角，由于加速度計(jì)在靜止時(shí)數(shù)據(jù)是精確的，但是當(dāng)設(shè)備運(yùn)動(dòng)起來后，數(shù)據(jù)就不再精 確了，角加速度計(jì)計(jì)算物體的旋轉(zhuǎn)運(yùn)動(dòng)，至此只用一軸的數(shù)據(jù)，無論是單獨(dú)利用加速度計(jì)還 是單獨(dú)利用角加速度計(jì)都無法得到設(shè)備的角度，而且由于陀螺儀本身的精度和制造工藝等 問題，使得由硬件直接輸出的數(shù)據(jù)具有漂移和累積誤差，還有數(shù)據(jù)耦合過程中也會(huì)存在誤 差，因此輸出的角度數(shù)據(jù)無法長時(shí)間保持高精度，因此利用陀螺儀控制設(shè)備的技術(shù)難點(diǎn)在 于陀螺儀數(shù)據(jù)的融合運(yùn)算。
[0003] 當(dāng)前主要有以下幾種陀螺儀數(shù)據(jù)融合的方法，基于四元數(shù)融合法，基于卡爾曼算 法的數(shù)據(jù)融合，基于互補(bǔ)濾波的方式進(jìn)行數(shù)據(jù)融合。雖然基于卡爾曼算法的數(shù)據(jù)融合的精 度較高，但是計(jì)算量巨大，過程復(fù)雜，基于四元數(shù)法計(jì)算量比卡爾曼算法小一些，互補(bǔ)濾波 計(jì)算量最小但是精度要低一些，而且目前上面的這些融合算法都是在設(shè)備沒有做加速運(yùn)動(dòng) 的情況，在加速運(yùn)動(dòng)的情況下，加速度計(jì)的數(shù)據(jù)量融合了重力還有設(shè)備本身的加速度，無法 再利用它進(jìn)行姿態(tài)定位。


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

[0004] 為了解決上述技術(shù)問題，本發(fā)明的目的是：提供一種適用于設(shè)備存在加速度的陀 螺儀數(shù)據(jù)融合方法。
[0005] 本發(fā)明所采用的技術(shù)方案是：一種陀螺儀數(shù)據(jù)融合方法，包括有以下步驟： A、 在陀螺儀設(shè)備開啟時(shí)，利用靜止?fàn)顟B(tài)下加速度計(jì)輸出的三個(gè)軸矢量，計(jì)算三個(gè)軸矢 量和的模值； B、 在運(yùn)動(dòng)狀態(tài)下加速度計(jì)輸出的三個(gè)軸矢量，計(jì)算三個(gè)軸矢量和的模值； C、 利用上一時(shí)刻的姿態(tài)角、根據(jù)加速度計(jì)的輸出得到的姿態(tài)角、根據(jù)角速度計(jì)得到的 特定軸的角速度、上一時(shí)刻與當(dāng)前時(shí)刻的間隔時(shí)間以及數(shù)據(jù)融合參考系數(shù)，計(jì)算當(dāng)前的姿 態(tài)角。
[0006] 進(jìn)一步，所述步驟A中對(duì)三個(gè)軸矢量進(jìn)行零值偏移誤差補(bǔ)償后再計(jì)算三個(gè)軸矢量 和的模值。
[0007] 進(jìn)一步，所述數(shù)據(jù)融合參考系數(shù)根據(jù)上述步驟A、步驟B中的模值計(jì)算所得。
[0008] 進(jìn)一步，所述步驟C中當(dāng)前姿態(tài)角的計(jì)算公式為：

【權(quán)利要求】
1. 一種陀螺儀數(shù)據(jù)融合方法，其特征在于：包括有以下步驟： A、 在陀螺儀設(shè)備開啟時(shí)，利用靜止?fàn)顟B(tài)下加速度計(jì)輸出的三個(gè)軸矢量，計(jì)算三個(gè)軸矢 量和的模值； B、 在運(yùn)動(dòng)狀態(tài)下加速度計(jì)輸出的三個(gè)軸矢量，計(jì)算三個(gè)軸矢量和的模值； C、 利用上一時(shí)刻的姿態(tài)角、根據(jù)加速度計(jì)的輸出得到的姿態(tài)角、根據(jù)角速度計(jì)得到的 特定軸的角速度、上一時(shí)刻與當(dāng)前時(shí)刻的間隔時(shí)間以及數(shù)據(jù)融合參考系數(shù)，計(jì)算當(dāng)前的姿 態(tài)角。
2. 根據(jù)權(quán)利要求1所述的一種陀螺儀數(shù)據(jù)融合方法，其特征在于：所述步驟A中對(duì)三 個(gè)軸矢量進(jìn)行零值偏移誤差補(bǔ)償后再計(jì)算三個(gè)軸矢量和的模值。
3. 根據(jù)權(quán)利要求1所述的一種陀螺儀數(shù)據(jù)融合方法，其特征在于：所述數(shù)據(jù)融合參考 系數(shù)根據(jù)上述步驟A、步驟B中的模值計(jì)算所得。
4. 根據(jù)權(quán)利要求1所述的一種陀螺儀數(shù)據(jù)融合方法，其特征在于：所述步驟C中當(dāng)前 姿態(tài)角的計(jì)算公式為：
其中angles為上一時(shí)刻的姿態(tài)角，angle_accelerate為根據(jù)加速度計(jì)的輸出得到的 姿態(tài)角，angle_gyro_rate為根據(jù)角速度計(jì)得到的特定軸的角速度，T為上一時(shí)刻與當(dāng)前時(shí) 刻的間隔時(shí)間，K為數(shù)據(jù)融合參考系數(shù)，anglet為當(dāng)前的姿態(tài)角。
5. 根據(jù)權(quán)利要求4所述的一種陀螺儀數(shù)據(jù)融合方法，其特征在于：所述數(shù)據(jù)融合參考 系數(shù)K的計(jì)算公式為：
其中G為所述步驟A中的模值，G_為所述步驟B中的模值，C和D為常數(shù)因子。
6. 根據(jù)權(quán)利要求5所述的一種陀螺儀數(shù)據(jù)融合方法，其特征在于：對(duì)于不同取值范圍 的IG1-Gj,設(shè)置不同值的常數(shù)因子C和常數(shù)因子D。
【文檔編號(hào)】G01C19/02GK104236533SQ201410476399
【公開日】2014年12月24日 申請(qǐng)日期:2014年9月17日 優(yōu)先權(quán)日:2014年9月17日
【發(fā)明者】蓋增園, 王旭耀, 林羽凡, 李建奇, 雷波, 曾德祥 申請(qǐng)人:廣州博冠光電技術(shù)有限公司