Building an Autopilot Robot (Part 1)

Accelerometers in Action

In this first part of his two-part article series, Pedro lays the groundwork for his autopiloted four-wheeled robot project. Here, he shows the complete path in digital signal processing (DSP) to go to from raw acceleration measurements—obtained from a BeagleBone Blue IMU—to reliable speed estimation using accelerometer data.

This project is completely open-source. Its source code is hosted in a Github repository. Table 1 shows the project’s parts list. This project uses a BeagleBone Blue board as its logical unit, sensor unit and motor driver unit. I chose this board because of its rich set of resources, including on-board sensors, sensor interfaces, LiPo battery, motor control, wireless connectivity and powerful SiP. This enabled me to minimize the use of external and auxiliary circuits, and create a full robotics solution that’s compact and simple. Figure 1 shows BeagleBone Blue board and its list of features and peripherals.

For the software part of this project, the following are used:

Operating System: Debian Linux
Programming language: C
Compiler: GCC
Sensors and drivers API: Robotics Cape
Additional libraries: Paho MQTT
for Internet control and monitoring

Links to more information about those software elements are provided in RESOURCES at the end of the article.

MATHEMATICAL OVERVIEW

This project focuses on estimating speed using only accelerometer as a source of data. Mathematically, the variation of speed in accelerated movement can be achieved by integration of the acceleration over time, as shown in this formula:

 (1)

Also, the instant speed can be calculated by the sum between acceleration integration over time and initial speed, as shown here:

 (2)

Geometrically, integrating a function curve produces the same result as the graph area between function curve and the X-axis. This enables easy acceleration integration in a computing system. All the software has to do is calculate a trapezoidal area, considering current acceleration and the previous one. For better understanding, refer to Figure 2. This equation shows the geometric area calculation (acceleration function integration, from t0 to t1) is:

 (3)

Finally, once speed based on accelerometer data is calculated, the speed control tasks can do their work. To control speed, this project uses PID control. PID control is among the most commonly used control techniques in automation and industrial systems. Briefly, here’s how it works: A PID block controls a variable (called “process variable” or PV) using periodic error comparison between a current measurement and what value you desire for a variable (called “setpoint”) This takes into consideration three terms: P- proportional term, I – integral term and D – derivative term—and the gains (K_P, K_I and K_D), in a closed-loop system. The gains can be automatically or manually adjusted, depending on the desired software complexity. The main equations for every PID control system are shown here:

 (4)

 (5)

where:

t0: time instant of a previous measure of process variable
t1: time instant of a current measure of process variable

Although, it isn’t explicit in Equation 5, the derivative term also considers t0 and t1 for its calculation.

ACCELEROMETERS

An accelerometer is a device that allows to get acceleration in one, two or three axes (x, y and/or z). Because they’re so widely used, accelerometers are often part of an Inertial Measurement Unit (IMU). In the case of this project, the IMU chip contains an accelerometer that measures three-axis acceleration, meaning that it can measure acceleration in every spatial direction. In fact, the acceleration measured equals the acceleration that is applied to the accelerometer IC. Accelerometers provide communication interfaces that enable them to be used in computing systems—I²C interface being the most common of these.

Getting deeper into microelectronics, an accelerometer can be part of a group of systems called MEMS (Micro Electro Mechanical Systems). MEMS combine mechanical and electrical micrometric scale systems to get measurements that depend on inertial/mechanical stimulus. Popular examples of MEMS are accelerometers and gyroscopes. These technologies are used for sensors in drones, remote controlled vehicles and portable healthcare equipment. In short, MEMS make is possible to acquire mechanical and/or inertial measurements that computing systems can process. This makes them vital tools for sensing the “outside world” of embedded systems.

One drawback to accelerometers is that the raw measurements they produce aren’t so reliable. The data can consist of a very noisy signal that contains a lot of spikes. The graphs in Figure 3 provide an example of noise present in raw accelerometer measurements. Figure 3a shows raw accelerometer data (Y-axis) when an accelerometer is standing still. And Figure 3b shows raw accelerometer (Y-axis) data when accelerometer is submitted to harsh movements. From observing these graphs, it’s possible to infer that raw accelerometer measurements are very noisy and require DSP techniques to generate useful data.

By Figure 3 analysis it’s possible to infer that, even standing still, acceleration measurements rarely tend to produce a zero value. At a closer look, there’s a tiny offset in these measurements (systematic error). Although these acceleration distortions are tiny, they are relevant enough to ruin the speed estimations in further calculations.

One way to minimize this tiny offset in accelerometer measurements is to perform accelerometer calibration before any other acceleration usage and/or manipulation. Calibration consists of these steps, which must be done for every accelerometer axis used:

1. Several consecutive measurements are made and summed.

2. After the reading process, the measurements’ mean is determined. This mean corresponds to an acceleration offset.

3. Once this offset is calculated, it must be subtracted from every raw acceleration measure, what minimizes the systematic error.

SENSIBILITY ADJUSTMENT

Most accelerometers available on market support sensibility adjustment. Sensibility adjustment means configuring the acceleration range, usually in “G” units (1 G = 9.8 m/s²). The greater the range, the lower the sensibility. The sensibility adjustment must fit the acceleration limits of your project. In the particular case of this project, the maximum acceleration per axis is about 0.6 g. The options available for accelerometer sensibility range are: ±2 G, ±4 G, ±8 G and ±16 G. Therefore, the ±2 G range fits best for this project.

Based on Figure 3, it can be inferred that even when the accelerometer is standing still, its measurements are very noisy. Physically, acceleration means speed variation over time, so these tiny values of acceleration don’t make sense because the accelerometer is standing still (no movement condition). This fact brings us to the conclusion that “too low” acceleration values must be ignored from further calculations that require reliable accelerometer data. In DSP terms, “ignoring tiny values” means applying the discrimination window technique. This technique consists of setting to zero every measurement that fits in a zone called the discrimination window. The discrimination window and valid signal zones are shown in Figure 4.

Based on Figure 4, all measurements in both valid signal zones (valid positive signal zone and valid negative signal zone) will remain unmodified and all measurements that fit in discrimination window will be set to zero. This discrimination window technique ensures that low-amplitude noises are completely filtered. That makes the resulting signal more reliable for further calculations. Getting deeper into coding this technique, the discrimination window technique code is shown in Listing 1 (extracted of the current project’s code).

#define DISCRIMINATION_WINDOW     0.3 //m/s²

.
.
.


float verify_discrimination_window(float sample)
{
     if ( abs(sample) <= DISCRIMINATION_WINDOW )
          return 0.0;
     else
          return sample;
}      }
}

The majority of accelerometers available on the market offer some options for configuring sample rate. Sample rate in an accelerometer means the number of measurements that can be acquired per second. This sample rate is represented in Hertz units: Hz.

When estimating speed by using accelerometer data, every single measurement acquired matters, because speed will be calculated by integrating acceleration over time. Therefore, to ensure better results, the sample rate should be set as the maximum value allowed by the accelerometer. In this project’s case, the maximum sample rate allowed in accelerometer is 200 Hz. That means that one new accelerometer measurement sample is available every 5 ms.

Also keep in mind that the accelerometer measurements MUST be processed in regular period/time intervals. Being sloppy here can lead to incorrect speed estimations. Every accelerometer sample matters and every single calculation error will be carried on every speed calculation, ruining all calculated speed estimations. To ensure that the accelerometer data is being processed exactly when it’s ready (in 5 ms time intervals), like an IRQ-oriented calculation, the Robotics Cape API provides a very interesting resource called DMP mode.

In DMP mode, a software interrupt is triggered every time a sample is ready to be read, and every measure is stored in a FIFO queue (this ensures no accelerometer data is lost). Also, it allows the programmer to set a function to be called when this interruption comes up (very similar to IRQ-function determination on bare-metal firmware development). Therefore, the DMP mode ensures regularity on processing accelerometer data.

FILTERING ACCELEROMETER DATA

Now let’s look at filtering accelerometer data. This is one of the most crucial steps for speed estimation by accelerometer data. Even if all previous steps have been done well, if this step fails the speed estimation is ruined. To filter accelerometer data, two filters can be applied consecutively: a native accelerometer filter and a filter based on consecutive sampling;

Most of accelerometers offer native filtering—for example, low-pass filters. In this project, the accelerometer offers low-pass filters, with the following options: 184 Hz, 92 Hz, 41 Hz, 20 Hz, 10 Hz and 5 Hz. In the particular case of this project, the best results were achieved using the 41 Hz low-pass filter according to results determined by empirical tests.

For the filter based on consecutive sampling (no-native accelerometer filter), there are a lot of options. Every project can use different filters here, depending on project’s requirements. That said, there are two filters that deserve special attention when talking about filter acceleration measurements: the Kalman filter and the Moving Average filter.

The Kalman filter works great for accelerometer filtering, due to two particular characteristics. First, it provides automatic adjustment during use—the more it’s used, the more accurate it becomes. And, second, it has to capability to predict states. That means it can restore accelerometer measurements even with transients on measurements.

Even thought it’s a great filter, the Kalman filter is very complex to implement and requires a lot of CPU usage, which isn’t good for very high sample rate systems. In contrast, the Moving Average Filter is perhaps too simple. Its calculation is very intuitive and doesn’t require high CPU usage. Despite its simplicity, the Moving Average Filter works well for acceleration measurement filtering and can be easily implemented and monitored. With this project, the Moving Average Filter results were satisfactory and the performance was very good, so it’s a good choice. Figure 5a and Figure 5b show how effective all the signal handling applied has been so far for X- and Y-axis. Raw measurements are represented in blue and handled measurements are represented in red.

SPEED CALCULATION

Finally, after all the DSP steps we’ve discussed so far are done, the accelerometer data is now reliable for further calculations. To calculate speed based on accelerometer measurements, the formulas shown in Equations 3, 4 and 5 are required. The initial speed is defined as zero—naturally, because when powered on, the robot will be standing still.

Considering two consecutive acceleration measurements acquired, consider these formulas to calculate speed based on acceleration in two consecutive times (t0 and t1):

 (6)

 (7)

 (8)

 (9)

Therefore, the speed calculation (for each axis) can assume one of these possibilities:

Ascending acceleration (acc(t0) < acc(t1)): Equation 6

Descending acceleration (acc(t0) > acc(t1)): Equation 7

Both accelerations measurements are the same (acc(t0) = acc(t1)): Equation 8

It’s important to remember that all these calculations must be made on every acceleration sample acquired. Therefore, following the calculations above, it’s possible to determine the instant speed in X-axis and Y-axis. Here, we’re assuming the Z-axis is measuring gravity acceleration, which isn’t relevant on this project. As these instant speeds are modules of X- and Y-axis velocity, the module of the resulting velocity can be determined as shown in Equation 9.

Even with all the care and handling with DSP so far, something else can still ruin speed estimation: accelerometer hysteresis. Unfortunately, acceleration measurements hardly get back to zero acceleration when accelerometers are submitted to a cycle of increasing and decreasing accelerations and that shows clearly as hysteresis.

The consequences of ignoring this behavior are bad. Because speed is calculated strictly based on accelerometer data, the residual acceleration effect will cause significant errors on speed calculation—mainly when the accelerometer is standing still (no-movement condition). In this case, this residual acceleration will be part of an integration calculus of speed, so each residual acceleration measurement will increase the calculated speed a little bit. For better understanding, observe Equations 1 and 2. Therefore, even with the accelerometer standing still, there are plenty of chances of speed being above zero and getting higher and higher continuously.

To handle accelerometer hysteresis, a very simple technique can be applied: the movement end check. This technique consists of counting how many times acceleration (in each axis) is equal to zero and, if this counter reaches a predetermined limit, the speed of forced to zero. The code in Listing 2 (extracted from the current project’s code) implements the movement check end feature.

#define THRESHOLD_MOVEMENT_END_CHECK         25   

...

void refresh_movement_end_check_counter(float acc_t0, float acc_t1, unsigned int * ptrCounter)
{
        if ( (acc_t0 == 0.0) && (acc_t1 == 0.0) )
        {
          *ptrCounter = (*ptrCounter) + 1;

                if (*ptrCounter == UINT_MAX)
                    *ptrCounter = THRESHOLD_MOVEMENT_END_CHECK;
        }
       else
         *ptrCounter = 0;
}


...

void instant_speed_calculation(float accX_t0, float accY_t0, float accX_t1, float accY_t1)
{
     ...

     refresh_movement_end_check_counter(accX_t0, accX_t1, &accx_movement_end_check_counter);
     refresh_movement_end_check_counter(accY_t0, accY_t1, &accy_movement_end_check_counter);

      instant_speed_x_axis = 0.0;
    if (accx_movement_end_check_counter < THRESHOLD_MOVEMENT_END_CHECK)
         instant_speed_x_axis = speed_x_axis + calculate_delta_speed(accX_t0, accX_t1, (float)SAMPLE_RATE_ACC_HZ);

    instant_speed_y_axis = 0.0;
     if (accy_movement_end_check_counter < THRESHOLD_MOVEMENT_END_CHECK)
          instant_speed_y_axis = speed_y_axis + calculate_delta_speed(accY_t0, accY_t1, (float)SAMPLE_RATE_ACC_HZ);     
     
     speed_y_axis = instant_speed_x_axis;
     speed_y_axis = instant_speed_y_axis;
     
    ....
}

So now all the DSP techniques and approaches to estimate speed using accelerometer data are known and reliable speed estimated data can be calculated. But in what order must these DSP concepts be applied? First of all, before using any DSP techniques, the accelerometer must be configured correctly. Therefore, considering the native IMU from BeagleBone Blue board, the configurations (and values) must be set as shown in Table 2.

Once the IMU accelerometer is configured, now accelerometer calibration must be made. Reminder: for calibration ONLY, accelerometer samples must be acquired in normal (no-DMP) mode. After accelerometer calibration, the acceleration sampling begins (in DMP mode). Before any action on DSP, the moving average filter buffer must be filled. Finally, after this moving average buffer filling, the full DSP handling starts. For every sample acquired from now, the following steps must be done:

1. The acquired acceleration sample is applied to a discrimination window, resulting in a sample called “window discriminated sample.”

2. The window discriminated sample is applied to a moving average filter, resulting in a sample called “filtered sample.”

3. After two filtered samples, it’s time for speed calculation.

4. To avoid accelerometer hysteresis side-effects on speed calculation, the “movement end check” technique must be applied.

Important: these steps must be applied to X- and Y-axis accelerations simultaneously.

SPEED CONTROL – PID

Once speed is calculated, it can be controlled. To control speed, one of the best choices is using PID control—a widely used control solution in automation and industrial systems. PID control allows short-time and smooth reaching to a desired value of a variable—in this case, speed. This means that, even if a robot’s mass varies during its movement and/or pavement conditions change, its speed will reach the desired value. It’s like having someone controlling a robot’s gas pedal to keep constant speed.

As shown in Equations 4 and 5, PID control consists of a combination of three distinct terms: a proportional term (gain: K_P), an integral term (gain: K_I) and a derivative term (gain: K_D). Simply put, a PID controller works by continuously minimizing the error: process variable (what you want to monitor / want to control) minus setpoint (process variable desired value). In few words, each term of PID has a distinct purpose on PID control action, as follows:

Proportional term: This term is responsible for approximate the process variable from setpoint value quickly. The gain of proportional must be big enough to ensure quick approximation to setpoint value and precise enough to ensure the process variable doesn’t go too far from set point.

Integral term: This term is responsible for minimizing the error produced by proportional term. The gain of integral should be much smaller than proportional term gain, otherwise the time for process variable achieve setpoint value will be very large.

Derivative term: This term is the most peculiar one. Its purpose is related to minimize error in process variables that has highly increase or decrease over time. Also, derivative term can reduce overshoot (overshoot: when a process variable goes too far from setpoint and has a big delay to get next to it again) produced by integral term. The gain of derivative term must be empiric set in most of cases.

Like any control technique, PID control has some interesting practical “tricks.” When applying PID control you normalize the process variable when process variable controller allows normalized values. This is a good trick because it allows direct application of the PID final result to the controller. I adopted this trick is in this project, making the PID final result the input for the motor PWM controller.

At the end of the PID value calculation, you should check if its value is above the maximum allowed. Mainly on the first PID calculations (when process variable is far from setpoint value), the PID result may assume values that are too large, so this verification ensures valid results of PID controller.

While you might not expect so, the calculation of a PID term in a computer program isn’t that complex to code. All you need as inputs are the current error and previous error results. The code in Listing 3 (extracted of the current project’s code) shows how to code a PID control in C language.

...

typedef struct {
    float setpoint_pid;
    float last_error;
    float integral_acum;
}TPIDController;

static TPIDController PIDController;

...

float calc_PID(float process_variable, float time_step)
{
    float current_error;
    float p_factor;
    float i_factor;
    float d_factor;
    float pid_final_result;

    current_error = PIDController.setpoint_pid - process_variable;
    p_factor = KP_CONSTANT * current_error;

    i_factor = PIDController.integral_acum + (KI_CONSTANT * current_error * time_step);
    PIDController.integral_acum = i_factor;

    d_factor = KD_CONSTANT * ( (PIDController.last_error - current_error) / time_step );
    PIDController.last_error = current_error;

    pid_final_result = p_factor + i_factor + f_factor;
    
    if (pid_final_result > PID_MAX_LIMIT)
      pid_final_result = PID_MAX_LIMIT;

    if (pid_final_result < PID_MIN_LIMIT)
      pid_final_result = PID_MIN_LIMIT;

    return pid_final_result;
}

WHAT’S NEXT?

By now, doing speed estimation using accelerometer data and speed control using PID concepts should be clear in your mind. To directly apply these concepts and bring them to the real world, we’ll tackle an interesting project in the second part this article’s series: an autopilot electrical robot.

In this article, we presented (and solved) a challenging task that every embedded systems designer will face at least once when designing embedded systems projects: How to get reliable data from inertial sensor measurements—in particular, an accelerometer—and how to do speed estimation using the reliable accelerometer data. This task requires non-intuitive DSP techniques and mathematical approaches that must be carefully analyzed in order to get relevant accelerometers samples in real-world situations. We also discussed briefly how PID control works in speed control and provided some tips on using PID control.

By understanding these two concepts—speed estimation and speed control—we’ve laid the groundwork for building an autopilot electrical robot. In part 2 of this article series, we’ll dive into the actual building of this robot.” 

Read Part 2 Here

RESOURCES

Github repository: https://github.com/phfbertoleti/AutoPilot_Project

Software used in this project:
Operating System: Debian Linux
Programming language: C
Compiler: GCC
Sensors and drivers API: Robotics Cape
Additional libraries: Paho MQTT

Freescale/NXP, “Application Note: AN3397 – Implementing Positioning Algorithms Using Accelerometers”, 2007
Eclipse Paho MQTT official website
Robotics Cape API (Strawson Design) 
James Stewart, “Calculus: Early Transcendentals (6th Edition)”, Cengage Learning, 2006
David Halliday. Robert Resnick and Jearl Walker, “Fundamentals of Physics (10th edition)”, Wiley, 2013.

Beagleboard.org | www.beagleboard.org
Eclipse.org | www.eclipse.org

PUBLISHED IN CIRCUIT CELLAR MAGAZINE • OCTOBER 2018 #339 – Get a PDF of the issue