imu_tk源碼分析

目錄

• imu_tk源碼分析
• 前言
• 算法思路簡介
• 代碼解讀
• • test_imu_calib
  • calibrateAccGyro函數
  • calibration函數
• 參考論文

前言

imu_tk是一個開源的用于低精度IMU標定的代碼，github地址為：imu_tk，主要用了非線性優化方法做加速度計和陀螺儀的標定，標定參數包括bias（3個），標度因子（3個），非正交安裝誤差（3個），一共12個參數。

算法思路簡介

詳細的算法思路可以參考它給出的兩篇論文，加速度計的標定利用了加速度計在靜止狀態下的三軸數據模值等于重力加速度這一條件，采集加速度計在不同狀態的靜止數據，通過優化算法即可求出標定參數。加速度計單獨即可完成標定，陀螺儀的標定需要加速度計數據的參與，所以需要先標定好加速度計。在標定陀螺儀時，IMU從靜止狀態A，通過一段時間，到了靜止狀態B，通過陀螺儀可以算出來$T_{A\to B}$的旋轉矩陣，而通過加速度數據也可以算出來$T_{A\to B}$的旋轉矩陣，最小化這一差異即可實現優化求解。

代碼解讀

test_imu_calib

importAsciiData： 通過函數讀入.mat格式的文件，加速度數據和陀螺儀數據是分開讀取的。mu_tk::TIMESTAMP_UNIT_SEC是在imu_tk/io_utils.h文件中定義的一個枚舉類型，可以實現不同時間單位之間的轉換，TIMESTAMP_UNIT_SEC = 1，即為轉換為秒。這里要注意數據的單位。

setBias：加速度計設定了初始的bias。

setScale：陀螺儀設定了初始的標度因子。

setInitStaticIntervalDuration：采集的數據持續的時間，可以方便選擇部分時間用于IMU參數的標定。

setGravityMagnitude：設置重力加速度數值，不同地方的重力加速度是不同的，這塊可能需要改下，默認設置的數值是9.81744。

enableVerboseOutput：相當于一個debug開關，打開后會輸出更多的調試信息，方便調試。

enableAccUseMeans：函數默認設置為否，意思是把所有的加速度靜止段數據作為優化函數的輸入，而不是每一靜止段的平均值。

calibrateAccGyro：主要的代碼思路在這個函數中。

calibrateAccGyro函數

calibrateAcc函數是加速度計的標定。

initialInterval：根據設定的時間，找到對應的最后一個數據的索引，這里在尋找過程中用到了二分法。

dataVariance：求整個時間段的數據的方差，此方差是作為判斷IMU靜止狀態時的一個閾值使用，再一個這個方差是所有數據的方差。

staticIntervalsDetector：這是個靜止狀態監測器，利用數據方差（三個數據方差的模值）作為判斷靜止與否的閾值條件，對于第i個數據，通過計算其左領域，右臨域共win_size大小的數據方差， win_size是函數輸入參數，默認大小為101，按照100Hz的輸出數據速率，大約為1s的數據。之后得到靜止段數據所對應的開始索引和結束索引。

extractIntervalsSamples：該函數是找到靜止區域索引所對應的慣導數據，靜止區域的數據個數要大于interval_n_samps，也就是1s。如果使用平均值的模式，就用時間段的中間的時間戳作為這個平均值的時間戳。
注意以下代碼

if(verbose_output_)cout<<"Accelerometers calibration: extracted "<<extracted_intervals.size()<<" intervals using threshold multiplier "<<th_mult<<" -> ";// TODO Perform here a quality testif( extracted_intervals.size() < min_num_intervals_) //至少是12個數據(有12個參數需要標定){if( verbose_output_) cout<<"Not enough intervals, calibration is not possible"<<endl;continue;}

min_num_intervals_設定的是靜止區域的個數，靜止區域的個數要不小于待標定的參數的個數，對于加速度計，待標定參數個數是9個，如果更改標定模型，參數min_num_intervals_可能需要改變。
接下來就是用ceres優化庫，去解待標定參數，這里比較關鍵的是構建優化函數。

calibration函數

h文件中描述了加速度計和陀螺儀采用的標定模型。

參考論文

D. Tedaldi, A. Pretto and E. Menegatti, “A Robust and Easy to Implement Method for IMU Calibration without External Equipments”. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2014), May 31 - June 7, 2014 Hong Kong, China, Page(s): 3042 - 3049

A. Pretto and G. Grisetti, “Calibration and performance evaluation of low-cost IMUs”. In Proceedings of the 20th IMEKO TC4 International Symposium, Sep. 15 - 17, 2014 Benevento, Italy, pages: 429 - 434

總結

以上是生活随笔為你收集整理的imu_tk源码分析的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。