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

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: 90498stamp: secs: 1698127594nsecs: 155961838frame_id: "gyro_link"
orientation: x: 0.0049992394633591695y: 0.002799155190586991z: -0.1353550255298614w: -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.0013670891057699919y: 0.0030183307826519012z: -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.04501450061798096y: -0.09672745317220688z: 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((_ - bg_) * dt);}// 更新时间timestamp_ = imu.timestamp_;}/// 组成NavStateNavStated 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, gravityVec3d 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

#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) { _; });math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_, [this](const IMU& imu) { return imu.acce_; });// 以acce均值为方向，取9.8长度为重力LOG(INFO) << "mean acce: " << anspose();gravity_ = -mean_acce / () * 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: " << anspose() << " acce: " << anspose();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.Sec();imu.acce_.x()=imu_msg.linear_acceleration.x;imu.acce_.y()=imu_msg.linear_acceleration.y;imu.acce_.z()=imu_msg.linear_acceleration._.x()=imu_msg.angular_velocity._.y()=imu_msg.angular_velocity._.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.ientation.x=q.x();odom.ientation.y=q.y();odom.ientation.z=q.z();odom.ientation.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;
}