imu积分定位

imu积分定位的输入为imu传感器得到的线性加速度和角速度，输出为积分得到的位姿（位置和角度）。
具体公式为：

1.求旋转

设前一帧位姿的旋转用旋转矩阵表示为$R_1$，当前帧位姿的旋转用旋转矩阵表示为$R_2$
，假设对机器人进行了一次旋转，可以表示为如下：
$$R_2=R_1\ast R_{12}$$
用四元数计算如下
$$q_2=q_1\ast q_{12}$$
代码如下：

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

２.求平移

设前一帧位姿的平移表示为$t_1$，当前帧位姿的平移表示为$t_2$
，假设对机器人进行了一次平移，可以表示为如下：
求平移公式为
$t_2=t_1+v\ast t+\frac{1}{2}\ast a\ast t^2$
代码如下

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

３.vins中的偏移

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

3.1角速度计算

设前一帧的角速度表示为$w_1$，当前帧传感器获得的角速度表示为$w_2$,传感器偏移为$b_g$
则平均角速度为
$$w=\frac{1}{2}(w_1+w_2)-b_g$$

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

则在ｔ时刻的旋转角度为：
$\beta =wt$

3.2加速度计算

设前一帧的局部坐标系的加速度表示为$a_1$，加速度偏移为$b{1}_a$,重力加速度为$g$
前一帧在世界坐标系下的加速度$a_{1,g}$
$$a_{1,g}=q_1(a_1-b{1}_a)-g$$

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

设当前帧的局部坐标系的加速度表示为$a_2$，加速度偏移为$b{2}_a$,重力加速度为$g$
当前帧在世界坐标系下的加速度$a_{2,g}$
$$a_{2,g}=q_2(a_2-b{2}_a)-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;
}

可用代码２

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