加速度计和陀螺仪数据融合算法

发表于 | 分类于 | | 阅读次数

在这篇文章里我用Python实现了一个简单的互补滤波器来做角度融合，实现姿态解算。用到的传感器是MPU-6050，使用树莓派的I2C总线读取传感器的底层数据。

  • 这篇文章的大部分内容来源于我这学期的期末作业，题目是平衡车角度融合算法探究。当时的方案是：单片机+模拟加速度陀螺仪传感器，C语言单片机编程。考虑到当时没有上位机所以无法观察滤波的效果，所以我尝试用smbus使用I2C总线读取数据，用matplotlib绘图检验滤波器的性能。

系统概述

  • Raspberry 3 Model B ／ Ubuntu 16.04（ARM）
  • MPU-6050
系统图

系统图

MPU-6050和树莓派之间通过I2C总线相连，并由PI供电(5V)。

MPU-6050

MPU-6050

角度融合算法

加速度计

加速度计测量的是加速度，为什么可以测量角度呢？

加速度计主要测量的是测量设备的受力情况，也就是三轴运动情况，设计原理适合于空间运动判断。我们把加速度计想象成一个正方体盒子，里面放着一颗直径和盒子边长相等的球体。假设我们把盒子放在水平桌面上，那么仅有垂直方向会受到球的挤压，这时传感器对应垂直方向的接口就会输出相对应的电压。也就是说，加速度传感器是一种力传感器。
既然加速度计可以感应轴上的压力大小，那么牛顿早在400年前提出得力学定律就可以派上用场了。我们现在要计算倾斜角。三轴可以确定一个空间位置，计算任意一个平面与夹角并不复杂，这里我只算一个轴的角度，所以我们只需要知道加速度计上两个方向的分力即可。

陀螺仪

陀螺仪的作用是测角速度，我们很自然的联想到，若我这个角速度进行积分运算，就可以得到我们需要的角度了。

数据处理

有资料提到陀螺仪的数据需要除以131，原因是芯片资料上有这个参数:

Parameter Conditions TYP Unit
Sensitivity Scale Factor FS_SEL=0 131 LSB
FS_SEL=1 65.5 LSB
FS_SEL=2 32.8 LSB
FS_SEL=3 16.4 LSB

这里参考的文章是MPU6050数据采集及其意义和滤波，其主要意义在于选择合适的量程。

为何要使用角度融合算法

你可能会问加速度计不是可以计算角度嘛，那为什么还要多此一举使用陀螺仪呢？要解决这个问题，首先要对这两种传感器有更深刻的理解。
被测物体在运动过程中难免会发生一些抖动，还是联想上文提到的盒子的模型，试想，若你的盒子在不停的振动，六个面的压力会如何变化？也就是说，在我们使用加速度传感器求角度的时候结果中包含了一些噪声，且加速度计动态响应慢。再看陀螺仪，这种传感器是用来测量角速度的，我们对它积分可以求出角度。然而由于自身的硬件特性，陀螺仪存在温度漂移，陀螺仪计算出的角度误差会随时间增大，我们同样不希望出现这种结果。

简单实用的互补滤波

设计实现

简单却实用的互补滤波

  • 互补滤波的理论基础

首先我们先来复习一下两个概念：

  1. 低通滤波器
  2. 高通滤波器
  3. 带通滤波器

低通滤波器（英语：Low-pass filter）容许低频信号通过，但减弱（或减少）频率高于截止频率的信号的通过。’低’和’高’频率的含义，是相对于滤波器设计者所选择的截止频率而言的。 高通滤波器则相反，而带通滤波器则是高通滤波器和低通滤波器的组合。

互补滤波器其实就是一种带通滤波器的变种。

  • 积分器
1
angle += gyro*dt;

其中，angle为角度，gyro为陀螺仪角速度，dt为计算周期。gyro*dt 得到计算周期时间段内通过的角度，通过对该角度的积分（不断累加），得到系统运行以来的角度。

  • 低通滤波器

低通滤波器的目标是过滤掉短期波动，让长期变化得以保留。

1
angle = (0.98)*angle_last + (0.02)*x_acc;

其中，angle为当前角度，angle_last为前一次角度，x_acc为当前加速度计换算后的角度值。对x_acc进行低通滤波，让加速度计的长期变化得以保留，angle能追踪加速度计的长期变化。

代码实现

首先可以用

1
i2cdetect -y 1 #0

来判断芯片的地址，MPU-6050的芯片地址是0x68 ，用Python实现I2C 读写需要使用smbus-cffi，按照官网的教程安装即可。加速度计和陀螺仪的寄存器地址可以在MPU-6050 Register Map and Descriptions - InvenSense查到。

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
import smbus
import time
import math
import matplotlib.pyplot as plt
bus = smbus.SMBus(1)
address = 0x68
ACCEL_XOUT_H_Addr = 0x3B
ACCEL_XOUT_L_Addr = 0x3C
ACCEL_ZOUT_H_Addr = 0x3F
ACCEL_ZOUT_L_Addr = 0x40
GYRO_YOUT_H_Addr = 0x45
GYRO_YOUT_L_Addr = 0x46
accel = 0
angle = 0
count = 0
PI = 3.1415926
Ydata_1 = []
Ydata_2 = []
# 传感器数据处理
def sensor_data(addr_L,addr_H):
        data_L = bus.read_byte_data(address,addr_L)
        data_H = bus.read_byte_data(address,addr_H)
        if data_H > 128:
                return (data_H - 256) * 255 + data_L
        else:
                return data_H * 255 + data_L
while True:
        try:
                accel = atan2(sensor_data(ACCEL_XOUT_L_Addr,ACCEL_XOUT_H_Addr),sensor_data(ACCEL_ZOUT_L_Addr,ACCEL_ZOUT_H_Addr))*180/PI
                angle = 0.95*(angle + 0.1 * sensor_data(GYRO_YOUT_L_Addr,GYRO_YOUT_H_Addr)/131)+0.05*accel
                count = count + 1
                time.sleep(0.1) #采样周期为0.1s
        except IOError: #抛IOError异常不去管它继续执行
                continue 
        else:
                print angle,gyro
                Ydata_1.append(accel)
                Ydata_2.append(angle)
                if count == 100: #采样100次
                        break  
plt.plot(Ydata_1)
plt.plot(Ydata_2) 
plt.show()

程序运行的结果如下：
角度曲线绿色的曲线是加速度计得到的角度，而蓝色的线是经过互补滤波后得到的角度。从这个曲线上可以看出来互补滤波器的效果还是比较好的。

小结

我在最开始的时候并不明白所谓的卡尔曼滤波为何物，不过把它当作一个黑盒用也没有出现什么奇怪的问题。直到今天我仍然不懂卡尔曼滤波的原理，不过既然我们会互补滤波，那么为什么不用呢？仅仅需要一行代码，效果和卡尔曼滤波几乎完全相同，计算量要比卡尔曼滤波小的多。不过以后的专业课肯定会讲到各种滤波器，卡尔曼滤波这个坑估计迟早得跳吧(｡ì _ í｡)