Inertial Management Unit¶

The Zumo32U4 includes on-board inertial sensors that can be used in advanced applications, such as helping our Zumo detect collisions and determine its own orientation by implementing an inertial measurement unit (IMU).

Note

We used IMU as the main sensory-model for Segway.

We took the aid of following sensor components of IMU;

Gyroscope : ST L3GD20H 3-axis gyroscope.
Accelerometer : ST LSM303D compass module, which combines a 3-axis accelerometer and 3-axis magnetometer.

Note

Both sensor chips share an \(I^2C\) bus connected to the ATmega32U4’s \(I^2C\) interface.
Level shifters built into the main board allow the inertial sensors, which operate at 3.3 V, to be connected to the ATmega32U4 (operating at 5 V).

Gyroscope¶

We consider the following aspects of Gyroscope for IMU sensory-model;

Gyroscope provides the change in orientation of the Zumo (Roll, Yaw, Pitch). Integration of result provides the position details.
ST L3GD20H Gyroscope operation is based on angular momentum.
ST L3GD20H provides; * Selectable full-scale range of \(\pm245dps\)/\(\pm500dps\)/

\(\pm2000dps\), with the \(8.75\frac{mdps}{digit}\)/ \(17.5\frac{mdps}{digit}\)/\(70\frac{mdps}{digit}\) sensitivity, respectively.
- Selectable data sampling rate.
- Low-Pass filter to reduce noise with selectable cut-off frequencies.

Listing 1 Gyroscope configuration [9]¶

// Set up the L3GD20H gyro.
gyro.init();

// 800 Hz output data rate,
// low-pass filter cutoff 100 Hz.
gyro.writeReg(L3G::CTRL1, 0b11111010);

// 2000 dps full scale.
gyro.writeReg(L3G::CTRL4, 0b00100000);

// High-pass filter disabled.
gyro.writeReg(L3G::CTRL5, 0b00000000);

Note

All other register were left with their default value. Review [13] for more information regarding default values.

A more detailed descriptions of the configuration used is shown below;

L3G::CTRL1.DR[1:0] = 0x3 selects the \(800 Hz\) data sampling rate [13].

L3G::CTRL1.BW[1:0] = 0x3 selects \(100 Hz\) gyroscope data cut-off frequency [13].

L3G::CTRL1.PD = 0x1 selects the normal mode disabling power mode, so the signal will be always be sampled [13].

L3G::CTRL1.XEN = 0x0, L3G::CTRL1.YEN = 0x1 and L3G::CTRL1.ZEN = 0x0 enables only the needed gyroscope channel.

L3G::CTRL4.FS[1:0] = 0x2 selects the full-scale of \(\pm2000dps\) with a sensitivity of \(70mdps/digit\) [13].

L3G::CTRL5.HPen = 0x0 disable the High-Pass filter [13].

Accelerometer¶

We consider the following aspects of Accelerometer for IMU sensory-model;

ST LSM303D Accelerometer provides the linear acceleration based on vibration.
By virtue of linear acceleration, Accelerometer provides 3-dimensional position (X-,Y-,Z- axis). [14]
ST LSM303D provides \(\pm2\)/\(\pm4\)/\(\pm6\)/\(\pm8\)/ \(\pm16\) selectable linear acceleration full-scale. [14]
ST LSM303D provides \(3.125Hz\)/\(6.25Hz\)/\(12.5Hz\)/ \(25Hz\)/\(50Hz\)/\(100Hz\)/\(200Hz\)/\(400Hz\)/ \(800Hz\)/\(1600Hz\) selectable sampling rate. [14]

For the implementation of the Segway the sampling frequency, \(f_s = 50Hz\), and full-scale range, \(acc_{range} = \pm8g\), were selected. Therefore, the ST LSM303D configuration code is shown in Listing 2

Listing 2 Compass configuration [9]¶

// Set up the LSM303D accelerometer.
compass.init();

// 50 Hz output data rate
compass.writeReg(LSM303::CTRL1, 0x57);

// 8 g full-scale
compass.writeReg(LSM303::CTRL2, 0x18);

Note

All other register were left with their default value. Review [14] for more information regarding default values.

A more detailed descriptions of the configuration used is shown below;

LSM303::CTRL1.AODR[3:0] = 0x5 sets the \(f_s = 50Hz\). [14]

LSM303::CTRL1.BDU = 0x1 enables atomic update for the acceleration read register. Meaning that the entire register will be written at once [14].

LSM303::CTRL1.AXEN = 0x1, LSM303::CTRL1.AYEN = 0x1 and LSM303::CTRL1.AZEN = 0x1 enables all three acceleration channels. [14]. All three are needed because the magnitude of the acceleration vector is calculated to filter some measurement noise. Listing 4 shows how the magnitude is used to filter the noise.

LSM303::CTRL2.AFS[2:0] = 0x3 sets \(acc_{range} = \pm8g\).

Combine Gyroscope and Accelerometer¶

Gyroscope gives angular position but has tendency to drift over the period of time. Accelerometer gives Inertia, and ultimately position but it is slow. Hence, Accelerometer output is used to correct position obtained from Gyroscope on periodic interval of time.

First the Gyroscope is being sampled as frequently as possible. Then the data of the Gyroscope is integrated and to give the current Zumo32u4’s angle as fast as possible. Listing 3 shows how the sampling and integration was performed;

Listing 3 Gyroscope angle sampling and integration [9]¶

/** Zumos Gyro */
L3G gyro;


/**
* Reads the Gyro changing rate and integrate it adding it to the angle
*/
void sampleGyro() {
  // Figure out how much time has passed since the last update.
  static uint16_t lastUpdate = 0;
  uint16_t m = micros();
  uint16_t dt = m - lastUpdate;
  float gyroAngularSpeed = 0;
  lastUpdate = m;

  gyro.read();
  // Obtain the angular speed out of the gyro. The gyro's
  // sensitivity is 0.07 dps per digit.
  gyroAngularSpeed = ((float)gyroOffsetY - (float)gyro.g.y) * 70 / 1000.0;

  // Calculate how much the angle has changed, in degrees, and
  // add it to our estimation of the current angle.
  angularPosition += gyroAngularSpeed * dt / 1000000.0;
}

The selected sampling frequency for all sensors was \(f_s=50Hz\) meaning that every \(20ms\) the integrated angle from the gyroscope is corrected with the angle given by the Accelerometer. Listing 4 shows how the correction is performed.

Listing 4 Integrated gyroscope angle correction with accelerometer’s angle [9]¶

/** Zumos Accelerometer */
LSM303 compass;


/**
 * Read the acceleormeter and adjust the angle
 */
void sampleAccelerometer() {
  static uint16_t lastUpdate = 0;
  uint16_t m = micros();
  uint16_t dt = m - lastUpdate;
  float gyroAngularSpeed = 0;

  lastUpdate = m;

  compass.read();
  accelerometerAngle = atan2(compass.a.z, -compass.a.x) * 180 / M_PI;

  // Calculate the magnitude of the measured acceleration vector,
  // in units of g.
  LSM303::vector<float> const aInG = {
    (float)compass.a.x / 4096,
    (float)compass.a.y / 4096,
    (float)compass.a.z / 4096}
  ;
  float mag = sqrt(LSM303::vector_dot(&aInG, &aInG));

  // Calculate how much weight we should give to the
  // accelerometer reading.  When the magnitude is not close to
  // 1 g, we trust it less because it is being influenced by
  // non-gravity accelerations, so we give it a lower weight.
  float weight = 1 - 5 * abs(1 - mag);
  weight = constrain(weight, 0, 1);
  weight /= 10;


  // Adjust the angle estimation.  The higher the weight, the
  // more the angle gets adjusted.
  angularPosition = weight * accelerometerAngle + (1 - weight) * angularPosition;
  angularSpeed = (angularPosition - prevAngularPosition) * 1000000.0 / dt;
  prevAngularPosition = angularPosition;

}

Note

Note that angularPosition is derivated to get angularSpeed, because both quantities are needed by the state variable model used. For more information review the State Variable Model.
The sign of the angle has been changed from the one in the original balancing example [9] to match our reference framework.
src/SegwayLQR/ZumoIMU.ino holds the source code that handles the IMU.

Warning

All angles are given in degrees because during implementation it was proved that it was easier to catch bugs if the angle was in degrees. One reason for this was that degrees are scaled up with respect with radians it was easier to catch integer divisions causing the angle to be zero. Furthermore the use of degrees is a little more intuitive than radians.

ZumoIMU API¶

class ZumoIMU¶

float accelerometerAngle = 0¶: Accelerometer angle

L3G gyro¶: Zumo’s Gyro

LSM303 compass¶: Zumo’ss Accelerometer

float gyroOffsetY¶: Gyro’s bias

float prevAngularPosition = 0¶: Previous Angular position

void setupIMU()¶: Setup the Gyro and Accelerometer

void sampleGyro()¶: Reads the Gyro changing rate and integrate it adding it to the angle

void sampleAccelerometer()¶: Read the accelerometer and adjust the angle

void calibrateGyro()¶: Calibrate the Gyroscope. Get the bias.

[9]	(1, 2, 3, 4, 5) Pololu. Zumolibrary’s balancing example code. URL: https://github.com/pololu/zumo-32u4-arduino-library/tree/master/examples/Balancing.

[13]	(1, 2, 3, 4, 5, 6) STMicroelectronics. L3GD20H, MEMS motion sensor: three-axis digital output gyroscope. June 2012. URL: https://www.pololu.com/file/0J731/L3GD20H.pdf.

[14]	(1, 2, 3, 4, 5, 6, 7) STMicroelectronics. LSM303D, Ultra compact high performance e-Compass 3D accelerometer and 3D magnetometer module. June 2012. URL: https://www.pololu.com/file/0J703/LSM303D.pdf.