国产自产21区,亚洲97,免费毛片网,国产啪视频,青青青国产在线观看,国产毛片一区二区三区精品

一種基于mems的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)以及測(cè)量方法
【專利摘要】一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)以及測(cè)量方法，涉及一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量方法。本發(fā)明是為了解決現(xiàn)有的三維姿態(tài)慣性測(cè)量設(shè)備采用傳感器成本高、精度低的問(wèn)題。三軸陀螺儀計(jì)傳感器用于將測(cè)量的角速度數(shù)據(jù)發(fā)送給ARM處理器，三軸加速度計(jì)傳感器用于將測(cè)量的加速度數(shù)據(jù)發(fā)送給ARM處理器，三軸磁強(qiáng)計(jì)傳感器用于將測(cè)量的磁強(qiáng)數(shù)據(jù)發(fā)送給ARM處理器，溫度傳感器用于測(cè)量將獲得的三軸陀螺儀計(jì)傳感器溫漂數(shù)據(jù)發(fā)送給ARM處理器，ARM處理器用來(lái)對(duì)接收到的數(shù)據(jù)分別進(jìn)行前置低通數(shù)字濾波、前端數(shù)據(jù)處理和拓展卡爾曼濾波處理，獲得歐拉角三維姿態(tài)慣性數(shù)據(jù)或者四元數(shù)三維姿態(tài)慣性數(shù)據(jù)。它可用于導(dǎo)航控制領(lǐng)域。
【專利說(shuō)明】一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)以及測(cè)量方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種空間三維姿態(tài)測(cè)量方法，特別是涉及一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量方法。
【背景技術(shù)】
[0002]無(wú)人機(jī)、機(jī)器人、機(jī)械云臺(tái)、車輛船舶、虛擬現(xiàn)實(shí)、人體運(yùn)動(dòng)分析等在近年都取得了迅猛的發(fā)展。而在這些應(yīng)用中，三維姿態(tài)與方位的自主測(cè)量顯得極為重要。現(xiàn)有的三維姿態(tài)與方位傳感器中，MEMS傳感器成本低廉，但是測(cè)量數(shù)據(jù)由于存在較大的零偏、溫漂等誤差較大不能滿足以上應(yīng)用對(duì)于精度的要求；而一些MU模塊，在精度上能滿足要求，但是成本高昂，限制了在實(shí)際中的應(yīng)用。

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

[0003]本發(fā)明是為了解決現(xiàn)有的三維姿態(tài)慣性測(cè)量設(shè)備采用傳感器成本高、精度低的問(wèn)題?，F(xiàn)提供一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)以及該測(cè)量方法。
[0004]一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)，它包括微機(jī)電式慣性測(cè)量器件，它還包括ARM處理器和溫度傳感器，
[0005]所述的微機(jī)電式慣性測(cè)量器件包括三軸陀螺儀計(jì)傳感器、三軸加速度計(jì)傳感器和三軸磁強(qiáng)計(jì)傳感器，
[0006]三軸陀螺儀計(jì)傳感器，用于測(cè)量獲得角速度，并將測(cè)量獲得的角速度數(shù)據(jù)發(fā)送給ARM處理器，
[0007]三軸加速度計(jì)傳感器，用于測(cè)量獲得加速度，并將測(cè)量獲得的加速度數(shù)據(jù)發(fā)送給ARM處理器，
[0008]三軸磁強(qiáng)計(jì)傳感器，用于測(cè)量獲得磁強(qiáng)，并將測(cè)量獲得的磁強(qiáng)數(shù)據(jù)發(fā)送給ARM處理器，
[0009]溫度傳感器，用于測(cè)量獲得三軸陀螺儀計(jì)傳感器的溫漂，并將測(cè)量獲得的溫漂數(shù)據(jù)發(fā)送給ARM處理器，
[0010]ARM處理器，用來(lái)對(duì)接收到的角速度數(shù)據(jù)、加速度數(shù)據(jù)、磁強(qiáng)數(shù)據(jù)和溫度傳感器數(shù)據(jù)分別進(jìn)行前置低通數(shù)字濾波、前端數(shù)據(jù)處理和拓展卡爾曼濾波處理，還用于對(duì)濾除外界高頻干擾的所有數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，獲得歐拉角三維姿態(tài)慣性數(shù)據(jù)或者四元數(shù)三維姿態(tài)慣性數(shù)據(jù)。
[0011]該系統(tǒng)采用一個(gè)ARM處理器采集三軸陀螺儀計(jì)傳感器、三軸加速度計(jì)傳感器和三軸磁強(qiáng)計(jì)傳感器的測(cè)量數(shù)據(jù)及采集溫度傳感器獲得三軸陀螺儀計(jì)傳感器的溫漂，再通過(guò)對(duì)測(cè)量的數(shù)據(jù)進(jìn)行前置低通數(shù)字濾波與前端數(shù)據(jù)處理和進(jìn)行拓展卡爾曼濾波，濾除外界高頻干擾并進(jìn)行數(shù)據(jù)融合，對(duì)外實(shí)時(shí)輸出四元數(shù)或歐拉角三維姿態(tài)方位數(shù)據(jù)，該設(shè)備采用一個(gè)ARM處理器，集合一個(gè)三軸陀螺儀計(jì)傳感器、一個(gè)三軸加速度計(jì)傳感器和一個(gè)三軸磁強(qiáng)計(jì)傳感器并裝載一個(gè)溫度傳感器，其測(cè)量精度同比現(xiàn)有的采用方位傳感器要提高2倍以上，并且該系統(tǒng)采用三軸陀螺儀計(jì)傳感器、三軸加速度計(jì)傳感器和三軸磁強(qiáng)計(jì)傳感器同比現(xiàn)有的采用方位傳感器的成本低。它可用于導(dǎo)航控制領(lǐng)域。
【專利附圖】

【附圖說(shuō)明】
[0012]圖1為【具體實(shí)施方式】一所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)的原理示框圖；
[0013]圖2為【具體實(shí)施方式】五所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)的測(cè)量方法的流程圖。
【具體實(shí)施方式】
[0014]【具體實(shí)施方式】一:參照?qǐng)D1具體說(shuō)明本實(shí)施方式，本實(shí)施方式所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)，它包括微機(jī)電式慣性測(cè)量器件，它還包括ARM處理器4和溫度傳感器5，
[0015]所述的微機(jī)電式慣性測(cè)量器件包括三軸陀螺儀計(jì)傳感器1、三軸加速度計(jì)傳感器
2和三軸磁強(qiáng)計(jì)傳感器3，
[0016]三軸陀螺儀計(jì)傳感器1，用于測(cè)量獲得角速度，并將測(cè)量獲得的角速度數(shù)據(jù)發(fā)送給ARM處理器4，
[0017]三軸加速度計(jì)傳感器2，用于測(cè)量獲得加速度，并將測(cè)量獲得的加速度數(shù)據(jù)發(fā)送給ARM處理器4，
[0018]三軸磁強(qiáng)計(jì)傳感器3，用于測(cè)量獲得磁強(qiáng)，并將測(cè)量獲得的磁強(qiáng)數(shù)據(jù)發(fā)送給ARM處理器4，
[0019]溫度傳感器5，用于測(cè)量獲得三軸陀螺儀計(jì)傳感器I的溫漂，并將測(cè)量獲得的溫漂數(shù)據(jù)發(fā)送給ARM處理器4，
[0020]ARM處理器4，用來(lái)對(duì)接收到的角速度數(shù)據(jù)、加速度數(shù)據(jù)、磁強(qiáng)數(shù)據(jù)和溫漂數(shù)據(jù)分別進(jìn)行前置低通數(shù)字濾波、前端數(shù)據(jù)處理和拓展卡爾曼濾波處理，還用于對(duì)濾除外界高頻干擾的所有數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，獲得歐拉角三維姿態(tài)慣性數(shù)據(jù)輸出或者四元數(shù)三維姿態(tài)慣性數(shù)據(jù)輸出。
[0021]【具體實(shí)施方式】二:本實(shí)施方式與【具體實(shí)施方式】一所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)的不同點(diǎn)在于，ARM處理器4采用型號(hào)為STM32F405實(shí)現(xiàn)。
[0022]【具體實(shí)施方式】三:本實(shí)施方式與【具體實(shí)施方式】一所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)的不同點(diǎn)在于，三軸陀螺儀計(jì)傳感器I采用型號(hào)為MPU6050實(shí)現(xiàn)。
[0023]【具體實(shí)施方式】四:本實(shí)施方式與【具體實(shí)施方式】一所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)的不同點(diǎn)在于，三軸加速度計(jì)傳感器2和三軸磁強(qiáng)計(jì)傳感器3采用型號(hào)為AK8975實(shí)現(xiàn)。
[0024]【具體實(shí)施方式】五:米用【具體實(shí)施方式】一所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)實(shí)現(xiàn)三維姿態(tài)慣性的測(cè)量方法，它包括以下步驟:
[0025]步驟一、將三軸陀螺儀計(jì)傳感器1、三軸加速度計(jì)傳感器2和三軸磁強(qiáng)計(jì)傳感器3水平固定在轉(zhuǎn)臺(tái)上，使三軸陀螺儀計(jì)傳感器I的敏感軸X、三軸加速度計(jì)傳感器2的敏感軸X和三軸磁強(qiáng)計(jì)傳感器3的敏感軸X相互平行,三軸陀螺儀計(jì)傳感器I的敏感軸y、三軸加速度計(jì)傳感器2的敏感軸y和三軸磁強(qiáng)計(jì)傳感器3的敏感軸y相互平行，三軸陀螺儀計(jì)傳感器I的敏感軸z、三軸加速度計(jì)傳感器2的敏感軸z和三軸磁強(qiáng)計(jì)傳感器3的敏感軸z相互平行，分別對(duì)三軸陀螺儀計(jì)傳感器1、三軸加速度計(jì)傳感器2和三軸磁強(qiáng)計(jì)傳感器3進(jìn)行標(biāo)定，獲取硬件平臺(tái)的校準(zhǔn)參數(shù)；
[0026]步驟二、系統(tǒng)進(jìn)行初始化對(duì)準(zhǔn)，ARM處理器4周期性讀取三軸陀螺儀計(jì)傳感器I敏感軸X、y、z軸的數(shù)據(jù)gx, gy, gz、三軸加速度計(jì)傳感器2敏感軸X、y、z軸的數(shù)據(jù)ax, ay, az和三軸磁強(qiáng)計(jì)傳感器3敏感軸X、y、z軸的數(shù)據(jù)mx, my, mz和溫度傳感器的數(shù)據(jù)T ；
[0027]步驟三、利用讀到的三軸加速度計(jì)傳感器2敏感軸的數(shù)據(jù)和三軸磁強(qiáng)計(jì)傳感器3敏感軸的數(shù)據(jù)以及
[0028]利用俯仰角公式
【權(quán)利要求】
1.一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)，它包括微機(jī)電式慣性測(cè)量器件，其特征在于，它還包括ARM處理器(4 )和溫度傳感器(5 )， 所述的微機(jī)電式慣性測(cè)量器件包括三軸陀螺儀計(jì)傳感器(I)、三軸加速度計(jì)傳感器(2)和三軸磁強(qiáng)計(jì)傳感器(3)， 三軸陀螺儀計(jì)傳感器(1)，用于測(cè)量獲得角速度，并將測(cè)量獲得的角速度數(shù)據(jù)發(fā)送給ARM處理器(4)， 三軸加速度計(jì)傳感器(2)，用于測(cè)量獲得加速度，并將測(cè)量獲得的加速度數(shù)據(jù)發(fā)送給ARM處理器(4)， 三軸磁強(qiáng)計(jì)傳感器(3)，用于測(cè)量獲得磁強(qiáng)，并將測(cè)量獲得的磁強(qiáng)數(shù)據(jù)發(fā)送給ARM處理器⑷， 溫度傳感器(5)，用于測(cè)量獲得三軸陀螺儀計(jì)傳感器(I)的溫漂，并將測(cè)量獲得的溫漂數(shù)據(jù)發(fā)送給ARM處理器(4)， ARM處理器(4)，用來(lái)對(duì)接收到的角速度數(shù)據(jù)、加速度數(shù)據(jù)、磁強(qiáng)數(shù)據(jù)和溫漂數(shù)據(jù)分別進(jìn)行前置低通數(shù)字濾波、前端數(shù)據(jù)處理和拓展卡爾曼濾波處理，還用于對(duì)濾除外界高頻干擾的所有數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，獲得歐拉角三維姿態(tài)慣性數(shù)據(jù)輸出或者四元數(shù)三維姿態(tài)慣性數(shù)據(jù)輸出。
2.根據(jù)權(quán)利要求1所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)，其特征在于，ARM處理器采用型號(hào)為STM32F405實(shí)現(xiàn)。
3.根據(jù)權(quán)利要求1所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)，其特征在于，三軸陀螺儀傳感器采用型號(hào)為MPU6050實(shí)現(xiàn)。
4.根據(jù)權(quán)利要求1所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)，其特征在于，三軸加速度計(jì)傳感器和三軸磁強(qiáng)計(jì)傳感器采用型號(hào)為AK8975實(shí)現(xiàn)。
5.采用權(quán)利要求1的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)實(shí)現(xiàn)三維姿態(tài)慣性的測(cè)量方法，其特征在于，它包括以下步驟: 步驟一、將三軸陀螺儀計(jì)傳感器(I)、三軸加速度計(jì)傳感器(2)和三軸磁強(qiáng)計(jì)傳感器(3 )水平固定在轉(zhuǎn)臺(tái)上，使三軸陀螺儀計(jì)傳感器(I)的敏感軸X、三軸加速度計(jì)傳感器(2 )的敏感軸X和三軸磁強(qiáng)計(jì)傳感器(3)的敏感軸X相互平行，三軸陀螺儀計(jì)傳感器(I)的敏感軸y、三軸加速度計(jì)傳感器(2)的敏感軸y和三軸磁強(qiáng)計(jì)傳感器(3)的敏感軸y相互平行，三軸陀螺儀計(jì)傳感器(I)的敏感軸z、三軸加速度計(jì)傳感器(2)的敏感軸z和三軸磁強(qiáng)計(jì)傳感器(3)的敏感軸z相互平行，分別對(duì)三軸陀螺儀計(jì)傳感器(I)、三軸加速度計(jì)傳感器(2)和三軸磁強(qiáng)計(jì)傳感器(3)進(jìn)行標(biāo)定，獲取硬件平臺(tái)的校準(zhǔn)參數(shù)； 步驟二、系統(tǒng)進(jìn)行初始化對(duì)準(zhǔn)，ARM處理器(4)周期性讀取三軸陀螺儀計(jì)傳感器(I)敏感軸X、y、z軸的數(shù)據(jù)gx, gy, gz、三軸加速度計(jì)傳感器(2)敏感軸X、y、z軸的數(shù)據(jù)ax, ay, az和三軸磁強(qiáng)計(jì)傳感器(3)敏感軸X、y、z軸的數(shù)據(jù)mx, my, mz和溫度傳感器的數(shù)據(jù)T ； 步驟三、利用讀到的三軸加速度計(jì)傳感器(2)敏感軸的數(shù)據(jù)和三軸磁強(qiáng)計(jì)傳感器(3)敏感軸的數(shù)據(jù)以及 利用俯仰角公式
6.根據(jù)權(quán)利要求5所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)實(shí)現(xiàn)三維姿態(tài)慣性的測(cè)量方法，其特征在于，在步驟一中獲得三軸陀螺儀計(jì)傳感器(I )、三軸加速度計(jì)傳感器(2)和三軸磁強(qiáng)計(jì)傳感器(3)校準(zhǔn)參數(shù)的過(guò)程為: 獲得三軸陀螺儀計(jì)傳感器(I)的校準(zhǔn)參數(shù)的過(guò)程為:將電路板水平放置在轉(zhuǎn)臺(tái)上，首先測(cè)量系統(tǒng)上電后進(jìn)行ARM處理器和周圍電路、總線的初始化后，ARM處理器連續(xù)讀取三軸陀螺儀計(jì)傳感器各敏感軸(X，1，z)的數(shù)據(jù)和溫度傳感器(5)的數(shù)據(jù)，得到四個(gè)數(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, 然后依次將三軸陀螺儀計(jì)傳感器(I)的敏感軸X，1，Z與轉(zhuǎn)臺(tái)的轉(zhuǎn)軸保持平行，以不同的標(biāo)稱轉(zhuǎn)速轉(zhuǎn)動(dòng)轉(zhuǎn)臺(tái)，記錄下各轉(zhuǎn)速下三軸陀螺儀計(jì)傳感器(I)的敏感軸x,y, z的輸出ga,并以ga=kgagBa+gda，a =x, y, z的方式擬合陀螺儀的量程系數(shù)kga，a =x, y, z, 式中，ga為敏感軸上的實(shí)際輸出，gBa為標(biāo)稱轉(zhuǎn)速，gda為陀螺儀漂移； 獲得三軸加速度計(jì)傳感器(2)的校準(zhǔn)參數(shù)的過(guò)程為:從不同角度將電路板靜置，待靜置后用ARM處理器連續(xù)讀取三軸加速度計(jì)各敏感軸(X, y, z)的數(shù)據(jù)ax[n], ay[n], az[n],
利用 Matlab 工具箱，按照(kaxax [n] +ax0)2+ (kayay [n] +ay0)2+ (kazaz [η] +az0)2=G02 的方式擬合出加速度計(jì)的量程系數(shù)kaa和零偏常數(shù)aa(l，a =χ, y, z，其中Gtl為重力加速度記； 獲得三軸磁強(qiáng)計(jì)傳感器(3)的校準(zhǔn)參數(shù)的過(guò)程為:將磁強(qiáng)計(jì)放置在轉(zhuǎn)臺(tái)上，勻速轉(zhuǎn)速一圈，用ARM處理器連續(xù)讀取三軸磁強(qiáng)計(jì)傳感器(3)各敏感軸(X，y, z)的數(shù)據(jù)，得到一個(gè)數(shù)組m[n] = {mx[n], my[n], mz[n]},設(shè)取磁場(chǎng)向量模為I,利用Matlab工具箱，按照(m_c)tX (mTXU)T(m-c)=l的方式進(jìn)行最小二乘擬合,其中，m=m[n],三維向量c=(cxCI, cy0, cz0)是磁場(chǎng)強(qiáng)度的橢球球心偏移量，ca0, a =x, y, z為各軸上的偏移量；矩陣U是標(biāo)度因子與非正交度校正矩陣。
7.根據(jù)權(quán)利要求5所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)實(shí)現(xiàn)三維姿態(tài)慣性的測(cè)量方法，其特征在于，對(duì)步驟四中的三軸陀螺儀計(jì)傳感器(I)初始化數(shù)據(jù)、三軸加速度計(jì)傳感器(2)初始化數(shù)據(jù)和三軸磁強(qiáng)計(jì)傳感器(3)初始化數(shù)據(jù)進(jìn)行前置低通濾波與前端數(shù)據(jù)處理的過(guò)程為: 對(duì)前置低通濾波進(jìn)行一階低通數(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, λπ分別為三軸陀螺儀計(jì)傳感器(I)、三軸加速度計(jì)傳感器(2)和三軸磁強(qiáng)計(jì)傳感器(3)的低通濾波常數(shù)，ga—last為上一次低通濾波后的三軸陀螺儀計(jì)傳感器(I) α軸數(shù)據(jù)，ga是最新一次測(cè)量得到的三軸陀螺儀計(jì)傳感器(I) a各軸原始數(shù)據(jù)，aa last為上一次低通濾波后的三軸加速度計(jì)傳感器(2) α軸數(shù)據(jù)，aa是最新一次測(cè)量得到的三軸加速度計(jì)傳感器(2) a各軸原始數(shù)據(jù)，ma last為上一次低通濾波后的三軸磁強(qiáng)計(jì)傳感器(3) α各軸數(shù)據(jù)，ma是最新一次測(cè)量得到的三軸磁強(qiáng)計(jì)傳感器(3) a各軸原始數(shù)據(jù)，
根據(jù)公式:
8.根據(jù)權(quán)利要求5所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)實(shí)現(xiàn)三維姿態(tài)慣性的測(cè)量方法，其特征在于，對(duì)步驟五中利用步驟三得到的初始化數(shù)據(jù)％，Q1, Q2, Q3和步驟四得到的三軸加速度計(jì)傳感器(2)精確化的數(shù)據(jù)采用拓展卡爾曼濾波器在Λ t時(shí)間間隔內(nèi)進(jìn)行拓展卡爾曼濾波的時(shí)間更新，得到預(yù)測(cè)四元數(shù)三維姿態(tài)慣性數(shù)據(jù)的過(guò)程為:根據(jù)公式:
9.根據(jù)權(quán)利要求5所述的一種基于MEMS的高精度三維姿態(tài)慣性測(cè)量系統(tǒng)實(shí)現(xiàn)三維姿態(tài)慣性的測(cè)量方法，其特征在于，對(duì)步驟六中的利用步驟四得到的精確化的三軸加速度計(jì)傳感器(2)數(shù)據(jù)和精確化的三軸磁強(qiáng)計(jì)傳感器(3)數(shù)據(jù)，在上一時(shí)刻得到的預(yù)測(cè)四元數(shù)三維姿態(tài)慣性數(shù)據(jù)處做線性化展開(kāi)，進(jìn)行拓展卡爾曼濾波的測(cè)量更新的過(guò)程為: 將觀測(cè)
【文檔編號(hào)】G01C21/16GK103776451SQ201410076764
【公開(kāi)日】2014年5月7日 申請(qǐng)日期:2014年3月4日 優(yōu)先權(quán)日:2014年3月4日
【發(fā)明者】胡慶雷, 肖冰, 陳卓 申請(qǐng)人:哈爾濱工業(yè)大學(xué)