IMU校準(zhǔn)，到底校準(zhǔn)什么？

簡(jiǎn)介

最近在學(xué)習(xí)傳感器相關(guān)的知識(shí)，在國(guó)外的一篇論文學(xué)習(xí)過(guò)程中，覺(jué)得這篇論文的校準(zhǔn)方法是個(gè)不錯(cuò)的參考。這種校準(zhǔn)簡(jiǎn)單且比較魯棒的算法，操作簡(jiǎn)單，且除了偏移與比例系數(shù)，還可以估計(jì)出傳感器 xyz 軸相對(duì)于標(biāo)準(zhǔn)正交三軸的偏移(因?yàn)閭鞲衅鞯奈锢?xyz 軸可能不是標(biāo)準(zhǔn)正交的)，不過(guò)相比六面校準(zhǔn)，其算法的計(jì)算過(guò)程要復(fù)雜許多。其校準(zhǔn)過(guò)程也并不復(fù)雜：1）初始靜止一段時(shí)間2）旋轉(zhuǎn) IMU 到另一個(gè)角度之后靜止3）等待一段采樣時(shí)間4）繼續(xù)旋轉(zhuǎn)，重復(fù) 2）3）步驟，達(dá)到預(yù)定的采樣次數(shù)5）估計(jì)參數(shù)思路理想的 IMU，x,y,z 三軸是獨(dú)立正交的，理想的三軸坐標(biāo)系，加速度計(jì)的理想坐標(biāo)系可以記作 AOF，實(shí)際中的 IMU 的坐標(biāo)軸并不是標(biāo)準(zhǔn)正交的，其與標(biāo)準(zhǔn)正交坐標(biāo)系有一定的偏移，將各軸的偏移記作 β，之后建立誤差模型，建立代價(jià)函數(shù)，進(jìn)而求解估計(jì)參數(shù)。加速度計(jì)部分坐標(biāo)軸定義假設(shè)現(xiàn)有一個(gè)標(biāo)準(zhǔn)正交坐標(biāo)系，記作 s(B)，當(dāng)前坐標(biāo)軸記作 s(S) ，當(dāng)前軸可能是非正交的，從 y 軸到 z 軸的偏移記作 β(yz)，其他的以此類(lèi)推，那么 從 s(S) 轉(zhuǎn)換到正交坐標(biāo)系 s(B) 的表達(dá)式就可以寫(xiě)作如下形式：

T 是其中的旋轉(zhuǎn)矩陣，為：

以此為基礎(chǔ)，將加速度計(jì)的標(biāo)準(zhǔn)正交軸記作 a(B)，加速度計(jì)當(dāng)前坐標(biāo)軸記作 a(S) ，但上面的旋轉(zhuǎn)矩陣 T 有點(diǎn)復(fù)雜，需要對(duì)其進(jìn)行簡(jiǎn)化，現(xiàn)給出兩個(gè)坐標(biāo)軸選取時(shí)的條件：1）定義加速度計(jì)的標(biāo)準(zhǔn)正交軸為 AOF，加速度計(jì)實(shí)際的坐標(biāo)軸記作 AF(AF不一定正交)，將 AOF 的 x 軸與 AF 的 x 軸重合2）AOF 的 y 軸定義在 AF 的x 與 y 軸組成的平面上按照上述的兩個(gè)原則建立坐標(biāo)軸，那么在上述旋轉(zhuǎn)矩陣的 β(xz)，β(xy) 就消除為 0，因?yàn)锳OF 與 AF 的 x 軸已經(jīng)重合了，因此 x 軸相對(duì) y，z 軸的偏移就消除掉了。因?yàn)?AOF 的 y 軸在 AF 的 x，y 軸組成的平面上，因此 y 相對(duì)于 x 的偏移也消除掉了，即 β(yx) = 0。那么針對(duì)加速度計(jì)，上述坐標(biāo)軸的表達(dá)式就可以寫(xiě)作：

加速度計(jì)模型在加速度計(jì)的零偏與比例系數(shù)的基礎(chǔ)上，加上上述的坐標(biāo)軸偏移，就可以得到加速度計(jì)的模型。加速度計(jì)的比例系數(shù)矩陣記作 K(a)：

加速度計(jì)的零偏向量記作 b(a)：

加速度計(jì)整體的誤差模型就可以寫(xiě)作：

v 表示測(cè)量噪聲，a(O) 表示標(biāo)準(zhǔn)軸的加速度值，a(S) 表示真實(shí)軸上的加速度計(jì)測(cè)量值。此方程中，含有 9 個(gè)未知量，三軸的軸偏移，三軸的比例系數(shù)，三軸的零偏。將代估變量記作 θ：

將測(cè)量噪聲忽略掉，誤差模型就可以寫(xiě)作：

代價(jià)函數(shù)建立對(duì)于此方程，不用六面校準(zhǔn)，而是任意角度的加速度計(jì)數(shù)據(jù)的話，采樣一定量的數(shù)據(jù)之后，可以利用這個(gè)原理來(lái)列出其代價(jià)函數(shù)：靜止?fàn)顟B(tài)下，加速度計(jì)三軸的數(shù)據(jù)的平方和等于重力加速度 g其代價(jià)函數(shù)就可以寫(xiě)作：

以上的代價(jià)函數(shù)是有加法的，運(yùn)用最小二乘法來(lái)估計(jì)參數(shù)比較困難，因此使用 LM 算法來(lái)進(jìn)行非線性估計(jì)。陀螺儀部分陀螺儀的校準(zhǔn)需要依賴于加速度計(jì)的校準(zhǔn)?？傮w思路為：1.在進(jìn)行完上一步的加速度計(jì)估計(jì)參數(shù)的計(jì)算之后，利用加速度計(jì)已經(jīng)校準(zhǔn)過(guò)的參數(shù)矩陣對(duì)之前采樣的數(shù)據(jù)進(jìn)行校正2.在采樣時(shí)的每一次靜止時(shí)的加速度計(jì)數(shù)據(jù)進(jìn)行校正后，可以得到一個(gè)較為準(zhǔn)確的重力加速度向量值3.當(dāng)前靜止時(shí)的重力加速度向量記作初始重力加速度向量，然后在旋轉(zhuǎn)IMU時(shí)，使用陀螺儀的數(shù)據(jù)對(duì)重力向量進(jìn)行積分，可以得到一個(gè)旋轉(zhuǎn)后(到下一次靜止采集數(shù)據(jù))的重力向量預(yù)測(cè)值4.再次使用加速度計(jì)的數(shù)據(jù)得到一個(gè)重力加速度向量值5.利用加速度計(jì)得到的重力加速度向量與陀螺儀積分得到的重力加速度向量做差，平方的方式，建立代價(jià)函數(shù)6.求解代價(jià)函數(shù)，得到陀螺儀的估計(jì)參數(shù)誤差模型建立陀螺儀與加速度計(jì)的坐標(biāo)軸應(yīng)該選擇相同的參考坐標(biāo)軸，也就是 AOF ，但對(duì)于陀螺儀來(lái)說(shuō)，因?yàn)檫x取的加速度計(jì)的標(biāo)準(zhǔn)正交軸，一些坐標(biāo)軸的偏移量就不能消除了，還是需要加上，即：

其中，T 為旋轉(zhuǎn)矩陣，W(O) 表示陀螺儀的正交標(biāo)準(zhǔn)軸，與 AOF 重合，W(S) 是當(dāng)前的陀螺儀軸比例系數(shù)與偏移系數(shù)矩陣都與加速度計(jì)的形式是相同的：

誤差模型的形式也是相同的：

在后續(xù)的計(jì)算中，測(cè)量誤差 v 可以忽略。代價(jià)函數(shù)建立按照上述的步驟說(shuō)明，代價(jià)函數(shù)使用加速度計(jì)校準(zhǔn)后得到的準(zhǔn)確的重力加速度向量與陀螺儀積分得到的重力加速度向量的各項(xiàng)差的平方和來(lái)建立：

同樣，使用 LM 算法來(lái)進(jìn)行參數(shù)的估計(jì)