详解IMU标定经典论文：A Robust and Easy to Implement Method for IMU Calibration without External Equipments

本文介绍一篇 关于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=\left[\begin{array}{ccc}1& -{\alpha }_{yz}& {\alpha }_{zy}\\ 0& 1& -{\alpha }_{zx}\\ 0& 0& 1\end{array}\right]$ 表示从 ${\mathbf{a}}^S$ 到 ${\mathbf{a}}^O$ 的旋转变换。${\mathbf{b}}^a=\left[\begin{array}{l}{b}_x^a\\ b_y^a\\ b_z^a\end{array}\right]$ 为加速度偏差，${\mathbf{K}}^a=\left[\begin{array}{ccc}{s}_x^a& 0& 0\\ 0& s_y^a& 0\\ 0& 0& s_z^a\end{array}\right]$ 为加速度尺度系数，${\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=\left[\begin{array}{ccc}1& -{\gamma }_{yz}& {\gamma }_{zy}\\ {\gamma }_{xz}& 1& -{\gamma }_{zx}\\ -{\gamma }_{xy}& {\gamma }_{yx}& 1\end{array}\right]$，${\mathbf{K}}^g=\left[\begin{array}{ccc}{s}_x^g& 0& 0\\ 0& s_y^g& 0\\ 0& 0& s_z^g\end{array}\right]$，${\mathbf{b}}^g=\left[\begin{array}{l}{b}_x^g\\ b_y^g\\ b_z^g\end{array}\right]$，${\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;
	end
	
	residuals = 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;
	end
	
	res_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)$，然后我们计算标准差：
$$\varsigma (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}$$

我们通过比较方标准差$\varsigma (t)$ 是否大于某一阈值来区分静止和运动状态。我们将初始方差 ${\varsigma }_{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 })=\left[\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}\right]$$

四阶龙格库塔法原理为：
$$\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 Huai

	for i = 1:num_samples - 1
	    
	    % first Runge-Kutta coefficient
	    q_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 coefficient
	    q_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 coefficient
	    q_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 coefficient
	    q_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;
	    
	end
	
	R = 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$；
• 计算初始协方差 ${\varsigma }_{init}$ ;
• 设 $i=1:k$ ，根据等待时间 $t_{wait}$ 和阈值计算静止间隔、再根据静止间隔 $t_{wait}$ 和加速度数据得到估计参数；
• 最后选取残差最小对应的参数为加速度标定参数，然后再使用同样的静止周期计算陀螺仪标定参数；