For those who want quick access to some additional detail …
Description:
This series of Inertial Measurement Unit (IMU) subVIs is patterned after the WPI series for reading sensors, and includes several additional IMU Application subVIs. These application subVIs do the math required for sensor fusion, forming a basic Attitude and Heading Reference System (AHRS), with drift-free Roll, Pitch and Yaw outputs. An IMU typically consists of a 3-axis Accelerometer, Rate Gyroscope, and Magnetometer (sometimes the Mag is absent, resulting in reduced capability). Although technically incorrect, these devices are often referred to as 9 Degrees-of-Freedom (DoF) sensors (3 sensors with each 3 axes in XYZ = 9 DoF). The RoboBees IMU Series subVIs are:
OPEN - Performs IMU initialization, setup, and calibration of each axis of Rate Gyroscope, Accelerometer, and Magnetometer.
IMU RefNum Registry Set / Get - These are standard WPI-type functional globals used to pass IMU configuration data.
READ - Reads IMU sensors, generates and applies calibration values. Gyro output provides XYZ rates as well as integrating these rates for degree displacements. Accelerometer output provides XYZ acceleration and indication of motion and freefall.
RESET - Resets Gyro degree displacement to zero for all axes.
COMPLE FILTER - Performs sensor fusion for basic AHRS using a complementary filter design.
TILT - Calculate Roll and Pitch from gravity vector and implements an optionally tilt-compensated Earth compass for Yaw.
These IMU subVIs are designed to be used with the Adafruit 9-DoF IMU Breakout - containing L3GD20 (Gyro) + LSM303 (Accelerometer and Magnetometer) chips (Product ID: 1714), or the Adafruit 10-DoF IMU Breakout - which adds the BMP180 chip (Product ID: 1604). The 10-DoF IMU adds a Barometer for air temperature and pressure measurements (for indicating altitude, not attitude), and is not currently implemented. The two IMU Application subVIs, COMPLE FILTER and TILT, are agnostic of IMU model, and compatible with separate sensors, as long as all sensor axes are aligned properly.
The motivation to fusing these sensors is to combine their best properties and mitigate the worst properties of each. The overall sensor fusion design is provided in block diagram form (attached). A major benefit is effective elimination of the drift inherent in the Rate Gyro when used alone.
Design:
The IMU OPEN subVI sets up each sensor based on the respective Set Cluster settings. This includes setting the sensor range, output data rate, enabling the High Pass Filter (Gyro and Accelerometer only), and selecting a High Pass Cut off Frequency. Each sensor can also be selectively enabled/disabled. All sensors (except the Barometer), need to be enabled for a fully functional AHRS. The IMU breakout board from Adafruit.com places all sensors on a common I2C bus for communication. The sensor chip’s I2C Addresses (in hex) and respective data sheets for I2C register references are:
Chip - Sensor - I2C Address - Data Sheet Reference
LSM303DLHC - Accelerometer - 0x19 - http://www.adafruit.com/datasheets/LSM303DLHC.PDF
LSM303DLHC - Magnetometer - 0x1E - see above
L3GD20(H) - Rate Gyroscope - 0x6B - http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/DM00036465.pdf
BMP180 - Barometer - 0x77 - http://www.adafruit.com/datasheets/BST-BMP180-DS000-09.pdf
Note: Adafruit modified both breakout board configurations in Oct of 2014 to upgrade the Gyro sensor from L3GD20 to L3GD20H (improved performance specifications). This code should be compatible with each Gyro variant. Guidance on the Gyro startup sequence is given in section 3.1 of ST MicroElectronics App Note AN4506 for L3GD20H :
http://www.st.com/web/en/resource/technical/document/application_note/DM00119037.pdf
After the sensors are set up, enabled sensors are turned on, IMU configuration data is loaded into an IMU Cluster, and sensor calibration begins.
Calibration:
Raw readings from an IMU are not very useful until they are conditioned by carefully determined calibration parameters. Since reading the digital sensor values is a bit complicated, the IMU READ subVI is simply called by IMU OPEN to produce the raw measurement values used to characterize the sensor. The calibration is only good for the range selected during the cal process.
The calibration process used here is determining the values ‘m’ and ‘b’ for the assumed linear sensor model Y=mX+b for each parameter/axis pair, where ‘X’ is the raw sensor reading, ‘m’ and ‘b’ are cal parameters, and ‘Y’ is the corrected sensor reading. The sensor is ‘exercised’ eight times to allow the Gyro readings to reach steady-state, which improves the calibration. With the default cal sample size of 30, the Gyro/Accelerometer cal process takes about 2.5 seconds, which is about 0.5 seconds longer than the default cal time for the OPEN subVI for an analog Gyro. The Gyro and Accelerometer cal is performed first, and the robot needs to be completely still during this time (as is required with an analog Gyro). Since the cal is conducted within the OPEN subVI, and OPEN is generally placed in Begin (in FIRST Robotics) before the robot can move itself, this requirement is easily fulfilled. Care, however, must still be taken to ensure no external bumps occur during this cal. This code can detect if a physical disturbance occurred, and recalibrate automatically as mitigation (no extra charge).
Recalling the model Y=mX+b, for the IMU Gyro, ‘m’ is taken from the spec sheet as the gain (sensitivity) for each axis. Value ‘b’, which is bias from zero, is determined by taking the statistical mean of a sample of Gyro rate measurements while the sensor is still.
For the Accelerometer, ‘m’ should be determined off-robot, as it requires the sensor to be moved in all 3 dimensions. These values are either determined experimentally off-robot (to improve upon the standard sensitivity values given in the sensor spec sheet), or assumed to be a value of one for each axis. To determine experimentally, find the maximum and minimum values of each axis as they are rotated (slowly) through vertical. The axis sensitivity is half of the signed difference between max and min. Value ‘b’ is determined by taking the statistical mean of a sample of Accelerometer measurements while the sensor is still.
The fun really begins with the Magnetometer calibration. Proper Mag cal requires the sensor to be rotated in all 3 dimensions, but unlike the Accelerometer, the Mag must be rotated with the rest of the robot to account for how the robot itself disturbs the Earth’s magnetic field (this is a lot easier when the Mag sensor is installed in a smart phone.). Since one typically would not have available a 3-axis non-ferrous robot gimbal, the cal process is designed to support a 2-D calibration in the X-Y (horizontal) plane only.
In another departure from the Gyro/Accel cal, sample data from the Mag must be taken as the robot is moving. This process is optional to allow the ‘m’ and ‘b’ Mag cal parameters to be determined during final testing, and then loaded into the Mag Set Cluster for operations (i.e. competition). If selected, the Mag cal process first instructs you to begin rotating the robot (manually) with a pop-up dialog box. Selecting OK initiates data collection, and provides a second dialog box to allow you to signal when you’ve completed a 360 degree rotation.
Once complete, the X and Y axis ‘m’ and ‘b’ parameters are calculated and displayed on the IMU OPEN Front Panel as ‘scale’ and ‘zero’, respectively. An XY graph of the data is also provided to allow visual inspection of the cal result - look for a nicely shaped circle (see example at left), if not, repeat the cal. If you like the plot, load the X and Y axis scale and zero parameters into the Mag Set Cluster, retain the default scale (1) and zero (0) values for the Mag Z axis, and turn off Mag cal by deasserting MagCal.
Magnetometer measurement error is generally classified into two categories, Hard Iron and Soft Iron effects. Only Hard Iron effects are compensated here, as they appear to dominate. Soft Iron effect compensation is a future improvement. Performance is adequate as long as sources of Soft Iron distortion (non-magnetized Iron, Nickle, and Cobalt) are kept away from sensor (or vice versa). If an ellipse is displayed, there is Soft Iron magnetic field interference. Ensure the cal plot has equally sized X and Y axis ranges prior to visually inspecting the cal plot and making an ellipse determination. If Soft Iron is detected, either relocate the IMU or discover and remove the offending material. Run all motors and other actuators to verify that the Yaw output of the Complementary Filter is not significantly affected. If so, it may be possible to relocate the IMU or shield the motor with mu-metal or similar. Do not shield the IMU!
Future Improvements:
Barometer, move freefall into interrupt/DIO, DCM/Kalman filter, Soft Iron Calibration, 3D cal, Auto ranging, FPGA integration, Fuse Mag-based Yaw with differential encoders to correct for external-to-robot ferrous sources, SPI interface for faster read response.
Implementation:
Code as per example on upper left. Experimentally determine values for Accelerometer scale as described in above section on calibration, and load into the Accel Set Cluster (or default to one). Set appropriate ranges, fastest data rates (if polling), and enable each sensor. Install sensor on robot and initially set MagCal to True. Determine Mag cal parameters as described above and load these values into Mag Set Cluster. Set MagCal to False to skip future Mag cals.
The Mag cal values are strongly a function of where the sensor is located on the robot, and what is installed nearby, so it’s important to recalibrate fairly often, certainly after relocating the sensor, and again after the robot is completely assembled. Be sure you actuate all motors and mechanisms to test for interaction with the Yaw indication, which uses the Mag. Be aware that another robot that comes close to the Magnetometer might disturb the local magnetic field enough to cause error in Yaw – the solution to this difficulty is left to the enterprising student of sensor fusion.
Enabling the Gyro and Accelerometer High Pass Filters (HPF) can have a deleterious effect on dynamic response, as they can introduce lag. HPFs default to off, but advanced control system designers may find them useful. Experiment cautiously.
The Accelerometer senses both gravitation and movement (study Einstein’s principle of Equivalence). Roll and Pitch will be affected by movement, as will Yaw should Tilt-Compensation be enabled (this is selected with the Comple Filter subVI).
Configuring these IMU subVIs per the example creates a simple Attitude and Heading Reference System (AHRS). The range of Pitch/Roll/Yaw values are constrained in range, however, as follows: Pitch range is -90<P<+90, Roll range is -180<R<+180, and Yaw range is -180<Y<+180. Although over-rotating Pitch and Yaw beyond their range will result in erroneous readings, this should be an adequate operating range for most robot body applications. These range constraints may or may not be adequate for an actuator or mechanism. Yaw is a bit different … North is 0 degrees, and East/West is -90/+90 deg, respectively. South is both -180 and +180 deg, as these values are congruent. It is ok to over-rotate Yaw, as this portion of the Complementary Filter is designed with logic to smoothly transition across the +/-180 degree discontinuity (auto-wrap).
It is important to note that while the R/P/Y values are constrained, the Gyro displacement values in X/Y/Z are not. Gyro deg outputs from the IMU READ subVI will wind (not wrap) - i.e. they will continue to count above/below 360 degrees. Use the LabVIEW Quotient & Remainder (modulus) function to convert these outputs to a wrapping behavior, if desired.
Implementation considerations for IMU Read:
This design performs numerical integration off-FPGA for the Digital Gyro (unlike the Analog Gyro subVIs, which typically utilize the FPGA to perform accumulation for superior speed resulting in less integration error). Testing indicates this off-FPGA approach may be adequate for FRC application, however, it is important to ensure the code runs without any timing issues. This is why a Timed Loop is recommended for the IMU Read within Periodic Tasks (executes at a higher priority). Non-uniformity in read iteration is mitigated by design, as each dt is uniquely calculated by time stamping each read event. This means that while some variation in timing can be tolerated (the occasional late iteration), excessively long durations (periods) between successive reads will fail to capture the actual rate profile as it changes resulting in integration error.
In short, use a Timed Loop structure to iterate IMU READs, and set the loop period for a relatively fast iteration (<75 ms). Shorter periods provide improved accuracy of degree displacements, however setting this too short can impact the execution of your remaining code (especially when using Timed Loops). One way of determining an optimized period is to set it while monitoring the “Finished Late?” Loop parameter, ensuring this value stays False. It helps if the CPU Utilization remains low (<75%), which can be monitored with the RT Get CPU Loads subVI (Real Time menu). Study LabVIEW determinism for additional insight.
----- The RoboBees FRC Team 836! ----- You too can conquer inertia!! … consider a career in STEM!
Sensor Fusion.pdf (102 KB)
Sensor Fusion.pdf (102 KB)
