一、MPU6050 引脚功能说明

在淘宝上买了一款MPU6050(如图)。它上面有8个引脚，这些引脚的作用如下：

引脚名称	功能描述	典型连接方式	注意事项
VCC	电源输入（3.3V）	接树莓派 3.3V (Pin 1)	严禁接5V，会烧毁芯片
GND	电源地	接树莓派 GND (Pin 6/9/14等)	确保共地
SCL	I²C时钟线（Serial Clock）	接树莓派 GPIO3 (SCL, Pin 5)	需4.7kΩ上拉电阻（开发板通常已集成）
SDA	I²C数据线（Serial Data）	接树莓派 GPIO2 (SDA, Pin 3)	与SCL同步上拉
XDA	辅助I²C数据线（连接外部磁力计如HMC5883L）	接从设备SDA	不使用时可悬空
XCL	辅助I²C时钟线（连接外部磁力计）	接从设备SCL	需外部上拉电阻
AD0	I²C地址选择脚： • 接地=0x68（默认） • 接VCC=0x69（多设备时用）	通常接地（Pin 9）	多MPU6050时需区分地址
INT	中断输出（可用于数据就绪/运动检测中断）	接GPIO输入（如GPIO4, Pin 7)	需在代码中配置中断触发条件

---

二、具体目标

在树莓派4B上通过MPU6050获取IMU数据并集成到ROS中

三、实现方法

需完成硬件连接、驱动配置、ROS节点开发三个步骤。以下是详细实现方案：

---

1. 硬件连接（I2C接口）

MPU6050引脚	树莓派4B引脚	颜色标识
VCC	3.3V (Pin 1)	红色
GND	GND (Pin 6)	黑色
SDA	GPIO2 (Pin 3)	蓝色
SCL	GPIO3 (Pin 5)	绿色

注意：

• 避免使用5V电源，防止MPU6050损坏

• 短接MPU6050的AD0引脚到GND（I2C地址为0x68）

---

2. 系统配置

启用I2C接口

sudo raspi-config
# 选择 Interfacing Options → I2C → Yes
sudo reboot

安装依赖库

sudo apt-get install i2c-tools libi2c-dev python3-smbus
pip3 install RPi.GPIO numpy

检测设备

sudo i2cdetect -y 1
# 应显示68地址的设备

---

3. 数据读取（Python驱动）

创建 mpu6050_driver.py：

#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import Imu
from smbus2 import SMBus
import math

class MPU6050:
    def __init__(self, bus=1, address=0x68):
        self.bus = SMBus(bus)
        self.address = address
        # 唤醒MPU6050
        self.bus.write_byte_data(self.address, 0x6B, 0x00)
        # 配置加速度计±8g
        self.bus.write_byte_data(self.address, 0x1C, 0x10)
        # 配置陀螺仪±1000°/s
        self.bus.write_byte_data(self.address, 0x1B, 0x10)

    def read_raw_data(self, addr):
        high = self.bus.read_byte_data(self.address, addr)
        low = self.bus.read_byte_data(self.address, addr+1)
        value = (high << 8) | low
        return value if value < 32768 else value - 65536

    def get_imu_data(self):
        # 加速度 (m/s²)
        accel_x = self.read_raw_data(0x3B) / 4096.0 * 9.8
        accel_y = self.read_raw_data(0x3D) / 4096.0 * 9.8
        accel_z = self.read_raw_data(0x3F) / 4096.0 * 9.8
        
        # 角速度 (rad/s)
        gyro_x = math.radians(self.read_raw_data(0x43) / 32.8)
        gyro_y = math.radians(self.read_raw_data(0x45) / 32.8)
        gyro_z = math.radians(self.read_raw_data(0x47) / 32.8)
        
        return accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z

def imu_publisher():
    rospy.init_node('mpu6050_node')
    pub = rospy.Publisher('/imu/data_raw', Imu, queue_size=10)
    imu = MPU6050()
    
    rate = rospy.Rate(50)  # 50Hz
    while not rospy.is_shutdown():
        msg = Imu()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = "imu_link"
        
        # 读取数据
        ax, ay, az, gx, gy, gz = imu.get_imu_data()
        
        # 填充消息
        msg.linear_acceleration.x = ax
        msg.linear_acceleration.y = ay
        msg.linear_acceleration.z = az
        msg.angular_velocity.x = gx
        msg.angular_velocity.y = gy
        msg.angular_velocity.z = gz
        
        # 发布数据
        pub.publish(msg)
        rate.sleep()

if __name__ == '__main__':
    try:
        imu_publisher()
    except rospy.ROSInterruptException:
        pass

---

4. ROS集成

创建ROS包

catkin_create_pkg mpu6050_driver rospy sensor_msgs

添加启动文件launch/mpu6050.launch：

<launch>
  <!-- 静态TF (根据实际安装位置调整) -->
  <node pkg="tf" type="static_transform_publisher" name="base_to_imu"
        args="0.1 0 0.15 0 0 0 base_link imu_link 100" />

  <!-- IMU驱动节点 -->
  <node pkg="mpu6050_driver" type="mpu6050_driver.py" name="mpu6050_node"
        output="screen" respawn="true">
    <param name="i2c_bus" value="1" />
    <param name="i2c_address" value="0x68" />
  </node>

  <!-- 数据滤波 (可选) -->
  <node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter">
    <param name="use_mag" value="false"/>
    <remap from="/imu/data_raw" to="/imu/data"/>
  </node>
</launch>

---

5. 校准与验证

零偏校准

# 在MPU6050类中添加校准方法
def calibrate(self, samples=500):
    offset = [0]*6
    for _ in range(samples):
        data = self.get_imu_data()
        for i in range(6):
            offset[i] += data[i]
    self.offset = [x/samples for x in offset]

可视化验证

rostopic echo /imu/data      # 查看原始数据
rosrun rviz rviz            # 添加Imu显示插件

---

---

四、常见问题解决

• I2C设备未发现

检查：sudo i2cdetect -y 1

解决：重新检查接线，确认上拉电阻（4.7kΩ）是否接好

• 数据漂移严重

校准：将机器人静止放置，运行校准程序

滤波：调整imu_filter_madgwick参数

<param name="gain" value="0.1"/>
<param name="zeta" value="0.01"/>

• 数据频率低

优化：减少Python开销，改用C++驱动（如RTIMULib）

• 数据噪声大？

解决方案：缩短导线长度，增加0.1μF去耦电容

• 读取数据全为零？

检查步骤：

确认sudo i2cdetect -y 1显示设备地址

验证是否执行了唤醒操作：

bus.write_byte_data(0x68, 0x6B, 0x00)  # PWR_MGMT_1寄存器

---

通过以上步骤，树莓派4B可稳定获取MPU6050数据并发布为标准ROS消息。实际部署时建议将IMU与里程计融合（如robot_localization包），提升定位精度。

五、进阶

5.1 扩展功能详解

• I²C通信（SCL+SDA）

协议：标准I²C协议，树莓派默认总线1（/dev/i2c-1）

地址： sudo i2cdetect -y 1   # 输出 68 或 69（由AD0引脚决定）

• 辅助I²C（XDA+XCL）

用途：级联其他I²C传感器（如磁力计）构成9轴IMU

配置：需在MPU6050寄存器中启用旁路模式（Python示例）：

bus.write_byte_data(0x68, 0x37, 0x02)  # INT_PIN_CFG寄存器

• 中断引脚（INT）

可通过配置以下寄存器实现：

0x38（INT_ENABLE）：启用数据就绪中断

0x37（INT_PIN_CFG）：设置中断触发方式

典型应用：

# Raspberry Pi GPIO中断示例
GPIO.setup(7, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(7, GPIO.FALLING, callback=isr_handler)

5.2 性能优化技巧

1. 硬件滤波

在MPU6050的ACCEL_CONFIG(0x1C)和GYRO_CONFIG(0x1B)寄存器中配置低通滤波

2. 提高I2C速率（默认100kHz → 400kHz）

sudo nano /boot/config.txt
# 添加：dtparam=i2c_arm=on,i2c_arm_baudrate=400000
sudo reboot

3. 使用DMP（Digital Motion Processor，需C++驱动）

// 启用MPU6050内置数字运动处理器
mpu.setDMPEnabled(true);

MPU6050内置DMP可硬件解算姿态（需配置寄存器）：

# 启用DMP（需加载官方固件）
bus.write_byte_data(0x68, 0x6B, 0x00)       # 解除睡眠
bus.write_byte_data(0x68, 0x6A, 0x00)       # 禁用主I2C
bus.write_byte_data(0x68, 0x37, 0x02)       # 启用旁路
bus.write_byte_data(0x68, 0x38, 0x01)       # 启用DMP中断

实际开发中建议优先使用成熟库（如RTIMULib或MPU6050.py）简化开发。