imu的静止零偏噪声标定与积分

示例使用的Imu为轮趣科技 n100 mini其中imu出来的数据的坐标系是基于ROS坐标系的
在这里插入图片描述

Eigen::Quaterniond q_ahrs(ahrs_frame_.frame.data.data_pack.Qw,
                          ahrs_frame_.frame.data.data_pack.Qx,
                          ahrs_frame_.frame.data.data_pack.Qy,
                          ahrs_frame_.frame.data.data_pack.Qz);

Eigen::Quaterniond q_r =                          
    Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitZ()) * 
    Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitY()) * 
    Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitX());

Eigen::Quaterniond q_rr =                          
    Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitZ()) * 
    Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitY()) * 
    Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitX());

Eigen::Quaterniond q_out =  q_r * q_ahrs * q_rr

为什么要右乘q_rr？

回答：你可以把*q_rr去掉，重新编译后，通过TF转换看到，实际上这个坐标是倒着的，还需要绕X轴转180°才是我们ROS里面用到的坐标系。

只左乘q_r的话是可以完成TF的坐标变换的，但是我们通常在ros里用到的坐标系是前左上，所以还要通过右乘q_rr来修正坐标系的位姿

备注：这款IMU是九轴IMU，融合了磁力计，也就是原始的yaw角指北是0度，范围为0-360°，顺时针增大。转换到imu单品ROS标准下的坐标后航向应该特殊处理为东北天坐标系。
在这里插入图片描述

输入的单帧Imu示例

  seq: 90498
  stamp: 
    secs: 1698127594
    nsecs: 155961838
  frame_id: "gyro_link"
orientation: 
  x: 0.0049992394633591695
  y: 0.002799155190586991
  z: -0.1353550255298614
  w: -0.9907805919647217
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.0013670891057699919
  y: 0.0030183307826519012
  z: -0.002960443962365389
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.04501450061798096
  y: -0.09672745317220688
  z: 9.795211791992188
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

imu_integration.h

#ifndef SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H
#define SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H

#include "eigen_types.h"
#include "imu.h"
#include "nav_state.h"

namespace sad {

/**
 * 本程序演示单纯靠IMU的积分
 */
class IMUIntegration {
   public:
    IMUIntegration(const Vec3d& gravity, const Vec3d& init_bg, const Vec3d& init_ba)
        : gravity_(gravity), bg_(init_bg), ba_(init_ba) {}

    // 增加imu读数
    void AddIMU(const IMU& imu) {
        double dt = imu.timestamp_ - timestamp_;
        if (dt > 0 && dt < 0.1) 
        {
            // 假设IMU时间间隔在0至0.1以内
            p_ = p_ + v_ * dt + 0.5 * gravity_ * dt * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt;
            v_ = v_ + R_ * (imu.acce_ - ba_) * dt + gravity_ * dt;
            R_ = R_ * Sophus::SO3d::exp((imu.gyro_ - bg_) * dt);
        }
        // 更新时间
        timestamp_ = imu.timestamp_;
    }

    /// 组成NavState
    NavStated GetNavState() const { return NavStated(timestamp_, R_, p_, v_, bg_, ba_); }

    SO3 GetR() const { return R_; }
    Vec3d GetV() const { return v_; }
    Vec3d GetP() const { return p_; }

   private:
    // 累计量
    SO3 R_;
    Vec3d v_ = Vec3d::Zero();
    Vec3d p_ = Vec3d::Zero();

    double timestamp_ = 0.0;

    // 零偏，由外部设定
    Vec3d bg_ = Vec3d::Zero();
    Vec3d ba_ = Vec3d::Zero();

    Vec3d gravity_ = Vec3d(0, 0, -9.8);  // 重力
};

}  // namespace sad

#endif  // SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H

static_imu_init.h

#ifndef SLAM_IN_AUTO_DRIVING_STATIC_IMU_INIT_H
#define SLAM_IN_AUTO_DRIVING_STATIC_IMU_INIT_H

#include "eigen_types.h"
#include "imu.h"
#include "odom.h"

#include <deque>

namespace sad {

/**
 * IMU水平静止状态下初始化器
 * 使用方法：调用AddIMU, AddOdom添加数据，使用InitSuccess获取初始化是否成功
 * 成功后，使用各Get函数获取内部参数
 *
 * 初始化器在每次调用AddIMU时尝试对系统进行初始化。在有odom的场合，初始化要求odom轮速读数接近零；没有时，假设车辆初期静止。
 * 初始化器收集一段时间内的IMU读数，按照书本3.5.4节估计初始零偏和噪声参数，提供给ESKF或者其他滤波器
 */
class StaticIMUInit {
   public:
    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_ = false;  // 是否使用odom来判断车辆静止（部分数据集没有odom选项）
    };

    /// 构造函数
    StaticIMUInit(Options options = Options()) : options_(options) {}

    /// 添加IMU数据
    bool AddIMU(const IMU& imu);
    /// 添加轮速数据
    bool AddOdom(const Odom& 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_; }

   private:
    /// 尝试对系统初始化
    bool TryInit();

    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;    // 静止的初始时间
};

}  // namespace sad

#endif  // SLAM_IN_AUTO_DRIVING_STATIC_IMU_INIT_H

static_imu_init.cc

#include "math_utils.h"


namespace sad {

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_;
    }

    // 记入初始化队列
    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();
    }

    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_; });

    // 以acce均值为方向，取9.8长度为重力
    LOG(INFO) << "mean acce: " << mean_acce.transpose();
    gravity_ = -mean_acce / mean_acce.norm() * options_.gravity_norm_;

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

    // 检查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;
    }

    // 估计测量噪声和零偏
    init_bg_ = mean_gyro;
    init_ba_ = mean_acce;

    LOG(INFO) << "IMU 初始化成功，初始化时间= " << current_time_ - init_start_time_ << ", bg = " << init_bg_.transpose()
              << ", ba = " << init_ba_.transpose() << ", gyro sq = " << cov_gyro_.transpose()
              << ", acce sq = " << cov_acce_.transpose() << ", grav = " << gravity_.transpose()
              << ", norm: " << gravity_.norm();
    LOG(INFO) << "mean gyro: " << mean_gyro.transpose() << " acce: " << mean_acce.transpose();
    init_success_ = true;
    return true;
}

}  // namespace sad

main.cpp

#include<ros/ros.h>
#include<imu_integration.h>
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"
#include<nav_msgs/Odometry.h>
#include<static_imu_init.h>
// 该实验中，我们假设零偏已知
Vec3d gravity(-0.050622,0.102474,-9.809334);  // 重力方向
Vec3d init_bg(-0.005136,-0.000981,-0.003482);
Vec3d init_ba(-0.000118,0.000239,-0.022851);
sad::IMUIntegration imu_integ1(gravity, init_bg, init_ba);

ros::Publisher pub_;

sad::StaticIMUInit imu_init;  // 使用默认配置



sad::IMU imu_format(const sensor_msgs::Imu &imu_msg)
{

    sad::IMU imu;
    imu.timestamp_=imu_msg.header.stamp.toSec();
    imu.acce_.x()=imu_msg.linear_acceleration.x;
    imu.acce_.y()=imu_msg.linear_acceleration.y;
    imu.acce_.z()=imu_msg.linear_acceleration.z;
    imu.gyro_.x()=imu_msg.angular_velocity.x;
    imu.gyro_.y()=imu_msg.angular_velocity.y;
    imu.gyro_.z()=imu_msg.angular_velocity.z;
    return imu;

}
void IMUCallback(const sensor_msgs::Imu &imu_msg)
{
    sad::IMU imu_out=imu_format(imu_msg);

    /// IMU 处理函数
    if (!imu_init.InitSuccess()) 
    {
        imu_init.AddIMU(imu_out);
        return;
    }
    //imu_init.GetInitBg(), imu_init.GetInitBa(), imu_init.GetGravity()
    static bool imu_inited = false;

    if(!imu_inited)
    {
    
        ROS_INFO("bg:%f,%f,%f",imu_init.GetInitBg().x(),imu_init.GetInitBg().y(),imu_init.GetInitBg().z());
        ROS_INFO("ba:%f,%f,%f",imu_init.GetInitBa().x(),imu_init.GetInitBa().y(),imu_init.GetInitBa().z());
        ROS_INFO("g:%f,%f,%f",imu_init.GetGravity().x(),imu_init.GetGravity().y(),imu_init.GetGravity().z());
        imu_inited=true;
    }

    imu_integ1.AddIMU(imu_out);
    Eigen::Matrix3d R=imu_integ1.GetR().matrix();
    nav_msgs::Odometry odom;
    odom.header.frame_id="odom";
    odom.header.stamp=imu_msg.header.stamp;
    odom.child_frame_id="base_link";
    odom.pose.pose.position.x=imu_integ1.GetP().x();
    odom.pose.pose.position.y=imu_integ1.GetP().y();
    odom.pose.pose.position.z=imu_integ1.GetP().z();
    Eigen::Quaterniond q;
    q=R.block<3,3>(0,0);
    odom.pose.pose.orientation.x=q.x();
    odom.pose.pose.orientation.y=q.y();
    odom.pose.pose.orientation.z=q.z();
    odom.pose.pose.orientation.w=q.w();
    odom.twist.twist.linear.x=imu_integ1.GetV().x();
    odom.twist.twist.linear.y=imu_integ1.GetV().y();
    odom.twist.twist.linear.z=imu_integ1.GetV().z();
    pub_.publish(odom);
}

int main(int argc, char** argv) 
{
    ros::init(argc, argv, "imu_integration");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/imu", 100, IMUCallback);
    pub_=n.advertise<nav_msgs::Odometry>("/imu_integration_odom",10);
    ros::spin();
    return 0;
}