惯导系统静止初始化方法与代码实现并在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 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_;
    }

点击惯导系统静止初始化方法与代码实现并在gazebo中测试——古月居可查看全文