imu_tk源碼分析

目錄

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

前言

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

算法思路簡介

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

代碼解讀

test_imu_calib

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

setBias：加速度計(jì)設(shè)定了初始的bias。

setScale：陀螺儀設(shè)定了初始的標(biāo)度因子。

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

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

enableVerboseOutput：相當(dāng)于一個debug開關(guān)，打開后會輸出更多的調(diào)試信息，方便調(diào)試。

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

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

calibrateAccGyro函數(shù)

calibrateAcc函數(shù)是加速度計(jì)的標(biāo)定。

initialInterval：根據(jù)設(shè)定的時間，找到對應(yīng)的最后一個數(shù)據(jù)的索引，這里在尋找過程中用到了二分法。

dataVariance：求整個時間段的數(shù)據(jù)的方差，此方差是作為判斷IMU靜止?fàn)顟B(tài)時的一個閾值使用，再一個這個方差是所有數(shù)據(jù)的方差。

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

extractIntervalsSamples：該函數(shù)是找到靜止區(qū)域索引所對應(yīng)的慣導(dǎo)數(shù)據(jù)，靜止區(qū)域的數(shù)據(jù)個數(shù)要大于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個數(shù)據(jù)(有12個參數(shù)需要標(biāo)定){if( verbose_output_) cout<<"Not enough intervals, calibration is not possible"<<endl;continue;}

min_num_intervals_設(shè)定的是靜止區(qū)域的個數(shù)，靜止區(qū)域的個數(shù)要不小于待標(biāo)定的參數(shù)的個數(shù)，對于加速度計(jì)，待標(biāo)定參數(shù)個數(shù)是9個，如果更改標(biāo)定模型，參數(shù)min_num_intervals_可能需要改變。
接下來就是用ceres優(yōu)化庫，去解待標(biāo)定參數(shù)，這里比較關(guān)鍵的是構(gòu)建優(yōu)化函數(shù)。

calibration函數(shù)

h文件中描述了加速度計(jì)和陀螺儀采用的標(biāo)定模型。

參考論文

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。