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

2024-01-25 15:20

本文主要是介绍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((imu.gyro_ - 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.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;
}

这篇关于imu的静止零偏噪声标定与积分的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/643681

相关文章

用Unity2D制作一个人物,实现移动、跳起、人物静止和动起来时的动画:中(人物移动、跳起、静止动作)

上回我们学到创建一个地形和一个人物,今天我们实现一下人物实现移动和跳起,依次点击,我们准备创建一个C#文件 创建好我们点击进去,就会跳转到我们的Vision Studio,然后输入这些代码 using UnityEngine;public class Move : MonoBehaviour // 定义一个名为Move的类,继承自MonoBehaviour{private Rigidbo

微积分-积分应用5.4(功)

术语“功”在日常语言中用来表示完成一项任务所需的总努力量。在物理学中,它有一个依赖于“力”概念的技术含义。直观上,你可以将力理解为对物体的推或拉——例如,一个书本在桌面上的水平推动,或者地球对球的向下拉力。一般来说,如果一个物体沿着一条直线运动,位置函数为 s ( t ) s(t) s(t),那么物体上的力 F F F(与运动方向相同)由牛顿第二运动定律给出,等于物体的质量 m m m 与其

解析apollo纵向控制标定表程序

百度apollo采用标定表描述车辆速度、加速度与油门/刹车之间的关系。该表可使无人车根据当前车速与期望加速度得到合适的油门/刹车开合度。除了文献《Baidu Apollo Auto-Calibration System - An Industry-Level Data-Driven and Learning based Vehicle Longitude Dynamic Calibrating

变速积分PID控制算法

变速积分PID控制算法 变速积分PID控制算法:变速积分PID的基本思想:变速积分的PID积分项表达式: 注:本文内容摘自《先进PID控制MATLAB仿真(第4版)》刘金琨 编著,研读此书受益匪浅,感谢作者! 变速积分PID控制算法: 在普通的PID控制算法中,由于积分系数 k i k_i ki​是常数,所以在整个控制过程中,积分增量不变。而系统对积分项的要求是,系统偏差大

梯形积分PID控制算法

梯形积分PID控制算法 梯形积分PID控制算法: 注:本文内容摘自《先进PID控制MATLAB仿真(第4版)》刘金琨 编著,研读此书受益匪浅,感谢作者! 梯形积分PID控制算法: 在PID控制律中积分项的作用是消除余差,为了减小余差,应提高积分项的运算精度,为此,可将矩形积分改为梯形积分。梯形积分的计算公式: ∫ 0 t e ( t ) d t = ∑ i = 0 k e

抗积分饱和PID控制算法

抗积分饱和PID控制算法 抗积分饱和PID控制算法:1.积分饱和现象:2.抗积分饱和算法: 注:本文内容摘自《先进PID控制MATLAB仿真(第4版)》刘金琨 编著,研读此书受益匪浅,感谢作者! 抗积分饱和PID控制算法: 1.积分饱和现象: 所谓积分饱和现象是指若系统存在一个方向偏差,PID控制器的输出由于积分作用的不断累加而加大,从而导致执行机构到达极限位置 X m

积分分离PID控制算法

积分分离PID控制算法 积分分离PID控制:积分分离控制基本思路:积分分离控制算法表示:积分分离式PID控制算法程序流程图: 注:本文内容摘自《先进PID控制MATLAB仿真(第4版)》刘金琨 编著,研读此书受益匪浅,感谢作者! 积分分离PID控制: 在普通的PID控制中引入积分环节的目的,主要为了消除静差,提高控制精度。但在过程启动、结束或大幅度增减设定时,短时间内系统输出

IMU腕带评估轮椅用户运动健康

近期,美国的研究团队利用惯性测量单元(IMU)和机器学习来准确评估手动轮椅使用者的运动健康状况,这在康复训练和慢性病管理领域具有广阔的应用前景。 研究小组将运用高性能的IMU传感器固定到轮椅使用者佩戴的手腕带上,用来监测并记录轮椅推进过程中的运动数据。实验设置了不同强度的六分钟推力测试,结果证实仅使用IMU传感器就能准确捕捉到轮椅使用者的速度、距离和节奏变化,为心血管健康评估提供

halcon 的图像坐标转到实际的机械坐标的标定

所谓手眼系统,就是人眼睛看到一个东西的时候要让手去抓取,就需要大脑知道眼睛和手的坐标关系。如果把大脑比作B,把眼睛比作A,把手比作C,如果A和B的关系知道,B和C的关系知道,那么C和A的关系就知道了,也就是手和眼的坐标关系也就知道了。 相机知道的是像素坐标,机械手是空间坐标系,所以手眼标定就是得到像素坐标系和空间机械手坐标系的坐标转化关系。 在实际控制中,相机检测到目标在图像中的像

IMU助力JAXA空间站机器人

近日,日本宇宙航空研究开发机构(JAXA)宣布,在国际空间站(ISS)实验舱“希望号”(Kibo)上部署的一款移动摄像机器人将采用Epson M-G370系列惯性测量单元(IMU)。IMU是一种能够检测物体运动状态的装置,通过测量加速度和角速度来确定物体的空间位置和姿态。这种技术对于在缺乏固定参照物的空间环境中尤为重要。 自2023年6月以来,“内部球形相机2号”(Internal Ball C