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

惯导系统静止初始化方法与代码实现并在gazebo中测试_第1张图片

前言

在进行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中测试——古月居可查看全文

你可能感兴趣的:(cocos2d,游戏引擎)