运动姿态传感器MPU6050数据获取和数据融合

Posted on | In
Words count in article: 1,331

MPU6050是一款运动和姿态传感器芯片，可以获取物体当前的三个加速度分量和三个旋转角速度。由于其体积小巧，功能强大，并且价格低，在平衡车和航模制作中应用广泛。

基于Python的数据获取和互补滤波

MPU6050数据获取

MPU6050使用i2c通讯，其数据获取已有大佬使用Python完成，见[Github mpu6050]，我们需要使用的是这里的mpu6050.py文件。

MPU6050数据互补滤波

通过MPU6050得到姿态角有两个途径：

  1. 通过加速度计可以得到加速度方向，进而求解Pitch(俯仰)和Roll(滚转)角度
  2. 通过陀螺仪得到角速度，角速度对时间的积分得到姿态角
    但这两种方式如果单独使用效果并不好，直接从加速度计得到的角度值包含大量噪声，抖动明显；直接由角速度积分得到的角度值虽然平滑，但陀螺仪的偏差会导致积分误差不断增大。通过将加速度计和陀螺仪分别得到的角度进行融合，可以得到质量较好的角度数据。常用的融合方式有卡尔曼滤波、互补滤波等，卡尔曼滤波高深难懂，互补滤波更容易理解，且效果也不差。
    互补滤波代码：
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    import math

    class Complimentary_Filter:
    def __init__(self, order1_ratio=0.75, order2_ratio=0.2):
    self.order1_ratio = order1_ratio
    self.order2_ratio = order2_ratio
    self.order1_pitch = 0.0 # angle around X axis
    self.order1_roll = 0.0 # angle around Y axis
    self.order2_pitch = 0.0 # angle around X axis
    self.order2_roll = 0.0 # angle around Y axis
    self.y_pitch=0.0
    self.y_roll=0.0
    return

    def combine_order1(self, dt, accel_data, gyro_data):

    # Turning around the X axis results in a vector on the Y-axis
    pitch_accel = math.atan2(accel_data[1], accel_data[2]) * 180 / math.pi
    # Turning around the Y axis results in a vector on the X-axis
    roll_accel = math.atan2(accel_data[0], accel_data[2]) * 180 / math.pi

    self.order1_pitch = (self.order1_pitch- gyro_data[2] * dt) * self.order1_ratio + pitch_accel * (1.0 - self.order1_ratio)
    self.order1_roll = (self.order1_roll- gyro_data[1] * dt)* self.order1_ratio + roll_accel * (1.0 - self.order1_ratio)
    return [pitch_accel, roll_accel, self.order1_pitch, self.order1_roll]

    def combine_order2(self, dt, accel_data, gyro_data):

    # Turning around the X axis results in a vector on the Y-axis
    pitch_accel = math.atan2(accel_data[1], accel_data[2]) * 180 / math.pi
    # Turning around the Y axis results in a vector on the X-axis
    roll_accel = math.atan2(accel_data[0], accel_data[2]) * 180 / math.pi

    x1=(pitch_accel-self.order2_pitch)*(1.0-self.order2_ratio)*(1.0-self.order2_ratio)
    self.y_pitch = self.y_pitch + x1*dt
    x2 = self.y_pitch + 2 * (1.0-self.order2_ratio) * (pitch_accel - self.order2_pitch) + gyro_data[2]
    self.order2_pitch = self.order2_pitch + x2*dt

    x1=(roll_accel-self.order2_roll)*(1.0-self.order2_ratio)*(1.0-self.order2_ratio)
    self.y_roll = self.y_roll + x1*dt
    x2 = self.y_roll + 2 * (1.0-self.order2_ratio) * (roll_accel - self.order2_roll) + gyro_data[1]
    self.order2_roll = self.order2_roll + x2*dt

    return [pitch_accel, roll_accel, self.order2_pitch, self.order2_roll]

树莓派获取MPU6050数据，融合滤波后绘图显示

采用树莓派3 B+，与MPU6050 I2C接口连接
代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
from pyqtgraph.Qt import QtGui, QtCore
from collections import deque
from filter import Complimentary_Filter as Filter
import pyqtgraph as pg
import math
import datetime
from mpu6050 import mpu6050

filter = Filter()
delta_T = 50
pen_width = 2
rad2deg = 180.0/math.pi
framenum = 200
x_label = range(0,framenum)
for i in range(framenum):
x_label[i] *= delta_T
acce_pitch_deque = deque(maxlen=framenum)
acce_roll_deque = deque(maxlen=framenum)
order1_filter_pitch_deque = deque(maxlen=framenum)
order1_filter_roll_deque = deque(maxlen=framenum)
order2_filter_pitch_deque = deque(maxlen=framenum)
order2_filter_roll_deque = deque(maxlen=framenum)

sensor = mpu6050(0x68)

for i in xrange(framenum):
acce_pitch_deque.append(0.0)
acce_roll_deque.append(0.0)
order1_filter_pitch_deque.append(0.0)
order1_filter_roll_deque.append(0.0)
order2_filter_pitch_deque.append(0.0)
order2_filter_roll_deque.append(0.0)

win = pg.GraphicsWindow(title = "MPU6050 Plotter")
win.resize(1200,600)
win.setWindowTitle('MPU6050 Pitch and Roll Plotter')
pg.setConfigOptions(antialias = True)

def addplot(title_name,left_name,unit_name,bottom_name,Y_range_min,Y_range_max):
subplot= win.addPlot(title = title_name)
subplot.showGrid(x = True,y = True)
subplot.setLabel('left', left_name, units = unit_name)
subplot.setLabel('bottom', bottom_name)
subplot.setYRange(Y_range_min,Y_range_max)
return subplot

acce_pitch_plotter = addplot('Acce_Pitch_Plotter','Pitch','deg','time/ms',-180,180)
curve_acce_pitch = acce_pitch_plotter.plot(pen = pg.mkPen(color = (255,0,255),width = pen_width))
acce_roll_plotter = addplot('Acce_Roll_Plotter','Roll','deg','time/ms',-180,180)
curve_acce_roll = acce_roll_plotter.plot(pen = pg.mkPen(color = (0, 255, 127),width = pen_width))

win.nextRow()

order1_filter_pitch_plotter = addplot('1st-order-filter_Pitch_Plotter','Pitch','deg','time/ms',-180,180)
curve_order1_filter_pitch = order1_filter_pitch_plotter.plot(pen = pg.mkPen(color = (255,0,255),width = pen_width))
order1_filter_roll_plotter = addplot('1st-order-filter_Roll_Plotter','Roll','deg','time/ms',-180,180)
curve_order1_filter_roll = order1_filter_roll_plotter.plot(pen = pg.mkPen(color = (0, 255, 127),width = pen_width))

win.nextRow()

order2_filter_pitch_plotter = addplot('2nd-order-filter_Pitch_Plotter','Pitch','deg','time/ms',-180,180)
curve_order2_filter_pitch = order2_filter_pitch_plotter.plot(pen = pg.mkPen(color = (255,0,255),width = pen_width))
order2_filter_roll_plotter = addplot('2nd-order-filter_Roll_Plotter','Roll','deg','time/ms',-180,180)
curve_order2_filter_roll = order2_filter_roll_plotter.plot(pen = pg.mkPen(color = (0, 255, 127),width = pen_width))

def update():
time1 = datetime.datetime.now()
acce_data = [sensor.get_accel_data().get('x'),sensor.get_accel_data().get('y'),sensor.get_accel_data().get('z')]
gyro_data = [sensor.get_gyro_data().get('x'),sensor.get_gyro_data().get('y'),sensor.get_gyro_data().get('z')]

order1_filter_pitch_roll_data = filter.combine_order1(delta_T/1000.0, acce_data, gyro_data)
order2_filter_pitch_roll_data = filter.combine_order2(delta_T/1000.0, acce_data, gyro_data)

acce_pitch_deque.append(order1_filter_pitch_roll_data[0])
acce_roll_deque.append(order1_filter_pitch_roll_data[1])

order1_filter_pitch_deque.append(order1_filter_pitch_roll_data[2])
order1_filter_roll_deque.append(order1_filter_pitch_roll_data[3])

order2_filter_pitch_deque.append(order2_filter_pitch_roll_data[2])
order2_filter_roll_deque.append(order2_filter_pitch_roll_data[3])

curve_acce_pitch.setData(x_label, list(acce_pitch_deque))
curve_acce_roll.setData(x_label, list(acce_roll_deque))

curve_order1_filter_pitch.setData(x_label, list(order1_filter_pitch_deque))
curve_order1_filter_roll.setData(x_label, list(order1_filter_roll_deque))

curve_order2_filter_pitch.setData(x_label, list(order2_filter_pitch_deque))
curve_order2_filter_roll.setData(x_label, list(order2_filter_roll_deque))

delta_time=datetime.datetime.now()-time1
#print delta_time.microseconds/1000.0

timer = QtCore.QTimer()
timer.timeout.connect(update)
timer.start(delta_T) #take a sample every * ms

if __name__ == '__main__':
import sys
if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
QtGui.QApplication.instance().exec_()

绘图结果

第一、二、三行分别表示加速度计数据直接求解得到的Pitch、Yaw角度，一阶互补滤波的角度，二阶互补滤波的角度。

其它资料

Arduino教程：MPU6050的数据获取、分析与处理