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

1 武漢大學(xué)衛(wèi)星導(dǎo)航定位技術(shù)研究中心，武漢市珞喻路129號，430079 2 武漢夢芯科技有限公司，武漢市高新大道980號，430079

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

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

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

1 在線校準(zhǔn)模型及算法

1.1 IMU誤差建模

(1)

(2)

(3)

1.2 損失函數(shù)

1.2.1 加速度計損失函數(shù)

將第k個加速度計需要校準(zhǔn)的參數(shù)定義為：

(4)

靜止?fàn)顟B(tài)下，加速度計的比力輸出只與當(dāng)?shù)刂亓铀俣扔嘘P(guān)，則加速度計輸出估計函數(shù)為：

(5)

將導(dǎo)航坐標(biāo)系定義為北東地，載體坐標(biāo)系定義為前右下，此時：

(6)

式中，φ、γ分別為橫滾角、俯仰角，g為當(dāng)?shù)氐闹亓铀俣取?/p>

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

(7)

1.2.2 陀螺儀損失函數(shù)

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

將第k個陀螺儀需要校準(zhǔn)的參數(shù)定義為：

(8)

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

(9)

(10)

1.3 算法設(shè)計

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

(11)

(12)

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

數(shù)值積分使用經(jīng)典的4階Runge-Kutta積分，通過對2個歷元之間的角速度積分得到當(dāng)前歷元的姿態(tài)四元素，并通過遞推得到2個靜態(tài)位置之間的旋轉(zhuǎn)矩陣[9]。

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

圖1 在線校準(zhǔn)算法流程Fig.1 Flow chart of online calibration algorithm

2 實驗與結(jié)果分析

為驗證本文算法的有效性，分別進(jìn)行仿真實驗、實測校準(zhǔn)實驗和車載導(dǎo)航實驗。

2.1 仿真實驗

表1 ICM42688P誤差標(biāo)稱參數(shù)Tab.1 Error nominal parameters of ICM42688P

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

(13)

(14)

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

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

圖3 殘差統(tǒng)計Fig.3 Residuals statistics

2.2 實測校準(zhǔn)實驗

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

圖4 IMU陣列及校準(zhǔn)平臺Fig.4 IMU array and calibration platform

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

表2 校準(zhǔn)實驗均值統(tǒng)計Tab.2 Statistics of mean value of calibration experiments

表3 IMU陣列誤差參數(shù)范圍統(tǒng)計Tab.3 Error parameter range statistics of IMU array

2.3 車載導(dǎo)航實驗

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

圖5展示了一組GNSS中斷后動態(tài)導(dǎo)航的平面、北向和東向誤差?？梢钥闯觯?zhǔn)后的導(dǎo)航誤差精度整體有所提升，尤其是誤差較大的路段。GNSS中斷后的不同時間段誤差發(fā)散程度不一致是由IMU的隨機誤差引起的，隨機誤差越大，誤差發(fā)散越快。校準(zhǔn)后，能補償IMU陣列的確定性誤差，進(jìn)行數(shù)據(jù)融合時能有效降低隨機誤差，在隨機誤差較大的路段，校準(zhǔn)后的IMU陣列能大幅度降低隨機誤差，改善效果較為明顯。

圖5 動態(tài)導(dǎo)航誤差Fig.5 Dynamic navigation errors

3組不同GNSS中斷實驗的平面誤差RMS統(tǒng)計結(jié)果如表4所示(t_sta為起始導(dǎo)航時刻，t_sta+50 s為起始中斷時刻)?？梢钥闯?，IMU陣列校準(zhǔn)后，3組中斷實驗精度均有提升，平均提升19.1%。精度提升數(shù)值較小可能是因為所使用的IMU陣列的誤差數(shù)值較小，同時直接平均的數(shù)據(jù)融合方式對隨機誤差的降低有限；另外，大量的數(shù)值積分會存在計算誤差，使得校準(zhǔn)的數(shù)值精度下降，進(jìn)而影響動態(tài)導(dǎo)航的精度。

表4 動態(tài)導(dǎo)航平面RMS值Tab.4 Plane RMS of dynamic navigation

3 結(jié) 語

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