Lab 3: Calibrating and Combining the Accelerometer and Gyroscope data In this lab we will learn how to read the current angle of your robot using a combination of a gyroscope and an accelerometer. The process outlined below is for the 6 DOF Arduino shield which contains the ADXL345 accelerometer and the ITG-­‐3200 gyroscope, but it would be the same for any accelerometer and gyroscope. An accelerometer is used to measure an object's acceleration relative to freefall so an object in freefall would experience zero g, where as an object sitting on the floor would experience 1g of acceleration in the upwards direction. This acceleration (or force) is required to counteract the acceleration of gravity acting downwards on the object, keeping it from falling. If the accelerations in various axes are known, trigonometry can be used to determine the angle of the object. A gyroscope is used to measure the angular velocity of an object (degrees/second). This value can be integrated to give degrees (the amount an object has rotated or the current angle). In order to control your system accurately, both the accelerometer and the gyroscope reading's are required. Once you have both of the these reading's there are various signal processing methods you can use to obtain the accurate angle. The two most accurate systems are the Kalman filter and a complementary filter. In this lab we will use a complementary filter. The steps for obtaining an accurate angle measurement from the raw gyroscope and accelerometer data are as follows: • Set up the device correctly and download the required libraries to read raw data • Calibrate the Accelerometer and Gyroscope so you know what they are outputting (what units, what sort of offset etc) • Convert the accelerometer reading into degrees (methods discussed below) • Convert the gyroscope reading into degrees (by integration) • Low pass filter the accelerometer angle • High pass filter the gyroscope angle • Combine the two reading's using the complementary filter technique Accelerometer and Gyroscope Set-­‐‑up To set up the accelerometer and gyroscope, sample Arduino code for the Six Dof Shield was downloaded from the Internet. The code is specific for the type of device and is written for an Arduino. Almost every model will have some sort of example code that contains libraries you can use. The libraries below create the FreeSixIMU class which contains the FreeSixIMU object and various functions that can be performed on it. The only function from this class that was used was getRawValues, which reads the raw (uncalibrated) values from the accelerometer and gyroscope. We will then filter and calibrate the data ourselves. The libraries for the 6dof shield can be found at: http://www.dfrobot.com/wiki/index.php/6_Dof_Shield_(DFR0209) The libraries need to be downloaded from the product page. This link is on the Wikipedia page under example code. They must then be added to the Arduino libraries folder so the function can access them (see lab 1 if you forgot how to do that). You will need the FreeSixIMU, FIMU_ADXL345 and the FIMU_ITG3200 .h and .cpp files. Once the libraries are in place, you can reference them from any function or class by using the #include argument. See example. #include <FreeSixIMU.h> #include <FIMU_ADXL345.h> #include <FIMU_ITG3200.h> After you have set up the libraries and can read raw data from the board, you are ready to start calibrating and filtering the data. The next sections discuss how to calibrate the accelerometer and gyroscope data so they return the robot angle in degrees instead of a value in LSBs (least significant bits). Accelerometer When setting up the accelerometer make sure to check the range and sensitivity. On the ADXL345 the default range is -­‐2000 to 2000 deg/s. This cannot be modified, but if you can change the range on your accelerometer you might want to decrease it so it is more sensitive to smaller changes as you will likely not see rotation values as large as 2000 deg/s. In order to obtain the angle from the accelerometer data, there are a few methods you can use. We will go over three methods in this lab. These methods are discussed below. Small Angle Approximation The small angle approximation method uses trig to calculate the angle, but assumes that sinθ ≈ θ. This is an accurate estimate for angles less than 30°. Given a normal motor controller, if the tilt angle is greater than 30 degrees, the controller will likely need to be running at full speed to correct this tilt and precision angle measurement is not required for this. Therefore using the small angle approximation will work well. The formula below uses this assumption to calculate the x-­‐angle from the accelerometer reading, the y-­‐angle is calculated the same way: x_acc = (float)(x_acc_ADC -­‐ x_acc_offset) * x_acc_scale*180/pi; Where the x_acc_ADC is the raw accelerometer reading. It was found using a function called getRawData() that came in the downloaded libraries for this accelerometer. The x_acc_scale is the conversion factor to convert the LSB reading into g's. It can be found in the accelerometer data sheet. The ADXL 345 conversion factor is 4mg/LSB for all g ranges, therefore it's x_acc_scale = 0.004. In order to use the trig calculations and small angle approximation, the acceleration value must be in g's. The offset value is the value the accelerometer reads when it is stationary and lying flat. It was obtained by averaging the reading's of the angle when the accelerometer was lying flat for 2 seconds. The 180/pi converts the g value into an angle in degrees. This method gives a fairly accurate reading for the angle. However, because it only uses one axis to calculate the angle it will give an angle between -­‐90 and 90 degrees. This should be fine for a balancing application because if you are off by more than 90 degrees than you likely won't be able to correct it anyway. If an angle between -­‐360 and 360 degrees is required, then the accelerometer reading from two axes must be used to calculate the angle. Manual Calibration Another method that is very simple and seems to work quite well is to develop your own x_acc_scale value to relate degrees to LSBs. You can then use the following formula: x_acc = float(x_acc_ADC -­‐ x_acc_offset) * x_acc_scale A good way to find the x_acc_scale for this case is to put the accelerometer in a position where it should read 90 degrees on a given axis (i.e. hold the accelerometer so the x-­‐axis is pointing up). Your conversion factor would then be equal to 90°/LSB reading. The LSB reading is the raw accelerometer output when it is rotated to 90 degrees. This method works well. However, like the small angle approximation method, it will only give an angle between -­‐90 and 90 degrees. In order to get the x_acc_scale, you will need to use a calibrate function like the one below. This function will read the raw values of the x-­‐axis of the accelerometer when it is positioned at 90 degrees (x-­‐axis pointing up). Remember ax = 1g when the x-­‐axis is pointing up because the g force will be up (counter acting free fall which would cause it to move down). Figure 1-­‐ Calibrate_Accleration function The acc_scale array has a length of two. Position zero is for the x scale and position one is for the y scaling value. It is a global array so you only need to run the calibrate_acceleration() function once at the beginning and then that value can be used throughout the function. Once you have found the scaling factor, you can use it to find the angle from the accelerometer raw data. Figure 2-­‐ Angle calculation from accelerometer data. Using manual calibration to find scaling factor The above function also uses the initial[] array. This is an array of all the initial values read from the robot when it is lying flat (i.e. the offset). Trigonometry Another method to calculate the angle is to use trigonometry without the small angle approximation. This is sometimes not ideal because it requires too much processing time. If you wish to do this, you can use a function like the one below. This code was found online. It inputs the raw x and y accelerometer readings in 32-­‐bit integers and calculates the angle. Figure 3-­‐ Function to calculate angle using arc tan This function uses both the x and y axis accelerometer reading's. Therefore it will give an angle between -­‐360 and 360 degrees. Gyroscope The gyroscope measures angular velocity. Its output is linear to the rate of rotation measured in degrees per second. In order to convert the output of the gyroscope into degrees per second you can use the conversion factor given in the data sheet. For the ITG-­‐3200, the conversion factor (a.k.a the sensitivity) is 14.375 LSB/(deg/s). The formula below gives the gyroscope data in degrees/s. gyro = (float)(gyro_ADC -­‐ gyro_offset)*gyro_scale; Where the gyro_offset is the value the gyroscope reads when it is lying stationary, the gyro_ADC is the raw gyroscope data (read using the getRawValues() function) and the gyro_scale is the conversion factor from LSBs to g's. Make sure to check the code in any libraries you are referencing to ensure the data has not been scaled before it is output. If it has, that will need to be taken into account when reading the data. Once you have the gyroscope data in degrees/s, you can integrate this value to get the angle in degrees using the formula below: gyro_angle = gyro_angle + gyro *dt; This formula takes the previous angle and adds the change in angle to give you the current angle with respect to where you started. To find the change in angle you take the speed you rotated (the gyroscope reading) and multiply it by how long you rotated (dt). To find dt, try using either the micros() or millis() function. The function below shows you how to implement the micros() function to find dt and use it to integrate the gyroscope data. Figure 4-­‐ Gyroscope data Integration Complementary Filter The Complementary filter method uses both the accelerometer and the gyroscope to measure the angle and combines the results to get a very accurate angle reading. Both types of sensors (accelerometer and gyroscope) are sensitive to noise at certain frequencies. This noise needs to be filtered out to obtain an accurate reading. The accelerometer is sensitive to high frequency noise caused by vibration therefore, it needs to be low pass filtered. The gyroscope is also sensitive to high frequency noise, however, after integration of the gyroscope data , this high frequency noise becomes low frequency drift. Drift causes the data to rise linearly over time. In order to eliminate the drift, a high pass filter should be used. A high pass filter lets high frequency signals pass and filters out low frequency (below the cut-­‐off frequency) signals. By filtering the two reading's you have eliminated half the angle data. You removed all the high frequency data from the accelerometer reading and all the low frequency data from the gyroscope reading. Therefore to obtain the complete angle you now need to add the two halves together. The sum of these two reading's gives you the band pass filtered angle data, which is very accurate. See figure below. Figure 5-­‐ Complementary filter with cutoff frequency ωc This figure shows a low pass filter (left) and a high pass filter (right) being added together to produce a complementary filter. Notice that the low pass filter has removed, attenuated below a set value, all the frequencies above ωc (the cutoff frequency) and the high pass filter has removed all the frequencies below ωc. Note that the cutoff frequency of the low pass filter and high pass filter is the same so when they are added together you get all the data (high and low frequency), but without the noise. If the cutoff frequencies were not the same, then the two filtered data sets would not add to give the complete data set. Low pass filter The purpose of the a low pass filter is to let through low frequency data (long term changes) and filter out high frequency data (short term changes). The basic code to implement the low pass filter can be found on the low pass filter Wikipedia page: http://en.wikipedia.org/wiki/Low-­‐pass_filter. This page has a good explanation of what the filter is, how it works and how the equation below is derived. The low pass filter equation: filtered_angle[n] = α*angle + (1-­‐α)*filtered_angle[n-­‐1]; Where 'angle' is the angle calculated from the accelerometer data using one of the methods discussed above and α is the smoothing factor. This equation smoothes the data by attenuating the high frequency noise. The smaller the α value, the more smooth the curve will appear. High Pass Filter The high pass filter, passes high frequency signals and attenuates low frequency signals. In this case we are trying to eliminate the gyroscope drift, which is low frequency. Once again, the Wikipedia high pass filter page is a very good source and contains a good explanation of how it works as well as the formula derivation: http://en.wikipedia.org/wiki/High-­‐pass_filter High pass filter equation: filtered_angle[n] = β*(filtered_angle[n-­‐1] + angle[n] + angle[n-­‐1]); Where the angle is obtained from integrating the raw gyroscope data and β is the time constant. In order for the low and high pass filters to have the same cutoff frequency, β must equal 1-­‐α, where α is the coefficient for the low pass filter equation. A high pass filter is the inverse of a low pass filter so another way to implement a high pass filter, is to take one minus the low pass filter. In this case you want the smoothing coefficient to remain α, since you are taking one minus the whole filter instead of just using a coefficient of 1-­‐α . The equation to implement this is shown below. If you simplify this equation (shown in the second line), you will see that taking one minus the low pass filtered data is the same as low pass filtering the data with a smoothing coefficient of 1-­‐α (where α is the smoothing coefficient used for the low pass filter). filtered_angle[n] = 1-­‐(α*angle + (1-­‐α)*filtered_angle[n-­‐1]); = β*angle + (1-­‐β)*filtered_angle[n-­‐1]; Combining the signals In order to get the full signal you now need to add the two filtered signals. 𝑎𝑛𝑔𝑙𝑒 = 𝑔𝑦𝑟𝑜 𝑎𝑛𝑔𝑙𝑒 + 𝑎𝑐𝑐 𝑎𝑛𝑔𝑙𝑒 This is assuming that the smoothing coefficients used in the low pass and high pass filters are α and 1-­‐α.. The smaller the α value, the more horizontal acceleration noise will be allowed to pass through. The following link explains the complementary filter in more detail and discusses briefly how to set and optimize the time constant: http://web.mit.edu/scolton/www/filter.pdf Note that when combining the accelerometer and gyroscope angles that the gyroscope y angle and the accelerometer x angle combine. This is because rotation around the y-­‐axis (i.e. the gyroscope reading) will cause a change in the x-­‐axis angle and vice versa. See figure below. ROLL ( y ) YAW X Z Y PITCH ( x ) angle_x = α*gyro_angle_y + (1-­‐α)acc_angle_x; angle_y = α*gyro_angle_x + (1-­‐α)acc_angle_y; In order to avoid confusion, you might want to rename the angles roll and pitch instead of x and y. Roll is rotation about the x-­‐axis (i.e. changes the y angle) and pitch is rotation about the y-­‐axis (i.e. changes x angle). Lab Requirements For this lab you will need to hook up your accelerometer and gyroscope, read the raw data, integrate the gyroscope data and then filter and combine both the accelerometer and gyroscope data. You will need to be able to demonstrate this to the T.A by showing the output angle as your rotate your accelerometer/gyroscope between -­‐90 and 90 degrees. Print-­‐off screen shots of all your code and hand it in after your demonstration.
