ROS【IMU】姿态检测与解算

前言

                                                            

                                                                               图 1 IMU 与姿态检测

惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。在导航中有着很重要的应用价值。

pitch、yaw、roll三个角的区别

pitch是围绕Y轴旋转，也叫做俯仰角。

yaw是围绕Z轴旋转，也叫偏航角。

roll是围绕X轴旋转，也叫翻滚角。

---

消息类型定义

类型：sensor_msgs/Imu.msg

# This is a message to hold data from an IMU (Inertial Measurement Unit)
#
# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
#
# If the covariance of the measurement is known, it should be filled in (if all you know is the 
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
# data a covariance will have to be assumed or gotten from some other source
#
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 
# estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each 
# covariance matrix, and disregard the associated estimate.

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 

加载方式：

from sensor_msgs.msg import Imu

 

解释

文档描述了Imu的消息结构，其中姿态（orientation）类型为四元数（geometry_msgs/Quaternion）；角速度（angular_velocity）和线加速度（linear_acceleration）的类型为三维向量（geometry_msgs/Vector3）。

四元数（Quaternion）转欧拉角（Euler-Angles）

四元数是一种姿态的表达方式，与欧拉角相比，它规避了“万向节锁”的问题。《四元素、欧拉角及方向余弦矩阵的相互转化公式》讲解了四元数与欧拉角的相互转换。这个视频形象地介绍了万向节锁的问题。

转换程序：

#!/usr/bin/env python
# license removed for brevity

import rospy
from sensor_msgs.msg import Imu
import math

def imu_cb(imu_data):
    # Read the quaternion of the robot IMU
    x = imu_data.orientation.x
    y = imu_data.orientation.y
    z = imu_data.orientation.z
    w = imu_data.orientation.w

    # Read the angular velocity of the robot IMU
    w_x = imu_data.angular_velocity.x
    w_y = imu_data.angular_velocity.y
    w_z = imu_data.angular_velocity.z

    # Read the linear acceleration of the robot IMU
    a_x = imu_data.linear_acceleration.x
    a_y = imu_data.linear_acceleration.y
    a_z = imu_data.linear_acceleration.z

    # Convert Quaternions to Euler-Angles
    rpy_angle = [0, 0, 0]
    rpy_angle[0] = math.atan2(2 * (w * x + y * z), 1 - 2 * (x**2 + y**2))
    rpy_angle[1] = math.asin(2 * (w * y - z * x))
    rpy_angle[2] = math.atan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))
    return

if __name__ == '__main__':
    rospy.init_node('imu_node', anonymous=True)
    rospy.Subscriber("/pigot/imu", Imu, imu_cb)
    rospy.spin()

值得注意的是：

在很多场合我们使用实际的IMU获得sensor_msgs/Imu.msg中的orientation明明是四元数（Quaternion），却直接赋值的是欧拉角，即imu_data.orientation.x、imu_data.orientation.y、imu_data.orientation.z直接为角度值，找了一些资料，发现好像都可以，也就是说ROS内部会自行转换。

资料

• 小明工坊：【ROS-Gazebo】IMU插件使用与数据采集——以四足机器人pigot为例
• Imu 格式消息官方文档