MEMSIMU的入門與應用

MEMSIMU的入門與應用1整理課件一、導航系統(tǒng)根本概念二、MEMS慣性器件；三、MEMS慣性器件的簡易標定；四、四元數(shù)；五、基于乘性四元數(shù)EKF濾波器2整理課件一、導航系統(tǒng)根本概念1.海里國際上采用1852m作為標準海里長度，1nm=1852m2.慣性級導航系統(tǒng)運行1h過程中在水平面上定位誤差等于1nm〔簡記1nm/h〕的慣性導航系統(tǒng)稱為慣性級導航系統(tǒng)3.慣性級陀螺地球自轉(zhuǎn)角速率為15.04107度/h.其千分之一為0.015度/h,稱為毫地轉(zhuǎn)率；將精度到達0.015度/h的陀螺稱為慣性級陀螺，往往也以1meru的量級(0.01度/h)表示慣性級陀螺精度4.加速度計高精度：<10-4g;中精度：10-4g至1mg;低精度：>1mg;慣性級導航系統(tǒng)對陀螺的最低要求為0.01度/h，加速度計精度的最低要求為1x10-4g3整理課件二.MEMS慣性器件1.消費級MEMS慣性器件InvensenseMPU6000/MPU6050Invensense公司的MPU6000/MPU6050六軸慣性器件是世界上首個六軸集成的低本錢MEMS慣性器件，該器件推出市場之后在各個領(lǐng)域〔特別是四軸飛行器〕得到了廣泛應用4整理課件二.MEMS慣性器件1.消費級MEMS慣性器件在2021年，apple的iPhone4采用了ST公司的三軸陀螺儀L3G4200D和三軸加速度計LIS331DLH作為的慣性傳感器。其它廠商隨即跟進，陀螺儀與加速度計逐漸成為智能的標配傳感器iPhone4IMU手機型號MEMS加速度計MEMS陀螺iPhone4LIS331DLHL3G4200D紅米MPU6050iPhone6MPU6700三星S6MPU6500三星S7LSM6SD3小米5ICG-20660L5整理課件二.MEMS慣性器件2.工業(yè)級MEMS慣性器件ADIS16405ADIS16488AXsensMti6整理課件二.MEMS慣性器件2.常見MEMS慣性器件參數(shù)比照加速度量程(最大)非線性度初始偏置軸間靈敏度噪聲密度偏置溫度系數(shù)輸出噪聲價格LSM303±16g

±60mg

0.15mg/sqrtHZ

±0.01量程/℃

MPU6050±16g

0.5%

±3%

±2%

0.4mg/sqrtHZ

±0.02量程/℃

MPU6500±16g

0.5%

±2%

0.3mg/sqrtHZ±0.026%量程/℃

BMI180±16g

0.5%

±25?mg

0.18mg/sqrtHZ

ADIS16405±18g

0.5%

±50mg

0.2°

0.5mg/sqrtHZ

±0.3mg/℃9mg

4500

ADIS16488±18g

0.5%

±0.035°

0.063mg/sqrtHZ

±0.0025mg/℃1.29mg

14500

7整理課件陀螺量程(最大)非線性度(量程）對準誤差初始偏置°/s線性加速度影響角速度隨機游走速率噪聲密度偏置溫度系數(shù)輸出噪聲°/srms價格（RMB）L3G4200D±2000

0.2%

±75°/s

0.03±0.04°/s/℃

MPU6050±2000

0.2%

±2%量程±20°/s

0.1

0.005±20°/s

0.05

MPU6500±2000

0.1%±2%量程±5°/s

0.01±0.24°/s/℃0.1

BMI180±2000

0.1%±3°/s

0.007

MAX21000±20000.2%1%±2°/s0.0090.450.009±2%停產(chǎn)ADIS16405±350±0.1%

±0.05°±3°/s

0.007

2

0.044±0.01°/s/℃0.8

4500

ADIS16488A±480

±0.01%

±0.05°

0.26

0.0059±0.0025

0.135

14500

二.MEMS慣性器件2.常見MEMS慣性器件參數(shù)比照8整理課件二.MEMS慣性器件2.MEMS慣性器件的常見技術(shù)參數(shù)〔2〕非線性度〔3〕初始偏置誤差MEMS慣性器件在開機后系統(tǒng)穩(wěn)定輸出后觀測信息中包含的誤差，可以通過靜態(tài)情況下的初始化求取均值消除〔1〕量程MEMS慣性器件在開機后系統(tǒng)穩(wěn)定輸出后觀測信息中包含的誤差，可以通過靜態(tài)情況下的初始化求取均值消除9整理課件二.MEMS慣性器件2.MEMS慣性器件的常見技術(shù)參數(shù)〔4〕對齊誤差參考文獻：MEMS陀螺儀中主要噪聲源的預測和管理ADI公司應用工程師MarkLooney三軸式陀螺儀對齊誤差MEMSIMU通常具有兩種類型的對齊誤差，它們相互關(guān)聯(lián)，但在系統(tǒng)級建模中具有不同應用：軸到封裝和軸到軸?！拜S到封裝對齊誤差〞描述陀螺儀相對于器件封裝上特定機械特性的對齊情況。將IMU安裝到系統(tǒng)后，如果系統(tǒng)無法支持慣性對齊，那么軸到封裝對齊誤差將成為整體對齊誤差的主要因素之一。系統(tǒng)與IMU的機械接口的機械缺陷也會增加整體對齊誤差?！拜S到軸對齊誤差〞描述各個陀螺儀旋轉(zhuǎn)軸相對于其他兩個陀螺儀的相對對齊精度。在系統(tǒng)可以實現(xiàn)簡單的對齊過程時，此參數(shù)影響最大，此時通常沿系統(tǒng)的慣性參考系中的一個軸直線移動整個組件(IMU已安裝在系統(tǒng)平臺上)，同時需要觀察傳感器。10整理課件二.MEMS慣性器件2.MEMS慣性器件的常見技術(shù)參數(shù)〔6〕隨機游走，噪聲密度隨機游走過程：當前觀測值完全由下一時刻觀測值加上現(xiàn)時噪聲決定，即n時刻的輸出由n-1時刻的輸出和隨機誤差和加和決定陀螺角度游走過程：寬帶角速率白噪聲積分的結(jié)果，即陀螺從0時刻累積的總角增量誤差表現(xiàn)為隨機游走，而每一時刻的等效角速率誤差表現(xiàn)為白噪聲噪聲密度：傳感器固有噪聲代表的是陀螺儀在靜態(tài)慣性和環(huán)境條件下運行時其輸出中的隨機振動。MEMSIMU數(shù)據(jù)手冊通常會提供速率噪聲密度(RND)參數(shù)來描述陀螺儀相對于頻率的固有噪聲。此參數(shù)通常使用單位°/s/√Hz，是預測特定濾波器配置固有噪聲的關(guān)鍵。單位：°/√hr功率譜N2的國際單位：〔rad/s〕2/Hz11整理課件〔6〕隨機游走，噪聲密度二.MEMS慣性器件2.MEMS慣性器件的常見技術(shù)參數(shù)信號方差強度q、功率譜密度S、隨機游走系數(shù)N之間的簡單換算關(guān)系：1、令陀螺輸出角速率方差的單位為〔rad/s〕2，離散系統(tǒng)等效鼓勵白噪聲的方差等于連續(xù)時間系統(tǒng)白噪聲方差強度與離散化周期的乘積：2、功率譜密度S〔單位〔rad/s〕2/Hz〕與方差q的關(guān)系：Qk=q*TsS=q/Ts3、角度隨機游走N〔單位°/√h〕與功率譜密度S的關(guān)系：N=√(36002*q)/6012整理課件二.MEMS慣性器件2.MEMS慣性器件的常見技術(shù)參數(shù)〔7〕偏置溫度系數(shù)〔溫飄〕MEMS慣性器件的零偏會受到環(huán)境溫度的影響而產(chǎn)生變化，針對這種影響，一種方案是對器件進行溫度標定，另一種方式是采用恒溫模式大疆

精靈3IMU13整理課件二.MEMS慣性器件2.MEMS陀螺誤差模型陀螺的誤差主要由兩局部構(gòu)成：漂移和刻度系數(shù)誤差。陀螺的隨機漂移誤差主要分為三種分量：逐次啟動漂移、慢變漂移、快變漂移陀螺的逐次啟動漂移與系統(tǒng)開機時刻的電氣參數(shù)、環(huán)境條件等隨機性因素相關(guān)。一旦系統(tǒng)啟動完畢，逐次啟動漂移造成的誤差量將保持在某一個固定值上14整理課件陀螺的慢變漂移是由于其工作過程中環(huán)境條件的隨機改變所造成的緩慢變化的誤差，由于其變換較為緩慢，與前后時刻的陀螺漂移存在一定的相關(guān)性，隨著時間點的接近依賴關(guān)系更加明顯，因此可以用一階馬爾可夫過程描述：快變漂移是在上述兩個漂移分量根底上的雜亂無章的高頻跳變，這種漂移分量可以抽象化為白噪聲過程：15整理課件二.MEMS慣性器件3.MEMS加速度計誤差模型速度計的誤差模型在組合導航系統(tǒng)設(shè)計中，一般只考慮隨機常值誤差，忽略相關(guān)誤差，因此加速度計誤差模型如式所示：16整理課件

MEMS陀螺儀誤差修正模型

MEMS加速度計誤差修正模型圖3.13KTD-565多功能三軸轉(zhuǎn)臺

(3.1)

(3.2)

三.導航系統(tǒng)傳感器標定3.1MEMSIMU加速度計、陀螺儀實驗室標定17整理課件位置坐標軸方向重力加速度gX軸Y軸Z軸X軸Y軸Z軸1東天南-0.008848-1.018200-0.0059962東北天-0.0317980.000362-1.0331963地東南0.9841780.029072-0.0051414西地南-0.0600091.019633-0.0091865天西南-1.054107-0.022041-0.0098946南西地-0.0366170.0000161.008865序號角速率輸入值(°/s)角速率輸出值(°/s)X軸Y軸Z軸X軸Y軸Z軸1200019.9564080.6324330.0200652-2000-20.030685-0.463136-0.04640430200-0.48642920.0317690.08837640-2000.542587-19.9020260.05770250020-0.005255-0.05406820.171637600-200.069309-0.054871-19.959693表4.2MEMS陀螺儀標定時各個軸向輸出值表4.1MEMS加速度計標定時各個軸向輸出值三.導航系統(tǒng)傳感器標定3.1MEMSIMU加速度計、陀螺儀實驗室標定18整理課件(3-3)

(3-4)

(3-5)

(3-6)

(3-7)

(3-8)

將載體分別靜止放置6個位置，采集加速度計的數(shù)據(jù):由式(3-3)、(3-4)、(3-5)可解修正模型的各個參數(shù)：加速度計標定參數(shù)求解方程三.導航系統(tǒng)傳感器標定3.1MEMSIMU加速度計、陀螺儀實驗室標定19整理課件三.導航系統(tǒng)傳感器標定3.1MEMSIMU加速度計、陀螺儀實驗室標定由于該MEMS陀螺儀無法敏感地球自轉(zhuǎn)角速度，IMU的陀螺X、Y、Z軸在三次轉(zhuǎn)動中的輸入角速度為：陀螺的標度因數(shù)和常值零偏：MEMS陀螺儀標定采用速率標定方式。將MEMSIMU固定于三軸轉(zhuǎn)臺的平臺上，通過給定固定速率值測量其對應輸出的方式進行實驗。具體步驟如下：〔1〕將待標定IMU安裝至三軸轉(zhuǎn)臺，IMU的軸向與轉(zhuǎn)臺軸向平行，系統(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〕中各次角速率實驗中的角速率平均值，用于標定計算。20整理課件三.導航系統(tǒng)傳感器標定3.1MEMSIMU加速度計、陀螺儀實驗室標定X軸MEMS陀螺標定試驗中，IMU的Y軸和Z軸角速率輸出：將X軸正反轉(zhuǎn)時分別采集的Y軸和Z軸數(shù)據(jù)的正反輸出分別求差，可得陀螺的安裝誤差系數(shù)同理，其它安裝誤差系數(shù)為：21整理課件MEMS陀螺儀輸出數(shù)學模型：MEMS加速度計輸出數(shù)學模型：(3-10)

(3-11)

三.導航系統(tǒng)傳感器標定3.1MEMSIMU加速度計、陀螺儀實驗室標定22整理課件三.導航系統(tǒng)傳感器標定3.2MEMSIMU加速度計六面法標定STDT00536-pointtumblesensorcalibration將MEMSIMU放置于六面立方體上，按照圖示順序翻轉(zhuǎn)6次，分別采集6個狀態(tài)的加速度計靜態(tài)數(shù)據(jù)，用于校正23整理課件三.導航系統(tǒng)傳感器標定3.3MEMSIMU加速度計、陀螺儀不依托額外設(shè)備實現(xiàn)標定參考論文：ArobustandeasytoimplementmethodforIMUcalibrationwithoutexternalequipmDEMO鏈接：s:///alberto_pretto/imu_tk視頻地址：s://youtube/watch?v=XqQCbkncVYI24整理課件三.導航系統(tǒng)傳感器標定3.3MEMSIMU加速度計、陀螺儀無外部基準標定

該校正方法是基于一個多位置方案能夠修正加速度計和陀螺儀的刻度因數(shù)誤差和對準誤差，同時也能夠估計傳感器的誤差。只需要將傳感器放置在一組不同的靜態(tài)姿態(tài)位置即可實現(xiàn)。通過一個有效的和快速的標定協(xié)議,利用有效的靜態(tài)檢測算子來可靠地檢測傳感器的靜態(tài)時間間隔,在這個方法中，我們假設(shè)當?shù)刂亓铀俣萭和溫度是恒定的。在校正中，我們第一次在靜態(tài)測量樣本間隔中進行加速度計校正。然后,我們利用這些結(jié)果，通過數(shù)值積分的方法實現(xiàn)陀螺儀的參數(shù)校正。校正數(shù)據(jù)采集流程25整理課件3.3MEMSIMU加速度計、陀螺儀無外部基準標定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，為正交坐標系；對于小角度的旋轉(zhuǎn)，非正交坐標系下的測量值SS可以旋轉(zhuǎn)變換至正交坐標系下的測量值SB：26整理課件3.3MEMSIMU加速度計、陀螺儀無外部基準標定3.3.1系統(tǒng)誤差模型

令載體坐標系BF與AOF坐標系方向一致〔重合〕，在該模型下，xz、xy、yz為0，旋轉(zhuǎn)矩陣可簡化為：如上所述,陀螺儀和加速度計的測量值應該引用同一個參考系,仍以AOF作為基準：Wo和ws表示在AOF坐標系和GF坐標系下輸出的角速度ao和as表示在AOF坐標系和GF坐標系下輸出的加速度27整理課件3.3MEMSIMU加速度計、陀螺儀無外部基準標定3.3.1系統(tǒng)誤差模型

刻度因數(shù)：零偏：加速度計誤差模型：陀螺誤差模型：其中，va和vg分別為加速度計的陀螺輸出的測量噪聲28整理課件3.3MEMSIMU加速度計、陀螺儀無外部基準標定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ù)。29整理課件3.3MEMSIMU加速度計、陀螺儀無外部基準標定3.3.2系統(tǒng)根本標定模型為了校準三軸陀螺，我們需要估計以下未知參數(shù)變量：在陀螺的校正過程中，我們需要采用加速度計的數(shù)據(jù)作為量，fai表示四元數(shù)的更新運算以下的代價函數(shù)估計陀螺的未知參數(shù)：其中，uak為通過修正后的加速度計數(shù)據(jù)計算獲得的單位四元數(shù)，ugk為通過陀螺更新獲得的四元數(shù)，通過LM法(Levenberg-Marquardt)求取上式的最小值,獲得陀螺的校正參數(shù)。30整理課件3.3MEMSIMU加速度計、陀螺儀無外部基準標定3.3.3系統(tǒng)校正步驟

〔1〕靜態(tài)狀態(tài)檢測在該校正方案中，區(qū)分IMU的運動狀態(tài)是校正執(zhí)行的關(guān)鍵，本系統(tǒng)在時間間隔Twait中，采用加速度計的方差構(gòu)建靜態(tài)檢測算子，判斷系統(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é)果31整理課件3.3.3系統(tǒng)校正步驟

〔3〕Allan方差3.3MEMSIMU加速度計、陀螺儀無外部基準標定XsensMti陀螺的Allan方差曲線陀螺零偏的漂移可以用Allan方差進行描述，Allan方差可定義為：X(t,k)：t時間間隔的平均值K：時間間隔的數(shù)量，計算陀螺每個軸的Allan方差，從1s至225s。由圖像可知，在t=50s時Allan方差收斂，在0至50s陀螺誤差的主要因素是角度隨機游走，曲線收斂后證明角度隨機游走對陀螺零偏的影響因素減小，取這段時間的陀螺原始數(shù)據(jù)求取均值能夠較好的反映陀螺的常值零偏信息。32整理課件3.3.3系統(tǒng)校正步驟

〔3〕Allan方差3.3MEMSIMU加速度計、陀螺儀無外部基準標定柳貴福.光學陀螺輸入輸出特性建模及補償技術(shù)研究[D].哈爾濱工程大學,2002.P5133整理課件3.3.3系統(tǒng)校正步驟

〔3〕Allan方差MEMS陀螺光纖陀螺激光陀螺〔2〕光纖陀螺的Allan方差曲線主要表現(xiàn)為斜率1/2〔0.005s和200s〕和斜率1/2〔1000s以上〕兩段，斜率為0的時間很短，可以認為零偏不穩(wěn)定影響很小或不存在〔3〕激光陀螺在整個Allan方差曲線上主要表現(xiàn)為斜率為-1的量化噪聲〔1〕MEMS陀螺的Allan方差雙對數(shù)曲線表現(xiàn)為明顯的3段：0.01~1s之間斜率為-1/2,5s~100s斜率為0,100s以上斜率為1/2，分別對應角速隨機游走、零偏不穩(wěn)定性和角速率隨機游走慣性儀器測試與數(shù)據(jù)分析P145-P14634整理課件3.3.3系統(tǒng)校正步驟

〔4〕校正流程3.3MEMSIMU加速度計、陀螺儀無外部基準標定1.將IMU靜止放置，初始化50秒至1分鐘；2.將IMU旋轉(zhuǎn)一個位置，靜止20秒鐘；3.重復步驟2的操作，將IMU旋轉(zhuǎn)至與之前不同的位置，可以參考六面法的旋轉(zhuǎn)位置，重復步驟2的次數(shù)到達10次后結(jié)束；4.將IMU靜止放置20秒，結(jié)束數(shù)據(jù)采集35整理課件3.3.4demo代碼3.3MEMSIMU加速度計、陀螺儀無外部基準標定當?shù)刂亓铀俣韧勇菘潭纫驍?shù)初始化加速度計零偏初始化36整理課件3.3.4demo代碼3.3MEMSIMU加速度計、陀螺儀無外部基準標定原始數(shù)據(jù)路徑：imu_tk\bin\test_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.htmls:///alberto_pretto/imu_tkCeres-slover數(shù)學庫安裝：Ceres-slover數(shù)學庫安裝：37整理課件該方案優(yōu)點：不需要額外的設(shè)備輔助標定〔例如三軸轉(zhuǎn)臺〕，對IMU的擺放位置沒有特殊的要求〔相比于六面法標定〕，可以實現(xiàn)IMU的三軸陀螺的標定該方案缺點：加速度計標定時要預先設(shè)定傳感器的零偏，假設(shè)設(shè)置于實際值有很大偏差校正結(jié)果誤差很大三.導航系統(tǒng)傳感器標定3.3MEMSIMU加速度計、陀螺儀無外部基準標定38整理課件三.導航系統(tǒng)傳感器標定3.4加速度計計算姿態(tài)角Ax2+Ay2+Az2=g2令加速度計三軸輸出數(shù)據(jù)為Ax，Ay，Az；當?shù)刂亓铀俣葹間，那么有如下關(guān)系：令旋轉(zhuǎn)后坐標系Xb軸與水平面XOY的夾角為pitch，那么重力加速度在該軸的分量為：Ax=sin(pitch)*gsin(pitch)=Ax/g;Cos(pitch)=√〔1-sin〔pitch〕2〕=√〔1-〔Ax/g〕2〕39整理課件三.導航系統(tǒng)傳感器標定北東地〔NED〕坐標系東北天〔ENU〕坐標系EU

ON俯仰橫滾航向40