IMU标定——椭球拟合

lightningwl

已于 2022-10-24 20:51:47 修改

阅读量4.3k

点赞数 6

分类专栏： IMU 文章标签： 1024程序员节

于 2022-10-24 20:50:49 首次发布

本文链接：https://blog.csdn.net/lightningwl/article/details/127499017

版权

IMU 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

之前一直在忙项目，今天趁着程序员日，浅浅谈一下自己这段时间对IMU标定的一些见解吧。

九轴传感器标定

1.1 IMU误差

随机误差我们一般通过均值、方差、Allan方差来评价，但是由于随机误差不是这篇文章的侧重点，所以关于随机误差大家，可以打开matlab运行下面这段代码，浅浅看一下高斯白噪声和随机游走噪声具体长什么样子。

rng(0); % 随机数种子
n1 = randn(10000,1); % 高斯白噪声1
n2 = randn(10000,1); % 高斯白噪声2
n3 = randn(10000,1); % 高斯白噪声3
w1 = cumsum(n1); % 随机游走1
w2 = cumsum(n2); % 随机游走2
w3 = cumsum(n3); % 随机游走3
figure(1);plot([n1 n2 n3], '.');
title('高斯白噪声');
legend('$n_1$','$n_2$','$n_3$', 'interpreter', 'latex');
figure(2);plot([w1 w2 w3]);
title('随机游走');
legend('$w_1$', '$w_2$', '$w_3$', 'interpreter', 'latex');

运行结果也可以给大家贴出来做一下参考。

然后就是重点，我们来分析一下IMU的确定性误差，这里因为我目前接触到的大多数IMU基本很少出现XYZ轴不正交和加速度计与陀螺仪坐标轴不重合的问题，所以这两个问题我们就不谈了。

关于温飘，一般我们会用温度及温度变化率多项式拟合温度产生的误差，这个式子就是： $B=aT^{3}+bT^{2}+cT+d\Delta T^{2}+e\Delta T+fT\Delta T+g$

上面这几个都不是我们这次讨论的重点，现在我们来好好讨论一下怎么解决零偏和尺度误差。

1.2 IMU简单校准

1.2.1 加速度计校准零偏与尺度误差

最简单的校准步骤如下：

将六个面朝上，分别获取每个面的最值为： $x_{min},x_{max},y_{min},y_{max},z_{min},z_{max}$
计算各个面的零偏与尺度误差，以x轴为例： $b_{x}=\frac{x_{min}+x_{max}}{2}$ ， $k_{x}=9.8/\frac{x_{max}-x_{min}}{2}$
则校准后的值为： $x_{c}=k_{x}x_{m}+b_{x}$

1.2.2 校准陀螺仪零偏

最简单的校准步骤如下：

静止一段时间，采集N组数据，求其平均值，为初始零偏： $b_{\omega x}=\frac{1}{N}\sum_{i=1}^{N}\omega _{x},b_{\omega y}=\frac{1}{N}\sum_{i=1}^{N}\omega _{y},b_{\omega z}=\frac{1}{N}\sum_{i=1}^{N}\omega _{z}$
将采集的数据减去初始零偏为校准后的值： $\omega _{x}=\omega _{mx}-b_{\omega x},\omega _{y}=\omega _{my}-b_{\omega y},\omega _{z}=\omega _{mz}-b_{\omega z}$

上述校准过程我们可以发现，陀螺仪我们是可以做到这样校准的，但是加速度计的校准我们在实际生活中无法保证每次只有固定一个面朝上而其他面不受影响，所以针对加速度计和磁力计，我们一般采用椭球拟合去校准。

1.2.3 椭球拟合流程

椭球方程： $(\frac{x-x0}{A})^{2}+(\frac{y-y0}{B})^{2}+(\frac{z-z0}{C})^{2}=1$
展开可得： $x^{2}+ay^{2}+bz^{2}+cx+dy+ez+f=0$
定义误差： $e_{i}=x_{i}^{2}+ay_{i}^{2}+bz_{i}^{2}+cx_{i}+dy_{i}+ez_{i}+f$
目标函数： $_{v={a,b,c,d,e,f}}^{min}\textrm{E(v)}=\sum_{i=1}^{N}e_{i}^{2}(v)=\sum_{i=1}^{N}(\alpha _{i}^{T}V-\beta _{i})^{2}$
根据线性最小二乘理论，最优解： $v^{*}=(M^{T}M)^{-1}M^{T}p$ ，其中 $M=\begin{bmatrix}\alpha _{1}^{T} \\ \alpha _{2}^{T} \\ ... \\ \alpha _{N}^{T} \end{bmatrix}$ ， $p=\begin{bmatrix}\beta _{1} \\ \beta _{2} \\ ... \\ \beta _{N} \end{bmatrix}$

1.2.4 椭球拟合加速度计校准步骤

获取N次静止时加速度计数据，第i次为： $x_{i},y_{i},z_{i}$
根据 $e_{i}=\begin{bmatrix}y_{i}^{2} & z_{i}^{2} & x_{i} & y_{i} & z_{i} & 1 \end{bmatrix}\begin{bmatrix}a \\ b \\ c \\ d \\ e \\ f \end{bmatrix}-(-x_{i}^{2})$ 计算 $\alpha _{i}^{T}$ 与 $\beta _{i}$ ，其中 $\begin{bmatrix}y_{i}^{2} & z_{i}^{2} & x_{i} & y_{i} & z_{i} & 1 \end{bmatrix}$ 为 $\alpha _{i}^{T}$ ， $\begin{bmatrix}a \\ b \\ c \\ d \\ e \\ f \end{bmatrix}$ 为v， $(-x_{i}^{2})$ 为 $\beta _{i}$
获取M与p
计算最优解 $v^{*}$ ，即a,b,c,d,e,f
计算 $A,B,C,x_{0},y_{0},z_{0}$ ，算得 $x_{0}=-\frac{c}{2},y_{0}=-\frac{d}{2a},z_{0}=-\frac{e}{2b},A=\sqrt{x_{0}^{2}+ay_{0}^{2}+bz_{0}^{2}-f},B=\frac{A}{\sqrt{a}},C=\frac{A}{\sqrt{b}}$
则校准结果为： $x^{'}=g\frac{x-x_{0}}{A},y^{'}=g\frac{y-y_{0}}{B},z^{'}=g\frac{z-z_{0}}{C}$

下面大家可以打开matlab，根据这段代码有更深入的理解：

%% 数据读取
dat = [ 9.9604 -0.0975 0.0549; 
-9.6576 0.0974 -0.0145;
0.0889 9.7972 -0.0812;
0.0340 -9.8285 0.0264;
0.0909 -0.0595 9.9975;
0.1680 0.0215 -9.7350];
x = dat(:,1);
y = dat(:,2);
z = dat(:,3);
%% 最小二乘法参数估计
N = length(dat);
M = zeros(N, 6);
p = zeros(N, 1);
for k = 1:N
M(k,:) = [y(k)^2 z(k)^2 x(k) y(k) z(k) 1]; % 矩阵M
p(k) = -x(k)^2; % 向量p
end
v = inv(M'*M) * M' * p; % 计算最优解
x0 = -v(3) / 2; % 拟合出的x0
y0 = -v(4) / (2*v(1)); % 拟合出的y0
z0 = -v(5) / (2*v(2)); % 拟合出的z0
A = sqrt(x0*x0 + v(1)*y0*y0 + v(2)*z0*z0 - v(6)); % 拟合出的x方向上的轴半径A
B = A/sqrt(v(1)); % 拟合出的y方向上的轴半径B
C = A/sqrt(v(2)); % 拟合出的z方向上的轴半径C
dat_cali = zeros(size(dat));
scale = 9.8 ./ [A, B, C]; % 尺度因子
offset = [x0, y0, z0]; % 偏移
for k = 1:N
dat_cali(k,:) = scale .* (dat(k,:) - offset); % 校准结果
end

1.2.5 椭球拟合磁力计校准步骤

与加速度计椭球拟合步骤大致一样

转动磁力计，获取N次数据，第i次为： $x_{i},y_{i},z_{i}$
根据 $e_{i}=\begin{bmatrix}y_{i}^{2} & z_{i}^{2} & x_{i} & y_{i} & z_{i} & 1 \end{bmatrix}\begin{bmatrix}a \\ b \\ c \\ d \\ e \\ f \end{bmatrix}-(-x_{i}^{2})$ 计算 $\alpha _{i}^{T}$ 与 $\beta _{i}$ ，其中 $\begin{bmatrix}y_{i}^{2} & z_{i}^{2} & x_{i} & y_{i} & z_{i} & 1 \end{bmatrix}$ 为 $\alpha _{i}^{T}$ ， $\begin{bmatrix}a \\ b \\ c \\ d \\ e \\ f \end{bmatrix}$ 为v， $(-x_{i}^{2})$ 为 $\beta _{i}$
获取M与p
计算最优解 $v^{*}$ ，即a,b,c,d,e,f
计算 $A,B,C,x_{0},y_{0},z_{0}$ ，算得 $x_{0}=-\frac{c}{2},y_{0}=-\frac{d}{2a},z_{0}=-\frac{e}{2b},A=\sqrt{x_{0}^{2}+ay_{0}^{2}+bz_{0}^{2}-f},B=\frac{A}{\sqrt{a}},C=\frac{A}{\sqrt{b}}$
则校准结果为： $x^{'}=g\frac{x-x_{0}}{A},y^{'}=g\frac{y-y_{0}}{B},z^{'}=g\frac{z-z_{0}}{C}$

这里给大家提供一个matlab的函数，可以直接调用进行椭球拟合：

function [scale, offset, cali_data] = func_lms_calibrate(data)

    x = data(:,1);
    y = data(:,2);
    z = data(:,3);

    %% 最小二乘法参数估计
    N = length(data);
    M = zeros(N, 6);
    p = zeros(N, 1);

    for k = 1:N
        M(k,:) = [y(k)^2 z(k)^2 x(k) y(k) z(k) 1];          % 矩阵M
        p(k) = -x(k)^2;                                     % 向量p
    end

    v = inv(M'*M) * M' * p;                                 % 计算最优解

    x0 = -v(3) / 2;                                         % 拟合出的x0
    y0 = -v(4) / (2*v(1));                                  % 拟合出的y0
    z0 = -v(5) / (2*v(2));                                  % 拟合出的z0

    A = sqrt(x0*x0 + v(1)*y0*y0 + v(2)*z0*z0 - v(6));       % 拟合出的x方向上的轴半径A
    B = A/sqrt(v(1));                                       % 拟合出的y方向上的轴半径B
    C = A/sqrt(v(2));                                       % 拟合出的z方向上的轴半径C

    cali_data = zeros(size(data));
    scale = [A, B, C];                               % 尺度因子
    offset = [x0, y0, z0];                                  % 偏移
    for k = 1:N
        cali_data(k,:) = 1 ./ scale .* (data(k,:) - offset);         % 校准结果
    end
    
end

下面展示一下用这个方法拟合出来的磁力计数据效果：