SLAM从入门到精通（IMU数据的读取）_六轴陀螺仪 slam

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        上一篇文章我们说过，对于差速轮来说，旋转的计算很多程度上依赖于theta=tan(theta)这个公式来进行的。但是，我们也知道，如果机器人转弯很快的话，这个公式其实是不成立的。所以，对于旋转这件事情来说，我们能想到的，就是用IMU来对角度来进行补偿和计算的。

        试想一下，如果在地面上，我们尚可通过电机的编码器数据计算来获取里程计数据信息，那么对于天上的飞机或者其他飞行器来说，就没有这个便利了。这个时候，IMU设备就派上用场了。IMU也称之为陀螺仪，有三轴、六轴、九轴之说。一般三轴IMU包括了三个方向的角度信息；六轴陀螺仪则在三轴的基础上，添加了三个方向的线加速度信息；九轴陀螺仪则在六轴陀螺仪的基础之上，进一步添加了三个方向的磁力计信息。对于速度不是特别高的机器人设备来说，实时获取三个方向的角度信息就可以了，简单处理就是和里程计做一个卡尔曼滤波即可。

1、ros下的imu消息格式

        对于imu来说，我们可以通过index.ros.org网站，借助于关键词sensor_msgs，找到https://github.com/ros/common_msgs这个地址。进一步在sensor_msgs/msg/Imu.msg里面，就可以发现我们想找的信息。

Header header
 
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes
 
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes
 
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z 

        除了通常的header之外，ros中的imu消息包还有三种数据。orientation是方向信息，不过是用四元数来表示的，orientation_covariance表示方向的协方差。angular_velocity是角速度信息，angular_velocity_covariance是角速度的协方差。linear_acceleration是三个方向的线加速度，linear_acceleration_covariance是线加速度的协方差信息。一般来说，我们用的比较多的是orientation和angular_velocity。

2、编写测试代码imu_node.cpp

 
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"
 
void IMUCallback(sensor_msgs::Imu msg)
{
	if(msg.orientation_covariance[0] < 0)
	{
		return;
	}
 
	tf::Quaternion quarternion(
		msg.orientation.x,
		msg.orientation.y,
		msg.orientation.z,
		msg.orientation.w
	);
 
	double roll;
	double pitch;
	double yaw;
	tf::Matrix3x3(quarternion).getRPY(roll, pitch, yaw);
	
	roll = roll*180.0f/M_PI;
	pitch = pitch*180.0f/M_PI;
	yaw = yaw*180.0f/M_PI;
	ROS_INFO("roll = %0.3f, pitch=%0.3f, yaw=%0.3f", roll, pitch, yaw);
	
}
 
int main(int argc, char* argv[])
{
	setlocale(LC_ALL, "");
	ros::init(argc, argv, "imu_node");
	
	ros::NodeHandle n;
	ros::Subscriber imu_sub = n.subscribe("/imu/data", 10, IMUCallback);
	ros::spin();
 
	return 0;
}

        代码不复杂，主要是订阅/imu/data信息，回调函数是ImuCallback。在ImuCallback函数中，我们获取orientation的四元数信息。如果想要获取yaw信息，则要把quaternion转换成矩阵后，再用getRPY生成。这个时候yaw是弧度信息，如果要直观显示的话，需要进一步转成角度信息。

3、添加CMakeLists.txt内容

add_executable(imu_node src/imu_node.cpp)
target_link_libraries(imu_node ${catkin_LIBRARIES})
add_dependencies(imu_node beginner_tutorials_generate_messages_cpp)

4、编译生成imu_node

        编译imu_node的方法很简单，就是直接输入catkin_make即可。

5、开始测试

        测试之前，有几点需要注意。首先，利用roslaunch打开wpb_simple.launch，

roslaunch wpr_simulation wpb_simple.launch

        接着打开imu_node，当然在此之前最好输入./devel/setup.sh，        

rosrun beginner_tutorials imu_node

        这个时候因为机器人还没有旋转，所以imu打印的参数中，都是信息为0。因此，我们可以继续输入rqt命令，让小车转起来，

rosrun rqt_robot_steering rqt_robot_steering

        通过窗口输入旋转的命令，这个时候，就可以看到console的打印命令了，yaw数据也开始发生了变化。这说明我们的imu数据已经被采集到了。