imu积分定位

SLAM 专栏收录该内容
50 篇文章 4 订阅

imu积分定位的输入为imu传感器得到的线性加速度和角速度,输出为积分得到的位姿(位置和角度)。
具体公式为:

1.求旋转

设前一帧位姿的旋转用旋转矩阵表示为 R 1 R_{1} R1,当前帧位姿的旋转用旋转矩阵表示为 R 2 R_{2} R2
,假设对机器人进行了一次旋转,可以表示为如下:
R 2 = R 1 ∗ R 12 R_{2}=R_{1}*R_{12} R2=R1R12
用四元数计算如下
q 2 = q 1 ∗ q 12 q_{2}=q_{1}*q_{12} q2=q1q12
代码如下:

tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);

2.求平移

设前一帧位姿的平移表示为 t 1 t_{1} t1,当前帧位姿的平移表示为 t 2 t_{2} t2
,假设对机器人进行了一次平移,可以表示为如下:
求平移公式为
t 2 = t 1 + v ∗ t + 1 2 ∗ a ∗ t 2 t_{2}=t_{1}+v*t+\frac{1}{2}*a*t^2 t2=t1+vt+21at2
代码如下

tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;

3.vins中的偏移

vins中的将角速度偏移和线加速度的偏移作为优化量,因此预测过程(航迹推导过程)中需要加入偏移。

3.1角速度计算

设前一帧的角速度表示为 w 1 w_{1} w1,当前帧传感器获得的角速度表示为 w 2 w_{2} w2,传感器偏移为 b g b_{g} bg
则平均角速度为
w = 1 2 ( w 1 + w 2 ) − b g w=\frac{1}{2}(w_{1}+w_{2})-b_{g} w=21(w1+w2)bg

  Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;

则在t时刻的旋转角度为:
β = w t \beta=wt β=wt

3.2加速度计算

设前一帧的局部坐标系的加速度表示为 a 1 a_{1} a1,加速度偏移为 b 1 a b1_{a} b1a,重力加速度为 g g g
前一帧在世界坐标系下的加速度 a 1 , g a_{1,g} a1,g
a 1 , g = q 1 ( a 1 − b 1 a ) − g a_{1,g}=q_{1}(a_{1}-b1_{a})-g a1,g=q1(a1b1a)g

Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;

设当前帧的局部坐标系的加速度表示为 a 2 a_{2} a2,加速度偏移为 b 2 a b2_{a} b2a,重力加速度为 g g g
当前帧在世界坐标系下的加速度 a 2 , g a_{2,g} a2,g
a 2 , g = q 2 ( a 2 − b 2 a ) − g a_{2,g}=q_{2}(a_{2}-b2_{a})-g a2,g=q2(a2b2a)g

  Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;

imu积分定位实践代码如下

void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
    double t = imu_msg->header.stamp.toSec();
    if (init_imu)
    {
        latest_time = t;
        init_imu = 0;
        return;
    }
    double dt = t - latest_time;
    latest_time = t;

    double dx = imu_msg->linear_acceleration.x;
    double dy = imu_msg->linear_acceleration.y;
    double dz = imu_msg->linear_acceleration.z;
    Eigen::Vector3d linear_acceleration{dx, dy, dz};

    double rx = imu_msg->angular_velocity.x;
    double ry = imu_msg->angular_velocity.y;
    double rz = imu_msg->angular_velocity.z;
    Eigen::Vector3d angular_velocity{rx, ry, rz};

    Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;

    Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
    tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);

    Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;

    Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);

    tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
    tmp_V = tmp_V + dt * un_acc;

    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

可用代码2


#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>

#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>
#include <signal.h>
#include <unistd.h>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include <opencv2/core/core.hpp>
#include<opencv2/opencv.hpp>
//********para of imu*******
Eigen::Vector3d tmp_P=Eigen::Vector3d(0, 0, 0); //t
Eigen::Quaterniond tmp_Q=Eigen::Quaterniond::Identity();//R
Eigen::Vector3d tmp_V=Eigen::Vector3d(0, 0, 0);
double g_last_imu_time = -1.0;
ros::Publisher g_imu_path_pub;
nav_msgs::Path g_imu_path;

using namespace std;
void ShowIMUPositionCallBack(Eigen::Vector3d imu_linear_acc, Eigen::Vector3d&  imu_angular_vel,clock_t t)
{
    std::cout<<imu_angular_vel(2)<<std::endl;
    double dt =(double) (t - g_last_imu_time)/CLOCKS_PER_SEC;
    g_last_imu_time = t;
    tmp_V+=tmp_Q*imu_linear_acc*dt;

    //  Eigen::Quaterniond wheelR=Eigen::Quaterniond(1,0,0,0.5*wheel_angular_vel(2)*dt);

    //  Eigen::Vector3d wheelt= g_wheel_predicted_rot*wheel_linear_vel*dt;
    Eigen::Vector3d acc=  tmp_Q*imu_linear_acc;
    tmp_P = tmp_P + tmp_Q* tmp_V*dt+0.5*dt*dt*acc ;
     tmp_Q = tmp_Q * Eigen::Quaterniond(1, 0 , 0, 0.5*imu_angular_vel(2)*dt);
    cout<<"tmp_Q eular"<<(180/M_PI)*tmp_Q.matrix().eulerAngles(0,1,2)<<endl;

    //pub path
    geometry_msgs::PoseStamped this_pose_stamped;
    this_pose_stamped.pose.position.x =tmp_P(0);
    this_pose_stamped.pose.position.y = tmp_P(1);
    this_pose_stamped.pose.position.z = tmp_P(2);

    this_pose_stamped.pose.orientation.x = tmp_Q.x();
    this_pose_stamped.pose.orientation.y = tmp_Q.y();
    this_pose_stamped.pose.orientation.z = tmp_Q.z();
    this_pose_stamped.pose.orientation.w = tmp_Q.w();
    this_pose_stamped.header.stamp= ros::Time::now();
    this_pose_stamped.header.frame_id="imu_path";
    std::cout<<" dt: " <<  dt <<std:: cout<<"x="<<tmp_P(0)<<" y="<<tmp_P(1)<<" z="<<tmp_P(2)<<std::endl;

    g_imu_path.poses.push_back(this_pose_stamped);
    g_imu_path_pub.publish(g_imu_path);

}

main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath");
    std::cout<<"start benchmark"<<std::endl;
   ros::NodeHandle ph;
    g_imu_path_pub = ph.advertise<nav_msgs::Path>("imu_path",1, true);
    g_imu_path.header.frame_id="world";
    double imu_angular_vel_rz=0.1;
     double const_imu_linear_acc_x = 0.0;
     double const_imu_linear_acc_y =0.0;
     Eigen::Vector3d imu_angular_vel={ 0, 0, imu_angular_vel_rz};
     Eigen::Vector3d imu_linear_acc={const_imu_linear_acc_x, const_imu_linear_acc_y,0};

    clock_t current_time,init_time;
    init_time=clock();
    current_time=clock();

    bool init_flag=false;
    tmp_V={0.1,0,0};
    cv::RNG rng;                        // OpenCV随机数产生器
    while (1)
    {
          if(false==init_flag)
          {
              current_time=init_time;
              g_last_imu_time=current_time;
              init_flag=true;
          }else{
              current_time = clock();
          }
          //add noise
      // imu_linear_acc(0)+= rng.gaussian ( 0.0001);
      cout<<"imu_angular_vel"<<imu_angular_vel(2)<<std::endl;
       ShowIMUPositionCallBack( imu_linear_acc, imu_angular_vel,current_time);
        sleep(0.1);
    }

    return 0;
}
  • 12
    点赞
  • 7
    评论
  • 58
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

©️2021 CSDN 皮肤主题: 创作都市 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值