IMU learning records

IMU A record of learning

---

One 、IMU Brief introduction

1.IMU principle

When we get home at night , When the power failure is found at home , When the eyes can't see anything in the dark , Only based on your own experience , Take small steps with extreme caution , And keep touching things around with your hands （ For example, refrigerator. ）, To determine where you are .

IMU The principle of is very similar to taking small steps in the dark . In the dark , Because of the error between the estimation of the step size and the actual distance , When you take more and more steps , The difference between the estimated position and the actual position will be farther and farther . As shown in the figure below ：

When taking the first step , estimated position （ Where blacks are ） And the actual location （ Where the white man is ） It is quite close to ; But as the number of steps increases , The difference between the estimated position and the actual position is becoming larger and larger . The little man in the picture only moves in one direction , It's one-dimensional . According to this method, it is extended to three-dimensional , Is the principle of the inertial measurement unit .

Academic language is ： Based on Newton's laws of Mechanics , By measuring the acceleration of the carrier in the inertial reference system , Integrate it with time , And transform it into the navigation coordinate system , You can get the speed in the navigation coordinate system 、 Information such as yaw angle and position .

2.GPS+IMU

In driverless systems ,GPS The update frequency of is generally 10Hz,IMU The update frequency of is generally 100Hz.

When two sensors work together , The frequency can be given 100Hz Positioning output of . The following figure is the schematic diagram of two sensor data fusion :

The information processing flow of the software running on the controller is similar to the following figure in terms of time dimension . stay 0~100ms In the cycle of , Use IMU Conduct 9 Estimation of secondary position , Wait for the new GPS When positioning data comes in , Then make corrections , In this way, high-frequency positioning result output can be realized .

Two 、IMU information acquisition

1.xsense_driver

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

Run the command ：
sudo chmod 777 /dev/ttyUSB0 // Assign permissions to the serial port ,USB Transfer to serial port authorization command
Really not line USB Plug and unplug the port several times , At least that's what I did .

2.rostopic list

3.rostopic

imu data type ：
The message header / Four yuan number / Quaternion residual / angular velocity / Angular velocity residual / Linear velocity / Linear velocity residual

4. Quaternion to Euler angle

imu There is no Euler angle in the output information , For the convenience of observation, I wrote one end of the code , Quaternion to Euler angle .
Can be used directly ROS Function of quaternion to Euler angle in

The code is as follows ：

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

The Euler angle data are as follows ：

4. function rqt

Because of the rotation on the plane , So only around Z Axis data changes , The other two axes are zero .

5. function rviz

Need to load ：rviz_imu_plugin
This is a display sensor_msgs::Imu News rviz plug-in unit
adopt rviz You can display the pose at this time , But the change of displacement cannot be observed

summary

This is the first blog , The learning process may also need a lot of summary , In the future, this will be a place to summarize ！