Posts Tagged ‘ uav ’

PID Controller in MATLAB

I’ve had several people ask me for a MATLAB implementation of a PID controller. I took my previous PID controller post and ported it to MATLAB. Because MATLAB is not designed for software like this, I made a single PID instance code set. In other words, the following code only represents one controller.

Here is the code for the update function (you must place it in a file named pid_update.m)

function pid_update(curr_error, dt)
    global windupGuard;
    global proportional_gain;
    global integral_gain;
    global derivative_gain;
    global prev_error;
    global int_error;
    global control;

    % integration
    int_error = int_error + (curr_error * dt);

    % integration windup guarding
    if (int_error < -(windupGuard))
         int_error = -(windupGuard);
    elseif (int_error > windupGuard)
         int_error = windupGuard;
    end

    % differentiation
    diff = ((curr_error - prev_error) / dt);

    % scaling
    p_term = (proportional_gain * curr_error);
    i_term = (integral_gain     * int_error);
    d_term = (derivative_gain   * diff);

    % summation of terms
    control = p_term + i_term + d_term;

    % save current error as previous error for next iteration
    prev_error = curr_error;

Here is some VERY basic code just used to call the update function once.

global windupGuard;
global proportional_gain;
global integral_gain;
global derivative_gain;
global prev_error;
global int_error;
global control;

% set these as needed
windupGuard = 10.0;
proportional_gain = 4.0;
integral_gain = 5.0;
derivative_gain = 3.0;

% this is the zeroize function
prev_error = 0.0;
int_error = 0.0;

% call update function
pid_update(19, 1.0)

Here’s a question for those of you here reading this: Why on earth are you using MATLAB for a PID controller?

Advertisements

Proportional-Integral-Derivative (PID) Controller

PID Intro:

The heart of any real control system has a feedback controller.  When the system’s process is unknown or hard to model, a Proportional-Integral-Derivative (PID) controller is an efficient method for control.  If the system’s process is known, a custom controller will yield higher efficiency.  However, modeling complex systems in attempt to design a custom controller may result in a much higher development time and cost than can be afforded.  In both cases, a PID controller is a very efficient method for controlling all types of systems.

thanks to wikipedia.org for the diagram

PID Theory:

PID controllers can be understood(partially) without an in depth lecture on control theory. PID controllers use 3 sub-controllers combined into 1 controller using a simple sum. The 3 controllers are:

  • Proportional:
    The proportional section of a PID controller is a basic intuitive approach to feedback control. A naive approach to feedback control would say, the farther away from perfect the system is, the more it should work to get perfect. In a perfect world without friction, momentum, etc., this system alone would work great! This is proportional control. However, our world is imperfect and we need to add smarter feedback compensation.
  • Integral:
    The integral section of a PID controller compensates environment imperfections such as friction, wind, and other such imperfections that would resist the system to reach its perfect state. An integral controller works by keeping a sum of all the error the system has seen. In calculus, this is equivalent to the area underneath the curve up to the current point. The controller increases its control signal as the summed error gets larger.
  • Derivative:
    The derivative section of a PID controller compensates environment imperfections such as momentum which causes the system to overshoot its perfect state. The derivative controller lessens its control signal as the speed in which it is achieving its perfect state increases. In calculus, this is the slope of the error signal.

Example:

Let’s consider an example to show how a PID controller could be used. Our example will be an airborne GPS navigation system in a helicopter. The aircraft knows where it is and where it is being told to be. Using basic subtraction, it can figure out the distance to the desired location. This is the error signal. It indicates how far from perfect it is.

Lets say that the PID controller is controlling the forward thrust of the helicopter and that the direction is automatically set by something else (in other words, the helicopter always points the direction it should).

The proportional controller would just set the thrust according to the distance away from the target location. This is a simple approach but most likely will not work by itself. As the helicopter gains momentum, it will become harder to slow down. Using proportional control only, the helicopter will overshoot its target and possibly oscillate around the target location. Another issue is wind. The thrust controller should compensate for wind because a tail wind could cause an positional overshoot and a head wind might cause it to never get there.

The integral controller continues to sum the error it incurs and adjusts appropriately. In the head wind example, the thrust would increase until the aircraft could reach its target destination.

The derivative controller measures the speed in which the error signal is changing, which for the helicopter is simply the relative ground velocity. As the helicopter approaches its destination the derivative controller reduces the thrust allowing the helicopter not to overshoot its target position. In the event of tail wind, it reduces the thurst even more so that it can’t be pushed past the target.

For the helicopter example, all 3 sub-controllers must be used. To use all of them, you just sum the control signals of all 3.

Implementation:

If you think this sounds hard to implement, you are wrong. PID controllers were designed to be generic and easily adapted to all systems. As mentioned before, PID controllers have 3 sub-controllers. Each controller has a parameter that must be tuned, called ‘gain’. To add a PID controller to a system, you just need to attach the generic PID controller and tune the 3 values.

The following C code is an efficient PID controller.

typedef struct {
    double windup_guard;
    double proportional_gain;
    double integral_gain;
    double derivative_gain;
    double prev_error;
    double int_error;
    double control;
} PID;

void pid_zeroize(PID* pid) {
    // set prev and integrated error to zero
    pid->prev_error = 0;
    pid->int_error = 0;
}

void pid_update(PID* pid, double curr_error, double dt) {
    double diff;
    double p_term;
    double i_term;
    double d_term;

    // integration with windup guarding
    pid->int_error += (curr_error * dt);
    if (pid->int_error < -(pid->windup_guard))
        pid->int_error = -(pid->windup_guard);
    else if (pid->int_error > pid->windup_guard)
        pid->int_error = pid->windup_guard;

    // differentiation
    diff = ((curr_error - pid->prev_error) / dt);

    // scaling
    p_term = (pid->proportional_gain * curr_error);
    i_term = (pid->integral_gain     * pid->int_error);
    d_term = (pid->derivative_gain   * diff);

    // summation of terms
    pid->control = p_term + i_term + d_term;

    // save current error as previous error for next iteration
    pid->prev_error = curr_error;
}

Windup Guard:

As you may have noticed, the code has a feature called windup guard. This is a critical feature that must be used in most control systems. Windup guarding is simply just setting a cap on the maximum value that the integrated error can be. This is typically most needed for startup conditions and situations for switching in and out of control. Let’s look at our helicopter example again. Consider the situation where the helicopter can be piloted by a person or the PID autonomous controller. If the human operator had a target destination only a few feet away from the current position but held the helicopter still, the integral portion of the PID controller would continue to sum the error seen. Eventually this error would grow very large. If the pilot then switched to autonomous mode, the controller’s output thrust signal would be huge, most likely causing a wreck. There are two solutions to this problem. The first is using a cap for the maximum absolute value of the integrated value. This is the windup guard. Another solution would be to call the pid_zeroize function each time the GPS target is set and each time the autonomous control system is enabled. In either case, setting a safe maximum value using a windup guard is good practice.

Optimization:

This code can be optimized if the pid_update function is called at the exact same rate every time. In this case, all references to ‘dt’ can be removed. This will change the optimal values for the integral and derivative gains but once tuned, will respond the same way. In this code, this optimization only removes one multiplication operation and one division operation.
It is possible to change this code to run on fixed-point arithmetic rather than floating-point. This dramatically increases the efficiency but system limits and precision may be compromised. Only use fixed-point if you are sure that you are using enough precision.

EDIT: fixed bug on line 31. changed multiply to divide.

RC Receiver Interface

There are numerous RC Receiver interfacing techniques published on the web.  My quadcopter design is using only one processor for all its computation.  For this reason, I spent extra time designing the interfaces to be as efficient as possible.  Even though RC Receiver interfaces aren’t super complicated, they can cause real-time scheduling issues because of the number of interrupts and the time spent inside these interrupt routines.

Typical RC Receivers output the channel values in a given sequence of pulse width modulated (PWM) signals.  The transmitters and receivers I have tested are Spektrum made.  All Spektrum models seem to have the same design style.  After a small bit of testing, I found that the several channels of signals are output in the same order everytime and have a small time separation.  Since the standard for RC PWM is 50Hz, each pulse comes every 20 milliseconds.  The top 5 lines of the picture above show an example output of a 5 channel Spektrum made receiver.

In a microcontroller, the most accurate way to measure incoming pulses is to use an input capture with a timer.  Measurement occurs when a free running timer value is latched into a register when an event occurs.  Obviously, a higher frequency timer yields a more precise pulse measuring system.  Since most microcontrollers only have a few input capture pins, some special circuitry must be used to measure several pulses.  Since the Spektrum systems do not overlap their output pulses, a simple OR gate structure can be used to combine all the signals into one signal.

Since my quadcopter will be using the Spektrum DX5e and AR500, I’m using a quad 2-input OR gate IC (MC14071BCP), to create this combined signal.  As shown in the timing diagram at the top, there are still 2 voltage transistions per pulse.  This results in 10 interrupts for this design style.  For synchronization purposes, there is always a time between the last pulse and the beginning of the first pulse that is longer than the longest possible pulse.  To capture the values sent by the transmitter, the software is setup for an interrupt to occur on every transition of the signal.  All of the interrupts, except the last one, have a very small amount of time to process.  The last interrupt copies out the five pulse width values and triggers an availability flag so that the rest of the software can have access to it.

An interesting thing that I noticed while testing is that Spektrum has a constant delay of 60 microseconds between each pulse.  To make the system even more efficient, turning off the falling edge interrupts for the first four pulses then back on for the last interrupt results in 4 less interrupts.  For the first 4 pulse width values, 60 microseconds would just be subtracted off.

For my system, this OR gate structure serves another purpose.  The AR500 receiver runs on 5 volts, however, the microcontroller (LPC1768) runs on 3.3 volts.  Of course there are many ways for converting one to another but the MC14071BCP IC has a wide voltage range and is tolerant to 5 volt inputs when running at 3.3 volts.  Without extra circuitry, it will convert the five 5 volt signals from the AR500 into one 3.3 volt signal that represents all five channels.

In conclusion, I’ve found that using a simple OR gate IC is very efficient for pin usage, timer utilization, voltage translation, and interrupt time efficiency.

Update:

I implemented this RC receiver interface on the LPC1768 connected to a AR500.  My results are accurate to about 60-70ns.  This leads me to believe that the AR500 is running on the typical ATmega8 (or 168 or 328) microcontroller running at 16MHz (1/16MHz = 62.5 ns).

Update:

Due to popular request, here is the code I used on the LPC1768 (right-click download, then change the “.pdf” extension to “.zip”):
https://nicisdigital.files.wordpress.com/2013/09/rc_receiver_interface-zip.pdf

Quadcopter Software Design

Before I begin discussing the quadcopter design, here is a diagram that shows the high-level software structure for my design:

All future software design posts will reference to this diagram.  Here is a short explanation of the necessity and function of each block:

PWM Decoder Driver:
In order to interpret flight commands from a standard RC Transmitter/Receiver, any multi-rotor design must have some sort of PWM decoder.  My decoder will be expecting a single signal containing all PWM channels (details here).  The decoder driver will translate the several channels of pulse widths into values to be interpreted by the command translator.

Command Translator:
Depending on the flight mode, the PWM values given by the transmitter are interpreted differently.  The command translator determines which mode the user is flying in and translates the commands accordingly.

I2C Master:
The inertial sensors I’ve chosen to use all run on the I2C protocol.  This high-speed protocol will be a great interface to use because of its addressing scheme and its interrupt time efficiency.  The I2C protocol is more complicated that most serial protocols but the LPC1768 has built-in hardware to handle the physical layer.  The I2C interrupt scheme for this hardware is very efficient with each interrupt needing only a few lines of code.

Sensor Fusion:
As mentioned in an earlier post, I will be implementing several sensor fusion algorithms to find out which has the best performance.  The first one I will implement is the Extended Kalman Filter (EKF).  The EKF algorithm is widely known to be one of the best methods for state (in my case aircraft attitude) estimation.  This filter will take the sensor readings from the various sensors and output an estimation of the current aircraft attitude.  For my initial design it will be important to measure:

  • Roll – Side to side angle relative to the horizon
  • Pitch – Front to back angle relative to the horizon
  • Roll Rate – Side to side rotational rate relative to the horizon
  • Pitch Rate – Front to back rotational rate relative to the horizon
  • Yaw Rate – Horizontal rotational rate relative to magnetic heading
  • Magnetic Heading – Direction relative to magnetic north

Proportional-Integral-Derivative (PID) Stabilization:
Once the desired aircraft attitude is determined from the transmitter commands and the actual aircraft attitude is determined from the sensor fusion algorithm, PID controllers are used to determine the fastest way to make the desired attitude become the actual attitude.  PID controllers are very efficient for control of a system in which an accurate physical model is unknown.  Using calculus to determine error slopes and areas, PID controllers compensate for environmental noise and disturbances while overcoming steady state error and oscillations.  PID controllers are VERY simple to design and code but are VERY hard to tune and calibrate.  (see this post)

Multi-Rotor Throttle Mixing (MRTM):
Standard helicopters use Cyclic Collective Pitch Mixing (CCPM) to adjust the aircraft attitude.  It works by adjusting the pitch of the blades depending on their current angular position.  I came up with term MRTM, as I haven’t found a term defined for multi-rotor helicopters.  MRTM works by adjusting the speed of several propellers in such a way that results in the same aircraft control and a typical helicopter (a post will soon follow that explains this in detail).  MRTM can be used to control the attitude of many styles of multi-rotor helicopters.

PWM Encoder Driver:
Once everything has been computed, the PWM Encoder takes the control values and generates a pulse width modulated (PWM) output for each motor.  This is the exact opposite process of the PWM Decoder.  The LPC1768 has good support for PWM output.  I’ll be able to output all channels of PWM without using any interrupts or processing time.  Can’t beat that!

Impressive Multi-rotor Designs

Fast Agile Quadcopter

MikroKopter HexaKopter (6 Propellers)

GRASP Lab (University of Pennsylvania)

ETH Flying Machine Arena (Quadcopter Ping Pong)

Shrediquette (FPV Flying in Africa)

Quadcopter Doing Flips (KapteinKUK)