想做用PYB做ROS机器人的主控，陀螺仪和加速度计怎么能少呢？
MPU6050模块
MPU6050是一种非常流行的空间运动传感器芯片，可以获取器件当前的三个加速度分量和三个旋转角速度。由于其体积小巧，功能强大，精度较高，不仅被广泛应用于工业，同时也是航模爱好者的神器，被安装在各类飞行器上驰骋蓝天。
MPU6050芯片内自带了一个数据处理子模块DMP，已经内置了滤波算法，在许多应用中使用DMP输出的数据已经能够很好的满足要求。在使用时只需要将偏移量减去即可使用，如果对精度有更高的要求可以使用上位机或这里使用卡尔曼滤波再次对数据进行优化。
大家在购买的时候，如果只需要陀螺仪和加速度计的话，就买和上图或者下图一样的六轴的GY-521六轴MPU6050模块即可，某宝上只需要4块钱左右即可买到还包邮，挺便宜的，一定不要买错型号，否则你在使用的时候一定会让你欲哭无泪。这里的库你可能不能直接用了。
硬件连接
接线和MPU数据的方向示意如图所示：
| PYB | MPU6050 |
|---|---|
| VCC | VCC |
| GND | GND |
| SCL(X9, IIC1) | SCL |
| SDL(X10, IIC1) | SDL |
代码
MPU6050库文件
mpu6050.py
import machine
class accel():
def __init__(self, i2c, addr=0x68):
self.iic = i2c
self.addr = addr
self.iic.start()
self.iic.writeto(self.addr, bytearray([107, 0]))
self.iic.stop()
def get_raw_values(self):
self.iic.start()
a = self.iic.readfrom_mem(self.addr, 0x3B, 14)
self.iic.stop()
return a
def get_ints(self):
b = self.get_raw_values()
c = []
for i in b:
c.append(i)
return c
def bytes_toint(self, firstbyte, secondbyte):
if not firstbyte & 0x80:
return firstbyte << 8 | secondbyte
return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)
def get_values(self):
raw_ints = self.get_raw_values()
vals = {
}
vals["AcX"] = self.bytes_toint(raw_ints[0], raw_ints[1])
vals["AcY"] = self.bytes_toint(raw_ints[2], raw_ints[3])
vals["AcZ"] = self.bytes_toint(raw_ints[4], raw_ints[5])
vals["Tmp"] = self.bytes_toint(raw_ints[6], raw_ints[7]) / 340.00 + 36.53
vals["GyX"] = self.bytes_toint(raw_ints[8], raw_ints[9])
vals["GyY"] = self.bytes_toint(raw_ints[10], raw_ints[11])
vals["GyZ"] = self.bytes_toint(raw_ints[12], raw_ints[13])
return vals # returned in range of Int16
# -32768 to 32767
def val_test(self): # ONLY FOR TESTING! Also, fast reading sometimes crashes IIC
from time import sleep
while 1:
print(self.get_values())
sleep(0.05)
主程序
from machine import I2C,Pin
i2c = I2C(scl=Pin("X9"), sda=Pin("X10"))
accel = mpu6050.accel(i2c)
accel_dict = accel.get_values()
print(accel_dict)
运行结果：
参数解析
六轴MPU6050输出所有参数：
- 加速度计的X轴分量：
ACC_X - 加速度计的Y轴分量：
ACC_Y - 加速度计的Z轴分量：
ACC_Z - 当前温度：
TEMP - 绕X轴旋转的角速度：
GYR_X - 绕Y轴旋转的角速度：
GYR_Y - 绕Z轴旋转的角速度：
GYR_Z
MPU6050芯片的座标系是这样定义的：令芯片表面朝向自己，将其表面文字转至正确角度，此时，以芯片内部中心为原点，水平向右的为X轴，竖直向上的为Y轴，指向自己的为Z轴。
三轴加速度计
加速度计的三轴分量ACC_X、ACC_Y和ACC_Z均为16位有符号整数，分别表示器件在三个轴向上的加速度，取负值时加速度沿座标轴负向，取正值时沿正向。
三个加速度分量均以重力加速度g的倍数为单位，能够表示的加速度范围，即倍率可以统一设定，有4个可选倍率：2g、4g、8g、16g。在这里不介绍如何进行倍率修改，如果想修改只需将不同倍率对应的标志位写入对应寄存器即可。倍率默认设定为2g，以ACC_X为例，在倍率为2g的时候，ACC_X的最小值为-32768，最大值为32768。当ACC_X为32768时，当前加速度为沿X轴正方向2倍的重力加速度。
加速度计算公式如图所示：
三轴陀螺仪
绕X、Y和Z三个座标轴旋转的角速度分量GYR_X、GYR_Y和GYR_Z均为16位有符号整数。从原点向旋转轴方向看去，取正值时为顺时针旋转，取负值时为逆时针旋转。
三个角速度分量均以“度/秒”为单位，能够表示的角速度范围，即倍率可统一设定，有4个可选倍率：250度/秒、500度/秒、1000度/秒、2000度/秒。以GYR_X为例，若倍率设定为250度/秒，则意味着GYR取正最大值32768时，当前角速度为顺时针250度/秒；若设定为500度/秒，取32768时表示当前角速度为顺时针500度/秒。显然，倍率越低精度越好，倍率越高表示的范围越大。倍率默认设定为250度/秒。具体修改方式此处不做讲解。
角速度的换算公式为：
数据处理与实现
MPU6050芯片提供的数据夹杂有较严重的噪音，在芯片处理静止状态时数据摆动都可能超过2%。除了噪音，各项数据还会有偏移的现象，也就是说数据并不是围绕静止工作点摆动，因此要先对数据偏移进行校准 ，再通过滤波算法消除噪音。
校准
校准是比较简单的工作，我们只需要找出摆动的数据围绕的中心点即可。我们以GRY_X为例，在芯片处理静止状态时，这个读数理论上讲应当为0，但它往往会存在偏移量，比如我们以10ms的间隔读取了10个值如下：
-158.4, -172.9, -134.2, -155.1, -131.2, -146.8, -173.1, -188.6, -142.7, -179.5
取这10个值的均值，也就是这个读数的偏移量为-158.25。在获取偏移量后，每次的读数都减去偏移量就可以得到校准后的读数了。当然这个偏移量只是估计值，比较准确的偏移量要对大量的数据进行统计才能获知，数据量越大越准，但统计的时间也就越慢。一般校准可以在每次启动系统时进行，那么你应当在准确度和启动时间之间做一个权衡。
三个角速度读数GYR_X、GYR_Y和GYR_Z均可通过统计求平均的方法来获得，但三个加速度分量就不能这样简单的完成了，因为芯片静止时的加速度并不为0。
加速度值的偏移来自两个方面，一是由于芯片的测量精度，导至它测得的加速度向量并不垂直于大地；二是芯片在整个系统（如无人机）上安装的精度是有限的，系统与芯片的座标系很难达到完美重合。前者我们称为读数偏移，后者我们称为角度偏移。因为读数和角度之间是非线性关系，所以要想以高精度进行校准必须先单独校准读数偏移，再把芯片固定在系统中后校准角度偏移。然而，由于校准角度偏移需要专业设备，且对于一般应用来说，两步校准带来的精度提升并不大，因此通常只进行读数校准即可。
读数校准方法：
- 为了尽量避免读数偏移带来的影响，首先将MPU6050牢牢地固定在系统中，并使二者座标系尽可能的重合。
此时，我们认为芯片的ACC_X和ACC_Y的理论值应为0，ACC_Z的理论值应为-16384（默认2g的倍率）。
由于ACC_X和ACC_Y的理论值应为0，与角速度量的校准类似，这两个读数偏移量可用统计均值的方式校准。ACC_Z则需要多一步处理，即在统计偏移量的过程中，每次读数都要加上16384，再进行统计均值校准。
卡尔曼滤波
对于夹杂了大量噪音的数据，卡尔曼滤波器的效果无疑是最好的。如果不想考虑算法细节，可以直接使用Arduino的Klaman Filter库完成。在我们的模型中，一个卡尔曼滤波器接受一个轴上的角度值、角速度值以及时间增量，估计出一个消除噪音的角度值。跟据当前的角度值和上一轮估计的角度值，以及这两轮估计的间隔时间，我们还可以反推出消除噪音的角速度。
卡尔曼滤波建议在上位机实现，Micropython本身对矩阵的运算并没有特别好的支持，在上位机可以借助numpy实现快速卡尔曼滤波运算。
参考文章：