IMU的学习记录

---

一、IMU简单介绍

1.IMU原理

当我们晚上回到家，发现家里停电时，眼睛在黑暗中什么都看不见的情况下，只能根据自己的经验，极为谨慎地走小碎步，并不断用手摸周围的东西（比如冰箱），用以确定自己所在的位置。

IMU的原理和黑暗中走小碎步很相似。在黑暗中，由于自己对步长的估计和实际走的距离存在误差，走的步数越来越多时，自己估计的位置与实际的位置相差会越来越远。就像下图所示：

走第一步时，估计位置（黑人所在位置）与实际位置（白人所在位置）还比较接近；但随着步数增多，估计位置与实际位置的差别越来越大。图中的小人只朝一个方向移动，是一维的。根据此方法推广到三维，就是惯性测量单元的原理。

学术上的语言是：以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。

2.GPS+IMU

在无人驾驶系统中，GPS的更新频率一般为10Hz，IMU的更新频率一般为100Hz。

两个传感器共同工作时，可以给出频率100Hz的定位输出。下图是两传感器数据融合的原理图:

跑在控制器上的软件对信息的处理流程在时间维度上类似下图。在0~100ms的周期中，使用IMU进行9次位置的估计，待新的GPS定位数据进来时，则进行修正，以此实现高频率的定位结果输出。

二、IMU信息采集

1.xsense_driver

提示
[ERROR] [1551165884.109790]: Fatal: could not find proper MT device

运行命令：
sudo chmod 777 /dev/ttyUSB0 //给串口赋权限，USB转串口的赋权限命令
实在不行USB口重新插拔几次，至少我是这么做的。

2.rostopic list

3.rostopic

imu数据类型：
消息头/四元数/四元数残差/角速度/角速度残差/线速度/线速度残差

4.四元数转欧拉角

imu输出信息中没有欧拉角，为了方便观察所以写了一端代码，进行四元数转欧拉角。
可以直接采用ROS中四元数转欧拉角的函数

代码如下：

#include "ros/ros.h"
#include "geometry_msgs/Vector3.h"
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/Quaternion.h"
#include "tf/transform_datatypes.h"

ros::Publisher rpy_publisher;
ros::Subscriber quat_subscriber;

void MsgCallback(const sensor_msgs::Imu msg)
{
    

    tf::Quaternion quat;

    quat = tf::Quaternion(msg.orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w);
    
    // the tf::Quaternion has a method to acess roll pitch and yaw
    double roll, pitch, yaw;
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
 
    // the found angles are written in a geometry_msgs::Vector3
    geometry_msgs::Vector3 rpy;
    rpy.x = roll;
    rpy.y = pitch;
    rpy.z = yaw;
 
    // this Vector is then published:
    rpy_publisher.publish(rpy);
    ROS_INFO("published rpy angles: roll=%f pitch=%f yaw=%f", rpy.x, rpy.y, rpy.z);
}
 
int main(int argc, char **argv)
{
    
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    rpy_publisher = n.advertise<geometry_msgs::Vector3>("rpy_angles", 1000);
    quat_subscriber = n.subscribe("/imu/data", 1000, MsgCallback);
 

    ROS_INFO("waiting for quaternion");
    ros::spin();
    return 0;
}

转欧拉角数据如下：

4.运行rqt

由于在平面上进行旋转，所以只有绕Z轴的数据变化，其余两个轴都为零。

5.运行rviz

需要加载：rviz_imu_plugin
这是一个显示sensor_msgs::Imu 消息的rviz插件
通过rviz可以显示此时的位姿，但是不能观察到位移的变化

总结

这是第一篇博客，学习的过程可能也需要多多总结，以后这就作为一个总结的地方吧！