从零手写VIO第二章作业（IMU数据仿真及Allan方差标定）

最新推荐文章于 2024-04-02 10:24:15 发布

生滚海鲜粥

最新推荐文章于 2024-04-02 10:24:15 发布

阅读量1.8k

点赞数 3

分类专栏： C++ SLAM 文章标签： slam 传感器 IMU 惯性导航

本文链接：https://blog.csdn.net/qq_37340588/article/details/107328959

版权

C++ 同时被 2 个专栏收录

12 篇文章 0 订阅

订阅专栏

SLAM

4 篇文章 0 订阅

订阅专栏

IMU数据仿真及Allan方差标定

IMU仿真代码介绍
IMU仿真数据验证
Allan方差标定

IMU仿真代码介绍

一、根据参数初始化

设定IMU数据采集频率
设定数据采集时间，对于allan方差标定，默认4小时
设定陀螺仪gyro和角速度计acc的bias随机游走的方差
设定陀螺仪gyro和角速度计acc的高斯白噪声方差

class Param{

public:

    Param();

    // time
    int imu_frequency = 200;
    int cam_frequency = 30;
    double imu_timestep = 1./imu_frequency;
    double cam_timestep = 1./cam_frequency;
    double t_start = 0;
    double t_end = 3600 * 4;  // 4 hours for allan

    // noise
    double gyro_bias_sigma = 0.00005;
    double acc_bias_sigma = 0.0005;

    //double gyro_bias_sigma = 1.0e-5;
    //double acc_bias_sigma = 0.0001;

    double gyro_noise_sigma = 0.015;    // rad/s * 1/sqrt(hz)
    double acc_noise_sigma = 0.019;      //　m/(s^2) * 1/sqrt(hz)

    double pixel_noise = 1;              // 1 pixel noise

    // cam f
    double fx = 460;
    double fy = 460;
    double cx = 255;
    double cy = 255;
    double image_w = 640;
    double image_h = 640;


    // 外参数
    Eigen::Matrix3d R_bc;   // cam to body
    Eigen::Vector3d t_bc;     // cam to body

};

二、构造运动模型（以例程为例）

2.1加速度模型

1. 设定质量快运动轨迹方程（以例程中椭圆为例）
$\bold{position}(t)= [ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5, z * sin( K1 * K * t ) + 5]$
$K$ 为 $x ， y$ 轴运动频率， $K 1$ 表示 $z$ 轴正弦频率是 $x ， y$ 轴的 $K 1$ 倍, $ellipse_x$ , $ellipse_y$ 为椭圆长轴和短轴

    float ellipse_x = 15;
    float ellipse_y = 20;
    float z = 1;           // z轴做sin运动
    float K1 = 10;          // z轴的正弦频率是x，y的k1倍
    float K = M_PI/ 10;    // 20 * K = 2pi 　　由于我们采取的是时间是20s, 系数K控制yaw正好旋转一圈，运动一周

    // translation
    // twb:  body frame in world frame
    Eigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5,  z * sin( K1 * K * t ) + 5);

2.求出质量块速度方程
在1中我们知道质量快的位矢为时间的函数，对位矢求导即为速度

Eigen::Vector3d dp(- K * ellipse_x * sin(K*t),  K * ellipse_y * cos(K*t), z*K1*K * cos(K1 * K * t));

3.求出质量块加速度方程
对2中速度求导即为加速度

Eigen::Vector3d ddp( -K2 * ellipse_x * cos(K*t),  -K2 * ellipse_y * sin(K*t), -z*K1*K1*K2 * sin(K1 * K * t));

2.2旋转模型

1.设定质量块的旋转角

光有轨迹，速度，加速度只能够用来生成加速度计数据，不能用来生成陀螺仪数据。为此我们还要设定质量快在不同时间是如何旋转的。特别地，程序中利用欧拉角来表示不同时刻body坐标系相对于惯性系的旋转角度。

欧拉角是有顺序的，各种博客经常称之为顺规。我们常用yaw，pitch，roll角来描述物体绕不同轴的角度。更加值得注意的是，在不同坐标系下yaw，pitch，roll所绕的轴不同。在东北天坐标系中，yaw，pitch，roll对应的是绕载体坐标系的z，y，x轴。在北东地坐标系中，yaw，pitch，roll对应的是绕载体坐标系的y, x, z轴。在IMU仿真程序中，我们采用东北天坐标系，欧拉角顺规为yaw，pitch，roll。具体的值为

$\vartheta$ = $kroll * cos(t) , kpitch * sin(t) , K*t）^T$

2.利用旋转角求得角速度 $\omega$

虽然程序在初始化欧拉角时是按roll，pitch，yaw写的，但实际旋转时是按照yaw，pitch，roll旋转的，这一点可在euler2Rotation函数中体现（下面会具体介绍）。值得注意的是这里的欧拉角为静态欧拉角，是body系在不同时刻相对于惯性系所转角度。

光有静态欧拉角还不够，我们最终目的是利用欧拉角仿真出陀螺仪所测得的角速度。那么如何利用欧拉角求得角速度 $\omega$ 。

令 $\phi$ ， $\theta$ ， $\psi$ 分别为yaw， pitch， roll角（即绕z，y，x轴）。从i系到b系绕各个轴的旋转矩阵分别为

$R(\phi) = \left[\begin{array}{ccc}\cos \phi & \sin \phi & 0 \\ -\sin \phi & \cos \phi & 0 \\ 0 & 0 & 1\end{array}\right]$

$R(\theta) = \left[\begin{array}{ccc} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \end{array}\right]$

$R(\psi) = \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \psi & \sin \psi \\ 0 & -\sin \psi & \cos \psi \end{array}\right]$

可以看出，这些旋转都是被动旋转，即空间中向量不变，我们只是观察空间中某固定向量在不同坐标系下的坐标。那么从i系到b系的旋转矩阵可通过如下方式获得

$\begin{aligned} \mathbf{x}_{b} &=R(\psi) R(\theta) R(\phi) \mathbf{x} \\ &=\left[\begin{array}{ccc} c \theta c \phi & c \theta s \phi & -s \theta \\ -c \psi s \phi+s \psi s \theta c \phi & c \psi c \phi+s \psi s \theta s \phi & s \psi c \theta \\ s \psi s \phi+c \psi s \theta c \phi & -s \psi c \phi+c \psi s \theta s \phi & c \psi c \theta \end{array}\right] \\ &=R(\phi, \theta, \psi) \mathbf{x} \end{aligned}$

即 $R_{bI} = R(\phi, \theta, \psi)$
所以 $R_{Ib} = R_{bI}^{-1} = R_{bI}^{T}$

Eigen::Matrix3d euler2Rotation( Eigen::Vector3d  eulerAngles)
{
    double roll = eulerAngles(0);
    double pitch = eulerAngles(1);
    double yaw = eulerAngles(2);

    double cr = cos(roll); double sr = sin(roll);
    double cp = cos(pitch); double sp = sin(pitch);
    double cy = cos(yaw); double sy = sin(yaw);

    Eigen::Matrix3d RIb;

    RIb<< cy*cp ,   cy*sp*sr - sy*cr,   sy*sr + cy* cr*sp,
            sy*cp,    cy *cr + sy*sr*sp,  sp*sy*cr - cy*sr,
            -sp,         cp*sr,           cp*cr;
    return RIb;
}

所以这也对应了上面所说的，程序中的欧拉角顺规是yaw-pitch-roll。

下面继续介绍如何从i系下欧拉角速度求出body系下角速度

$\begin{aligned} \boldsymbol{\omega}=R(\psi) R(\theta)\left\{\begin{array}{c} 0 \\ 0 \\ \frac{d \phi}{d t} \end{array}\right\}+R(\psi)\left\{\begin{array}{c} 0 \\ \frac{d \theta}{d t} \\ 0 \end{array}\right\}+\left\{\begin{array}{c} \frac{d \psi}{d t} \\ 0 \\ 0 \end{array}\right\} \\ =\left[\begin{array}{ccc} 1 & 0 & -\sin \theta \\ 0 & \cos \psi & \sin \psi \cos \theta \\ 0 & -\sin \psi & \cos \psi \cos \theta \end{array}\right]\left\{\begin{array}{c} \frac{d \psi}{d t} \\ \frac{d \theta}{d t} \\ \frac{d \phi}{d t} \end{array}\right\} \end{aligned}$

在第三次旋转时是绕x轴旋转，这时的x轴即为body系的x轴，所以我们在 $\left\{\begin{array}{l} \frac{d \psi}{d t} \\ 0 \\ 0 \end{array}\right\}$ 前不用乘任何旋转矩阵。在第二次旋转时是绕y轴旋转，因为在绕y轴旋转后还要再绕x轴旋转，所以需要我们在欧拉角速度前乘一个绕x轴的旋转矩阵 $R(\psi)$ 以使我们此次绕y轴的欧拉角速度旋转到body系下，最终得到 $R(\psi)\left\{\begin{array}{l}0 \\ \frac{d \theta}{d t} \\ 0\end{array}\right\}$ 。 $R(\psi) R(\theta)\left\{\begin{array}{c}0 \\ 0 \\ \frac{d \phi}{d t}\end{array}\right\}$ 也是同样分析。

至此我们完成了从i系欧拉角速度到body系下角速度的变化，由此我们即可完成陀螺仪角速度的仿真。

Eigen::Matrix3d eulerRates2bodyRates(Eigen::Vector3d eulerAngles)
{
    double roll = eulerAngles(0);
    double pitch = eulerAngles(1);

    double cr = cos(roll); double sr = sin(roll);
    double cp = cos(pitch); double sp = sin(pitch);

    Eigen::Matrix3d R;
    R<<  1,   0,    -sp,
            0,   cr,   sr*cp,
            0,   -sr,  cr*cp;

    return R;
}

当然，以上的推导都是基于理想情况下无噪声数据，如果考虑高斯白噪声和bias以及bias的随机游走，我们的测量值应该是
$\begin{array}{l} \tilde{\boldsymbol{\omega}}^{b}=\boldsymbol{\omega}^{b}+\mathbf{b}^{g}+\mathbf{n}^{g} \\ \tilde{\mathbf{a}}^{b}=\mathbf{q}_{b w}\left(\mathbf{a}^{w}+\mathbf{g}^{w}\right)+\mathbf{b}^{a}+\mathbf{n}^{a}\\ \mathbf{\dot{b}(t)}=n_{b}(t) \end{array}$

高斯白噪声以及bias随机游走的方差可通过程序设定，已在第一部分介绍。

IMU仿真数据验证

为了验证我们所得到的IMU数据是否正确，我们需要对IMU数据进行积分，如果我们积分后的轨迹与最开始设定的轨迹贴近，即可认为IMU得到的数据是准确的。因为我们是对离散采样后的结果进行积分，所以轨迹不可能完全一样，但是根据积分方式的不同，我们可将积分方式分为欧拉法和中值法。后者更能贴合实际轨迹，下面介绍具体原理。
通过IMU我们可以获得三个轴的加速度和body系下的角加速度，有了这些值，我们不仅可以获得轨迹（P），还可以获得速度（V）和旋转角（Q），即PVQ。

一、欧拉法

使用欧拉法，即两个相邻时刻 k 到 k+1 的位姿是用第 k 时刻的测量
值 $a, ω$ 来计算。
$\begin{array}{l} \mathbf{p}_{w b_{k+1}}=\mathbf{p}_{w b_{k}}+\mathbf{v}_{k}^{w} \Delta t+\frac{1}{2} \mathbf{a} \Delta t^{2} \\ \mathbf{v}_{k+1}^{w}=\mathbf{v}_{k}^{w}+\mathbf{a} \Delta t \\ \mathbf{q}_{w b_{k+1}}=\mathbf{q}_{w b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \omega \delta t \end{array}\right] \end{array}$
其中，
$\begin{array}{l} \mathbf{a}=\mathbf{q}_{w b_{k}}\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right)-\mathbf{g}^{w} \\ \boldsymbol{\omega}=\boldsymbol{\omega}^{b_{k}}-\mathbf{b}_{k}^{g} \end{array}$
可以看出，我们的k+1时刻的PVQ是用k时刻测量值求得，但k到k+1时刻之间的加速度是不断变化的，只取一个时刻进行积分肯定是不够精确的。

程序代码

Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
Qwb = Qwb * dq;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = Vw + acc_w * dt;

实验结果

利用欧拉法对IMU无噪声数据进行积分

利用欧拉法对IMU有噪声数据进行积分

二、中值法

使用 mid-point 方法，即两个相邻时刻 k 到 k+1 的位姿是用两个时刻的测量值 $a, ω$ 的平均值来计算。

$\begin{array}{l} \mathbf{p}_{w b_{k+1}}=\mathbf{p}_{w b_{k}}+\mathbf{v}_{k}^{w} \Delta t+\frac{1}{2} \mathbf{a} \Delta t^{2} \\ \mathbf{v}_{k+1}^{w}=\mathbf{v}_{k}^{w}+\mathbf{a} \Delta t \\ \mathbf{q}_{w b_{k+1}}=\mathbf{q}_{w b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \boldsymbol{\omega} \delta t \end{array}\right] \end{array}$
其中，
$\begin{array}{l} \mathbf{a}=\frac{1}{2}\left[\mathbf{q} w b_{k}\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right)-\mathbf{g}^{w}+\mathbf{q}_{w b_{k+1}}\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)-\mathbf{g}^{w}\right] \\ \boldsymbol{\omega}=\frac{1}{2}\left[\left(\boldsymbol{\omega}^{b_{k}}-\mathbf{b}_{k}^{g}\right)+\left(\boldsymbol{\omega}^{b_{k+1}}-\mathbf{b}_{k}^{g}\right)\right] \end{array}$

程序代码

Eigen::Quaterniond Qwbk, Qwbk1;
Eigen::Vector3d midOmega;

midOmega = (imudata[i].imu_gyro + imudata[i-1].imu_gyro)/2;

dtheta_half =  midOmega * dt /2.0;

dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
dq.normalize();

//calculate qwbk and qwbk+1
Qwbk = Qwb;
Qwbk1 = Qwb * dq;

//PVQ
Eigen::Vector3d acc_w = 0.5 * (Qwbk * imudata[i-1].imu_acc + gw + Qwbk1 * imupose.imu_acc + gw);
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = Vw + acc_w * dt;
Qwb = Qwbk1;

实验结果

利用中值法对IMU无噪声数据进行积分

利用中值法对IMU有噪声数据进行积分

三、实验结论

分别利用中值法和欧拉法对无噪声数据进行积分，从结果可以看出对IMU数据进行积分可以得到物体轨迹。同时利用中值法积分结果更接近实际轨迹。通过对有噪声数据进行积分可以看出由于我们对噪声不断积分，使得误差不断累积，导致在刚开始时轨迹接近真实轨迹，但随着时间的不断推移，误差越来越大直至积分所得轨迹完全偏离实际轨迹。

Allan方差标定

一、IMU噪声分类

以陀螺仪误差为例，陀螺仪误差主要包含以下五项：
1. 量化噪声：陀螺仪在AD采样时所引入的噪声。噪声大小与采样频率有关，频率越高，量化噪声越小。

Allan方差：
$\log _{10} \sigma_{Q N}(\tau)=\log _{10}(\sqrt{3} Q)-\log _{10} \tau$

2. 角度随机游走：由于测得的角速率里包含白噪声，对白噪声进行积分后得到的不是白噪声，而是一个马尔可夫过程，即这一次的误差是在上一次误差的基础上累加一个随机的白噪声得到的。角度误差所包含的这种马尔可夫性质的误差就叫做角度随机游走。

Allan方差：
$\log _{10} \sigma_{A R W}(\tau)=\log _{10} N-1 / 2 * \log _{10} \tau$

3.角速率随机游走：和角度随机游走类似，由于对角加速率进行积分的同时会对角加速率携带的白噪声也会进行积分，导致角速率随机游走。

Allan方差：
$\log _{10} \sigma_{R R W}(\tau)=\log _{10}(K / \sqrt{3})+1 / 2 * \log _{10} \tau$

4.零偏不稳定性噪声：bias并不是固定不变的，其导数服从高斯分布。这也就意味着零偏是会发生变化的，具体变化多少时服从高斯分布的。

Allan方差：
$\log _{10} \sigma_{B I}(\tau)=\log _{10}(2 B / 3)$

5.速率斜坡：是一种趋势性误差。随机误差是随机的，无法用确定性模型消除，而趋势性误差是可以直接拟合消除的。速率斜坡是一种趋势性误差，由温度变化导致，可通过温补消除。

Allan方差：
$\log _{10} \sigma_{R R}(\tau)=\log _{10}(R / \sqrt{2})+\log _{10} \tau$

二、Allan方差曲线绘制

在一中我们可以看到Allan方差是采样间隔的函数。每种Allan方差在 $log(\tau)-log(\sigma_{Allan})$ 坐标系下都对应一条直线。我们将这些直线相加并绘制便可以得到Allan方差曲线。五种误差的直线斜率分别为-1，-1/2，1/2，0，1。最终得到的Allan方差曲线便是五条直线叠加的结果。
具体流程如下：

保持传感器绝对静止获取数据。
对数据进行分段，设定时间段的时长为 $k\tau$ ，如下图所示。

3.对每段 $k\tau$ 内的IMU数据进行平均，以模拟 $k\tau$ 采样间隔下所获数据。

4.计算采样间隔 $k\tau$ 时所对应的Allan方差 $\sigma(k\tau)$ ，在图上找到 $(k\tau,\sigma(k\tau))$ 点。

5.对上述步骤的 $k$ 取不同值，得到一系列点，最终绘制艾伦曲线。

三、利用IMU仿真数据进行Allan方差标定

将IMU仿真代码中物体速度，加速度设为零

Eigen::Vector3d dp(0,  0, 0);       // position导数　in world frame
Eigen::Vector3d ddp( 0,  0, 0);     // position二阶导数

IMU仿真代码中高斯噪声方差设定值：
加速度的高斯白噪声方差为0.019，陀螺仪的高斯白噪声方差为0.015。加速度bias的随机游走噪声为 $5e^{-4}$ ，陀螺仪bias的随机游走噪声为 $5e^{-5}$

double gyro_noise_sigma = 0.015;    // rad/s * 1/sqrt(hz)
double acc_noise_sigma = 0.019;      //　m/(s^2) * 1/sqrt(hz)
// noise
double gyro_bias_sigma = 0.00005;
double acc_bias_sigma = 0.0005;

3.将IMU仿真数据送入MATLAB Allan方差标定程序中
实验结果：

加速度计

加速度计标定结果

陀螺仪

陀螺仪标定结果

更改IMU仿真代码参数，进行标定，得到如下结果

IMU	噪声设定值	噪声标定值	随机游走设定值	随机游走标定值
加速度计	0.019	0.019146	$5e^{-4}$	$4.9e^{-4}$
陀螺仪	0.015	0.015536	$5e^{-5}$	$8.4e^{-5}$
加速度计	0.0019	0.001938	$5e^{-5}$	$3.7e^{-5}$
陀螺仪	0.0015	0.001506	$5e^{-6}$	$4e^{-6}$

5.实验结论
从上表中可以看出高斯噪声标定结果与真实值十分相近，bias随机游走标定值与IMU仿真数据中的设定值也在同一数量级。值得注意的是，IMU仿真数据噪声设定值为连续系统的方差，从标定结果来看，kalibr_allan标定结果也是连续系统的噪声方差。

生滚海鲜粥

关注

3
点赞
踩
27

收藏

觉得还不错? 一键收藏
0
评论
从零手写VIO第二章作业（IMU数据仿真及Allan方差标定）

这里写目录标题IMU仿真代码介绍一、根据参数初始化二、构造运动模型（以例程为例）2.1加速度模型2.2旋转模型三级目录IMU仿真代码介绍一、根据参数初始化设定IMU数据采集频率设定数据采集时间，对于allan方差标定，默认4小时设定陀螺仪gyro和角速度计acc的bias随机游走的方差设定陀螺仪gyro和角速度计acc的高斯白噪声方差class Param{public: Param(); // time int imu_frequency = 200;
复制链接

扫一扫