基于PE-ANGO 的MIMU 現場標定方法

喬美英，姚文豪，高柯飛，杜 衡，趙開東

（1.河南理工大學 電氣工程與自動化學院，焦作 454003；2.河南省智能裝備直驅技術與控制國際聯合實驗室，焦作 454003）

近年來隨著微機電系統（Micro Electro Mechanical System,MEMS）的快速發展，微慣性測量單元（Miniature Inertial Measurement Unit,MIMU）因低成本、小尺寸、功耗低和可靠性高等優點被廣泛應用在運動檢測及生物醫學等領域[1-3]，然而此類傳感器通常受到各種誤差的影響[4]，需要進行不定期的標定。傳統的標定方法是使用高精度設備提供的可靠基準信息來完成[5]，不滿足低成本傳感器的使用需求，且標定步驟通常較為繁瑣，不利于非專業人員操作。因此，需要研究一種標定精度高、標定步驟簡單的傳感器現場快速標定方法。

國內外已有學者對現場標定方法做了大量研究。針對加速度計現場標定，其依據的事實是：理想情況下，加速度計測量的比力矢量等于當地重力加速度矢量。如Shokri-Ghaleh 等人[6]提出使用不等極限布谷鳥優化算法獲取加速度計誤差參數，該方法不需要外部設備輔助。Qureshi 等人[7]從誤差模型中推導出代價函數方程，使用牛頓迭代法估算加速度計誤差參數。該方法迭代速度快，然而對初值有著嚴格的要求且容易陷入局部最優。Hassan 等人[8]采用廣義非線性最小二乘法估計加速度計誤差參數，提高了收斂速度，但數據采集時間較長。喬美英等人[9]通過改進布谷鳥優化算法，提高了加速度計標定的精度和速度，然而其數據采集依賴雙軸轉臺。低成本陀螺儀因無法檢測地球自轉速度[10]，使得其標定基準很難確定。部分標定方法仍需要借助勻角速度羅盤[11]等類似設備為陀螺儀提供基準，從而建立與加速度計標定類似的目標函數，并使用優化算法求得誤差參數。此外，使用優化算法雖然無需求解目標函數的導數信息，在標定精度上表現較好，但其較為依賴初始值的質量，質量較差的初始解可能會影響算法的尋優性能，甚至使算法陷入局部最優。

本文在上述研究的基礎上，提出一種無需使用任何輔助設備的MIMU 現場標定方法。相比于傳統方法，本文綜合考慮了常見誤差（比例因子誤差、非正交誤差和零偏）以及傳感器之間的非對準誤差。具體實現過程為：首先對加速度計和陀螺儀建立誤差模型，模型包含了常見誤差以及非對準誤差。然后推導了用于加速度計和陀螺儀現場標定的目標函數，并采用基于先驗知識增強的自適應北蒼鷹優化（Prior Knowledge Enhancement Adaptive North Goshawk Optimization,PE-ANGO）算法進行參數尋優。為了進一步方便MIMU 現場標定，本文引入了手持MIMU采集數據的方法，重新設計了靜態檢測器的檢測閾值、檢測周期和初始靜止時間，通過手持MIMU 采集36次靜態位置信息，即可實現加速度計和陀螺儀的現場標定。

1 MIMU 誤差模型

1.1 加速度計誤差模型

靜止狀態下，加速度計無論處于何種姿態，其測量矢量應與當地重力矢量完全一致?；谶@一事實，加速度計現場標定可以在不借助任何設備輔助的情況下完成。加速度計標定通常先建立加速度計的測量模型，接著利用當地重力矢量推導出標定加速度計參數的目標約束函數，從而將加速度計標定問題轉化為多參數尋優問題。

加速度計測量時通常受到多種誤差源影響，其中最重要的是比例因子誤差、軸間非正交誤差以及零偏誤差[4]。因此，加速度計測量模型可以建模為：

其中，ma=[maxmaymaz]T為加速度計測量值；ua=[uaxuayuaz]T為加速度計測量的真實值；表示比例因子誤差；ba=[baxbaybaz]T表示加速度計零偏誤差；非正交誤差Ka可以轉化為上三角矩陣或下三角矩陣[12]，表達式如式(2)所示。

其中，θyz、θzy、θzx分別表示加速度計三個測量軸的非正交偏差。

一般的標定方法是基于靜止時加速度計測量的比力矢量與當地重力加速度一致的事實進行的?；诙辔恢梅硕ǖ乃枷?，通過采集N組測量數據并最小化式(3)所示的目標函數J(a)，從而得到誤差參數[6]。

其中，G=[GaxGayGaz]T表示當地重力矢量。

總目標函數中待求解的變量共9 個，將其整理成向量的形式，如式(4)所示。

于是，加速度計的標定問題就轉化為求J(a)的最小化問題。

1.2 陀螺儀誤差模型

陀螺儀獨立標定時的一般誤差模型可以表示為：

其中，mg=[mgxmgymgz]T表示陀螺儀測量值；ug=[ugxugyugz]T表示真實的角速度；Kg表示陀螺儀測量坐標系的非正交誤差矩陣；Sg表示陀螺儀測量軸的比例因子誤差矩陣，是一個三維對角矩陣；bg=[bgxbgybgz]T表示陀螺儀處于靜止狀態時測量軸的輸出偏差，即零偏。

陀螺儀的獨立標定無法解決傳感器之間的非對準誤差，該誤差會造成導航定位誤差累積，使得最終結果偏離真實軌跡。因此，將非對準誤差考慮進陀螺儀誤差模型：

其中，R表示陀螺儀測量坐標系與加速度計坐標系之間的非對準誤差矩陣，其本質上是一個旋轉矩陣。

為了計算方便，將式(6)改寫為式(7)形式：

于是，陀螺儀誤差模型中待標定項為ER和bg，共12 個標定參數。

陀螺儀的標定分兩步進行，首先對陀螺儀的零偏誤差進行估計。靜止情況下，陀螺儀的真實值應滿足式(9)所示的關系。

其中，Nb表示靜置期間采集的數據總數。

利用式(10)可以將bg從陀螺儀測量模型中去除，接著處理誤差矩陣ER。由于加速度計和陀螺儀都可以測量載體姿態變化，且理想情況下，加速度計和陀螺儀測量的載體姿態變化應完全一致[13]。依據這一事實，可以分析移動一次MIMU 前后加速度計和陀螺儀測量單位重力矢量的關系。假設單位重力矢量在加速度計測量坐標系的初始位置為，與之對應的，單位重力矢量在陀螺儀測量坐標系的初始位置為，需要注意的是是用已標定加速度計測量得到的。通過將MIMU 旋轉一定角度，將會得到下一個靜止狀態的單位重力矢量位置。參考Chao 等人[10]的方法，由加速度計測量得到的下一個靜止狀態單位重力矢量位置為：

傳統描述陀螺儀坐標系旋轉的方法是使用順序歐拉角旋轉，比如假設陀螺儀旋轉時先繞x軸旋轉，再繞y軸旋轉，最后繞z軸旋轉。這種旋轉次序由人為主觀確定，在傳感器測量運動期間會引入系統誤差。為了提高陀螺儀測量結果表征的精度，本文引入同步正交旋轉角度矢量（Simultaneous Orthogonal Rotation Angle）方法[14]表示陀螺儀的旋轉。

其中，Φ示同步正交旋轉角度矢量；Tt表示陀螺儀采樣間隔；Ax、Ay、Az分別表示陀螺儀繞x、y、z旋轉的角度。

假設Φ的旋轉軸v在采樣間隔Tt內保持恒定，則其旋轉軸v和模值φ可用來表示陀螺儀實際旋轉軸和角度，即：

利用Φ的旋轉軸v和模值φ，可以建立陀螺儀本征坐標系中與該旋轉相關的旋轉矩陣：

其中，c和s分別代表cos(φ)和sin(φ)。

由陀螺儀計算旋轉之后的單位矢量位置為：

綜上所述，可以建立陀螺儀標定的目標函數J(g)為：

其中，Ns表示靜態位置總數。

與加速度計標定相同，將陀螺儀待標定參數組成一個向量：

由此，可以將陀螺儀標定問題轉化為求目標函數J(g)最小化參數問題。

2 先驗知識增強的北蒼鷹優化算法

優化問題的求解方法通?？煞譃榇_定性方法和隨機方法。確定性方法基于目標函數的導數信息進行優化問題的求解，能夠精確求解最優線性或凸非線性問題，然而對于復雜問題的求解容易給出局部最優解?；陔S機的優化算法在解決多參數優化問題時通常能給出較為精確的結果，但算法結果常受限于初始值質量以及全局搜索和局部搜索之間的平衡。北蒼鷹優化算法由Dehghani 等人于2021 年提出，該算法對北方蒼鷹狩獵行為進行數學建模，并將狩獵過程分為獵物識別與攻擊階段（全局搜索階段）和獵物逃逸與追逐階段（局部搜索階段）。北蒼鷹優化算法相較于其他傳統優化算法，如粒子群優化算法（Particle Swarm Optimization,PSO）、遺傳算法（Genetic Algorithm,GA）、鯨魚優化算法（Whale Optimization Algorithm,WOA）等，具有參數少、迭代速度快和精確度高的優點[15]。

本文對傳統北蒼鷹優化（North Goshawk Optimization,NGO）算法進行改進，提出了基于先驗知識增強的自適應北蒼鷹優化算法（PE-ANGO）。首先針對初始值問題，提出使用先驗知識預測最優解位置，并在最優解附近模擬爆炸現象產生算法初始種群；然后針對算法全局搜索與局部搜索平衡問題，通過改進自適應參數R增強算法局部搜索的能力。

先驗知識的獲取來源于傳感器上次標定的結果或傳感器技術手冊。通過計算一階后向差分預測此次標定最優解的位置，并模擬爆炸現象在預測的最優個體位置附近產生初始種群。這種方法可以集中算法注意力去搜索最有潛力的區域，具體做法為：假設此次正在進行第k次標定，將k-1次的最優個體位置與k-2次的最優個體位置進行一階后向差分，利用所得差分矢量補償第k-1次標定得到的最優個體位置，從而預測第k次最優個體位置。以此位置為中心，模擬爆炸現象生成第k次標定的初始種群。此做法可以理解為北蒼鷹種群除隨機選取獵物之外，還會學習獵物移動的趨勢，從而提前預測獵物移動方向和位置，極大地提高了下次狩獵的成功幾率與速度。

該方法將之前標定獲得的最優個體位置作為先驗知識，并利用第k-1次與k-2次最優個體位置構造一階后向差分方程，如式(21)所示。

其中，ΔX表示前兩次標定最優個體位置的差分矢量；Xk-1,best、Xk-2,best分別表示第k-1次和第k-2次標定的最優個體位置。

若無k-2次最優個體位置，則令Xk-2,best=Xk-1,best。將ΔX補償到第k-1次最優個體位置，以此預測第k次最優個體位置，預測公式如式(22)所示。

圖1 爆炸效果圖Fig.1 The explosion rendering

爆炸現象基于正態分布進行模擬，利用MATLAB得到符合正態分布的一組隨機數：

其中，aver代表正態分布的均值，即預測最優個體的某一維度的值。σ表示正態分布的標準差，該值決定了爆炸的范圍，該值越大，則爆炸范圍越大，粒子落在遠處的幾率就大，反之就小。X為m×n維符合正態分布的隨機數矩陣。

以二維坐標系原點(0,0)為中心，模擬爆炸現象在x、y軸的[ -1 1]內生成200 個二維數據點，觀察不同標準差對數據點分布的影響，如圖2 所示。

圖2 不同標準差對數據點分布的影響Fig.2 Effect of different standard deviation on the distribution of data points

在實際應用中，標準差σ的選取與信任先驗知識的程度有關，越信任先驗知識，該值選取越小，即盡可能集中在預測的最優個體位置附近生成初始種群，相反則應取值大一點。

此外，為了平衡算法全局搜索與局部搜索的關系，改進了原算法中參數R的自適應取值。NGO 中參數R的大小將影響北蒼鷹的捕獵行為。R越大，種群個體的搜索半徑就越大，有利于快速搜索問題解空間中較優區域，使算法具有較好的跳出局部最優的能力。然而在算法后期，較大的搜索半徑會拖延算法的收斂速度，甚至出現算法發散的情況。因此，參數R的值應在算法前期盡可能取大一些以保證算法全局尋優能力，在算法中后期應取小一些以使算法在較優區域做更精細的搜索。改進的自適應因子R為：

其中，t表示當前迭代次數；T表示最大迭代次數；w表示自適應因子的衰減程度，即衰減因子。

原算法中，參數R以線性函數形式調整，難以平衡算法的全局搜索和局部搜索階段。本算法結合原算法優秀的全局搜索能力，增大局部搜索階段在算法迭代過程中的占比，平衡算法全局與局部搜索，提高了算法的收斂速度和尋優精度。

3 基于手持MIMU 數據采集的標定過程

3.1 手持MIMU 數據采集方式

為了使標定步驟簡單快速、易于操作，本文引入了手持MIMU 進行數據采集的方法。2014 年，Tedaldi等[16]提出了確定靜態檢測閾值、靜止周期和初始靜止時間的方法，使用的靜態檢測算子c(t)如式(25)所示。

在靜態位置檢測期間，確定c(t)是否小于檢測閾值。如果c(t)小于檢測閾值，則認為加速度計處于靜止位置；否則，加速度計被認為是移動的。檢測閾值應在現場確定，以減少不穩定因素可能導致閾值設置不合適，從而影響數據采集效率。另外，設置靜止周期用于采集傳感器在準靜態位置一段時間內的測量值，并將該時段內測量值的平均值作為該位置的數據點，這種做法可以降低傳感器的噪聲干擾。靜止周期也應通過實驗確定。本文得到合適的檢測閾值為1.74×10-4g，靜止周期為0.4 s。初始靜止時間用于確定陀螺儀零偏誤差，使用Allan 方差分析法確定了初始靜止時間為18 s。

針對準靜態位置的選取，給出了以下細節：首先為了防止計算過程中方程出現奇異性，在考慮不理想位置數據點的情況下，測量位置應嚴格不低于待標定參數的個數。此外，為了方便非專業人員操作，本方法并不限制特定的靜態位置次序，但需要注意的是，為了降低陀螺儀測量噪聲影響，在旋轉MIMU 時應盡量使角度大一點，且旋轉速度不宜過快。最后，由于傳感器的數學模型忽略了耦合誤差，所以靜態位置的選擇應盡可能對稱，以減少這些誤差的影響。移動過程除上述之外沒有其他要求。

在實驗中，為了獲得更多樣化的測量數據，靜態位置應盡可能覆蓋多個方向。IMU 需要移動和暫停至少18 次，這是待校準參數數量的兩倍。本文在空間中將IMU 視為正方體，并選取了正方體6 個平面和12個邊作為靜態位置，并重復一次采集步驟，共采集了36 個靜態位置數據。這種做法可以減少異常值的影響，同時覆蓋盡可能多的方向。

3.2 標定步驟

基于上述MIMU 數據采集方法，可以方便地進行加速度計和陀螺儀數據采集。數據采集基于實驗室自主設計的MIMU 核心板，通過MIMU 核心板上的LED閃爍提示用戶成功完成一次靜態數據的采集。

具體的標定步驟實施如下：

步驟一：手持MIMU 按3.1 所述36 個靜態位置移動，每個靜態位置數據采集成功后MIMU 核心板會通過LED 閃爍提示用戶，此時可以移動到下一個靜態位置進行數據采集。將3.1 所述36 個靜態位置采集完成之后進行標定工作；

步驟二：進行加速度計的標定工作，基于1.1 節提出的加速度計目標函數，并使用PE-ANGO 進行參數尋優可以快速得到加速度計誤差參數；

步驟三：使用第二步得到的加速度計誤差參數補償加速度計輸出數據，并按照1.2 節所述方法標定陀螺儀；

步驟四：得到所有誤差參數，并補償傳感器輸出。

4 實驗驗證

4.1 基于PE-ANGO 的加速度計標定仿真實驗

為了驗證所提算法的性能，以加速度計標定為例設計了仿真實驗。仿真環境為MATLAB R2021b，實驗選取單位球體上1000 個點作為不同位置下的重力矢量，以此模擬當地重力加速度即真實值，并按照說明書提供的參考誤差參數（如表1 所示）進行設定。

表1 加速度計參考誤差參數（由說明書提供）Tab.1 Reference error parameters of accelerometer(provided by the manual)

假設加速度計實際誤差參數與說明書提供的參考誤差參數相差不大，實際誤差參數的設定如表2 所示。

表2 加速度計實際誤差參數Tab.2 Actual error parameters of accelerometer

將表1設定為第k-1次標定誤差參數，結合式(21)和式(22)，由于第k-2次誤差參數不可獲取，因此令Xk-1,Best=Xk-2,Best，此時預測得到=Xk-1,Best，模擬爆炸現象在預測得到的最優個體附近產生算法初始種群，PE-ANGO 算法初始參數設置如表3 所示。

表3 PE-ANGO 初始參數設置Tab.3 Initial parameter setting for PE-ANGO

為了對比NGO 和PE-ANGO 的收斂速度，設定相同的收斂精度，分別使用兩種算法進行仿真標定實驗，兩種算法的迭代次數與目標函數值變化如圖3 所示。

圖3 NGO 和PE-ANGO 迭代曲線對比Fig.3 Comparison of NGO and PE-ANGO iteration curves

由圖3 可知，使用PE-ANGO 標定時，算法初始種群目標函數值極低，極大地增強了算法的收斂速度，這得益于使用先驗知識預測最優個體位置并模擬爆炸現象產生初始種群的策略。由于第k次標定的最優個體位置已被預測，且仿真時加速度計處于理想環境，因此，σ值取的較小。在實際應用中，如果傳感器長時間處于惡劣環境下，此時應降低對預測值的信任，選擇較大的σ值以增強算法在爆炸外圍的搜索能力。

為了對比NGO 和PE-ANGO 的魯棒性及穩定性，設定相同的最大迭代次數，分別使用兩種算法進行50次仿真標定實驗，兩種算法每次仿真標定完成所需的迭代次數如圖4 所示。

圖4 NGO 和PE-ANGO 標定50 次仿真結果Fig.4 Simulation results of NGO and PE-ANGO calibration for 50 times

在50 次仿真實驗中，分別使用NGO 和PE-ANGO標定所需平均迭代次數和標準差（Standard Deviation,SD）如表4 所示。

表4 相同收斂精度下NGO 與PE-ANGO 的50 次仿真統計結果Tab.4 Simulation statistical results of NGO and PE-ANGO with the same convergence accuracy for 50 times

設定迭代次數為400 次的條件下，再進行50 次NGO 和PE-ANGO 的標定仿真實驗，對比兩種算法的收斂精度，標定仿真實驗的結果如表5 所示。均方根誤差（Root Mean Square Error,RMSE）說明了PE-ANGO算法可以精確地標定出加速度計誤差參數。此外，通過標準差也可以看出PE-ANGO 的魯棒性優于NGO。與NGO 算法相比，PE-ANGO 算法的標定精度提高了一個數量級。

表5 相同迭代次數下50 次仿真標定實驗結果Tab.5 Experimental results of simulation calibration with the same number of iterations for 50 times

4.2 實測實驗

為了驗證本文方法的有效性，設計了MIMU 實測實驗。實驗使用基于MPU9250 的自主設計MIMU 核心板進行數據采集，如圖5 所示。室內溫度24 ℃。

圖5 MIMU 核心板Fig.5 MIMU core circuit board

加速度計18 個靜態位置數據點的分布情況如圖6所示，其包含了正方體的6 個平面和12 個邊的數據。

然后將MIMU 按照第3 節所述方法進行標定，靜態檢測閾值設置為1.74×10-4g，靜止周期設置為0.4 s，初始靜止時間設置為18 s。利用標定得到的誤差參數補償加速度計數據點，并繪制補償后數據點的分布情況，如圖7 所示。

圖7 加速度計校準后數據點分布Fig.7 Data point distribution after accelerometer calibration

為了驗證本文方法對陀螺儀標定的有效性，另外采集一組MIMU 測量數據，通過對補償前后陀螺儀測量的角速度進行積分，估計出橫滾角和俯仰角。除此之外，使用文獻[5]提出的優化多位置標定方法標定MIMU 并估計的橫滾角和俯仰角與本文方法標定MIMU 并估計的橫滾角和俯仰角進行對比。圖8 展示了三種方法估計的橫滾角和俯仰角。

圖8 基于三種方法的姿態估計對比Fig.8 Comparison of attitude estimation based on three methods

關于MIMU 運動情況說明如下：

1）在開始的15 s 內，將MIMU 水平靜置。此時可以觀察到三種方法估計的橫滾角和俯仰角都很接近參考值。

2）在15～25 s 內，將MIMU 拿起并旋轉，此時姿態誤差開始累積；

3）在25～35 s 內，將MIMU 恢復原狀并靜置，可以觀察到采用本文方法補償后的橫滾角和俯仰角更接近初始位置，表明本文方法對降低陀螺儀誤差是有效的。表6 給出了三種方法最終累積的姿態偏差。由表6可以看出，采用本文方法對MIMU 標定后，與未校準方法相比，俯仰角累積誤差的抑制效果提高了約89%，橫滾角累積誤差的抑制效果提高了約87%。與文獻[5]提出的優化多位置標定方法相比，本文方法對俯仰角和橫滾角累積誤差的抑制效果分別提高了71%和68%。

表6 三種方法的姿態偏差Tab.6 Three methods of attitude deviation

5 結論

本文提出了一種對MIMU 現場標定的新方法。首先，為了解決傳統矢量約束目標函數導致的方向性誤差，建立了標量約束目標函數。為了增強智能優化算法初始解的質量，提出了一種基于PE-ANGO 算法用于目標函數的多參數尋優。此外，為了便于非專業人員進行MIMU 現場標定工作，引入了手持MIMU 進行傳感器數據采集。為了驗證本文方法的性能，分別進行了仿真和實驗。

仿真結果表明，利用本文算法可以有效獲取較為精確的傳感器誤差參數，其精度滿足低成本IMU 的使用。通過統計數據分析證明了PE-ANGO 相較于NGO不僅有收斂速度和精度的優勢，其穩定性和魯棒性也優于NGO。實際測量實驗驗證了本文方法的有效性，與傳統標定方法對比，本文方法對俯仰角和橫滾角累積誤差的抑制效果分別提高了71%和68%，表明本文方法可以在不借助復雜設備的前提下標定加速度計誤差參數，能夠替代傳統方法應用于傳感器現場標定。

然而，基于智能優化算法的標定技術目前仍處于探索階段，未來將繼續專注傳感器誤差標定，爭取實現慣性傳感器標定的在線應用。