imu_tk源碼分析

目錄

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

前言

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

算法思路簡介

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

代碼解讀

test_imu_calib

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

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

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

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

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

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

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

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

calibrateAccGyro函數

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

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

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

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

extractIntervalsSamples：該函數是找到靜止區(qū)域索引所對應的慣導數據，靜止區(qū)域的數據個數要大于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_設定的是靜止區(qū)域的個數，靜止區(qū)域的個數要不小于待標定的參數的個數，對于加速度計，待標定參數個數是9個，如果更改標定模型，參數min_num_intervals_可能需要改變。
接下來就是用ceres優(yōu)化庫，去解待標定參數，這里比較關鍵的是構建優(yōu)化函數。

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源码分析的全部內容，希望文章能夠幫你解決所遇到的問題。

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