一種基于mems的高精度三維姿態(tài)慣性測量系統(tǒng)以及測量方法
【專利摘要】一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)以及測量方法,涉及一種基于MEMS的高精度三維姿態(tài)慣性測量方法。本發(fā)明是為了解決現(xiàn)有的三維姿態(tài)慣性測量設(shè)備采用傳感器成本高、精度低的問題。三軸陀螺儀計傳感器用于將測量的角速度數(shù)據(jù)發(fā)送給ARM處理器,三軸加速度計傳感器用于將測量的加速度數(shù)據(jù)發(fā)送給ARM處理器,三軸磁強計傳感器用于將測量的磁強數(shù)據(jù)發(fā)送給ARM處理器,溫度傳感器用于測量將獲得的三軸陀螺儀計傳感器溫漂數(shù)據(jù)發(fā)送給ARM處理器,ARM處理器用來對接收到的數(shù)據(jù)分別進行前置低通數(shù)字濾波、前端數(shù)據(jù)處理和拓展卡爾曼濾波處理,獲得歐拉角三維姿態(tài)慣性數(shù)據(jù)或者四元數(shù)三維姿態(tài)慣性數(shù)據(jù)。它可用于導(dǎo)航控制領(lǐng)域。
【專利說明】一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)以及測量方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種空間三維姿態(tài)測量方法,特別是涉及一種基于MEMS的高精度三維姿態(tài)慣性測量方法。
【背景技術(shù)】
[0002]無人機、機器人、機械云臺、車輛船舶、虛擬現(xiàn)實、人體運動分析等在近年都取得了迅猛的發(fā)展。而在這些應(yīng)用中,三維姿態(tài)與方位的自主測量顯得極為重要?,F(xiàn)有的三維姿態(tài)與方位傳感器中,MEMS傳感器成本低廉,但是測量數(shù)據(jù)由于存在較大的零偏、溫漂等誤差較大不能滿足以上應(yīng)用對于精度的要求;而一些MU模塊,在精度上能滿足要求,但是成本高昂,限制了在實際中的應(yīng)用。
【發(fā)明內(nèi)容】
[0003]本發(fā)明是為了解決現(xiàn)有的三維姿態(tài)慣性測量設(shè)備采用傳感器成本高、精度低的問題?,F(xiàn)提供一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)以及該測量方法。
[0004]一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng),它包括微機電式慣性測量器件,它還包括ARM處理器和溫度傳感器,
[0005]所述的微機電式慣性測量器件包括三軸陀螺儀計傳感器、三軸加速度計傳感器和三軸磁強計傳感器,
[0006]三軸陀螺儀計傳感器,用于測量獲得角速度,并將測量獲得的角速度數(shù)據(jù)發(fā)送給ARM處理器,
[0007]三軸加速度計傳感器,用于測量獲得加速度,并將測量獲得的加速度數(shù)據(jù)發(fā)送給ARM處理器,
[0008]三軸磁強計傳感器,用于測量獲得磁強,并將測量獲得的磁強數(shù)據(jù)發(fā)送給ARM處理器,
[0009]溫度傳感器,用于測量獲得三軸陀螺儀計傳感器的溫漂,并將測量獲得的溫漂數(shù)據(jù)發(fā)送給ARM處理器,
[0010]ARM處理器,用來對接收到的角速度數(shù)據(jù)、加速度數(shù)據(jù)、磁強數(shù)據(jù)和溫度傳感器數(shù)據(jù)分別進行前置低通數(shù)字濾波、前端數(shù)據(jù)處理和拓展卡爾曼濾波處理,還用于對濾除外界高頻干擾的所有數(shù)據(jù)進行數(shù)據(jù)融合,獲得歐拉角三維姿態(tài)慣性數(shù)據(jù)或者四元數(shù)三維姿態(tài)慣性數(shù)據(jù)。
[0011]該系統(tǒng)采用一個ARM處理器采集三軸陀螺儀計傳感器、三軸加速度計傳感器和三軸磁強計傳感器的測量數(shù)據(jù)及采集溫度傳感器獲得三軸陀螺儀計傳感器的溫漂,再通過對測量的數(shù)據(jù)進行前置低通數(shù)字濾波與前端數(shù)據(jù)處理和進行拓展卡爾曼濾波,濾除外界高頻干擾并進行數(shù)據(jù)融合,對外實時輸出四元數(shù)或歐拉角三維姿態(tài)方位數(shù)據(jù),該設(shè)備采用一個ARM處理器,集合一個三軸陀螺儀計傳感器、一個三軸加速度計傳感器和一個三軸磁強計傳感器并裝載一個溫度傳感器,其測量精度同比現(xiàn)有的采用方位傳感器要提高2倍以上,并且該系統(tǒng)采用三軸陀螺儀計傳感器、三軸加速度計傳感器和三軸磁強計傳感器同比現(xiàn)有的采用方位傳感器的成本低。它可用于導(dǎo)航控制領(lǐng)域。
【專利附圖】
【附圖說明】
[0012]圖1為【具體實施方式】一所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)的原理示框圖;
[0013]圖2為【具體實施方式】五所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)的測量方法的流程圖。
【具體實施方式】
[0014]【具體實施方式】一:參照圖1具體說明本實施方式,本實施方式所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng),它包括微機電式慣性測量器件,它還包括ARM處理器4和溫度傳感器5,
[0015]所述的微機電式慣性測量器件包括三軸陀螺儀計傳感器1、三軸加速度計傳感器
2和三軸磁強計傳感器3,
[0016]三軸陀螺儀計傳感器1,用于測量獲得角速度,并將測量獲得的角速度數(shù)據(jù)發(fā)送給ARM處理器4,
[0017]三軸加速度計傳感器2,用于測量獲得加速度,并將測量獲得的加速度數(shù)據(jù)發(fā)送給ARM處理器4,
[0018]三軸磁強計傳感器3,用于測量獲得磁強,并將測量獲得的磁強數(shù)據(jù)發(fā)送給ARM處理器4,
[0019]溫度傳感器5,用于測量獲得三軸陀螺儀計傳感器I的溫漂,并將測量獲得的溫漂數(shù)據(jù)發(fā)送給ARM處理器4,
[0020]ARM處理器4,用來對接收到的角速度數(shù)據(jù)、加速度數(shù)據(jù)、磁強數(shù)據(jù)和溫漂數(shù)據(jù)分別進行前置低通數(shù)字濾波、前端數(shù)據(jù)處理和拓展卡爾曼濾波處理,還用于對濾除外界高頻干擾的所有數(shù)據(jù)進行數(shù)據(jù)融合,獲得歐拉角三維姿態(tài)慣性數(shù)據(jù)輸出或者四元數(shù)三維姿態(tài)慣性數(shù)據(jù)輸出。
[0021]【具體實施方式】二:本實施方式與【具體實施方式】一所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)的不同點在于,ARM處理器4采用型號為STM32F405實現(xiàn)。
[0022]【具體實施方式】三:本實施方式與【具體實施方式】一所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)的不同點在于,三軸陀螺儀計傳感器I采用型號為MPU6050實現(xiàn)。
[0023]【具體實施方式】四:本實施方式與【具體實施方式】一所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)的不同點在于,三軸加速度計傳感器2和三軸磁強計傳感器3采用型號為AK8975實現(xiàn)。
[0024]【具體實施方式】五:米用【具體實施方式】一所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)實現(xiàn)三維姿態(tài)慣性的測量方法,它包括以下步驟:
[0025]步驟一、將三軸陀螺儀計傳感器1、三軸加速度計傳感器2和三軸磁強計傳感器3水平固定在轉(zhuǎn)臺上,使三軸陀螺儀計傳感器I的敏感軸X、三軸加速度計傳感器2的敏感軸X和三軸磁強計傳感器3的敏感軸X相互平行,三軸陀螺儀計傳感器I的敏感軸y、三軸加速度計傳感器2的敏感軸y和三軸磁強計傳感器3的敏感軸y相互平行,三軸陀螺儀計傳感器I的敏感軸z、三軸加速度計傳感器2的敏感軸z和三軸磁強計傳感器3的敏感軸z相互平行,分別對三軸陀螺儀計傳感器1、三軸加速度計傳感器2和三軸磁強計傳感器3進行標(biāo)定,獲取硬件平臺的校準(zhǔn)參數(shù);
[0026]步驟二、系統(tǒng)進行初始化對準(zhǔn),ARM處理器4周期性讀取三軸陀螺儀計傳感器I敏感軸X、y、z軸的數(shù)據(jù)gx, gy, gz、三軸加速度計傳感器2敏感軸X、y、z軸的數(shù)據(jù)ax, ay, az和三軸磁強計傳感器3敏感軸X、y、z軸的數(shù)據(jù)mx, my, mz和溫度傳感器的數(shù)據(jù)T ;
[0027]步驟三、利用讀到的三軸加速度計傳感器2敏感軸的數(shù)據(jù)和三軸磁強計傳感器3敏感軸的數(shù)據(jù)以及
[0028]利用俯仰角公式
【權(quán)利要求】
1.一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng),它包括微機電式慣性測量器件,其特征在于,它還包括ARM處理器(4 )和溫度傳感器(5 ), 所述的微機電式慣性測量器件包括三軸陀螺儀計傳感器(I)、三軸加速度計傳感器(2)和三軸磁強計傳感器(3), 三軸陀螺儀計傳感器(1),用于測量獲得角速度,并將測量獲得的角速度數(shù)據(jù)發(fā)送給ARM處理器(4), 三軸加速度計傳感器(2),用于測量獲得加速度,并將測量獲得的加速度數(shù)據(jù)發(fā)送給ARM處理器(4), 三軸磁強計傳感器(3),用于測量獲得磁強,并將測量獲得的磁強數(shù)據(jù)發(fā)送給ARM處理器⑷, 溫度傳感器(5),用于測量獲得三軸陀螺儀計傳感器(I)的溫漂,并將測量獲得的溫漂數(shù)據(jù)發(fā)送給ARM處理器(4), ARM處理器(4),用來對接收到的角速度數(shù)據(jù)、加速度數(shù)據(jù)、磁強數(shù)據(jù)和溫漂數(shù)據(jù)分別進行前置低通數(shù)字濾波、前端數(shù)據(jù)處理和拓展卡爾曼濾波處理,還用于對濾除外界高頻干擾的所有數(shù)據(jù)進行數(shù)據(jù)融合,獲得歐拉角三維姿態(tài)慣性數(shù)據(jù)輸出或者四元數(shù)三維姿態(tài)慣性數(shù)據(jù)輸出。
2.根據(jù)權(quán)利要求1所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng),其特征在于,ARM處理器采用型號為STM32F405實現(xiàn)。
3.根據(jù)權(quán)利要求1所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng),其特征在于,三軸陀螺儀傳感器采用型號為MPU6050實現(xiàn)。
4.根據(jù)權(quán)利要求1所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng),其特征在于,三軸加速度計傳感器和三軸磁強計傳感器采用型號為AK8975實現(xiàn)。
5.采用權(quán)利要求1的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)實現(xiàn)三維姿態(tài)慣性的測量方法,其特征在于,它包括以下步驟: 步驟一、將三軸陀螺儀計傳感器(I)、三軸加速度計傳感器(2)和三軸磁強計傳感器(3 )水平固定在轉(zhuǎn)臺上,使三軸陀螺儀計傳感器(I)的敏感軸X、三軸加速度計傳感器(2 )的敏感軸X和三軸磁強計傳感器(3)的敏感軸X相互平行,三軸陀螺儀計傳感器(I)的敏感軸y、三軸加速度計傳感器(2)的敏感軸y和三軸磁強計傳感器(3)的敏感軸y相互平行,三軸陀螺儀計傳感器(I)的敏感軸z、三軸加速度計傳感器(2)的敏感軸z和三軸磁強計傳感器(3)的敏感軸z相互平行,分別對三軸陀螺儀計傳感器(I)、三軸加速度計傳感器(2)和三軸磁強計傳感器(3)進行標(biāo)定,獲取硬件平臺的校準(zhǔn)參數(shù); 步驟二、系統(tǒng)進行初始化對準(zhǔn),ARM處理器(4)周期性讀取三軸陀螺儀計傳感器(I)敏感軸X、y、z軸的數(shù)據(jù)gx, gy, gz、三軸加速度計傳感器(2)敏感軸X、y、z軸的數(shù)據(jù)ax, ay, az和三軸磁強計傳感器(3)敏感軸X、y、z軸的數(shù)據(jù)mx, my, mz和溫度傳感器的數(shù)據(jù)T ; 步驟三、利用讀到的三軸加速度計傳感器(2)敏感軸的數(shù)據(jù)和三軸磁強計傳感器(3)敏感軸的數(shù)據(jù)以及 利用俯仰角公式
6.根據(jù)權(quán)利要求5所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)實現(xiàn)三維姿態(tài)慣性的測量方法,其特征在于,在步驟一中獲得三軸陀螺儀計傳感器(I )、三軸加速度計傳感器(2)和三軸磁強計傳感器(3)校準(zhǔn)參數(shù)的過程為: 獲得三軸陀螺儀計傳感器(I)的校準(zhǔn)參數(shù)的過程為:將電路板水平放置在轉(zhuǎn)臺上,首先測量系統(tǒng)上電后進行ARM處理器和周圍電路、總線的初始化后,ARM處理器連續(xù)讀取三軸陀螺儀計傳感器各敏感軸(X,1,z)的數(shù)據(jù)和溫度傳感器(5)的數(shù)據(jù),得到四個數(shù)組&[11],gy[n], gz[n]和 T [η], 利用Matlab數(shù)據(jù)擬合工具箱按公式: gda=aaT3+baT2+caT+da, 擬合陀螺儀的零漂和溫漂,得到陀螺儀的零漂dx,dy, dz,一次溫漂常數(shù)cx,cy, cz, 二次溫漂常數(shù)bx, by, bz和三次溫漂常數(shù)ax, ay, az,式中,gda為α軸上的漂移,包括零漂和溫漂,a=x,y,z, 然后依次將三軸陀螺儀計傳感器(I)的敏感軸X,1,Z與轉(zhuǎn)臺的轉(zhuǎn)軸保持平行,以不同的標(biāo)稱轉(zhuǎn)速轉(zhuǎn)動轉(zhuǎn)臺,記錄下各轉(zhuǎn)速下三軸陀螺儀計傳感器(I)的敏感軸x,y, z的輸出ga,并以ga=kgagBa+gda,a =x, y, z的方式擬合陀螺儀的量程系數(shù)kga,a =x, y, z, 式中,ga為敏感軸上的實際輸出,gBa為標(biāo)稱轉(zhuǎn)速,gda為陀螺儀漂移; 獲得三軸加速度計傳感器(2)的校準(zhǔn)參數(shù)的過程為:從不同角度將電路板靜置,待靜置后用ARM處理器連續(xù)讀取三軸加速度計各敏感軸(X, y, z)的數(shù)據(jù)ax[n], ay[n], az[n],
利用 Matlab 工具箱,按照(kaxax [n] +ax0)2+ (kayay [n] +ay0)2+ (kazaz [η] +az0)2=G02 的方式擬合出加速度計的量程系數(shù)kaa和零偏常數(shù)aa(l,a =χ, y, z,其中Gtl為重力加速度記; 獲得三軸磁強計傳感器(3)的校準(zhǔn)參數(shù)的過程為:將磁強計放置在轉(zhuǎn)臺上,勻速轉(zhuǎn)速一圈,用ARM處理器連續(xù)讀取三軸磁強計傳感器(3)各敏感軸(X,y, z)的數(shù)據(jù),得到一個數(shù)組m[n] = {mx[n], my[n], mz[n]},設(shè)取磁場向量模為I,利用Matlab工具箱,按照(m_c)tX (mTXU)T(m-c)=l的方式進行最小二乘擬合,其中,m=m[n],三維向量c=(cxCI, cy0, cz0)是磁場強度的橢球球心偏移量,ca0, a =x, y, z為各軸上的偏移量;矩陣U是標(biāo)度因子與非正交度校正矩陣。
7.根據(jù)權(quán)利要求5所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)實現(xiàn)三維姿態(tài)慣性的測量方法,其特征在于,對步驟四中的三軸陀螺儀計傳感器(I)初始化數(shù)據(jù)、三軸加速度計傳感器(2)初始化數(shù)據(jù)和三軸磁強計傳感器(3)初始化數(shù)據(jù)進行前置低通濾波與前端數(shù)據(jù)處理的過程為: 對前置低通濾波進行一階低通數(shù)字濾波,得到:
S a _last= λ gga Iast+ (1- λ g) g α,a a last-λ aaa —last+ (1-λ a) a α,ma last-λ mma _last+ (1-λ m)ma, a =X, y, Z,0 ≤λg≤ 1,0≤ λa≤ 1,0≤ λm≤l, 其中,Xg, Xa, λπ分別為三軸陀螺儀計傳感器(I)、三軸加速度計傳感器(2)和三軸磁強計傳感器(3)的低通濾波常數(shù),ga—last為上一次低通濾波后的三軸陀螺儀計傳感器(I) α軸數(shù)據(jù),ga是最新一次測量得到的三軸陀螺儀計傳感器(I) a各軸原始數(shù)據(jù),aa last為上一次低通濾波后的三軸加速度計傳感器(2) α軸數(shù)據(jù),aa是最新一次測量得到的三軸加速度計傳感器(2) a各軸原始數(shù)據(jù),ma last為上一次低通濾波后的三軸磁強計傳感器(3) α各軸數(shù)據(jù),ma是最新一次測量得到的三軸磁強計傳感器(3) a各軸原始數(shù)據(jù),
根據(jù)公式:
8.根據(jù)權(quán)利要求5所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)實現(xiàn)三維姿態(tài)慣性的測量方法,其特征在于,對步驟五中利用步驟三得到的初始化數(shù)據(jù)%,Q1, Q2, Q3和步驟四得到的三軸加速度計傳感器(2)精確化的數(shù)據(jù)采用拓展卡爾曼濾波器在Λ t時間間隔內(nèi)進行拓展卡爾曼濾波的時間更新,得到預(yù)測四元數(shù)三維姿態(tài)慣性數(shù)據(jù)的過程為:根據(jù)公式:
9.根據(jù)權(quán)利要求5所述的一種基于MEMS的高精度三維姿態(tài)慣性測量系統(tǒng)實現(xiàn)三維姿態(tài)慣性的測量方法,其特征在于,對步驟六中的利用步驟四得到的精確化的三軸加速度計傳感器(2)數(shù)據(jù)和精確化的三軸磁強計傳感器(3)數(shù)據(jù),在上一時刻得到的預(yù)測四元數(shù)三維姿態(tài)慣性數(shù)據(jù)處做線性化展開,進行拓展卡爾曼濾波的測量更新的過程為: 將觀測
【文檔編號】G01C21/16GK103776451SQ201410076764
【公開日】2014年5月7日 申請日期:2014年3月4日 優(yōu)先權(quán)日:2014年3月4日
【發(fā)明者】胡慶雷, 肖冰, 陳卓 申請人:哈爾濱工業(yè)大學(xué)