|
 本文介紹一篇 關(guān)于IMU標(biāo)定的經(jīng)典論文，論文收錄于ICRA14,，在論文中作者介紹了如何不適用外部設(shè)備標(biāo)定 IMU 加速度和角速度偏差、尺度系數(shù),、軸偏移參數(shù),。論文鏈接： https:///paper/2021503353 https:///paper/2211578699 項目鏈接：https://github.com/JzHuai0108/imu_tk_matlab2.Basic Calibration Framework 加速度標(biāo)定我們需要估計的未知參數(shù)為：此時我們可以忽略測量噪聲,，則加速度誤差模型簡化為：正如在傳統(tǒng)的多位置策略中,，我們將 IMU 置于 個不同的位置，在每一個靜止周期內(nèi)讀取加速度測量值 ,，我們可以使用以下?lián)p失函數(shù)來估計加速度誤差模型參數(shù)：其中,，是當(dāng)?shù)刂亓铀俣确怠p失函數(shù)程序為： 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
我們使用同樣的靜止周期來標(biāo)定陀螺儀,。在這里我們通過對初始靜止時刻角速度值求平均來得到角速度偏差,。這樣我們需要求解的參數(shù)簡化為：我們使用標(biāo)定后的加速度數(shù)據(jù)作為參考，給定一個初始的重力向量,，對角速度數(shù)據(jù)進(jìn)行積分,，我們可以估計最終的重力向量，則損失函數(shù)可以寫為：其中,，是標(biāo)定后的加速度向量,，是估計后的重力向量。角速度積分這里使用的是 RK4,，不過目前 IMU 的采樣頻率都很高了,，一般很少再使用了,。IMU 標(biāo)定的準(zhǔn)確性非常依賴于靜止和運動時間間隔的準(zhǔn)確區(qū)分，為了標(biāo)定加速度計我們使用靜止周期,，標(biāo)定陀螺儀我們使用兩個靜態(tài)周期之間的動態(tài)時間間隔,。我們這里使用基于方差的靜止檢測器，對于時間周期長度秒,，我們有加速度,，然后我們計算標(biāo)準(zhǔn)差：我們通過比較方標(biāo)準(zhǔn)差是否大于某一閾值來區(qū)分靜止和運動狀態(tài)。我們將初始方差擴(kuò)大整數(shù)倍來作為閾值,。下圖是靜止檢測器的檢測結(jié)果,，這里整數(shù)倍為6倍。B. Runge-Kutta Integration下面簡單介紹下四階龍格庫塔法,，這里主要用在陀螺儀的標(biāo)定,。四元數(shù)微分方程為：最終得到積分后的四元數(shù),，還需要再轉(zhuǎn)化為單位四元數(shù)，整個 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
為了避免標(biāo)定參數(shù)估計中的不可觀察性,，至少需要收集 IMU 9個不同姿態(tài)的數(shù)據(jù),，姿態(tài)數(shù)越多，標(biāo)定結(jié)果越準(zhǔn)確,。整個標(biāo)定算法如下,，需要知道采集好的加速度數(shù)據(jù)和角速度數(shù)據(jù)，初始靜止時間,，以及運動后的靜止時間,。根據(jù)計算后的陀螺儀偏差得到無偏角速度數(shù)據(jù),；設(shè),根據(jù)等待時間和閾值計算靜止間隔,、再根據(jù)靜止間隔和加速度數(shù)據(jù)得到估計參數(shù)；最后選取殘差最小對應(yīng)的參數(shù)為加速度標(biāo)定參數(shù),，然后再使用同樣的靜止周期計算陀螺儀標(biāo)定參數(shù),；
|
