MEMS IMU的入門與應(yīng)用.ppt

1、MEMS IMU的入門與應(yīng)用,一、導(dǎo)航系統(tǒng)基本概念 二、MEMS慣性器件； 三、MEMS慣性器件的簡易標定； 四、四元數(shù)； 五、基于乘性四元數(shù)EKF濾波器,一、導(dǎo)航系統(tǒng)基本概念,1.海里,國際上采用1852m作為標準海里長度，1nm=1852m,2.慣性級導(dǎo)航系統(tǒng),運行1h過程中在水平面上定位誤差等于1nm（簡記1nm/h）的慣性導(dǎo)航系統(tǒng)稱為慣性級導(dǎo)航系統(tǒng),3.慣性級陀螺,地球自轉(zhuǎn)角速率為15.04107度/h.其千分之一為0.015度/h,稱為毫地轉(zhuǎn)率；將精度達到0.015度/h的陀螺稱為慣性級陀螺，往往也以1meru的量級(0.01度/h)表示慣性級陀螺精度,4.加速度計,高精度：1mg;

2、,慣性級導(dǎo)航系統(tǒng)對陀螺的最低要求為0.01度/h，加速度計精度的最低要求為1x10-4g,二.MEMS慣性器件,1.消費級MEMS慣性器件,Invensense MPU6000/MPU6050,Invensense公司的MPU6000/MPU6050六軸慣性器件是世界上首個六軸集成的低成本MEMS慣性器件，該器件推出市場之后在各個領(lǐng)域（特別是四軸飛行器）得到了廣泛應(yīng)用,二.MEMS慣性器件,1.消費級MEMS慣性器件,在2010年，apple的iPhone4采用了ST公司的三軸陀螺儀L3G4200D和三軸加速度計LIS331DLH作為手機的慣性傳感器。其它手機廠商隨即跟進，陀螺儀與加速度計逐漸

3、成為智能手機的標配傳感器,iPhone4 IMU,二.MEMS慣性器件,2.工業(yè)級MEMS慣性器件,ADIS16405,ADIS16488A,Xsens Mti,二.MEMS慣性器件,2.常見MEMS慣性器件參數(shù)對比,二.MEMS慣性器件,2.常見MEMS慣性器件參數(shù)對比,二.MEMS慣性器件,2.MEMS慣性器件的常見技術(shù)參數(shù),（2）非線性度,（3）初始偏置誤差,MEMS慣性器件在開機后系統(tǒng)穩(wěn)定輸出后觀測信息中包含的誤差，可以通過靜態(tài)情況下的初始化求取均值消除,（1）量程,MEMS慣性器件在開機后系統(tǒng)穩(wěn)定輸出后觀測信息中包含的誤差，可以通過靜態(tài)情況下的初始化求取均值消除,二.MEMS慣性器件

4、,2.MEMS慣性器件的常見技術(shù)參數(shù),（4）對齊誤差,參考文獻：MEMS陀螺儀中主要噪聲源的預(yù)測和管理 ADI公司 應(yīng)用工程師 Mark Looney,三軸式陀螺儀對齊誤差,MEMS IMU通常具有兩種類型的對齊誤差，它們相互關(guān)聯(lián)，但在系統(tǒng)級建模中具有不同應(yīng)用： 軸到封裝和軸到軸。 “ 軸到封裝對齊誤差”描述陀螺儀相對于器件封裝上特定機械特性的對齊情況。將IMU安裝到系統(tǒng)后，如果系統(tǒng)無法支持慣性對齊，則軸到封裝對齊誤差將成為整體對齊誤差的主要因素之一。系統(tǒng)與IMU的機械接口的機械缺陷也會增加整體對齊誤差。 “ 軸到軸對齊誤差”描述各個陀螺儀旋轉(zhuǎn)軸相對于其他兩個陀螺儀的相對對齊精度。在系統(tǒng)可以實

5、現(xiàn)簡單的對齊過程時，此參數(shù)影響最大，此時通常沿系統(tǒng)的慣性參考系中的一個軸直線移動整個組件(IMU已安裝在系統(tǒng)平臺上)，同時需要觀察傳感器。,二.MEMS慣性器件,2.MEMS慣性器件的常見技術(shù)參數(shù),（6）隨機游走，噪聲密度,隨機游走過程：當前觀測值完全由下一時刻觀測值加上現(xiàn)時噪聲決定，即n時刻的輸出由n-1時刻的輸出和隨機誤差和加和決定,陀螺角度游走過程：寬帶角速率白噪聲積分的結(jié)果，即陀螺從0時刻累積的總角增量誤差表現(xiàn)為隨機游走，而每一時刻的等效角速率誤差表現(xiàn)為白噪聲,噪聲密度：傳感器固有噪聲代表的是陀螺儀在靜態(tài)慣性和環(huán)境條件下運行時其輸出中的隨機振動。 MEMS IMU數(shù)據(jù)手冊通常會提供速率

6、噪聲密度(RND)參數(shù)來描述陀螺儀相對于頻率的固有噪聲。此參數(shù)通常使用單位/s/Hz，是預(yù)測特定濾波器配置固有噪聲的關(guān)鍵。,單位： /hr,功率譜N的國際單位： （rad/s）/Hz,（6）隨機游走，噪聲密度,二.MEMS慣性器件,2.MEMS慣性器件的常見技術(shù)參數(shù),信號方差強度q、功率譜密度S、隨機游走系數(shù)N之間的簡單換算關(guān)系：,1、令陀螺輸出角速率方差的單位為（rad/s），離散系統(tǒng)等效激勵白噪聲的方差等于連續(xù)時間系統(tǒng)白噪聲方差強度與離散化周期的乘積：,2、功率譜密度S（單位（rad/s）/Hz）與方差q的關(guān)系：,Qk=q*Ts,S=q/Ts,3、角度隨機游走N（單位/h）與功率譜密度S的

7、關(guān)系：,N= (3600*q)/60,二.MEMS慣性器件,2.MEMS慣性器件的常見技術(shù)參數(shù),（7）偏置溫度系數(shù)（溫飄）,MEMS慣性器件的零偏會受到環(huán)境溫度的影響而產(chǎn)生變化，針對這種影響，一種方案是對器件進行溫度標定，另一種方式是采用恒溫模式,大疆 精靈3 IMU,二.MEMS慣性器件,2.MEMS陀螺誤差模型,陀螺的誤差主要由兩部分構(gòu)成：漂移和刻度系數(shù)誤差。,陀螺的隨機漂移誤差主要分為三種分量：逐次啟動漂移、慢變漂移、快變漂移,陀螺的逐次啟動漂移與系統(tǒng)開機時刻的電氣參數(shù)、環(huán)境條件等隨機性因素相關(guān)。一旦系統(tǒng)啟動完畢，逐次啟動漂移造成的誤差量將保持在某一個固定值上,陀螺的慢變漂移是由于其工作

8、過程中環(huán)境條件的隨機改變所造成的緩慢變化的誤差，由于其變換較為緩慢，與前后時刻的陀螺漂移存在一定的相關(guān)性，隨著時間點的接近依賴關(guān)系更加明顯，因此可以用一階馬爾可夫過程描述：,快變漂移是在上述兩個漂移分量基礎(chǔ)上的雜亂無章的高頻跳變，這種漂移分量可以抽象化為白噪聲過程：,二.MEMS慣性器件,3.MEMS加速度計誤差模型,速度計的誤差模型在組合導(dǎo)航系統(tǒng)設(shè)計中，一般只考慮隨機常值誤差，忽略相關(guān)誤差，因此加速度計誤差模型如式所示：, = 0 0 0 + 1 2 1 2 1 2 . ,MEMS陀螺儀誤差修正模型, = 0 0 0 + 1 2 1 2 1 2 . ,MEMS加速度計誤差修正模型,圖3.1

9、3KTD-565多功能三軸轉(zhuǎn)臺,陀螺零偏： 0 、 0 、 0 標度因數(shù)： 、 、 安裝誤差： 1 、 2 、 1 、 1 、 2,加速度計零偏： 0 、 0 、 0 標度因數(shù)： 、 、 安裝誤差： 1 、 2 、 1 、 2 、 1 、 2,(3.1),(3.2),三.導(dǎo)航系統(tǒng)傳感器標定,3.1 MEMS IMU加速度計、陀螺儀實驗室標定,表4.2 MEMS陀螺儀標定時各個軸向輸出值,表4.1 MEMS加速度計標定時各個軸向輸出值,三.導(dǎo)航系統(tǒng)傳感器標定,3.1 MEMS IMU加速度計、陀螺儀實驗室標定,(3-3),(3-4),(3-5),(3-6),(3-7),(3-8),將載體分別靜止

10、放置6個位置，采集加速度計的數(shù)據(jù):,由式(3-3)、(3-4)、(3-5)可解修正模型的各個參數(shù)：,加速度計標定參數(shù)求解方程,三.導(dǎo)航系統(tǒng)傳感器標定,3.1 MEMS IMU加速度計、陀螺儀實驗室標定,三.導(dǎo)航系統(tǒng)傳感器標定,3.1 MEMS IMU加速度計、陀螺儀實驗室標定,由于該MEMS陀螺儀無法敏感地球自轉(zhuǎn)角速度，IMU的陀螺X、Y、Z軸在三次轉(zhuǎn)動中的輸入角速度為：,陀螺的標度因數(shù)和常值零偏：,MEMS陀螺儀標定采用速率標定方式。將MEMS IMU固定于三軸轉(zhuǎn)臺的平臺上，通過給定固定速率值測量其對應(yīng)輸出的方式進行實驗。具體步驟如下： （1）將待標定IMU安裝至三軸轉(zhuǎn)臺，IMU的軸向與轉(zhuǎn)臺

11、軸向平行，系統(tǒng)開機初始化； （2）X軸陀螺標定數(shù)據(jù)采集。令轉(zhuǎn)臺X軸正轉(zhuǎn)，Y、Z軸靜止，速率穩(wěn)定后記錄X、Y、Z軸的MEMS陀螺輸出數(shù)據(jù)；之后轉(zhuǎn)臺X軸反轉(zhuǎn)，角速率與之前相同，Y、Z軸靜止，速率穩(wěn)定后記錄X、Y、Z軸的MEMS陀螺輸出數(shù)據(jù)，完畢后轉(zhuǎn)臺停止旋轉(zhuǎn)； （3）Y、Z軸陀螺標定數(shù)據(jù)采集，參考步驟（2）的方式進行操作； （4）求取步驟（2）、（3）中各次角速率實驗中的角速率平均值，用于標定計算。,三.導(dǎo)航系統(tǒng)傳感器標定,3.1 MEMS IMU加速度計、陀螺儀實驗室標定,X軸MEMS陀螺標定試驗中，IMU的Y軸和Z軸角速率輸出：,將X軸正反轉(zhuǎn)時分別采集的Y軸和Z軸數(shù)據(jù)的正反輸出分別求差，可得陀

12、螺的安裝誤差系數(shù),同理，其它安裝誤差系數(shù)為：,MEMS陀螺儀輸出數(shù)學(xué)模型：,MEMS加速度計輸出數(shù)學(xué)模型：,(3-10),(3-11),三.導(dǎo)航系統(tǒng)傳感器標定,3.1 MEMS IMU加速度計、陀螺儀實驗室標定,三.導(dǎo)航系統(tǒng)傳感器標定,3.2 MEMS IMU加速度計六面法標定,ST DT0053 6-point tumble sensor calibration,將MEMS IMU放置于六面立方體上，按照圖示順序翻轉(zhuǎn)6次，分別采集6個狀態(tài)的加速度計靜態(tài)數(shù)據(jù)，用于校正,三.導(dǎo)航系統(tǒng)傳感器標定,3.3 MEMS IMU加速度計、陀螺儀不依托額外設(shè)備實現(xiàn)標定,參考論文：A robust and e

13、asy to implement method for IMU calibration without external equipm DEMO鏈接：/alberto_pretto/imu_tk,視頻地址：,三.導(dǎo)航系統(tǒng)傳感器標定,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,該校正方法是基于一個多位置方案能夠修正加速度計和陀螺儀的刻度因數(shù)誤差和對準誤差，同時也能夠估計傳感器的誤差。只需要將傳感器放置在一組不同的靜態(tài)姿態(tài)位置即可實現(xiàn)。 通過一個有效的和快速的標定協(xié)議,利用有效的靜態(tài)檢測算子來可靠地檢測傳感器的靜態(tài)時間間隔,在這個方法中，我們假設(shè)

14、當?shù)刂亓铀俣萭和溫度是恒定的。 在校正中，我們第一次在靜態(tài)測量樣本間隔中進行加速度計校正。然后,我們利用這些結(jié)果，通過數(shù)值積分的方法實現(xiàn)陀螺儀的參數(shù)校正。,校正數(shù)據(jù)采集流程,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,3.3.1 系統(tǒng)誤差模型,令三軸加速度計和陀螺的坐標系為AF和GF，該坐標系為非正交坐標系； 令理想三軸加速度計和陀螺的坐標系為AOF和GOF，該坐標系為正交坐標系； 二者之間關(guān)系為： X軸方向AOF坐標系與AF坐標系一致； AOF的Y軸位于AF的X軸和Y軸構(gòu)成的平面中； GOF和GF的關(guān)系與上面類似；,校正數(shù)據(jù)采集流程,令載體坐標系為BF，為正交坐標系；對于小角度

15、的旋轉(zhuǎn)，非正交坐標系下的測量值SS可以旋轉(zhuǎn)變換至正交坐標系下的測量值SB：,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,3.3.1 系統(tǒng)誤差模型,令載體坐標系BF與AOF坐標系方向一致（重合），在該模型下，xz、xy、yz為0，旋轉(zhuǎn)矩陣可簡化為：,如上所述,陀螺儀和加速度計的測量值應(yīng)該引用同一個參考系,仍以AOF作為基準：,Wo和ws表示在AOF坐標系和GF坐標系下輸出的角速度,ao和as表示在AOF坐標系和GF坐標系下輸出的加速度,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,3.3.1 系統(tǒng)誤差模型,刻度因數(shù)：,零偏：,加速度計誤差模型：,陀螺誤差模型：,其中，va和

16、vg分別為加速度計的陀螺輸出的測量噪聲,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,3.3.2 系統(tǒng)基本標定模型,為了校準三軸加速度計，我們需要估計以下未知參數(shù)變量：,定義以下函數(shù)，其中忽略了加速度計的輸出噪聲：,放置IMU于M個位置，維持臨時的靜止，可獲得M個靜態(tài)狀態(tài)的加速度計數(shù)據(jù)（在非正交坐標系A(chǔ)F中測量），對每個靜態(tài)狀態(tài)的加速度計數(shù)據(jù)求取均值，可用以下的代價函數(shù)估計加速度計的未知參數(shù)：,其中，g為當?shù)氐闹亓铀俣?，通過LM法(Levenberg-Marquardt)求取上式的最小值,獲得加速度計的校正參數(shù)。,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,3.3.2

17、系統(tǒng)基本標定模型,為了校準三軸陀螺，我們需要估計以下未知參數(shù)變量：,在陀螺的校正過程中，我們需要采用加速度計的數(shù)據(jù)作為已知量，fai表示四元數(shù)的更新運算,以下的代價函數(shù)估計陀螺的未知參數(shù)：,其中，uak為通過修正后的加速度計數(shù)據(jù)計算獲得的單位四元數(shù)，ugk為通過陀螺更新獲得的四元數(shù)，通過LM法(Levenberg-Marquardt)求取上式的最小值,獲得陀螺的校正參數(shù)。,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,3.3.3 系統(tǒng)校正步驟,（1）靜態(tài)狀態(tài)檢測,在該校正方案中，區(qū)分IMU的運動狀態(tài)是校正執(zhí)行的關(guān)鍵，本系統(tǒng)在時間間隔Twait中，采用加速度計的方差構(gòu)建靜態(tài)檢測算子，判

18、斷系統(tǒng)的運動狀態(tài)：,通過判斷s(t)與系統(tǒng)設(shè)定閾值的大小，實現(xiàn)系統(tǒng)靜止和運動狀態(tài)的判斷，其中系統(tǒng)設(shè)定閾值在系統(tǒng)初始化階段Tinit，采用加速度計的原始數(shù)據(jù)進行計算,IMU數(shù)據(jù)運動與靜止狀態(tài)判斷結(jié)果,3.3.3 系統(tǒng)校正步驟,（3）Allan方差,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,Xsens Mti陀螺的Allan方差曲線,陀螺零偏的漂移可以用Allan方差進行描述，Allan方差可定義為：,X(t,k)：t時間間隔的平均值 K：時間間隔的數(shù)量，,計算陀螺每個軸的Allan方差，從1s至225s。由圖像可知，在t=50s時Allan方差收斂，在0至50s陀螺誤差的主要因素是

19、角度隨機游走，曲線收斂后證明角度隨機游走對陀螺零偏的影響因素減小，取這段時間的陀螺原始數(shù)據(jù)求取均值能夠較好的反映陀螺的常值零偏信息。,3.3.3 系統(tǒng)校正步驟,（3）Allan方差,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,柳貴福. 光學(xué)陀螺輸入輸出特性建模及補償技術(shù)研究D. 哈爾濱工程大學(xué), 2002. P51,3.3.3 系統(tǒng)校正步驟,（3）Allan方差,MEMS陀螺,光纖 陀螺,激光陀螺,（2）光纖陀螺的Allan方差曲線主要表現(xiàn)為斜率1/2（0.005s和200s）和斜率1/2（1000s以上）兩段，斜率為0的時間很短，可以認為零偏不穩(wěn)定影響很小或不存在,（3）激光陀螺

20、在整個Allan方差曲線上主要表現(xiàn)為斜率為-1的量化噪聲,（1）MEMS陀螺的Allan方差雙對數(shù)曲線表現(xiàn)為明顯的3段：0.011s之間斜率為-1/2,5s100s斜率為0,100s以上斜率為1/2，分別對應(yīng)角速隨機游走、零偏不穩(wěn)定性和角速率隨機游走,慣性儀器測試與數(shù)據(jù)分析 P145-P146,3.3.3 系統(tǒng)校正步驟,（4）校正流程,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,1.將IMU靜止放置，初始化50秒至1分鐘；,2.將IMU旋轉(zhuǎn)一個位置，靜止20秒鐘；,3.重復(fù)步驟2的操作，將IMU旋轉(zhuǎn)至與之前不同的位置，可以參考六面法的旋轉(zhuǎn)位置，重復(fù)步驟2的次數(shù)達到10次后結(jié)束；,4

21、.將IMU靜止放置20秒，結(jié)束數(shù)據(jù)采集,3.3.4 demo代碼,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,當?shù)刂亓铀俣?陀螺刻度因數(shù)初始化,加速度計零偏初始化,3.3.4 demo代碼,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,原始數(shù)據(jù)路徑：imu_tkbintest_data,加速度計原始數(shù)據(jù)：xsens_acc.mat;陀螺原始數(shù)據(jù)：xsens_gyro.mat,數(shù)據(jù)格式：時間戳（單位為秒） X軸數(shù)據(jù) Y軸數(shù)據(jù) Z軸數(shù)據(jù) 文本文檔格式：以UTF-8無BOM格式編碼，否則程序運行會報錯,/building.html,

22、/alberto_pretto/imu_tk,Ceres-slover數(shù)學(xué)庫安裝：,Ceres-slover數(shù)學(xué)庫安裝：,該方案優(yōu)點：不需要額外的設(shè)備輔助標定（例如三軸轉(zhuǎn)臺），對IMU的擺放位置沒有特殊的要求（相比于六面法標定），可以實現(xiàn)IMU的三軸陀螺的標定,該方案缺點：加速度計標定時要預(yù)先設(shè)定傳感器的零偏，若設(shè)置于實際值有很大偏差校正結(jié)果誤差很大,三.導(dǎo)航系統(tǒng)傳感器標定,3.3 MEMS IMU加速度計、陀螺儀無外部基準標定,三.導(dǎo)航系統(tǒng)傳感器標定,3.4 加速度計計算姿態(tài)角,Ax+Ay+Az=g,令加速度計三軸輸出數(shù)據(jù)為Ax，Ay，Az；當?shù)刂亓?/p>

23、速度為g，則有如下關(guān)系：,令旋轉(zhuǎn)后坐標系Xb軸與水平面XOY的夾角為pitch，則重力加速度在該軸的分量為：,Ax = sin(pitch)*g sin(pitch)=Ax/g; Cos(pitch)= （1-sin（pitch） =（1-（ Ax/g ）,三.導(dǎo)航系統(tǒng)傳感器標定,北東地（NED）坐標系,東北天（ENU）坐標系,俯仰,橫滾,航向,四元數(shù)法的能夠?qū)d體的剛體運動進行簡潔的描述,漢密爾頓四元數(shù)規(guī)則,JPL四元數(shù)規(guī)則,四.四元數(shù),四元數(shù)的逆 1 等于其共軛 ,四元數(shù)的乘法的表現(xiàn)形式,四.四元數(shù), 為 反對稱陣：,令：,由四元數(shù)表示的載體姿態(tài)矩陣:,四.四元數(shù),四元數(shù)的姿態(tài)微分方程形式:,式中：,四元數(shù)的增量更新采用四階龍格-庫塔法:,四階龍格-庫塔法中的加和運算，會使更新得到的增量四元數(shù)為非單位四元數(shù),需要進行歸一化,四.四元數(shù),四元數(shù)更新:,乘性四元數(shù)：,加性四元數(shù)：,四.四元數(shù),令系統(tǒng)的誤差四元數(shù)為 ，由于誤差四元數(shù) 在系統(tǒng)姿態(tài)更新中非常小，因此可以用小角度旋轉(zhuǎn)近似