If I want to use PYB as the master control of ROS robot, how can I reduce the gyroscope and accelerometer?
Article Directory
MPU6050 module
MPU6050 is a very popular space motion sensor chip, which can obtain the current three acceleration components and three rotational angular velocities of the device. Because of its small size, powerful functions and high precision, it is not only widely used in industry, but also an artifact for model aircraft enthusiasts. It is installed on various types of aircraft to fly in the blue sky.
The MPU6050 chip comes with a data processing sub-module DMP, which has built-in filtering algorithms. In many applications, the data output by DMP can meet the requirements well. When in use, you only need to subtract the offset to use. If you have higher requirements for accuracy, you can use the upper computer or use Kalman filter to optimize the data again.
When you buy, if you only need the gyroscope and accelerometer, you can buy the six-axis GY-521 six-axis MPU6050 module as shown in the picture above or below. It only costs about 4 yuan on a certain treasure. It's very cheap, and you must not buy the wrong model, otherwise it will make you cry without tears when you use it. You may not be able to use the library directly here.
Hardware connection
The direction of wiring and MPU data is shown in the figure:
| PYB | MPU6050 |
|---|---|
| VCC | VCC |
| GND | GND |
| SCL(X9, IIC1) | SCL |
| SDL(X10, IIC1) | SDL |
Code
MPU6050 library file
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)
Main program
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)
operation result:
Parameter analysis
Six-axis MPU6050 outputs all parameters:
- X-axis component of accelerometer:
ACC_X - Y-axis component of accelerometer:
ACC_Y - The Z-axis component of the accelerometer:
ACC_Z - Current Temperature:
TEMP - Angular velocity of rotation around the X axis:
GYR_X - Angular velocity of rotation around Y axis:
GYR_Y - Angular velocity of rotation around the Z axis:
GYR_Z
The coordinate system of the MPU6050 chip is defined as follows: make the surface of the chip face you, and turn the surface text to the correct angle. At this time, the center of the chip is the origin, the horizontal right is the X axis, and the vertical upward is Y The axis that points to itself is the Z axis.
Three-axis accelerometer
The three-axis components ACC_X, ACC_Y and ACC_Z of the accelerometer are all 16-bit signed integers, which respectively represent the acceleration of the device in the three axes. When the value is negative, the acceleration is along the negative direction of the coordinate axis, and when the value is positive, it is along the positive direction. .
The three acceleration components are in units of multiples of the gravitational acceleration g, and the acceleration range that can be expressed, that is, the magnification can be set uniformly, and there are 4 optional magnifications: 2g, 4g, 8g, 16g. How to modify the magnification is not introduced here. If you want to modify, just write the flag bits corresponding to different magnifications into the corresponding registers. The default setting of the magnification is 2g . For ACC_Xexample, when the magnification is used 2g, ACC_Xthe minimum value is -32768 and the maximum value is 32768. When it ACC_Xis 32768, the current acceleration is 2 times the acceleration of gravity along the positive direction of the X axis.
The acceleration calculation formula is shown in the figure:
Three-axis gyroscope
Angular velocity component around the X, Y and Z three coordinate axes of rotation GYR_X, GYR_Yand GYR_Zare 16-bit signed integer. Viewed from the origin to the direction of the rotation axis, a positive value means clockwise rotation, and a negative value means counterclockwise rotation.
The three angular velocity components are all in "degree/second", the range of angular velocity that can be expressed, that is, the magnification can be set uniformly, and there are 4 optional magnifications: 250 degrees/second, 500 degrees/second, 1000 degrees/second, 2000 Degrees/second. For GYR_Xexample, if the magnification is set to 250 degrees/second, it means that when GYR takes the positive maximum value of 32768, the current angular velocity is 250 degrees/second clockwise; if it is set to 500 degrees/second, when it is set to 32768, it means the current angular velocity It is 500 degrees/sec clockwise. Obviously, the lower the magnification, the better the accuracy, and the higher the magnification, the larger the range. The default setting of the magnification is 250 degrees/sec . The specific modification method is not explained here.
The conversion formula of angular velocity is:
Data processing and realization
The data provided by the MPU6050 chip is mixed with serious noise, and the data swing may exceed 2% when the chip is processing the static state. In addition to noise, the data will also have an offset phenomenon, that is to say, the data does not swing around the stationary operating point, so the data offset must be calibrated first, and then the noise must be eliminated through the filtering algorithm.
calibration
Calibration is a relatively simple task, we only need to find the center point around the swing data. Let's take GRY_X as an example. When the chip is in a static state, the reading should theoretically be 0, but it often has an offset. For example, we read 10 values at 10ms intervals as follows:
-158.4, -172.9, -134.2, -155.1, -131.2, -146.8, -173.1, -188.6, -142.7, -179.5
Take the average of these 10 values, that is, the offset of this reading is -158.25. After obtaining the offset, subtract the offset from each reading to get the calibrated reading. Of course, this offset is only an estimated value. A more accurate offset can only be obtained by performing statistics on a large amount of data. The larger the amount of data, the more accurate, but the slower the statistical time. Generally, calibration can be performed every time the system is started, so you should make a trade-off between accuracy and start-up time.
The three angular velocity readings GYR_X, GYR_Y and GYR_Z can all be obtained through statistical averaging, but the three acceleration components cannot be done so simply because the acceleration when the chip is stationary is not zero.
The offset of the acceleration value comes from two aspects. One is that the acceleration vector measured by the chip is not perpendicular to the ground due to the measurement accuracy of the chip; the other is that the accuracy of the chip in the entire system (such as a drone) is limited. Yes, the coordinate system of the system and the chip is difficult to achieve perfect coincidence. The former is called reading offset, and the latter is called angular offset. Because there is a non-linear relationship between reading and angle, if you want to calibrate with high accuracy, you must first calibrate the reading offset separately, and then fix the chip in the system and calibrate the angle offset. However, because the calibration of the angle offset requires professional equipment, and for general applications, the accuracy improvement brought by the two-step calibration is not large, so usually only the reading calibration is required.
Reading calibration method:
- In order to avoid the influence of the reading offset as much as possible, first fix the MPU6050 firmly in the system, and make the two coordinate systems coincide as much as possible.
At this time, we believe that the theoretical value of theACC_XsumACC_Yof the chips should be 0, andACC_Zthe theoretical value should be -16384 (default 2g magnification).
BecauseACC_XandACC_Ythe theoretical value should be 0, with an angular velocity similar to the calibration, two calibration offsets the reading means available statistical mean.ACC_ZOne more step is needed, that is, in the process of counting the offset, 16384 must be added to each reading, and then the statistical mean calibration is performed.
Kalman filter
For data with a lot of noise, the effect of Kalman filter is undoubtedly the best. If you don't want to consider the details of the algorithm, you can directly use the Klaman Filter library of Arduino. In our model, a Kalman filter accepts an axis angle value, angular velocity value and time increment, and estimates an angle value that eliminates noise. Based on the current angle value and the angle value estimated in the previous round, as well as the interval time between the two rounds of estimation, we can also infer the angular velocity to eliminate noise.
Kalman filtering is recommended to be implemented on the host computer. Micropython itself does not have particularly good support for matrix operations. On the host computer, you can use numpy to implement fast Kalman filter operations.
Reference article: