1.树莓派6DOF-IMU扩展板快速上手

0x00 扩展板简介

现在虽然市面上有很多IMU芯片，但是在使用的时候非常的麻烦。尤其是接线，大多数都需要使用USB转TTL模块来通过杜邦线接到主控机上。而且模块买到手后，还需要经过加速度、陀螺仪的标定才能使用。对于很多初学者来说，基本上就会一脸茫然、不知所措。所以，我们ROS小课堂就自己开发了，可以直插在树莓派上使用的IMU扩展板，大大的减少了接线的麻烦。而且想要在ROS中使用IMU信息，还需要经过编程。现在我们配套的ROS代码已经完成，所以只要拿到模块后，3分钟就能立刻与自己的ROS系统集成在一起使用了。

0x01 IMU扩展板性能参数

扩展板是6DOF的传感器，不能输出3轴地磁参数，因此可以输出的参数包括：

三轴加速度、三轴陀螺仪、三轴欧拉角、四元数

扩展板特色

1.集成高精度陀螺仪、加速度计，采用高性能的微处理器和先进的动力学解算与卡尔曼滤波算法，能够快速求解出模块当前的实时运动姿态。

2.采用先进的数字滤波技术，能有效降低测量噪声，提高测量精度。集成了姿态解算器，配合动态卡尔曼滤波算法，能够在动态环境下准确输出模块的当前姿态，姿态测量精度静态0.05度，动态0.1度，稳定性极高。

3.支持串口和IIC两种接口，现在代码已经实现了IIC接口与树莓派进行传感器数据传输，可以根据需要选择串口进行数据输出。

4.具有四路扩展接口，其中端口2具备GPS连接能力，可以与符合NMEA-0183标准的串口GPS数据配合形成GPS-IMU组合导航单元。其他三路接口可配置为模拟输入、数字输入、数字输出或PWM输出功能。

性能参数

1.工作电流：<25mA

2.树莓派CPU资源消耗：< 1%（功耗消耗超低）

3.数据回传速率：0.1~200Hz（默认出厂设置为10Hz，考虑到树莓派的处理速度，不建议设置速度太高）

4.串口波特率设置：2400~921600 bps（默认出厂设置为9600 bps）

5.工作温度范围：-40° ～ 85°

0x02 IMU扩展板配套源码

下面来分享给大家部分配套的IMU代码，当购买我们的扩展板后，即可下载完整的配套代码，而且提供技术支持，保证可以在ROS上运行起来，输出正确的传感器数据。下面首先来看下启动IMU的launch文件：

<!---
  Copyright:2016-2019 https://www.corvin.cn ROS小课堂
  Author: corvin
  Description:该launch启动文件是为了启动扩展板，使其进入正常工作状态。启动后，就会在
    /imu话题上发布imu传感器信息。需要该信息的节点，订阅该话题即可。同时，我们还可以使
    用dynamic_reconfigure来动态的修正z轴的角度。
  History:
    20191031:Initial this launch file.
-->
<launch>
  <arg name="imu_cfg_file" default="$(find rasp_imu_hat_6dof)/cfg/param.yaml"/>

  <node pkg="rasp_imu_hat_6dof" type="imu_node.py" name="imu_node" output="screen">
      <rosparam file="$(arg imu_cfg_file)" command="load"/>
  </node>
</launch>

获取传感器数据，然后组装成ROS中的IMU消息格式并发送到话题中的代码：

#!/usr/bin/env python
# -*- coding: UTF-8 -*-

# Copyright: 2016-2019 https://www.corvin.cn ROS小课堂
# Author: corvin
# Description: 为树莓派IMU扩展板所使用的配套代码，由于默认
#    扩展板与树莓派使用IIC连接。所以这里的代码是直接从IIC接口
#    中读取IMU模块的三轴加速度、角度、四元数，然后组装成ROS
#    中的IMU消息格式，发布到/imu话题中，这样有需要的节点可以
#    直接订阅该话题即可获取到imu扩展板当前的数据。
# History:
#    20191031: Initial this file.


import rospy
import string
import math
import time
import sys

from tf.transformations import quaternion_from_euler
from dynamic_reconfigure.server import Server
from rasp_imu_hat_6dof.cfg import imuConfig
from sensor_msgs.msg import Imu
from imu_data import MyIMU


degrees2rad = math.pi/180.0
imu_yaw_calibration = 0.0

# Callback for dynamic reconfigure requests
def reconfig_callback(config, level):
    global imu_yaw_calibration
    imu_yaw_calibration = config['yaw_calibration']
    rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration))
    return config

rospy.init_node("imu_node")

#Get DIY param
topic_name = rospy.get_param('~pub_topic', 'imu')
link_name  = rospy.get_param('~link_name', 'base_imu_link')
pub_hz     = rospy.get_param('~pub_hz', '10')


pub = rospy.Publisher(topic_name, Imu, queue_size=1)
srv = Server(imuConfig, reconfig_callback)  # define dynamic_reconfigure callback
imuMsg = Imu()

# Orientation covariance estimation:
# Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z
# Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause
# static roll/pitch error of 0.8%, owing to gravity orientation sensing
# error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025
# so set all covariances the same.
imuMsg.orientation_covariance = [
0.0025 , 0 , 0,
0, 0.0025, 0,
0, 0, 0.0025
]

# Angular velocity covariance estimation:
# Observed gyro noise: 4 counts => 0.28 degrees/sec
# nonlinearity spec: 0.2% of full scale => 8 degrees/sec = 0.14 rad/sec
# Choosing the larger (0.14) as std dev, variance = 0.14^2 ~= 0.02
imuMsg.angular_velocity_covariance = [
0.02, 0 , 0,
0 , 0.02, 0,
0 , 0 , 0.02
]

# linear acceleration covariance estimation:
# observed acceleration noise: 5 counts => 20milli-G's ~= 0.2m/s^2
# nonliniarity spec: 0.5% of full scale => 0.2m/s^2
# Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
imuMsg.linear_acceleration_covariance = [
0.04 , 0 , 0,
0 , 0.04, 0,
0 , 0 , 0.04
]

seq=0
# sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2.
accel_factor = 9.806/256.0


myIMU = MyIMU(0x50)
rate = rospy.Rate(pub_hz)

while not rospy.is_shutdown():
    myIMU.get_YPRAG()

    yaw_deg = float(myIMU.raw_yaw)
    yaw_deg = yaw_deg + imu_yaw_calibration
    if yaw_deg >= 180.0:
        yaw_deg -= 360.0

    #rospy.loginfo("yaw_deg: %f", yaw_deg)
    yaw = yaw_deg*degrees2rad
    pitch = float(myIMU.raw_pitch)*degrees2rad
    roll  = float(myIMU.raw_roll)*degrees2rad
    #rospy.loginfo("yaw:%f pitch:%f  rool:%f", yaw, pitch, roll)

    # Publish message
    imuMsg.linear_acceleration.x = float(myIMU.raw_ax)*accel_factor
    imuMsg.linear_acceleration.y = float(myIMU.raw_ay)*accel_factor
    imuMsg.linear_acceleration.z = float(myIMU.raw_az)*accel_factor

    imuMsg.angular_velocity.x = float(myIMU.raw_gx)*degrees2rad
    imuMsg.angular_velocity.y = float(myIMU.raw_gy)*degrees2rad
    imuMsg.angular_velocity.z = float(myIMU.raw_gz)*degrees2rad

    myIMU.get_quatern()
    imuMsg.orientation.x = myIMU.raw_q1
    imuMsg.orientation.y = myIMU.raw_q2
    imuMsg.orientation.z = myIMU.raw_q3
    imuMsg.orientation.w = myIMU.raw_q0

    #q = quaternion_from_euler(roll, pitch, yaw)
    #imuMsg.orientation.x = q[0]
    #imuMsg.orientation.y = q[1]
    #imuMsg.orientation.z = q[2]
    #imuMsg.orientation.w = q[3]

    imuMsg.header.stamp= rospy.Time.now()
    imuMsg.header.frame_id = link_name
    imuMsg.header.seq = seq
    pub.publish(imuMsg)

    seq = seq + 1
    rate.sleep()

0x03 IMU扩展板运行效果演示

下面我们就可以把IMU扩展板直接插在树莓派上查看运行效果了，如下视频所示：

0x04 参考资料

[1]. ROS下IMU消息格式定义. http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html

[2].razor_imu_9dof的ROS官网. http://wiki.ros.org/razor_imu_9dof

0x05 注意事项

[1].角度Z轴经过陀螺仪积分，会有累计误差。X、Y轴没有累计误差，误差控制在静态0.05°，动态0.1°。

[2].扩展板在发货时，已经把加速度计和陀螺仪标定好了，这样用户在拿到手上可以直接使用，不用再次花费时间标定。

0x06 问题反馈

大家在文章发现中有任何问题或者疑问，可以直接在文章末尾给我留言，或者关注ROS小课堂的官方微信公众号，在公众号中给我发消息反馈问题也行。我基本上每天都会处理公众号中的留言！当然如果你要是顺便给ROS小课堂打个赏，我也会感激不尽的，打赏30块还会被邀请进ROS小课堂的微信群，与更多志同道合的小伙伴一起学习和交流！

[wshop_reward]