Motor Control: The Velocity Estimator Saga

Last time, I’d managed to make current control work on my new board, and now it was time for servo position control.

After a bit of hacking around and trying random PID gains, I wrote some code to generate a position profile for the motor to track. Using this framework I could easily run sinusoid or step response tests and log data at 20kHz or some downscaled multiple. I did some step response tests and the results were really quite nice. This plot is a beautiful underdamped response. This data was logged at 2kHz.

5 P40 D7000

I increased the D term by a bit but unfortunately this caused quite a bit of noise. I was a bit mystified by this, as oscillations were present even with a low D value.

6 P180 D7000

Another plot of the noise.

9 P180 D10000

This was really annoying.  I couldn’t really figure out what was going on, so I decided to try tracking a 20 Hz sine wave and logging the control effort over a time period of 1 second.

contrller effort

In the plot above, the green axis is commanded Q axis current, with units of amps. As you can see, spikes of 20a are present. What the heck? This is horrendously noisy. After quite a bit of head scratching I bumped the data log rate up to 20kHz and sampled the position and control effort over just one sinusoid. The sinusoid is still a 20 Hz sine and the data was logged over 1/20th of a second.

ctrl effort vs position

What the heck! I figured out that this weird stepping signal is encoder quantization showing up in the derivative term of my PD controller.

What actually is happening here can only be understood through a deeper understanding of what the derivative term in PID actually is. In a textbook PID controller, a control effort is calculated by summing the error, the integral of the error, and the derivative of the error, each term multiplied by a tuned gain value. The derivative of the error, d/dt of position error, is in fact exactly the same as velocity error. This means that in order to properly calculate the D term, one must know the velocity of the motor- easier said than done.

The root problem of this is the fact that velocity is hard to find with a quantized encoder. Some motors use a separate tachometer to accurately measure velocity on top of their position encoder, which apparently works well but is pretty bulky. One option was to mount a gryo to the motor can to measure velocity, but this seemed a bit extra. In my code, the derivative term is calculated by observing the number of encoder ticks that happened in the last 10 main loop cycles, or 500 microseconds. This approach works great if the motor is spinning fast. However when the motor is going slower, the number of encoder edges in the last 10 cycles is almost always either just one or zero, leading to a completely binary derivative signal. Not good. This is not a problem that can easily be fixed, simply due to the fact that velocity information is really hard to get from a quantized encoder: velocity information simply does not exist between encoder edges, and information cannot be created if it doesn’t exist in the first place.

One semi promising option for finding velocity is to measure the time between successive encoder edges. The time between encoder edges can be used to form an relatively precise estimate for velocity. The upside to this is that the spacing between the pulses can be measured quite accurately. Ben had tried a similar edge timing scheme before, with mixed results. The largest problem is that velocity information is not available instantaneously, as it is calculated from the two previous encoder ticks (which could have happened a while ago, meaning the velocity could have changed!!). Therefore if the motor is rotating and suddenly stops, no new encoder edges will arrive and therefore no velocity information will be available. If naively calculated, this sudden motor stop will lead to not a calculated velocity ozero, but a constant, incorrect, old velocity. This is really just the fact that if no new encoder edges arrive, new information is just not available and cannot be created from nothing.

Furthermore, the CCR1 register does not reset to zero when no new edges arrive- meaning that zero velocity will actually be interpreted as some fixed velocity which is not zero. Furthermore, new velocity information is not the current velocity, but the velocity between the previous encoder ticks, meaning a delay is present in veocity measurements. Direction information is also not present, and must be obtained elsewhere.

I figured I’d give this scheme a shot, as it was summer and I didn’t have much to lose. I began by looking at Ben’s code. Ben used the 446’s TIM2 set up in input capture mode, meaning that a trigger signal will cause the timer’s current value to be stored in the CCR1 register before the timer is reset to zero. The great thing about timers is that, once set up, they run autonomously and therefore do not hog valuable computation time firing some interrupt. A major problem with Ben’s initial scheme is that the CCR1 register is only fired on a rising edge of the TI1 signal, which means that, as I am using a quadrature encoder, velocity information is only acquired once every four encoder ticks. I tried for an extremely long amount of time to figure out how to have internal triggering work in 4x mode with no avail. With this 1/4 resolution scheme, results left something to be desired:

interpolator 1

Blue is encoder position and green the extrapolated position. As you can see, velocity information is pretty bad and therefore the extrapolated position wanders off into oblivion.

Ben’s scheme used the rising edge of channel A of the encoder to trigger the internal trigger output of TIM3, to which TIM2 was slaved. After an entire weekend of solid register map reading, I concluded that unfortuantely internal triggering could take me no farther. I used wired to jump the encoder A and B channels onto PA15 and PB9, which I then mapped to TIM2. You’d think there would be a better way rather than just jumping pins but if there is, I couldn’t find it.

The wires are the ones closest, jumping over the 446.


And with that I fired it up (and some registers changed) and got some interesting results:

interpolator 3

Blue is encoder position, green is extrapolated position (moved up for visibility). As you can see the results are better, but a lot of saw-toothed wanering is still present. I tried averaging the past five velocities to obtain the new velocity:

interpolator 4 5 past velocities

This is quite a bit better. However it is still not perfect and therefore I decided to really go ham on this and make this position extrapolator really kick butt. I made a python script entitled “trajectory comparer” and grabbed motor data of position (blue) and the velocity exrapolator (red). As you can see the exrapolated position is crap and barely sticks to the encoder position.

1 base

First I sovled for CCR1 and compared it to the measured CCR1 from the motor. After a few code errors they matched quite well (they are the dark and light green signals):

2 solve for CCR1

Then I made a bunch of different extrapolators and compared them. I also clipped the extrapolaters when they would wander too far from the encoder position. They all look pretty similar but are marginally more or less good in some ways.

4 alllll the interpolators

I filtered the encoder position with a butterworth filter and compared my extrapolators to that. The best scheme turned out to be pretty stupid. I assumed the more spaced out the encoder ticks are, the more reliable they are as they are simply averaged over more time. Therefore, one can simply look at the last three CCR1 values, take the largest, and invert it to give velocity. This is not perfect in any way, but it turned out to actually work fine.

Onto real motor tests! And then, right then and there, reality decided to hit me in the face. The interpolator decidedly made things worse.

Without interpolator:

1 step old gains

With interpolator:

2 step with new interpolator


It just didn’t really work. In fact, it even made it worse!

I tried tracking a sinusoid. This is with the interpolator:

sine tracking with P

Without the interpolator:

sine tracking without P

The control effort was still super noisy, and performance was not improved.

From here, I decided to move to directly estimating the velocity and calculating the derivative based on that. The results, unfortunately, were just as bad.

As a final test (which maybe I should have done to start with…) was to directly measure CCR1 as the motor rotated at constant speed:

CCR1 vs position

As you can see, this signal is COMPLETE GARBAGE. I scoped an encoder channel and it turns out that the encoder ticks are, in fact, not evenly spaced at all, and thus, velocity data computed in the way I was doing it is in fact complete garbage.

Welp. Guess that was a complete waste of time. At least I learned quite a bit about timers and register diddling, and restructured some of my code to work better.

As a final bit of summer work, I decided to finally tune the current controller, which I’d been putting off for the past four months. I wrote a little script to use Ben’s equations for setting the gains, using a conservative value of w_c of pi/16. We tried measuring the resistance and inductance directly and plugging these values in directly. This just worked terribly, and caused either slow response or instability. The trick was to get measurements directly using the controller, which would mean that the units were always correct. First we measured the resistance by giving a constant 1 volt of Q amps and observing the current, which turned out to be almost exactly 12. This gave a phase resistance of 83 mOhm. Then, we used a step in Q from 0 to 1 volt and observed the rise time. The 63% rise time came out to be 260uS, giving an inductance of about 22 uH. These values gave me controller gains of K = 0.095 and KI = 0.172. These values worked great, and gave the controller a rise time of 10 samples, which apparently is good.

Tune in next time.