惯导系统静止初始化方法与代码实现并在gazebo中测试

前言
静止初始化方法
惯导静止初始化实现代码
在gazebo中进行测试

在这里插入图片描述

前言

在进行GPS加IMU的组合导航或者Lidar加IMU的组合导航时，用EKF或者ESKF的滤波方法时，需要提前知道惯导的测量噪声、初始零偏、重力方向等信息。

此时就需要对惯导进行一个初始化，来获取以上信息，常见的初始化方法为静止初始化法。例如无人机在上电后要进行自检，此时需要无人机静止一段时间，通过指示灯来提示自检是否完毕，在静止的过程中，则对惯导进行了初始化的方法。

静止初始化方法

在传统组合导航系统中，最常见的是使用静止初始化方法。

静止初始化就是把IMU放在某个地方静止一段时间。在静止时间内，由于物体本身没有任何运动，可以简单地认为IMU的陀螺仪只测到零偏，而加速度计则测到零偏与重力之和。

可以设置一个静止初始化流程来获取这些变量：
1、将IMU静止一段时间，比如10s。静止状态检测可以由轮速计进行判断，当两轮的轮速均小于阈值时，认为机器人静止。在没有轮速计的机器人上，也可以直接认为机器人静止，来测定相关变量。
2、统计静止时间内的陀螺仪与加速度计读数均值，记为 $\bar{d} _{gyr},\bar{d} _{acc}$
3、由于机器人并未发生转动，这段时间的陀螺仪均值可以取 $b_{g}=\bar{d} _{gyr}$
4、加速度的测量方程为
在这里插入图片描述
当机器人的实际加速度为零，旋转视为R=I时，加速度实际测到 $b_{a}-g$ ,其中 $b_{a}$ 为小量，g的长度可视为固定值。
在这些前提下，取方向为 $-\bar{d} _{acc}$ ，大小为9.8的矢量作为重力矢量，这一步确定了重力的朝向。
5、将这段时间的加速度计读数去掉重力，重新计算 $\bar{d} _{acc}$ 。
6、取 $b_{a}=\bar{d} _{acc}$
7、认为零偏不动，估计陀螺仪和加速度计的测量方差。该方差可用于ESKF的噪声参数

惯导静止初始化实现代码

声明静态惯导初始化的类

IMU水平静止状态下初始化器
使用方法：调用AddIMU, AddOdom添加数据，使用InitSuccess获取初始化是否成功
成功后，使用各Get函数获取内部参数

class StaticIMUInit {

下面为类的具体内容
++++++++++++++++++++++++++++++++++++++++++++++++++++

    //  可配置参数结构
    struct Options {
        Options() {}
        double init_time_seconds_ = 10.0;     // 静止时间
        int init_imu_queue_max_size_ = 2000;  // 初始化IMU队列最大长度
        int static_odom_pulse_ = 5;           // 静止时轮速计输出噪声
        double max_static_gyro_var = 0.5;     // 静态下陀螺测量方差
        double max_static_acce_var = 0.05;    // 静态下加计测量方差
        double gravity_norm_ = 9.81;          // 重力大小
        bool use_speed_for_static_checking_ = true;  // 是否使用odom来判断车辆静止（部分数据集没有odom选项）
    };

公共变量区，设置可配置参数结构体

具体参数：

静止时间（采集这么长时间数据后进行初始化）
初始化IMU队列最大长度（如在设置的静止时间内，队列满，再来数据则逐步省略掉最早的数据）
静止时轮速计输出噪声（轮速计输出小于此值认为静止）
静止下陀螺和加计测量方差最大值（计算结果大于此值则认为计算失败）
重力大小
是否使用odom来判断车辆静止

++++++++++++++++++++++++++++++++++++++++++++++++++++

    /// 判定初始化是否成功
    bool InitSuccess() const { return init_success_; }

    /// 获取各Cov, bias, gravity
    Vec3d GetCovGyro() const { return cov_gyro_; }
    Vec3d GetCovAcce() const { return cov_acce_; }
    Vec3d GetInitBg() const { return init_bg_; }
    Vec3d GetInitBa() const { return init_ba_; }
    Vec3d GetGravity() const { return gravity_; }

获得计算的方差、零偏、重力等

++++++++++++++++++++++++++++++++++++++++++++++++++++
私有变量区声明，要用到的变量

    Options options_;                 // 选项信息
    bool init_success_ = false;       // 初始化是否成功
    Vec3d cov_gyro_ = Vec3d::Zero();  // 陀螺测量噪声协方差（初始化时评估）
    Vec3d cov_acce_ = Vec3d::Zero();  // 加计测量噪声协方差（初始化时评估）
    Vec3d init_bg_ = Vec3d::Zero();   // 陀螺初始零偏
    Vec3d init_ba_ = Vec3d::Zero();   // 加计初始零偏
    Vec3d gravity_ = Vec3d::Zero();   // 重力
    bool is_static_ = false;          // 标志车辆是否静止
    std::deque<IMU> init_imu_deque_;  // 初始化用的数据
    double current_time_ = 0.0;       // 当前时间
    double init_start_time_ = 0.0;    // 静止的初始时间

下面来看各实现函数
++++++++++++++++++++++++++++++++++++++++++++++++++++
当惯导数据来时，添加惯导数据

如果已经初始化成功则直接返回

bool StaticIMUInit::AddIMU(const IMU& imu) {
    if (init_success_) {
        return true;
    }

通过轮速计判断机器人没有静止，则将用于初始化的惯导数据队列清空

    if (options_.use_speed_for_static_checking_ && !is_static_) {
        LOG(WARNING) << "等待静止";
        init_imu_deque_.clear();
        return false;
    }

如果惯导数据队列为空，那么则刚进入静止状态，记录初始静止时间

    if (init_imu_deque_.empty()) {
        // 记录初始静止时间
        init_start_time_ = imu.timestamp_;
    }

将数据加如用于初始化的imu队列数据

    // 记入初始化队列
    init_imu_deque_.push_back(imu);

计算静止时间数据有了多久

double init_time = imu.timestamp_ - init_start_time_;  // 初始化经过时间

如果时间大于了设定值，那么则用采集好的数据进入初始化函数

    if (init_time > options_.init_time_seconds_) {
        // 尝试初始化逻辑
        TryInit();
    }

时间没够，但是数据大于了设定队列长度，则删除掉队列最前数据

    // 维持初始化队列长度
    while (init_imu_deque_.size() > options_.init_imu_queue_max_size_) {
        init_imu_deque_.pop_front();
    }

运行到这则说明，静止的时间还没够，更新当前时间，整体函数返回false

   current_time_ = imu.timestamp_;
    return false;

++++++++++++++++++++++++++++++++++++++++++++++++++++
下面是轮速计数据来时的操作函数

bool StaticIMUInit::AddOdom(const Odom& odom) {
    // 判断车辆是否静止
    if (init_success_) {
        return true;
    }
    if (odom.left_pulse_ < options_.static_odom_pulse_ && odom.right_pulse_ < options_.static_odom_pulse_) {
        is_static_ = true;
    } else {
        is_static_ = false;
    }
    current_time_ = odom.timestamp_;
    return true;
}

主要就是根据数据，判断是否静止，将标志位进行设置

++++++++++++++++++++++++++++++++++++++++++++++++++++
然后就是当静止时间够后，进入的初始化函数

如果队列数据太短，则无法进行初始化

bool StaticIMUInit::TryInit() {
    if (init_imu_deque_.size() < 10) {
        return false;
    }

声明陀螺和加计的均值

Vec3d mean_gyro, mean_acce;

下面则计算均值和协方差

math::ComputeMeanAndCovDiag(init_imu_deque_, mean_gyro, cov_gyro_, [](const IMU& imu) { return imu.gyro_; });
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_, [this](const IMU& imu) { return imu.acce_; });

这里主要调用了数学库中的均值与协方差计算函数

++++++++++++++++++++++++++++++++++++++++++++++++++++

    LOG(INFO) << "mean acce: " << mean_acce.transpose();
    gravity_ = -mean_acce / mean_acce.norm() * options_.gravity_norm_;

以acce均值为方向，取9.8长度为重力

将加计读数减去重力，重新计算协方差

    // 重新计算加计的协方差
    math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_,
                                [this](const IMU& imu) { return imu.acce_ + gravity_; });

检测计算的 IMU 噪声是否过大，过大则直接返回 false

    // 检查IMU噪声
    if (cov_gyro_.norm() > options_.max_static_gyro_var) {
        LOG(ERROR) << "陀螺仪测量噪声太大" << cov_gyro_.norm() << " > " << options_.max_static_gyro_var;
        return false;
    }

    if (cov_acce_.norm() > options_.max_static_acce_var) {
        LOG(ERROR) << "加计测量噪声太大" << cov_acce_.norm() << " > " << options_.max_static_acce_var;
        return false;
    }

赋值IMU零偏

    init_bg_ = mean_gyro;
    init_ba_ = mean_acce;

至此初始成功，函数返回true

    init_success_ = true;
    return true;
}

在gazebo中进行测试

为了在gazebo中测试上面的代码，在无人机的测试环境中进行，用无人机的IMU，进行测试，所以需要在类的定义中，加入对无人机IMU的订阅话题，然后在回调函数中调用上面的代码函数。

        // 将ros的imu格式的数据 转为 设置的imu格式的数据
        sad::IMU imu;
        imu.timestamp_ = Imu_msg->header.stamp.toSec();
        imu.gyro_ << Imu_msg->angular_velocity.x,Imu_msg->angular_velocity.y,Imu_msg->angular_velocity.z;
        imu.acce_ << Imu_msg->linear_acceleration.x,Imu_msg->linear_acceleration.y,Imu_msg->linear_acceleration.z;

		//添加IMU数据，时间够了则自动进行计算
        AddIMU(imu);

由于上面方法函数已经写好，所以测试函数就比较简单，上面就是将ros的imu格式的数据转为设置的imu格式的数据
下面就是添加IMU数据，时间够了则自动进行计算。

计算的结果在上面的实现函数中，直接打印在终端，可以查看

编译完代码，则可以进行测试。

启动无人机仿真环境：
在这里插入图片描述
无人机在地面静止

启动测试代码
终端输出

I0307 10:13:05.245363 49670 static_imu_init.cc:73] mean acce: -0.220884 -0.193247 009.92608
I0307 10:13:05.245481 49670 static_imu_init.cc:95] IMU 初始化成功，初始化时间= 10, bg = 0-0.00141685 000.00568429 -1.93852e-05, ba = -0.00267846 -0.00234334 0000.120365, gyro sq = 1.13541e-05 1.16579e-05 1.17825e-05, acce sq = 0.00150067 0.00157587 0.00172446, grav = 0.218205 0.190904 -9.80571, norm: 9.81
I0307 10:13:05.245513 49670 static_imu_init.cc:99] mean gyro: 0-0.00141685 000.00568429 -1.93852e-05 acce: -0.00267846 -0.00234334 0000.120365