Control Theory - SoonerRobotics/SCR-Resources GitHub Wiki

PID Control

Applications: Motion Control, Optimization, Error Reduction
**Difficulty: **Easy / Intermediate

Overview

PID stands for Proportional, Integral, Derivative and is a common feedback controller used to control robots.

The general idea behind a PID is that you can construct an error curve to determine how to best optimize a system (and reduce error). Each component of the PID acts differently on the error curve to optimize the system. In practice, variations of the PID controller are used, such as: the P controller (purely proportional) and the PD controller (no integral component). In reality, the PID controller is just a general case of these specific controllers.

Use in Robotics / SCR

The PID controller is used to reduce control error in robotics. One good example implementation of PID is making a robot drive a certain distance.

Equations

PID.png

Image made by Justin Kleiber

The PID controller can be represented easily in seven simple equations. Here is what each of them do:

  1. We establish the error equation. In our example, error is the difference between where the robot is at time t, and where you want it to be
  2. We define the time step, which is the current time minus the last time the PID ran. This should be in seconds
  3. Proportional Control: The output of this equation is linearly related to how much error there is. If the robot is 100 meters away - lots of output compared to 1 centimeter away. This also has our first tuning constant, Kp, which we will talk about later
  4. Integral Control: The output here is linearly related to the cumulative error of the system since it started. This can introduce jitter into the output. Also we see our second tuning constant Ki.
  5. We define the integral of the error curve to be the trapezoidal Riemann sum approximation. We store the past value of the integration step in a variable and calculate only the current step in order to make the PID run faster
  6. Derivative Control: The output is linearly related to the derivative. This should be the rate at which the error is decaying or reducing in most cases. This dampens the output. The third and final tuning constant, Kd, lives here.
  7. We can approximate the derivative as the slope between the current error and the last measured error. On initialization, this will be really high unless you initialize the PID with a starting value, so make sure to do that.
  8. The output of the PID controller is the sum of equations 3, 4 and 6

Tuning

My personal tuning strategy for any PID is to start with a P controller (Ki = 0, Kd = 0). I normally set Kp = 0.5 or something else depending on the application. If the system overshoots the target with the P controller, I reduce the Kp variable, otherwise I increase it. Once the system is coming up just a bit short of the target, I add in Ki to make a PI controller.

Since Ki adds a lot of volatility, I normally start really small (Ki = 0.001). I adjust Ki up to make the system hit the target if it is short, otherwise I reduce it. At a certain point, Ki will introduce too much jitter and should be turned down. Ideally, the Ki can be tuned such that the PI controller is shakily getting to the target state. To remove the shaking, add dampening:

I also add Kd in a small quantity (Kd = 0.002). Increase Kd to dampen the control more, and decrease to add jitter or overshoot back in more. Adding too much Kd will make the controller undershoot - and then you'll need to reduce it.

In general, don't make the constants negative.

Calculating the Constants

You are actually supposed to be able to calculate the constants when using PID. Once I learn how to do that, I'll update this. It is taught in ECE 4413: Intro to Control Systems.

The reason these constants need to be calculated is because PIDs are used on safety critical systems like airplanes. You can't just fly the airplane into the sky and randomly initialize PID constants and tune it until the flight is smooth. The PID control would be too jittery (the plane banks super hard one way and then the other, destabilizing the flight) or the control wouldn't be enough (the plane doesn't turn far enough and runs into a mountain). This is not desirable at all.

That being said, our robots can be tuned by trial and error, although if that can be avoided it saves testing time

Caveats

  • PID controllers need to have a reset function. This should set the cumulative error variable to 0 so the error of previous control doesn't disrupt future controls.
  • The PID should be initialize with a start time and start state in order to do derivatives correctly
  • PID does not guarantee optimal control, but it does do pretty well with good tuning.
  • In some cases, cumulative error is too powerful, and it is better to set Ki = 0 and go with a PD controller.

Further Reading

Extended Kalman Filter

**Applications: **Sensor Fusion, Localization, Signal Processing
Difficulty: Hard

Overview

The idea behind an (Extended) Kalman Filter is that a system's can be modeled through mathematical equations, which are verified by measurement feedback. In this way, an Extended Kalman Filter is a feedforward and feedback system used to estimate the state of a system.

Note: There is a difference between a Kalman Filter and an Extended Kalman Filter. The extended version can handle non linear applications (such as turning) while the regular Kalman Filter cannot handle anything other than linear control applications. In SCR we mainly use the Extended Kalman Filter for this reason

How it is used in Robotics / SCR

SCR uses Extended Kalman Filters (EKFs) to automatically determine what location our robot is in (localization). The EKF first models the motion of the robot using past data such as the last known velocity, acceleration, and position. This is called the motion model - the step that uses math (Dynamics/Kinematics) to predict future values from past values (the feedforward component of the filter).

With the motion model producing a prediction of the robot's state, the robot's sensors are then read to determine what the robot observes as it's position. Sensors such as GPS, IMUs, and encoders may be used to find the location, velocity and acceleration of the robot. These sensor values are called measurements, and compose what we call the measurement model, or the feedback component of the filter. These sensor values are 'noisy' - a typical GPS sensor will have accuracy down to 5 meters. That means a normal GPS, if performing optimally in an environment with little interference will could still be off by quite a bit. That's where the Kalman gain comes in.

The Kalman gain is a value computed by the filter that determines how reliable each model is. Generally there is an uncertainty matrix for the motion model and an innovation metric for the measurement model (which determines how much new information we have in the measurement step). These things are considered when computing gain.

With gain computed, the EKF can choose how much to value each model. Once the two models are balanced, then the EKF can estimate the robot's location and update its uncertainty matrix.

Linearization of Non-linear data

In order to handle non-linear models, the EKF utilizes Jacobians to linearize the data it is given. Put simply, a Jacobian is a matrix of partial derivatives relating how each variable depends on every other variable. You can read more about computing Jacobians on Wikipedia: Jacobian matrix and determinant - Wikipedia

jacobian.svg

Image from Wikipedia

Equations

Note: all things with hats (the "^" symbol on top of the letter) indicate a prediction in the literature (sometimes a single letter is used to represent multiple things). You'll notice the x prediction for state k is formed from the k-1 state, and then is used in the update step.

kalman.png

Image screenshot from Wikipedia. Note: There are many different formulations of EKFs, and this is one of many.

The prediction state estimate uses an equation f(x, u), which is the kinematic equations that take a past state, x, and a control signal*, u*, and predict the next state.

The covariance matrix P is the uncertainty of the prediction

The innovation is the amount of new information gained from the measurement update. H(x) is used to determine the values of all sensors at the predicted state x. Generally, it is a safe bet to start off with the jacobian H as the identity matrix - for example predicted velocity generally corresponds to the measured velocity in a 1:1 fashion.

The Kalman Gain is the amount to trust the measurement model based on the motion model uncertainty, the observation jacobian H, and the noise of the sensors R. R is a diagonal square matrix with each value on the diagonal representing the noise of each sensor.

Tuning

The EKF is primarily tuned through adjusting the matrices H, Q, and R.

H, as mentioned above, is the matrix of linear relationships between the estimated state variables and the measurement values. This is determined mathematically, but in some cases it might make sense to approximate it and tune the values

Q is the process noise, which is the variance in the motion model's estimates. This is probably best to set as the identity matrix or another diagonal square matrix, and then tune the values in the diagonal based on the accuracy of the state predictions. The higher the values, the less certain you are about the motion model's accuracy

R is the observation noise - how inaccurate the sensors are. This can be estimated well at first from knowing the sensor tolerances and then tuned further. This is also normally a diagonal matrix. Higher values mean less accurate sensors

RoboMagellan EKF Application

The implementation of a Kalman Filter by RoboMagellan remains untested, but the code itself should give you a good idea of how to build a kalman filter: robomagellan-2019/EKF.cpp at ros · SoonerRobotics/robomagellan-2019 · GitHub

This was written in C++ using the Eigen linear algebra library. It is likely this model would have worked once tuned and tested, but the competition was cancelled. The motion model was based on the Ackermann steering model, and the EKF's equations were formulated from the Interactive Kalman Filter Tutorial below.

Further Readings and Additional Resources

⚠️ **GitHub.com Fallback** ⚠️