LM優化算法的IMU陣列在線校準

劉國念 江金光 吳家驥 龔一民 杜 瑩

1 武漢大學衛星導航定位技術研究中心，武漢市珞喻路129號，430079 2 武漢夢芯科技有限公司，武漢市高新大道980號，430079

低成本的MEMS IMU由于隨機誤差較大，應用于慣性導航系統時誤差增長較快。利用多個傳感器構建IMU陣列，通過不同的融合方式融合冗余的觀測數據能有效降低隨機誤差[1]。但由于集成電路制造工藝的缺陷，每個IMU的輸出都會存在因零偏、比例因子和非正交[2]偏離理想值引起的差異。此外，多個傳感器放置在一起時，安裝誤差也會導致IMU的輸出存在差異。因此，為了更好地運用IMU，在使用前需要對傳感器進行校準。

傳統的IMU陣列的校準需要通過昂貴的精密轉臺提供所需的已知激勵[3-5]。但轉臺的使用增加了IMU陣列的使用成本，因此，不使用轉臺的IMU陣列在線校準方法成為研究熱點。在靜態條件下，可利用地球重力加速度作為激勵校準加速度計[6]誤差參數，但由于地球自轉角速度數值較小，無法從MEMS IMU的噪聲中分離，缺少參考激勵使得陀螺儀誤差校準方法較少使用。Rohac等[7]使用激光陀螺獲得參考激勵的精確數值，但增加了額外成本。Carlsson等[8]提出一種塊坐標下降法(block coordinate descent, BCD)聯合估計誤差參數(包含IMU之間的距離)和載體的運動參數，但該方法需要提供較大激勵，難以控制輸入。

本文提出一種IMU陣列在線校準方法，通過對IMU陣列的誤差參數構建相應的損失函數，利用LM優化算法估計加速度計和陀螺儀的誤差參數。該方法無需任何外部參考設備，只需在手機支架上獲得一系列靜置和運動數據。

1 在線校準模型及算法

1.1 IMU誤差建模

(1)

(2)

(3)

1.2 損失函數

1.2.1 加速度計損失函數

將第k個加速度計需要校準的參數定義為：

(4)

靜止狀態下，加速度計的比力輸出只與當地重力加速度有關，則加速度計輸出估計函數為：

(5)

將導航坐標系定義為北東地，載體坐標系定義為前右下，此時：

(6)

式中，φ、γ分別為橫滾角、俯仰角，g為當地的重力加速度。

由式(6)可以看出，每個靜態位置存在橫滾角和俯仰角2個未知數，因此加速度計在每個位置多輸出了1個冗余量。若要校準加速度計的所有參數，至少需要放置12個或者9個非共面的位置。本文采用一種多個位置的靜態校準方式校準加速度計，將加速度計校準參數的損失函數建模為：

(7)

1.2.2 陀螺儀損失函數

式中，tc為冷水平均溫度，℃；t2′為冷水進口溫度，℃；t2″為冷水出口溫度，℃；Rec為冷水側雷諾數；V2為冷水流速，m/s；vc為冷水運動粘度，m/s；λc為冷水側導熱系數，W/m·K；Prc為冷水側普朗特數；Nu2為冷水側努塞爾系數；h2為冷水側換熱系數，W/m2·K.

將第k個陀螺儀需要校準的參數定義為：

(8)

由于MEMS工藝制造的IMU噪聲太大，地球自轉角速度無法分離而不能成為參考源。靜置的加速度輸出通過校準后具有很好的穩定性，且能得到各個靜態位置重力的投影值，本文將其作為陀螺儀校準的參考值。在假定陀螺儀是無偏的前提下，即陀螺儀的數據通過一定時間內的平均可以得到零偏，利用2個靜態位置之間轉動過程中陀螺儀的數據，通過數值積分方法得到2個靜態位置之間的旋轉矩陣。利用靜態位置和旋轉矩陣估計下一個靜態位置的重力投影估計值(式(9))，進而構建陀螺儀損失函數(式(10))：

(9)

(10)

1.3 算法設計

式(7)和式(10)是非線性函數，本文利用LM優化算法迭代求解出待定誤差參數最優解。LM優化算法對于初值的設定比較嚴格，初值誤差較大會使迭代進入局部最優。因此，除了陀螺儀的零偏，將加速度計和陀螺儀的確定性誤差初值都設定為理想值，N個靜態位置的橫滾角和俯仰角的初值使用加速度計的原始輸出計算：

(11)

(12)

其中，[p]i為向量p中的第i維數值。

數值積分使用經典的4階Runge-Kutta積分，通過對2個歷元之間的角速度積分得到當前歷元的姿態四元素，并通過遞推得到2個靜態位置之間的旋轉矩陣[9]。

圖1為在線校準算法流程，首先利用加速度計的方差構建一個靜態檢測器，將靜態和動態數據分離；然后使用LM算法和靜態數據對式(7)進行迭代求解，當整體誤差和小于閾值時，加速度計校準完成；最后利用加速度計校準后的靜態數據和動態數據對式(10)進行迭代求解，得到陀螺儀的誤差參數。

圖1 在線校準算法流程Fig.1 Flow chart of online calibration algorithm

2 實驗與結果分析

為驗證本文算法的有效性，分別進行仿真實驗、實測校準實驗和車載導航實驗。

2.1 仿真實驗

表1 ICM42688P誤差標稱參數Tab.1 Error nominal parameters of ICM42688P

利用文獻[10]的開源MATLAB慣導工具箱中的軌跡生成器進行如式(13)和(14)的拓展，得到IMU陣列的仿真數據，其頻率為100 Hz：

(13)

(14)

圖2 IMU陣列分布Fig.2 Distribution of IMU array

將校準結果與設定的誤差參數作差得到參數的殘差值，結果如圖3所示。從圖3(a)看出，加速度計和陀螺儀的零偏精度分別達到1.3×10-4m/s2和1.3×10-3dps；從圖3(b)看出，加速度計和陀螺儀的尺度變換矩陣精度分別達到3.3×10-5和7.9×10-5，校準結果與設定的誤差參數值一致性較好。

圖3 殘差統計Fig.3 Residuals statistics

2.2 實測校準實驗

實測校準實驗使用實驗室內部設計的一款IMU陣列，其中IMU使用InvenSense公司的ICM系列產品。為兼顧IMU陣列的性能和MCU管腳的使用情況，所設計的陣列由9個IMU構成3×3的矩陣，如圖4(a)所示。在IMU陣列的驅動設計過程中，使用串行外設接口(serial peripheral interface, SPI)通信協議，并利用ICM42688P內部的FIFO獲取原始數據，減少SPI片選信號的開關次數，提高系統的時間同步可靠性。加速度計量程設置為±2g，陀螺儀量程設置為±250 dps，量程范圍與常用的車載量程設置相同，便于將校準參數應用于車載導航解算中。

圖4 IMU陣列及校準平臺Fig.4 IMU array and calibration platform

陣列校準平臺如圖4(b)，該校準平臺上是一個手機支架，可以進行360°手動旋轉，并且在數據采集過程中，能使陣列數據在運動后短時間內穩定為靜態數據，同時保證轉動角速度控制在量程范圍內。為降低加速度的噪聲以及減少Runge-Kutta積分過程造成的誤差，將IMU陣列在校準平臺上的靜置時間設定為8～10 s，動態間隔時間設定為1～4 s，每組校準實驗采集35～50個靜態和動態數據。表2、3分別是8組校準實驗的均值和誤差參數范圍統計。可以看出，加速度計零偏及尺度變換波動范圍分別為-83～59 mg和-0.005～0.008，陀螺儀零偏及尺度變換波動范圍分別為-1.1～0.54dps和0.013～0.016。ICM42688P的各項誤差數值都偏小，因此對傳感器補償后性能提升有限制。

表2 校準實驗均值統計Tab.2 Statistics of mean value of calibration experiments

表3 IMU陣列誤差參數范圍統計Tab.3 Error parameter range statistics of IMU array

2.3 車載導航實驗

使用圖4(a)中的IMU陣列進行車載動態組合導航實驗。將IMU陣列數據通過直接平均的方式擬合成一個虛擬IMU，并利用虛擬IMU與GNSS進行松組合導航；通過后處理設置GNSS中斷，中斷時間為20 s，中斷間隔為60 s，比較GNSS中斷后校準前后的導航誤差。參考設備為高精度、具有雙頻RTK的NovAtel CPT6，定位精度可達cm級。由于ICM42688P的加速度計高程方向存在低頻噪聲、誤差較大，因此本文只評價導航結果的平面誤差。

圖5展示了一組GNSS中斷后動態導航的平面、北向和東向誤差。可以看出，校準后的導航誤差精度整體有所提升，尤其是誤差較大的路段。GNSS中斷后的不同時間段誤差發散程度不一致是由IMU的隨機誤差引起的，隨機誤差越大，誤差發散越快。校準后，能補償IMU陣列的確定性誤差，進行數據融合時能有效降低隨機誤差，在隨機誤差較大的路段，校準后的IMU陣列能大幅度降低隨機誤差，改善效果較為明顯。

圖5 動態導航誤差Fig.5 Dynamic navigation errors

3組不同GNSS中斷實驗的平面誤差RMS統計結果如表4所示(t_sta為起始導航時刻，t_sta+50 s為起始中斷時刻)。可以看出，IMU陣列校準后，3組中斷實驗精度均有提升，平均提升19.1%。精度提升數值較小可能是因為所使用的IMU陣列的誤差數值較小，同時直接平均的數據融合方式對隨機誤差的降低有限；另外，大量的數值積分會存在計算誤差，使得校準的數值精度下降，進而影響動態導航的精度。

表4 動態導航平面RMS值Tab.4 Plane RMS of dynamic navigation

3 結 語

本文提出一種在線IMU陣列校準方式，分別對加速度計和陀螺儀構建損失函數，利用求解非線性函數的LM優化算法，迭代得出IMU陣列的各個誤差項。仿真實驗結果顯示，加速度計校準的零偏和尺度變換誤差殘差精度達到1.3×10-4m/s2和3.3×10-5，陀螺儀校準的零偏和尺度變換誤差殘差精度達到1.3×10-3dps和7.9×10-5，誤差參數設定值與校準值一致性較好。3組實物測試結果顯示，校準后IMU陣列的動態導航精度平均提升19.1%，表明該方法能有效校準IMU陣列的誤差參數，提高動態導航性能。