Velocity PID(F) Best Practices - To Integrate, or Not To Integrate?

[calcmogul]

I'm the person who implemented the velocity PID controller in WPILib last summer. If I understand correctly, an integrator is necessary and sufficient to eliminate steady-state error in controlling a DC motor with a velocity PID controller in all cases. Here's some math:

I'll be using the model for a DC motor from http://ctms.engin.umich.edu/CTMS/ind...stemModelin g, which has the following transfer function from voltage (V) to angular velocity (theta_dot):

P(s) = theta_dot(s)/V(s)=K/((Js+b)(Ls+R)+K^2)

First, we'll try controlling it with a P controller defined as:

C(s) = Kp

When these are in unity feedback, the transfer function from the input voltage to the error is:

E(s)/V(s) = 1/(1+C(s)P(s))
E(s) = 1/(1+C(s)P(s))*V(s)
= 1/(1+Kp*K/((Js+b)(Ls+R)+K^2))*V(s)

(See http://ctms.engin.umich.edu/CTMS/Con...motorBlock.png for a diagram. I'm assuming that theta_dot_ref maps to the input voltage with a constant gain and that the measured signal is used as-is in the error calculation, so H(s) = 1).

In this case, we'll apply a step input, so V(s) = 1/s.

E(s) = 1/(1+Kp*K/((Js+b)(Ls+R)+K^2))*1/s

To find the steady-state value of a transfer function, we can apply the formula:

e_ss = lim_s->0 s*E(s)

e_ss = lim_s->0 s*1/(1+Kp*K/((Js+b)(Ls+R)+K^2))*1/s
= lim_s->0 1/(1+Kp*K/((Js+b)(Ls+R)+K^2))
= 1/(1+Kp*K/((0+b)(0+R)+K^2))
= 1/(1+Kp*K/(bR+K^2))

Notice that the steady-state error is non-zero. To fix this, an integrator must be included in the controller:

C(s) = Kp+Ki/s

Same steady-state calculations as before with the new controller:

E(s) = 1/(1+C(s)P(s))*V(s)
= 1/(1+(Kp+Ki/s)*K/((Js+b)(Ls+R)+K^2))*1/s

e_ss = lim_s->0 s(1/(1+(Kp+Ki/s)*K/((Js+b)(Ls+R)+K^2))*1/s)
= lim_s->0 1/(1+(Kp+Ki/s)*K/((Js+b)(Ls+R)+K^2))

multiplying by s/s:

= lim_s->0 s/(s+(Kp*s+Ki)*K/((Js+b)(Ls+R)+K^2))
= 0/(0+(0+Ki)*K/((0+b)(0+R)+K^2))
= 0/(Ki*K/(bR+K^2))

the denominator is non-zero, so e_ss = 0.

So mathematically speaking, an integrator is required to eliminate steady-state error in all cases for this model. If you don't want to use one, a feedforward can eliminate most if not all of the error if chosen carefully. Given that, there is always uncertainty, hence why feedback control exists.

Oblarg is correct that the behavior of the velocity PID tuning constants doesn't match the displacement PID. I tried to get something with close enough dynamics that teams could use a similar tuning procedure for both (except in the velocity case, there's no steady-state error so I made Ki not do anything).

[AdamHeard]

Originally Posted by calcmogul

So mathematically speaking, an integrator is required to eliminate steady-state error in all cases for this model. If you don't want to use one, a feedforward can eliminate most if not all of the error if chosen carefully. Given that, there is always uncertainty, hence why feedback control exists.

In the practical sense most teams are just looking for consistent RPM within a tight band near the setpoint.

If the system inertia is large enough compared to the motor power and the control frequency high enough you'll get high frequency oscillations of low magnitude around the setpoint (as Jared and Oblarg detailed above).

So, you need an integrator to eliminate steady state error in a theoretical sense, but not necessarily in a practical sense depending on the system.

[Jared Russell]

Originally Posted by calcmogul

Oblarg is correct that the behavior of the velocity PID tuning constants doesn't match the displacement PID. I tried to get something with close enough dynamics that teams could use a similar tuning procedure for both (except in the velocity case, there's no steady-state error so I made Ki not do anything).

This is totally reasonable, and in practice you need an integrator if your loop rate is slow or you are concerned about the effects of rapid oscillation on the lifetime of your mechanical components.

Originally Posted by calcmogul

e_ss = 1/(1+Kp*K/(bR+K^2))

e_ss approaches zero as Kp approaches infinity, and the closed-loop transfer function is stable for all positive values of Kp.

[Oblarg]

Originally Posted by calcmogul

I'm the person who implemented the velocity PID controller in WPILib last summer. If I understand correctly, an integrator is necessary and sufficient to eliminate steady-state error in controlling a DC motor with a velocity PID controller in all cases. Here's some math:

I'll be using the model for a DC motor from http://ctms.engin.umich.edu/CTMS/ind...stemModelin g, which has the following transfer function from voltage (V) to angular velocity (theta_dot):

P(s) = theta_dot(s)/V(s)=K/((Js+b)(Ls+R)+K^2)

First, we'll try controlling it with a P controller defined as:

C(s) = Kp

When these are in unity feedback, the transfer function from the input voltage to the error is:

E(s)/V(s) = 1/(1+C(s)P(s))
E(s) = 1/(1+C(s)P(s))*V(s)
= 1/(1+Kp*K/((Js+b)(Ls+R)+K^2))*V(s)

(See http://ctms.engin.umich.edu/CTMS/Con...motorBlock.png for a diagram. I'm assuming that theta_dot_ref maps to the input voltage with a constant gain and that the measured signal is used as-is in the error calculation, so H(s) = 1).

In this case, we'll apply a step input, so V(s) = 1/s.

E(s) = 1/(1+Kp*K/((Js+b)(Ls+R)+K^2))*1/s

To find the steady-state value of a transfer function, we can apply the formula:

e_ss = lim_s->0 s*E(s)

e_ss = lim_s->0 s*1/(1+Kp*K/((Js+b)(Ls+R)+K^2))*1/s
= lim_s->0 1/(1+Kp*K/((Js+b)(Ls+R)+K^2))
= 1/(1+Kp*K/((0+b)(0+R)+K^2))
= 1/(1+Kp*K/(bR+K^2))

Notice that the steady-state error is non-zero. To fix this, an integrator must be included in the controller:

C(s) = Kp+Ki/s

Same steady-state calculations as before with the new controller:

E(s) = 1/(1+C(s)P(s))*V(s)
= 1/(1+(Kp+Ki/s)*K/((Js+b)(Ls+R)+K^2))*1/s

e_ss = lim_s->0 s(1/(1+(Kp+Ki/s)*K/((Js+b)(Ls+R)+K^2))*1/s)
= lim_s->0 1/(1+(Kp+Ki/s)*K/((Js+b)(Ls+R)+K^2))

multiplying by s/s:

= lim_s->0 s/(s+(Kp*s+Ki)*K/((Js+b)(Ls+R)+K^2))
= 0/(0+(0+Ki)*K/((0+b)(0+R)+K^2))
= 0/(Ki*K/(bR+K^2))

the denominator is non-zero, so e_ss = 0.

So mathematically speaking, an integrator is required to eliminate steady-state error in all cases for this model. If you don't want to use one, a feedforward can eliminate most if not all of the error if chosen carefully. Given that, there is always uncertainty, hence why feedback control exists.

Oblarg is correct that the behavior of the velocity PID tuning constants doesn't match the displacement PID. I tried to get something with close enough dynamics that teams could use a similar tuning procedure for both (except in the velocity case, there's no steady-state error so I made Ki not do anything).

This is basically what I had figured after reading up on control theory over the past few days. The WPILib implementation certainly works (it is essentially what my team did for our drive last year); however, I don't know if anyone has any good data on how it performs c.f. the other method.

I'd like, when I find the time, to run some actual tests and find out what the practical difference in performance is between the two approaches in FRC contexts.

I know the WPILib implementation runs in its own thread and the loop frequency can be set manually - do you know what the fastest speed it can run is?

[AustinSchuh]

Originally Posted by Oblarg

I know the WPILib implementation runs in its own thread and the loop frequency can be set manually - do you know what the fastest speed it can run is?

If you want to run it much faster than 100 hz, you should start looking at monitoring the between-loop timing, and improve it. By default, WPILib threads don't run with SCHED_FIFO priority, so they can have a bit of timing jitter. We write our own to get full control, and can reliably hit +- 280 uS with a 5 ms loop. Without that, we've seen it miss cycles.

As you get faster and faster, you'll start to see noise show up in the velocity measurement due to the discrete differentiation. When that starts to happen, you'll need to add complexity to filter it out.

[Oblarg]

Originally Posted by AustinSchuh

As you get faster and faster, you'll start to see noise show up in the velocity measurement due to the discrete differentiation. When that starts to happen, you'll need to add complexity to filter it out.

What kind of filtering do people typically use? Would exponential smoothing suffice?

[AustinSchuh]

Originally Posted by Oblarg

What kind of filtering do people typically use? Would exponential smoothing suffice?

I'm pretty certain that 971 is not typical, but we run either a statespace observer or kalman filter to smooth the velocity out (Practically speaking, they are just different ways to do the same thing with different knobs). Most teams seem to just use PIDF (if they use anything at all).

The math for an observer boils down to

X_hat(n + 1) = A X_hat(n) + B u(n) + L * (Y - C X_hat(n))

A and B are derived from your model. C is the mapping from your state to your sensor readings. X_hat is the estimate of the state. Y is your sensor measurement, and L is the feedback gain that you get from tuning the filter.

I'm happy to go into more details if you are interested, and our code to implement this is publically available.

[calcmogul]

Originally Posted by Oblarg

What kind of filtering do people typically use? Would exponential smoothing suffice?

If you don't have a model to use with a state space observer, that's a good alternative. WPILib has a LinearDigitalFilter class that implements PIDSource, so it can be used with WPILib's PID controller. You can either use the exponential smoothing (single pole IIR) and moving average factory methods or design your own filter and pass the gains in.

For example, to find the gains for the single pole IIR filter, we discretized the transfer function Y(s)/X(s) = 1/(1+s*tau) where tau is the time constant. The cutoff frequency can be obtained with f_c = 1/(2*pi*tau).

I should warn you we tried using the moving average filter with a buffer length of 50 in the velocity PID controller (instead of a std::queue of length 50) and it failed WPILib's integration tests. I haven't had time to determine why, but my current guess is floating point precision errors from adding so many values together. More testing is needed.

[Jared Russell]

Note that the Talon SRX does velocity filtering for you (IIRC a 64-tap FIR filter, where each value is the change in position over the preceding 100 milliseconds, and the filter is updated at 1KHz).

[Oblarg]

Originally Posted by Jared Russell

Note that the Talon SRX does velocity filtering for you (IIRC a 64-tap FIR filter, where each value is the change in position over the preceding 100 milliseconds, and the filter is updated at 1KHz).

Out of curiosity, what does the window function look like? Flat moving average, or?

[Jared Russell]

Originally Posted by Oblarg

Out of curiosity, what does the window function look like? Flat moving average, or?

I assume flat moving average, but not positive.