IMU定位/位姿跟踪（IMU_localization or IMU_pose_tracking）

1 前言

IMU位姿跟踪。

2 安装

首先安装ROS，然后安装Eigen：

sudo apt-get install libeigen3-dev

从Github下载代码，并编译：

git pull https://github.com/Abekabe/verdict-crawler.git
cd verdict-crawler-master
catkin_make

3 代码解读

3.1 main函数

先从main函数开始：

• 定义用于在rviz中显示IMU运动轨迹的Marker的发布器line，话题为Imu_path
• 在函数ImuIntegrator中初始化line
• 订阅IMU数据，话题为/imu/data，在回调函数ImuCallback中进行处理

int main(int argc, char **argv) {
    ros::init(argc, argv, "hw3_node");
    ros::NodeHandle nh;
    ros::Publisher line = nh.advertise<visualization_msgs::Marker>("Imu_path", 1000);
    ImuIntegrator *imu_integrator = new ImuIntegrator(line);
    ros::Subscriber Imu_message = nh.subscribe("/imu/data", 1000, &ImuIntegrator::ImuCallback, imu_integrator);
    ros::spin();
}

3.2 头文件

添加头文件，包括ROS和Eigen，其中visualization_msgs/Marker.h是为了在rviz中显示IMU的运动轨迹。

#include "ros/ros.h"
#include "ros/time.h"
#include 
#include
#include 
#include 

定义结构体Pose，包括位置（position）和姿态（orientation）。

struct Pose
{
    /*
    Orientation orien;
    Position pos;
    */
    Eigen::Vector3d pos;
    Eigen::Matrix3d orien;
};

定义类ImuIntegrator，包含要用到的变量和函数。

class ImuIntegrator
{
private:
    Pose pose;
    ros::Time time;
    Eigen::Vector3d gravity;
    Eigen::Vector3d velocity;
    visualization_msgs::Marker path;
    ros::Publisher line_pub;
    double deltaT;
    bool firstT;
public:
    //! Constructor.
    ImuIntegrator(const ros::Publisher&);
    //! Destructor.
    ~ImuIntegrator();

    //! Callback function for dynamic reconfigure server.
    //void configCallback(node_example::node_example_paramsConfig &config, uint32_t level);

    //! Publish the message.
    void publishMessage();

    //! Callback function for subscriber.
    void ImuCallback(const sensor_msgs::Imu &msg);

    void setGravity(const geometry_msgs::Vector3 &msg);
    void updatePath(const Eigen::Vector3d &msg);
    void calcPosition(const geometry_msgs::Vector3 &msg);
    void calcOrientation(const geometry_msgs::Vector3 &msg);
};

3.3 源文件

定义构造函数ImuIntegrator：

• 传入的参数为ros::Publisher &pub，然后有赋值给line_pub，在publishMessage函数中用于发布path，这种用法，我还是第一次见
• 设置路径path的参数，注意frame_id为"/global"，在rviz中要设置正确

ImuIntegrator::ImuIntegrator(const ros::Publisher &pub)
{
    Eigen::Vector3d zero(0, 0, 0);
    pose.pos = zero;
    pose.orien = Eigen::Matrix3d::Identity();
    velocity = zero;
    line_pub = pub;  
    firstT = true;

    // Line strip is blue
    path.color.b = 1.0;
    path.color.a = 1.0;
    path.type = visualization_msgs::Marker::LINE_STRIP;
    path.header.frame_id = "/global";   
    path.ns = "points_and_lines";
    path.action = visualization_msgs::Marker::ADD;
    path.pose.orientation.w = 1.0;
    path.scale.x = 0.2;
    geometry_msgs::Point p;
    p.x = 0;
    p.y = 0;
    p.z = 0;
    path.points.push_back(p);  
}

回调函数ImuCallback：

• 记录第一帧IMU数据的加速度，在求解位置和速度时会用到
• 记录第一帧IMU的时间戳，用于计算第二帧与第一帧IMU数据的时间戳的差值deltaT
• 计算位置和姿态，更新路径并发布

void ImuIntegrator::ImuCallback(const sensor_msgs::Imu &msg)
{
    if (firstT) {
        time = msg.header.stamp;
        deltaT = 0;
        setGravity(msg.linear_acceleration);
        firstT = false;
    }
    else {
        deltaT = (msg.header.stamp - time).toSec();
        time = msg.header.stamp;
        calcOrientation(msg.angular_velocity);
        calcPosition(msg.linear_acceleration);
        updatePath(pose.pos);
        publishMessage();
    }
    //std::cout << pose.pos << std::endl;
}

将IMU的加速度赋给gravity：

void ImuIntegrator::setGravity(const geometry_msgs::Vector3 &msg)
{
    gravity[0] = msg.x;
    gravity[1] = msg.y;
    gravity[2] = msg.z;
}

更新路径，将最新的位置p添加（push_back）到容器path.path中：

void ImuIntegrator::updatePath(const Eigen::Vector3d &msg)
{
    geometry_msgs::Point p;
    p.x = msg[0];
    p.y = msg[1];
    p.z = msg[2];
    path.points.push_back(p);
}

发布路径path：

void ImuIntegrator::publishMessage()
{
    line_pub.publish(path);
}

计算姿态orientation：

void ImuIntegrator::calcOrientation(const geometry_msgs::Vector3 &msg)
{
    Eigen::Matrix3d B;
    B << 0,                 -msg.z * deltaT,    msg.y * deltaT,
        msg.z * deltaT,     0,                  -msg.x * deltaT,
        -msg.y * deltaT,    msg.x * deltaT,     0;
    double sigma = std::sqrt(std::pow(msg.x, 2) + std::pow(msg.y, 2) + std::pow(msg.z, 2)) * deltaT;
    //std::cout << "sigma: " << sigma << std::endl << Eigen::Matrix3d::Identity() + (std::sin(sigma) / sigma) * B << std::endl << pose.orien << std::endl;
    pose.orien = pose.orien * (Eigen::Matrix3d::Identity() + (std::sin(sigma) / sigma) * B - ((1 - std::cos(sigma)) / std::pow(sigma, 2)) * B * B);
}

计算位置position：

void ImuIntegrator::calcPosition(const geometry_msgs::Vector3 &msg)
{
    Eigen::Vector3d acc_l(msg.x, msg.y, msg.z);
    Eigen::Vector3d acc_g = pose.orien * acc_l;
    //Eigen::Vector3d acc(msg.x - gravity[0], msg.y - gravity[1], msg.z - gravity[2]);
    velocity = velocity + deltaT * (acc_g - gravity);
    pose.pos = pose.pos + deltaT * velocity;
}

我没有看明白位置，速度和姿态（PVQ）的求解模型，求解位置应该是一个匀速模型。

4 测试

roscore
rosbag play [your bag]
rosrun hw3_0410817 hw3_node

注意：IMU话题是/imu/data，发布的marker在/globa坐标系中。

我使用的IMU是Xsens MTi-300，万元级产品，精度很高，可以参考：https://blog.csdn.net/learning_tortosie/article/details/97806633 。

将IMU静止放置在桌面上，下图为IMU的运动轨迹，附录中为IMU的部分位置数据。

可以看出，在IMU静止时，估计的运动轨迹都在不断累计飘移，这是因为IMU输出的加速度和角速度是不稳定的，经积分后会发生飘移。

我想测试IMU在1s内的精度，但是我很难控制程序运行，移动IMU和程序停止的时间，只能大致判断精度还可以。。。

5 总结

IMU无法提供长时间定位，只能提供短时间的定位，但是这个时间应该多短？我没办法去评估。

所以，IMU需要与其他传感器进行组合定位，一般融合的方法是EKF。

6 附录

部分IMU位置数据：

position2 3.1667e-07 4.18823e-07 -5.58963e-07
position3 2.09688e-07 1.85788e-06 -3.51271e-07
position4 -4.72242e-07 4.25249e-06 1.98705e-06
position5 -1.57088e-06 7.82243e-06 4.7222e-06
position6 -2.77659e-06 1.27414e-05 7.24739e-06
position7 -4.03386e-06 1.8075e-05 1.08147e-05
position8 -5.72336e-06 2.42595e-05 1.68338e-05
position9 -7.95667e-06 3.07524e-05 2.34455e-05
position10 -9.89981e-06 3.78275e-05 2.9214e-05
position11 -1.05061e-05 4.44589e-05 3.48982e-05
position12 -1.03208e-05 5.24322e-05 4.23239e-05
position13 -9.19498e-06 6.20016e-05 5.14419e-05
position14 -7.40169e-06 7.03085e-05 5.92352e-05
position15 -5.68151e-06 7.75432e-05 6.65798e-05
position16 -3.18753e-06 8.80226e-05 7.73008e-05
position17 -2.23853e-06 9.92139e-05 8.83477e-05
position18 -1.46438e-06 0.000111323 9.99378e-05
position19 -1.30969e-06 0.000125296 0.000112212
position20 -6.75859e-07 0.000138647 0.000123822
position21 1.50035e-07 0.00015484 0.000136454
position22 1.33587e-06 0.000171044 0.000147891
position23 3.87662e-06 0.000189307 0.000159541
position24 7.53172e-06 0.000209289 0.000173188
position25 1.16375e-05 0.000229701 0.00018723
position26 1.57472e-05 0.000251215 0.000201975
position27 1.97467e-05 0.000275027 0.000218842
position28 2.4008e-05 0.00029846 0.000236118
position29 2.93382e-05 0.00032344 0.000252653
position30 3.60018e-05 0.000351552 0.000271231
position31 4.27963e-05 0.00037874 0.000289976
position32 4.9181e-05 0.000405715 0.000309522
position33 5.65672e-05 0.00043759 0.000331324
position34 6.3681e-05 0.00046902 0.000353178
position35 7.07884e-05 0.000501666 0.000374632
position36 7.86109e-05 0.000537786 0.000396655
position37 8.73642e-05 0.000573984 0.000418308
position38 9.7066e-05 0.000610461 0.000440244
position39 0.000108412 0.00064868 0.000462428
position40 0.000121827 0.000689065 0.000485004
position41 0.000137117 0.000732777 0.000511384
position42 0.000152824 0.000774498 0.000537384
position43 0.00016926 0.000818076 0.00056531
position44 0.000184104 0.000861311 0.000591705
position45 0.000199126 0.000907263 0.000618911
position46 0.000214609 0.000956573 0.000647622
position47 0.000230314 0.00100395 0.000675678
position48 0.000247648 0.0010528 0.000704976
position49 0.0002678 0.00110407 0.000736164
position50 0.000289648 0.00115695 0.000767179
position51 0.000312347 0.00120832 0.000797297
position52 0.000335818 0.0012617 0.000827277
position53 0.000360425 0.00131965 0.000860217
position54 0.000383238 0.00137545 0.000893443
position55 0.000408751 0.00143459 0.000927919
position56 0.00043595 0.00149476 0.000961811
position57 0.000464247 0.00155597 0.000996355
position58 0.000495352 0.00161997 0.00103337
position59 0.000529119 0.00168663 0.00107095
position60 0.000563666 0.00175463 0.00110999
position61 0.000599249 0.00182437 0.00114932
position62 0.000637557 0.00189728 0.0011909
position63 0.00067478 0.00196835 0.00123038
position64 0.000718714 0.00205123 0.00127614
position65 0.000760842 0.00213029 0.00131954
position66 0.000801729 0.00220358 0.00136052
position67 0.000845431 0.00227871 0.00140369
position68 0.0008954 0.00235944 0.00144874
position69 0.000946202 0.00244008 0.00149334
position70 0.000997498 0.0025202 0.00153673
position71 0.00105692 0.00261366 0.00158757
position72 0.00110558 0.00269061 0.00162998
position73 0.0011601 0.00277734 0.00167893
position74 0.00122223 0.00287494 0.0017331
position75 0.00128237 0.00296698 0.00178424
position76 0.00134042 0.0030536 0.00183115
position77 0.00140235 0.00314287 0.0018777
position78 0.00146934 0.00323922 0.00192884
position79 0.00153634 0.00333519 0.00198083
position80 0.0016017 0.00342902 0.00203058
position81 0.00167287 0.00353146 0.00208507
position82 0.00174496 0.00363303 0.00213974
position83 0.00181869 0.00373577 0.00219505
position84 0.00189358 0.00383985 0.00225007
position85 0.00197415 0.00395036 0.00230751
position86 0.00205397 0.00405701 0.00236433
position87 0.00213916 0.00416804 0.0024213
position88 0.00222626 0.0042789 0.00247849
position89 0.00231756 0.00439216 0.00253788
position90 0.00240924 0.00450443 0.0025953
position91 0.00250617 0.00462254 0.0026555
position92 0.00259161 0.00472709 0.00271057
position93 0.00270191 0.00486185 0.00278371
position94 0.00280109 0.00498487 0.00284746
position95 0.00289281 0.00509913 0.00290316
position96 0.00299311 0.00522353 0.00296323
position97 0.00309983 0.00535355 0.00302741
position98 0.00321182 0.00548599 0.00309327
position99 0.0033223 0.00561434 0.00315907
position100 0.00343897 0.00574678 0.00322572