详解IMU标定经典论文：一种易于实施、无需外部设备的IMU校准方法

InfoRich 2023-10-16 发布于江苏

展开全文

本文介绍一篇关于IMU标定的经典论文，论文收录于ICRA14，在论文中作者介绍了如何不适用外部设备标定 IMU 加速度和角速度偏差、尺度系数、轴偏移参数。

论文链接：

https:///paper/2021503353

https:///paper/2211578699

项目链接：https://github.com/JzHuai0108/imu_tk_matlab

1.Sensor Error Model

首先介绍传感器误差模型，令表示为理想情况下的加速度数据，表示为实际的加速度数据，

表示从到的旋转变换。为加速度偏差，

为加速度尺度系数，为加速度测量噪声，则可以得到加速度误差模型：

同样也可以得到角速度误差模型，令

为角速度测量噪声，则角速度误差模型为：

2.Basic Calibration Framework

加速度标定我们需要估计的未知参数为：

此时我们可以忽略测量噪声，则加速度误差模型简化为：

正如在传统的多位置策略中，我们将 IMU 置于个不同的位置，在每一个静止周期内读取加速度测量值，我们可以使用以下损失函数来估计加速度误差模型参数：

其中，是当地重力加速度幅值。损失函数程序为：

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

我们使用同样的静止周期来标定陀螺仪。在这里我们通过对初始静止时刻角速度值求平均来得到角速度偏差。这样我们需要求解的参数简化为：

我们使用标定后的加速度数据作为参考，给定一个初始的重力向量，对角速度数据进行积分，我们可以估计最终的重力向量，则损失函数可以写为：

其中，是标定后的加速度向量，是估计后的重力向量。角速度积分这里使用的是 RK4，不过目前 IMU 的采样频率都很高了，一般很少再使用了。

3.Calibration Procedure

A. Static Detector

IMU 标定的准确性非常依赖于静止和运动时间间隔的准确区分，为了标定加速度计我们使用静止周期，标定陀螺仪我们使用两个静态周期之间的动态时间间隔。我们这里使用基于方差的静止检测器，对于时间周期长度秒，我们有加速度，然后我们计算标准差：

我们通过比较方标准差是否大于某一阈值来区分静止和运动状态。我们将初始方差扩大整数倍来作为阈值。下图是静止检测器的检测结果，这里整数倍为6倍。

B. Runge-Kutta Integration

下面简单介绍下四阶龙格库塔法，这里主要用在陀螺仪的标定。四元数微分方程为：

其中，是一个反对称矩阵，形式为：

四阶龙格库塔法原理为：

各个参数为：

最终得到积分后的四元数，还需要再转化为单位四元数，整个 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 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;
end
R = inv(fromQtoR(q_next_k));
end