1 前言

IMU位姿跟踪。

2 安装

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

sudo apt-get install libeigen3-dev

从Github下载代码，并编译：

git clone https://github.com/Abekabe/IMU-Dead-Reckoning.git
cd IMU-Dead-Reckoning
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 <visualization_msgs/Marker.h>
#include<sensor_msgs/Imu.h>
#include <Eigen/Dense>
#include <cmath>

定义结构体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 bluepath.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

IMU定位/位姿跟踪（IMU_localization or IMU_pose_tracking）相关推荐

MO-LTR：基于单目RGB视频的多目标定位、跟踪与重建
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达标题:MO-LTR: Multiple Object Localization, Tracking a ...
李飞飞团队最新论文：基于anchor关键点的类别级物体6D位姿跟踪
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达简介作者提出了一种基于RGB-D的深度学习方法6PACK,能够实时的跟踪已知类别物体.通过学习用少量 ...
摄像头图像分析目标物体大小位置_一文读懂图像定位及跟踪技术
文 | 传感器技术在科学技术日新月异的今天,人们对机器设备的智能性.自主性要求也越来越高,希望其完全替代人的角色,把人们从繁重.危险的工作任务中解脱出来,而能否像人一样具有感知周围环境的能力已成为设 ...
传统的6d位姿估计fangfa1_李飞飞团队最新论文：基于anchor关键点的类别级物体6D位姿跟踪...
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达简介作者提出了一种基于RGB-D的深度学习方法6PACK,能够实时的跟踪已知类别物体.通过学习用少量 ...
雷达、定位、跟踪等信号处理邻域SCI期刊整理及推荐
雷达邻域SCI期刊整理及推荐:题名.刊物信息.撰写特点.审稿周期及投稿难度总结定位/跟踪邻域SCI期刊整理及推荐:题名.刊物信息.撰写特点.审稿周期及投稿难度总结估计/滤波/融合等信号处理邻域SC ...
多传感器融合SLAM：激光雷达、RGB相机、IMU定位与光度重建
作者 | 流川峰编辑 | 深蓝前沿点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[SLAM]技术交流群后台回复[SLAM综述]获 ...
基于 IMU 的位姿解算
前言 IMU,即惯性测量元件,以牛顿经典力学定律为基本工作原理,三轴加速度计和三轴陀螺仪作为敏感元件,上电时分别输出加速度和角速度. 一般情况下,将捷联固定IMU的运动对象称为载体.当载体位姿发生变化 ...
(完整项目源码)GPS定位源码整套管理系统，轨迹播放，车辆管理，电子围栏，报警记录，数据库/人员定位/宠物定位/物流跟踪/资产定位
(完整项目源码)GPS定位源码整套管理系统,轨迹播放,车辆管理,电子围栏,报警记录,Mysql数据库- 框架:spring+jpa+struts2 ,MVC架构设计,可扩展性强. 数据库:mysql5 ...
水下声呐定位声线跟踪算法
声波传播速度是海洋中最重要的声学参数,它是影响声波在海洋中传播的最基本的物理量参数.有关测量研究表明,水中声速是温度.盐度和静压力的函数,声速随着温度.盐度和压力的增加而增加.海洋的不均匀性和多变性强 ...

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

目录