智能机器人（35)：惯导驱动-mpu

1. 硬件
2. 软件
3. 软件 – ROS
3.1 arduino uno 部分
3.2 pc ros 部分
3.3 usage

MPU 6050 is a sensor based on MEMS technology with I2C protocol communication.
The accelerometer works on the principle of the piezoelectric effect.
The gyroscopes work on the principle of Coriolis acceleration.
REF： http://blog.csdn.net/eaibot/article/details/51398074/

1. 硬件

Arduino-UNO + MPU-6050

XCL，XDA:这2个pin是MPU6050做I2C的master时用来控制slave的。不接。
SDA，SCL：这2个pin是I2C接口。 接UNO的pin4(SDA)和pin5(SCL)。
AD0: 用来选定I2C地址的，接地时地址是0x68，接3.3V时是0x69. 接地。
INT：打断信号，MPU6050内建1024byte的FIFO buffer，当buffer有数据时可以interrupt通知arduino。接UNO的pin-2 (interrupt pin 0)
共享代码中都设置400KHz的I2C bus speed，100K应该也可。

2. 软件

下载：I2Cdev library： http://diyhacking.com/projects/I2Cdev.zip
下载：MPU 6050： http://diyhacking.com/projects/MPU6050.zip
两个库解压，复制到Arduino的libraries目录
File –> Examples –> MPU6050 –> Examples –> MPU6050_DMP6
编译，上传，打开串口监控设置波特率115200：
Send any character to begin DMP programming and demo:
ypr 28.37 -30.82 10.95
ypr 28.46 -30.82 10.95
ypr 28.56 -30.82 10.96
ypr 28.66 -30.82 10.96
…

注：
最初数据不准确。
注：
这里不是raw data，是DMP方式。
DMP即Digital Motion Processing，a built-in motion processor. It processes the values from the accelerometer and gyroscope to give accurate 3D values.

3. 软件 – ROS

REF： https://github.com/fsteinhardt/mpu6050_serial_to_imu/blob/master/README.md

3.1 arduino uno 部分

复制前面的ino代码，创建新项目。
修改部分：

*1 comment define OUTPUT_READABLE_YAWPITCHROLL：
#define OUTPUT_READABLE_YAWPITCHROLL ->
//#define OUTPUT_READABLE_YAWPITCHROLL
*2 uncomment OUTPUT_TEAPOT:
//#define OUTPUT_TEAPOT ->
#define OUTPUT_TEAPOT
*3 change code:
//uint8_t teapotPacket[14] = { ‘$’, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, ‘r’, ‘n’ };
———–>>>>>>>>>>>>>>>
uint8_t teapotPacket[28] = { ‘$’, 0x03, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, ‘r’, ‘n’ };
*4 change code:
teapotPacket[2] = fifoBuffer[0];
teapotPacket[3] = fifoBuffer[1];
teapotPacket[4] = fifoBuffer[4];
teapotPacket[5] = fifoBuffer[5];
teapotPacket[6] = fifoBuffer[8];
teapotPacket[7] = fifoBuffer[9];
teapotPacket[8] = fifoBuffer[12];
teapotPacket[9] = fifoBuffer[13];
Serial.write(teapotPacket, 14);
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
————>>>>>>>>>>>>>>>>>>>>>>>>
teapotPacket[2] = fifoBuffer[0];
teapotPacket[3] = fifoBuffer[1];
teapotPacket[4] = fifoBuffer[4];
teapotPacket[5] = fifoBuffer[5];
teapotPacket[6] = fifoBuffer[8];
teapotPacket[7] = fifoBuffer[9];
teapotPacket[8] = fifoBuffer[12];
teapotPacket[9] = fifoBuffer[13];
// gyro values
teapotPacket[10] = fifoBuffer[16];
teapotPacket[11] = fifoBuffer[17];
teapotPacket[12] = fifoBuffer[20];
teapotPacket[13] = fifoBuffer[21];
teapotPacket[14] = fifoBuffer[24];
teapotPacket[15] = fifoBuffer[25];
// accelerometer values
teapotPacket[16] = fifoBuffer[28];
teapotPacket[17] = fifoBuffer[29];
teapotPacket[18] = fifoBuffer[32];
teapotPacket[19] = fifoBuffer[33];
teapotPacket[20] = fifoBuffer[36];
teapotPacket[21] = fifoBuffer[37];
//temperature
int16_t temperature = mpu.getTemperature();
teapotPacket[22] = temperature >> 8;
teapotPacket[23] = temperature & 0xFF;
Serial.write(teapotPacket, 28);
teapotPacket[25]++; // packetCount, loops at 0xFF on purpose
编译，上传到arduino uno。

或者，
直接下载代码： https://github.com/fsteinhardt/mpu6050_serial_to_imu

解压， 对MPU6050.ino项目用Arduino IDE打开。
不过arduino带的wire版本和这里代码不兼容， wire的好几个函数改名了。
error: ‘class TwoWire’ has no member named ‘setClock’
需要修改两处：
// Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
和
// enable Arduino interrupt detection
Serial.println(F(“Enabling interrupt detection (Arduino external interrupt 0)…”));
//attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
attachInterrupt(0, dmpDataReady, RISING);
后面这个版本时间长了串口不稳定。

3.2 pc ros 部分

复制解压后的剩余代码到~/atkin_ws/src，
或者：
$ cd ~/catkin_ws/src/
$ git clone https://github.com/fsteinhardt/mpu6050_serial_to_imu
$ cd ~/catkin_ws/
$ catkin_make

如果需要：
$ sudo apt-get install ros-indigo-serial

Serial is a cross-platform, simple to use library for using serial ports on computers. This library provides a C++, object oriented interface for interacting with RS-232 like devices on Linux and Windows.
Not like ros-indigo-rosserial.

$ source ~/.bashrc
$ roslaunch mpu6050_serial_to_imu demo.launch
$ rostopic echo /imu_data

可以看到mpu6050的值转换为ROS的/imu 的消息：
header:
seq: 36872
stamp:
secs: 1508381861
nsecs: 177936189
frame_id: imu_link
orientation:
x: -0.0668025501072
y: -0.0957503132522
z: -0.175129953772
w: 0.977523017675
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: 0.0
y: -0.0266316109008
z: -0.0266316109008
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: 0.185614013672
y: 2.42735229492
z: 11.7942736816
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Published Topics
* imu/data ([sensor_msgs::Imu])
The measured accelerometer, gyro and orientation values.
* imu/temperature ([sensor_msgs::Temperature])
The measured temperature in degrees Celsius.

Services:
* set_zero_orientation ([std_srvs/Empty])
This service sets the current orientation as the new zero orientation
so that from now on only the difference to this orientation is sent.

TF Published:
The resulting orientation can be published as a tf transform,
the frame names can be set using parameters.

Parameters:
* broadcast_tf (bool, default: true)
If true: publish the orientation of the IMU as a tf transform.
* tf_parent_frame_id (string, default: “imu_base”)
Sets the name of the parent frame in the tf transform.
… WE CHANGE TO base_link …
* tf_frame_id (string, default: “imu_link”)
Sets the name of the own frame in the tf transform.
* imu_frame_id (string, default: “imu_link”)
Sets the name of the frame for imu messages.

3.3 usage

Needed a tf:

Sample Launch:

4. 校准和标定

Put MPU6050, as horizontal as possible
– Write down offsets for that particular MPU6050 and use them with library’s functions “…setoffset…” in projects.

四元数到角度 ： Node发布的是四元数、角速度、线加速度， 如果直观看左转右转的角度：

#! /usr/bin/python

import PyKDL
import rospy
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from math import *
import threading
import os
import subprocess
import yaml

def quat_to_angle(quat):
rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
return rot.GetRPY()[2]
def normalize_angle(angle):
res = angle
while res > pi:
res -= 2.0*pi
while res < -pi: res += 2.0*pi return res class CalibrateRobot: def __init__(self): self.lock = threading.Lock() self.sub_imu = rospy.Subscriber('/imu/data', Imu, self.imu_cb) self.last_imu_angle = 0 self.imu_angle = 0 while not rospy.is_shutdown(): rospy.sleep(0.1) rospy.loginfo("imu_angle:"+str(self.imu_angle*180/3.1415926)) def imu_cb(self, msg): with self.lock: angle = quat_to_angle(msg.orientation) self.imu_angle = angle self.imu_time = msg.header.stamp def main(): rospy.init_node('scan_to_angle') CalibrateRobot() if __name__ == '__main__': main() --- [INFO] [WallTime: 1508384569.829275] imu_angle:-12.3451541995 [INFO] [WallTime: 1508384569.930287] imu_angle:-12.3600526937 [INFO] [WallTime: 1508384570.031151] imu_angle:-12.3667251945 [INFO] [WallTime: 1508384570.132128] imu_angle:-12.3829488555 可见即使位置不变也是存在漂移的，这个偏移可以加回去。