本文介紹一篇 關于IMU 標定的經典論文，論文收錄于 ICRA14，在論文中作者介紹了如何不適用外部設備標定 IMU 加速度和角速度偏差、尺度系數、軸偏移參數。

論文鏈接：https://readpaper.com/paper/2021503353、https://readpaper.com/paper/2211578699

項目鏈接：https://github.com/JzHuai0108/imu_tk_matlab

---

1. Sensor Error Model

首先介紹傳感器誤差模型，令 ${\mathbf{a}}^O$ 表示為理想情況下的加速度數據，${\mathbf{a}}^S$ 表示為實際的加速度數據，${\mathbf{T}}^a=?\begin{array}{ccc}1& ?{\alpha }_{yz}& {\alpha }_{zy}\\ 0& 1& ?{\alpha }_{zx}\\ 0& 0& 1\end{array}?$ 表示從 ${\mathbf{a}}^S$ 到 ${\mathbf{a}}^O$ 的旋轉變換。${\mathbf{b}}^a=?\begin{array}{l}{b}_x^a\\ b_y^a\\ b_z^a\end{array}?$ 為加速度偏差，${\mathbf{K}}^a=?\begin{array}{ccc}{s}_x^a& 0& 0\\ 0& s_y^a& 0\\ 0& 0& s_z^a\end{array}?$ 為加速度尺度系數，${\mathbf{\nu }}^a$ 為加速度測量噪聲，則可以得到加速度誤差模型：
$${\mathbf{a}}^O={\mathbf{T}}^a{\mathbf{K}}^a\left({\mathbf{a}}^S+{\mathbf{b}}^a+{\mathbf{\nu }}^a\right)$$

同樣也可以得到角速度誤差模型，令 ${\mathbf{T}}^g=?\begin{array}{ccc}1& ?{\gamma }_{yz}& {\gamma }_{zy}\\ {\gamma }_{xz}& 1& ?{\gamma }_{zx}\\ ?{\gamma }_{xy}& {\gamma }_{yx}& 1\end{array}?$，${\mathbf{K}}^g=?\begin{array}{ccc}{s}_x^g& 0& 0\\ 0& s_y^g& 0\\ 0& 0& s_z^g\end{array}?$，${\mathbf{b}}^g=?\begin{array}{l}{b}_x^g\\ b_y^g\\ b_z^g\end{array}?$，${\mathbf{\nu }}^g$ 為角速度測量噪聲，則角速度誤差模型為：

$${\mathbf{\omega }}^O={\mathbf{T}}^g{\mathbf{K}}^g\left({\mathbf{\omega }}^S+{\mathbf{b}}^g+{\mathbf{\nu }}^g\right)$$

---

2. Basic Calibration Framework

加速度標定我們需要估計的未知參數為：
$${\theta }^{acc}=\left[{\alpha }_{yz},{\alpha }_{zy},{\alpha }_{zx},s_x^a,s_y^a,s_z^a,b_x^a,b_y^a,b_z^a\right]$$

此時我們可以忽略測量噪聲，則加速度誤差模型簡化為：
$${\mathbf{a}}^O={\mathbf{T}}^a{\mathbf{K}}^a\left({\mathbf{a}}^S+{\mathbf{b}}^a\right)$$

正如在傳統的多位置策略中，我們將 IMU 置于 $M$ 個不同的位置，在每一個靜止周期內讀取加速度測量值 ${\mathbf{a}}_k^S$，我們可以使用以下損失函數來估計加速度誤差模型參數：
$$\mathbf{L}\left({\mathbf{\theta }}^{acc}\right)=\sum _{k=1}^M{\left(\Vert \mathbf{g}{\Vert }^2?{\Vert h\left({\mathbf{a}}_k^S,{\mathbf{\theta }}^{acc}\right)\Vert }^2\right)}^2$$

其中，$||\mathbf{g}\Vert$ 是當地重力加速度幅值。損失函數程序為：

function [res_vector] = accCostFunctLSQNONLIN(E, a_hat, magnitude)misalignmentMatrix = [1, -E(1), E(2); 0, 1, -E(3); 0, 0, 1];scalingMtrix = diag([E(4), E(5), E(6)]);a_bar = misalignmentMatrix*scalingMtrix*(a_hat + (diag([E(7), E(8), E(9)])*ones(3, size(a_hat,2))));% Magnitude taken from tables if(nargin<3)magnitude = 9.81744;endresiduals = zeros(length(a_bar(1,:)), 1);for i = 1:length(a_bar(1,:))residuals(i,1) = (magnitude^2 - (a_bar(1,i)^2 + a_bar(2,i)^2 + a_bar(3,i)^2))^2;endres_vector = residuals;end

我們使用同樣的靜止周期來標定陀螺儀。在這里我們通過對初始靜止時刻角速度值求平均來得到角速度偏差。這樣我們需要求解的參數簡化為：
$${\mathbf{\theta }}^{gyro}=\left[{\gamma }_{yz},{\gamma }_{zy},{\gamma }_{xz},{\gamma }_{zx},{\gamma }_{xy},{\gamma }_{yx},s_x^g,s_y^g,s_z^g\right]$$

我們使用標定后的加速度數據作為參考，給定一個初始的重力向量，對角速度數據進行積分，我們可以估計最終的重力向量，則損失函數可以寫為：
$$\mathbf{L}\left({\mathbf{\theta }}^{gyro}\right)=\sum _{k=2}^M{\Vert {\mathbf{u}}_{a,k}?{\mathbf{u}}_{g,k}\Vert }^2$$

其中，${\mathbf{u}}_{a,k}$ 是標定后的加速度向量，${\mathbf{u}}_{g,k}$ 是估計后的重力向量。角速度積分這里使用的是 RK4，不過目前 IMU 的采樣頻率都很高了，一般很少再使用了。

---

3. Calibration Procedure

A. Static Detector

IMU 標定的準確性非常依賴于靜止和運動時間間隔的準確區分，為了標定加速度計我們使用靜止周期，標定陀螺儀我們使用兩個靜態周期之間的動態時間間隔。我們這里使用基于方差的靜止檢測器，對于時間周期長度 $t_{wait}$ 秒，我們有加速度 $\left({\mathbf{a}}_x^t,{\mathbf{a}}_y^t,{\mathbf{a}}_z^t\right)$，然后我們計算標準差：
$$?(t)=\sqrt{{\left[{\mathrm{var}}_{t_w}\left({\mathbf{a}}_x^t\right)\right]}^2+{\left[{\mathrm{var}}_{t_w}\left({\mathbf{a}}_y^t\right)\right]}^2+{\left[{\mathrm{var}}_{t_w}\left({\mathbf{a}}_z^t\right)\right]}^2}$$

我們通過比較方標準差$?(t)$ 是否大于某一閾值來區分靜止和運動狀態。我們將初始方差 ${?}_{init}$ 擴大整數倍來作為閾值。下圖是靜止檢測器的檢測結果，這里整數倍為6倍。

---

B. Runge-Kutta Integration

下面簡單介紹下四階龍格庫塔法，這里主要用在陀螺儀的標定。四元數微分方程為：
$$\mathbf{f}(\mathbf{q},t)=\dot{\mathbf{q}}=\frac{1}{2}\mathbf{\Omega }(\mathbf{\omega }(t))\mathbf{q}$$

其中，$\Omega (\mathbf{\omega })$ 是一個反對稱矩陣，形式為：
$$\mathbf{\Omega }(\mathbf{\omega })=?\begin{array}{cccc}0& ?{\omega }_x& ?{\omega }_y& ?{\omega }_z\\ {\omega }_x& 0& {\omega }_z& ?{\omega }_y\\ {\omega }_y& ?{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& ?{\omega }_x& 0\end{array}?$$

四階龍格庫塔法原理為：
$$\begin{array}{ll}{\mathbf{q}}_{k+1}={\mathbf{q}}_k+\Delta t\frac{1}{6}\left({\mathbf{k}}_1+2{\mathbf{k}}_2+2{\mathbf{k}}_3+{\mathbf{k}}_4\right)& \\ {\mathbf{k}}_i=\mathbf{f}\left({\mathbf{q}}^{(i)},t_k+c_i\Delta t\right),& \text{?for?}i=1\\ {\mathbf{q}}^{(i)}={\mathbf{q}}_k,& \text{?for?}i>1\\ {\mathbf{q}}^{(i)}={\mathbf{q}}_k+\Delta t{\sum }_{j=1}^{i?1}{a}_{ij}{\mathbf{k}}_j,& \end{array}$$

各個參數為：
$$\begin{array}{c}{c}_1=0,c_2=\frac{1}{2},c_3=\frac{1}{2},c_4=1\\ a_{21}=\frac{1}{2},a_{31}=0,a_{41}=0\\ a_{32}=\frac{1}{2},a_{42}=0,a_{43}=1\end{array}$$

最終得到積分后的四元數，還需要再轉化為單位四元數，整個RK4 程序為：

function [R] = rotationRK4(omega, dt)omega_x = omega(1,:);omega_y = omega(2,:);omega_z = omega(3,:);num_samples = length(omega_x);q_k = fromOmegaToQ([omega_x(1); omega_y(1); omega_z(1)], [dt])';q_next_k = q_k; % was [0; 0; 0; 0]; changed by Huaifor i = 1:num_samples - 1% first Runge-Kutta coefficientq_i_1 = q_k;OMEGA_omega_t_k = ...[0 -omega_x(i) -omega_y(i) -omega_z(i);omega_x(i) 0 omega_z(i) -omega_y(i);omega_y(i) -omega_z(i) 0 omega_x(i);omega_z(i) omega_y(i) -omega_x(i) 0 ];k_1 = (1/2)*OMEGA_omega_t_k*q_i_1;% second Runge-Kutta coefficientq_i_2 = q_k + dt*(1/2)*k_1;OMEGA_omega_t_k_plus_half_dt = ...[0 -(omega_x(i) + omega_x(i + 1))/2 -(omega_y(i) + omega_y(i + 1))/2 -(omega_z(i) + omega_z(i + 1))/2;(omega_x(i) + omega_x(i + 1))/2 0 (omega_z(i) + omega_z(i + 1))/2 -(omega_y(i) + omega_y(i + 1))/2;(omega_y(i) + omega_y(i + 1))/2 -(omega_z(i) + omega_z(i + 1))/2 0 (omega_x(i) + omega_x(i + 1))/2;(omega_z(i) + omega_z(i + 1))/2 (omega_y(i) + omega_y(i + 1))/2 -(omega_x(i) + omega_x(i + 1))/2 0 ];k_2 = (1/2)*OMEGA_omega_t_k_plus_half_dt*q_i_2;% third Runge-Kutta coefficientq_i_3 = q_k + dt*(1/2)*k_2;OMEGA_omega_t_k_plus_half_dt = ...[0 -(omega_x(i) + omega_x(i + 1))/2 -(omega_y(i) + omega_y(i + 1))/2 -(omega_z(i) + omega_z(i + 1))/2;(omega_x(i) + omega_x(i + 1))/2 0 (omega_z(i) + omega_z(i + 1))/2 -(omega_y(i) + omega_y(i + 1))/2;(omega_y(i) + omega_y(i + 1))/2 -(omega_z(i) + omega_z(i + 1))/2 0 (omega_x(i) + omega_x(i + 1))/2;(omega_z(i) + omega_z(i + 1))/2 (omega_y(i) + omega_y(i + 1))/2 -(omega_x(i) + omega_x(i + 1))/2 0 ];k_3 = (1/2)*OMEGA_omega_t_k_plus_half_dt*q_i_3;% forth Runge-Kutta coefficientq_i_4 = q_k + dt*1*k_3;OMEGA_omega_t_k_plus_dt = ...[0 -omega_x(i + 1) -omega_y(i + 1) -omega_z(i + 1);omega_x(i + 1) 0 omega_z(i + 1) -omega_y(i + 1);omega_y(i + 1) -omega_z(i + 1) 0 omega_x(i + 1);omega_z(i + 1) omega_y(i + 1) -omega_x(i + 1) 0 ];k_4 = (1/2)*OMEGA_omega_t_k_plus_dt*q_i_4;q_next_k = q_k + dt*((1/6)*k_1 + (1/3)*k_2 + (1/3)*k_3 + (1/6)*k_4);q_next_k = q_next_k/norm(q_next_k);q_k = q_next_k;endR = inv(fromQtoR(q_next_k));end

---

C. Complete Procedure

為了避免標定參數估計中的不可觀察性，至少需要收集IMU9個不同姿態的數據，姿態數越多，標定結果越準確。整個標定算法如下，需要知道采集好的加速度數據 ${\mathbf{a}}^S$ 和角速度數據 ${\mathbf{\omega }}^S$，初始靜止時間 $T_{init}$，以及運動后的靜止時間 $t_{wait}$。

• 首先根據初始時間計算陀螺儀偏差 ${\mathbf{b}}^g$；
• 根據計算后的陀螺儀偏差得到無偏角速度數據 ${\mathbf{\omega }}_{biasfree}^S$；
• 計算初始協方差 ${?}_{init}$ ;
• 設 $i=1:k$ ，根據等待時間 $t_{wait}$ 和閾值計算靜止間隔、再根據靜止間隔 $t_{wait}$ 和加速度數據得到估計參數；
• 最后選取殘差最小對應的參數為加速度標定參數，然后再使用同樣的靜止周期計算陀螺儀標定參數；
  

總結

以上是生活随笔為你收集整理的详解IMU标定经典论文：A Robust and Easy to Implement Method for IMU Calibration without External Equipments的全部內容，希望文章能夠幫你解決所遇到的問題。

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