Robot control systems
A thorough explanation on controlling position and speed of actuated robot components
This document will not go over:
- Collecting data/noise rejection
- Using control effort to move actuators
- Automatic tuning
- Gain scheduling
Code implementation will not be a focus, but wording and some concepts will be structured similarly to the WPILib implementation. Note all graphs are not exact, they are used only to show the general behavior and shape of a response curve
Preface
Notation
\[u(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{d}{dt} e(t) \]The above diagram is called a block diagram. Block diagrams can come in many forms, and are used to describe how a system functions. Plant is the term for the system that generates measurements, usually the physical robot, but could be a simulator. This diagram is for a PID controller, we will go in depth about it soon, but for now, this is just an example of a block diagram. In code, most values are variables, but here, they are often described as functions of time (t).
The above graph is called a response graph of response curve, it represents a graph of position over time(sometimes velocity, if so, it will be stated). The dashed line represents the setpoint, and the solid line represents the measurement.
Note that all graphs in this document are simplified models, meant to portray the shape and behavior, not actual data.
Situation
Imagine a simple system of a slider attached to an actuator. Gravity and friction can be ignored. Let R be the desired position of the slider, and Y be the position of the slider. The goal is to generate a control effort U that reduces the error, \(R-Y\), to 0 as fast as possible without overshooting, moving the slider to the desired position.
Limits
\[1.0 \geq U \geq -1.0 \]Y cannot be changed except by U
PID
Idea
Back to the diagram:
We will go over each element of this diagram (P, I, and D) individually, then we will combine them to get the final controller. We will then go over tuning the controller. Here is the equation for a pid controller
\[u(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{d}{dt} e(t) \] \[\begin{align} t &- \text{time}\\ r(t) &- \text{setpoint}\\ e(t) &- \text{error}\\ u(t) &- \text{output}\\ y(t) &- \text{measurement} \end{align} \]The equations and diagram show the variables as functions of time, we will assume t(time) is always equal to the current time, reducing the functions into variables
P
The P(proportional) term of the controller is the simplest, \(R-Y\), which is equivalent to \(E\). Then it is multiplied by the P gain, \(K_p\). when clamped, it will form a response curve like this:
If \(K_p\) is too high, the system will overshoot, causing a curve more like this:
D
The D(derivative) term is similar to the P term, but instead uses \(\frac{dE}{dt}\) (means rate of change or derivative), again, multiplied by a gain, \(K_d\). the D section causes U to increase if Y is moving away from R, accounting for a setpoint that is moving away, and decreases U if Y is moving toward R, slowing Y and reducing overshoot. If \(K_d\) is too high by a small amount, it can overreact, causing a sudden stutter, as seen in the graph below
If \(K_d\) is too high by a lot, its corrections could be strong enough to cause an even stronger force in the other direction, resulting in intense oscillations that amplify over time.
An important thing to note is that the oscillations and abnormalities caused by improper \(K_d\) tend to be seen as Y is moving to R, whereas \(K_p\) oscillations are centered at the setpoint. Behavior like this is a big reason you should clear the area around the robot, and have one hand on the disable button.
I
The last part of the PID controller is the I(integral) term. The I term adds up all past values of E (notated as \(\int_0^t e(\tau) d\tau\)), and multiplies it by the \(K_i\) gain. The I term is used to resolve steady-state error, the phenomenon where Y is very close to R, but not within the accepted range, usually caused by friction or external forces. If in this state, the P term can’t overcome friction, and the D term will do nothing, since there is no relative motion. Below is a response graph of a system encountering steady state error, the shaded area represents the past values of E, up to the current time
Windup
After a large change in setpoint, the I term may increase to a large value before reaching the setpoint, causing overshoot similar to P term overshoot
I zone
The I zone is one solution to integrator windup, where the I term is disabled until \(E\) is within the defined zone
I range
I range limits the output of the I term to prevent saturation
Tuning
A too large \(K_i\) can cause effects similar to if \(K_p\) is too high, a small overshoot may be seen:
If \(K_i\) (or \(K_p\)) is very high, stable oscillations can be seen:
Or, if \(K_i\) (or \(K_p\)) is extremely high, oscillations will intensify over time:
Note that these oscillations are centered around the setpoint, as opposed to D term oscillations, which are centered before the setpoint.
The I term can reduce or eliminate steady state error, but can harm stability and cause problems easily. If steady state error becomes a problem, feedforward and motion profiling are recommended before \(K_i\) is used
strengths/Weaknesses of PID
PID controllers are very useful to control position or velocity of a mechanism, but cannot account for real world conditions and limits. A PID can correct itself for conditions, but cannot predict them, causing lag. A PID loop also has no control on the maximum speed or acceleration of the mechanism.
A controller can predict conditions such as friction and gravity using feedforward, and motion profiling will allow it to follow speed and acceleration limits. This is why feedforward and motion profiling are seen along with PID in most systems.
Tuning
While mathematical systems do exist to tune PID controllers, it tends to be simpler to tune the controller manually. Tuning manually follows the following steps
- Set \(P\), \(I\), and \(D\) to 0
- Increase \(P\) until overshoot
- Decrease \(P\) slightly
- Increase \(D\) until stuttering
- Decrease \(D\) slightly
- Repeat from 2 until \(P\) and \(D\) are changing very little
- If steady state error, tune \(I\), otherwise, Done
For tuning \(I\):
- Increase \(I\) until overshoot or oscillations
- Decrease \(I\) slightly
The main idea is to have gains as high as possible without any problematic behavior(overshoot, stutter, oscillations)
If the system is affected by gravity or has large friction, the optimal tune may not be very good.
Cascading
Cascading PID controllers is the process of using the output of one as the setpoint of another. It is not commonly used in FRC, but can be seen when abstracting a system. An example of this is a PID controlling the angle of a robot using a swerve drive. The modules each have PIDs for angle and speed, both are given setpoints generated by the kinematics, which generates the setpoints based of the desired angular speed. The robot angle PID sends outputs to be processed by the kinematics, to form inputs for the module PIDs.
FeedForward
Idea
PID control is very effective, but requires measurements that may not be obtainable. PID also is very bad at accounting for physical effects on the system such as friction, gravity, and inertia. Feedforward control doesnt require a measurement to generate a control effort. A feedforward controller used gains and a mathematical model to estimate the force output required to achieve the set velocity. It is important to note that feedforward uses velocity setpoints.
\[u(t) = K_p \text{sgn}(r(t)) + K_v r(t) + K_a \frac{d}{dt} r(t) \] \[u(t) = K_d + K_p \text{sgn}(r(t)) + K_v r(t) + K_a \frac{d}{dt} r(t) \] \[u(t) = K_d \cos(\theta) + K_p \text{sgn}(r(t)) + K_v r(t) + K_a \frac{d}{dt} r(t) \]G
The G term is the simplest, it is a constant \(K_g\) that is added to account for gravity. On mechanisms with arms, \(K_g\) is scaled by the cosine of the angle (0 is horizontal) of the arm to account for the reduced effect of gravity when the arm is at higher angles. \(K_g\) is optional and should be 0 if there is no effect due to gravity
S
The S term is designed to effectively remove static friction by applying a constant force, \(K_s\), in the direction of the desired velocity. The direction is obtained by the signum operator, denoted \(\text{sgn}(x)\). The signum returns 1 if the input is positive, -1 if it is negative, and 0 if it is 0.
V
The \(K_v\) term should remove dynamic friction when multiplied by the set speed. Resulting in the output required to maintain the set velocity. The resistive forces tend to be linear in FRC.
A
The A term accounts for inertia. \(K_a\) represents the output required to maintain acceleration, obtained by getting the derivative of the set speed. The friction relation to acceleration is also nearly linear in FRC.
Strengths/weaknesses
Feedforward accounts for known factors in a system and responds to them before an error accumulates. Feedforward also does not need a measurement(unless on an arm) to work, allowing basic control of mechanisms without sensors. Due the the operation based off of a model, feedforward gains can be approximated by online calculators, and systems like SysId can calculate the gains based off mechanism response to a sequence of outputs.
Feedforward cannot correct itself for unexpected changes, causing a buildup of error over time from disturbances and slightly nonoptimal gains.
Generating setpoints
Feedforward uses velocity setpoints instead of position setpoints, allowing easy control of systems like flywheels and speed control, but bad for position control. Generating the velocity setpoints for position control is a common problem that can be solved with motion profiling.
Motion profiling
Idea
Motion profiling is the process of generating setpoints in both speed and position to move a mechanism from a given position to another given position while following speed and acceleration limits. This is useful for PID, as it effectively limits speed and acceleration of a mechanism to adhere to physical limits. Speed limiting and more controlled deceleration both reduce overshoots and allow faster moving with more control. Motion profiling also generates velocity setpoints for feedforward controllers. When profiling and feedforward are used on a system, the measurement will always be near the setpoint, allowing more aggressive PID gains to speed up corrections without overshoot.
Velocity controllers
Differences
PID can be given velocity as input, and will function the same, friction will cause more steady-state error, feedforward is recommended. Feedforward control requires a velocity setpoint to function, so it will be very easy to implement. Position control for feedforward should use motion profiling for setpoint generation.
Combination
PIDF
Feedforward and pid can simply be added together to determine the control output.
This combination can be more stable and reduce or remove the need for \(K_i\), and allow higher accuracy. Note that the controllers may need to be re-tuned.
Profiling PID(F)
The generated setpoints(pos and vel) from the profiler can be sent directly to the controller, the ff will need a velocity setpoint.
Tuning
Tuning FF without profiling will not be covered, as profiling is necessary for position control.
PID order
\(P\)
\(D\)
\(I\)
FF order
\(S/G\)
\(V\)
\(A\)
Profiled PIDF order
Start low Speed
Start low Accel
\(S/G\)
\(V\)
\(A\)
\(P\)
\(D\)
\(I\)
Increase speed
Increase accel
Repeat from \(K_v\)
See also
“Proportional–Integral–Derivative Controller.” Wikipedia, 30 Apr. 2025. Wikipedia, https://en.wikipedia.org/w/index.php?title=Proportional%E2%80%93integral%E2%80%93derivative\_controller\&oldid=1288158994 .
Interactive charts built with Recharts