IMU定位位姿跟踪（IMU_localizationorIMU_po_tracking）⽬录
1 前⾔
IMU位姿跟踪。
2 安装
800字
⾸先安装ROS，然后安装Eigen：
sudo apt-get install libeigen3-dev
从Github下载代码，并编译：
git clone /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.adverti<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<nsor_msgs/Imu.h>
#include<Eigen/Den>
#include<cmath>
定义结构体Po，包括位置（position）和姿态（orientation）。
struct Po
{
/*
Orientation orien;
Position pos;
*/
Eigen::Vector3d pos;
Eigen::Matrix3d orien;
};
定义类ImuIntegrator，包含要⽤到的变量和函数。
class ImuIntegrator
{
private:
Po po;
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 rver.
销售服务//void configCallback(node_example::node_example_paramsConfig &config, uint32_t level);
//! Publish the message.
void publishMessage();
//! Callback function for subscriber.
void ImuCallback(const nsor_msgs::Imu &msg);
void tGravity(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);
po.pos = zero;
一生不变歌词
velocity = zero;
line_pub = pub;
firstT = true;
// Line strip is blue
path.header.frame_id ="/global";
path.ns ="points_and_lines";
path.action = visualization_msgs::Marker::ADD;
ientation.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 nsor_msgs::Imu &msg)
{
if(firstT){
time = msg.header.stamp;
deltaT =0;
tGravity(msg.linear_acceleration);
firstT = fal;
}
el{
deltaT =(msg.header.stamp - time).toSec();
time = msg.header.stamp;
calcOrientation(msg.angular_velocity);
calcPosition(msg.linear_acceleration);
updatePath(po.pos);
publishMessage();
}
//std::cout << po.pos << std::endl;
}
将IMU的加速度赋给gravity：
void ImuIntegrator::tGravity(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 << po.orien << std::endl;
}
计算位置position：
void ImuIntegrator::calcPosition(const geometry_msgs::Vector3 &msg)
{
Eigen::Vector3d acc_l(msg.x, msg.y, msg.z);春景诗
Eigen::Vector3d acc_g = po.orien * acc_l;
//Eigen::Vector3d acc(msg.x - gravity[0], msg.y - gravity[1], msg.z - gravity[2]);
velocity = velocity + deltaT *(acc_g - gravity);
po.pos = po.pos + deltaT * velocity;
}
我没有看明⽩位置，速度和姿态（PVQ）的求解模型，求解位置应该是⼀个匀速模型。
4 测试
roscore
rosbag play [your bag]
rosrun hw3_0410817 hw3_node
注意：IMU话题是/imu/data，发布的marker在/globa坐标系中。
将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.0001
11323 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.0009072
63 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
