簡介

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

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

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

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

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

加速度計整體的誤差模型就可以寫作：

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

將測量噪聲忽略掉，誤差模型就可以寫作：

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

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

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

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

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

同樣，使用 LM 算法來進行參數(shù)的估計