Segfault Re/BOOTED Part 2: The Adaptive Face-Forward Compensator

chorl

Alright, so this is the post that will drill into some details of execution surrounding the analog balancing filter on Segfault. By the way, the first test video is now linked here: either [Youtube] or  [MIT TechTV]. I don’t get paid by  either for references, so take your pick. I generally don’t like being pedantically academic (or perhaps even mildly), so there won’t be (many?) proofs, derivations, calculations, or simulations. Most of the description is intended to be conceptual – if you really wanted to learn the math behind something, I’m sure there are plenty of resources around the Intertubes that can help you out.

Two Sensors One Filter

The idea of the balance filter is to combine the reading from both an accelerometer (with its sensitive axis horizontal and aligned perpendicular to the wheel axis, and a rate gyroscope with its sensitive axis of rotation parallel to the wheel axis. The reason that this is done is because the two sensors respond optimally to two different kinds of input.

The accelerometer acts as a long-term stable reference, since gravity ultimately points downwards. However, because its sensitive axis is also in line with the direction of vehicle movement, any vehicle acceleration will tend to contaminate the reading, as illustrated by my awesome artistic diagram below:

In this diagram, I’ve roughly equated the rest output voltage of the accelerometer as the blue pendulum. During linear acceleration, the reading is distorted (purple arrow) and becomes inaccurate. Used alone, this sensor would report an “angle” even if the vehicle were completely upright. That’s just the best case – any vibrations, bumps, motor discontinuities like backlash; pretty much any vehicle movement at all outside of steady state velocity (accel = 0) will couple into the reading.

So accelerometers are useful if you’re not moving. You can build a Segway-esque vehicle with just an accelerometer, but it stands a chance of becoming unstable if it accelerates too quickly. Or at all.

On the other hand, the gyroscope will handily tell you how fast you’re rotating, but for very low velocities and zero rotational velocities (standing still), they will tend to report a small nonzero reading. This is DC error (zero frequency, since it’s zero movement-related), contrasted with the accelerometer’s “AC” error (induced by some nonzero input that may be periodic).

Therefore, you can also build a Segway-esque vehicle with only a gyro, but it’ll never stay still.

Combining Readings

There are already plenty of established “sensor fusion” algorithms that can take any number of inertial sensors and produce a reading that is the weighed sum of some combination of them. Some of those algorithms, like the Kalman filter (which this balance filter is not), can dynamically change those weights so the system has a sensor it comes to “trust” the most for any given situation.

The classic case for two sensors like the above, and one that is easy to implement in the digital world as about 2 lines of code, is the following:

  • Low pass filter the accelerometer input, and then
  • Sum it with the high-pass filtered time-integral of the gyroscope input. After all, integrating velocity gives position – angular or otherwise.

If I may cite Mr. DIY Segway himself, it’s “easier explained in code“:

angle = (0.98)*(angle + gyro * dt) + (0.02)*(x_acc);

Assume the above line of code runs every dt seconds – typically 0.01 or 0.02 seconds or something, for a 100hz (or 50hz, etc. loop). Then that line says “Combine the integrated gyro reading with the accelerometer reading, but let the gyro dominate the majority of the result”. In this case, the gyro should be used for 98% of the update each timestep. What this accomplishes is that it makes sure the accelerometer reading is sufficiently tuned out for anything that is not standing still. For anything but the act of standing still, the gyro is more trustworthy.

The astute will also notice that “high pass” means “blocking DC error” – this is required so the integration doesn’t rise to infinity as it continuously sums a very tiny but still nontrivial error.

That works pretty well for most cases in software.

But I’m not using software.

My problems with the first (few) Segfault filters came from trying port that line of code into hardware. You think porting between operating systems and programming languages is hard some times; try porting it to op amps.

Essentially, what ultimately doomed those filter designs to failure (despite some of the outputs looking good) is that ideal differentiation and integration is impossible to achieve in real life, and especially near “DC” i.e. zero frequency, which is the dominant regime in which a Segway-like vehicle operates. Op amps coupled to capacitative loads can start oscillating and becoming unstable, which was clearly exhibited by the previous designs.

What did I do to fix that? Well I tacked on more low-pass filter stages to “lower the peak to peak ripple”. Which only really caused more phase lag and made everything worse.

What is this? Like seriously… what did this do? I don’t even remember. It was all so painful. That isn’t even counting the actual op amps used in the PI(D) loop itself, nor the op amps used in splitting the drive signals into left and right.

Another Approach

Early on this term, I was introduced to another implementation of the complementary filter which was mind bogglingly simple compared to the ball of parasitic oscillation I made before. It appeared as part of a paper on teaching undergraduate dynamics and controls. The author K. Byl (nee Lilienkamp) described the analysis and design of several “plants” that demonstrated control theory, one of which was a “hardware” inverted pendulum robot. If you’re interested in reading the whole thing (it’s like a REALLY REALLY EPIC build report), it’s on MIT’s Internet publications cubby.

I’ll just knock some of the interesting parts out and put them here. First off, from p. 298, figure 6.15:

What can be observed is:

  • It’s just two low-pass filter architectures.
  • By algebraic manipulation, the top and bottom transfer functions sum to 1. That is the “complementary” part.
  • The inputs differ only by one time derivative (a single zero at the origin if we are considering s-domain) and have the same magnitude (e.g. 1 V / radian of tilt and 1 V/ radian / s of angular rotation rate)
  • The derivative path has a gain equivalent to the time constant of the filters

From a visual/graphical perspective, it’s helpful to refer to some Bode plots of the system.

The input variable θ does not have any time derivatives innately associated with it. Therefore, in the frequency domain, its transfer function is just itself. Its magnitude is a constant value across all frequencies, a flat horizontal line.When multiplied in the s-domain with the transfer function of a first order low-pass filter (the blue line), there is therefore no change, and the output has the same slope and break frequency.

The single-zero-at-zero (H(s) = s) is, on a Bode plot, a straight line from the lower left towards the upper right.

When this is multiplied with the first order low pass filter transfer function, it effectively “rotates” the whole plot about the break frequency.

Moving towards increasing frequency in the first plot, a positive slope times a flat line is a positive slope. And a positive slope times a negative slope is a flat line. This is the green line in the first plot. Summing the result is the flat line marked in red, once again showing frequency independence.

However, mathematical manipulation of the transfer function s / (tau *s + 1) with the low pass filter transfer function  1 / (tau*s + 1) will come up short of 1. To compensate for this, the derivative path must have a gain (constant numerator factor) of tau. The block diagram is complete.

What this told me is that as long as my accelerometer and gyro gave me the same magnitude of output, I could just low pass filter them with gain and then sum them together.

No AC coupling or extra integrators required.

my turn

Knowing this, I immediately turned my attention back to whipping up a workable, breadboardable circuit for Segfault. I’ve shown this kind of picture before, two build reports ago, but this is the latest version as-implemented in Segfault. It might be better to have the image open in another window, since it’s enormous for legibility.

For the netlabel challenged, it reads like a standard English book. Left to right, then to the next “line”.

Several important design notes emerge from the circuit:

  • Ignore all the power supply junk. That was for my reference when breadboarding the circuit.
  • I manually found the “volts per tilt” of the accelerometer board to be about 30mV per radian, using our slick MITERS adjustable angle block. The datasheet claims a sensitivity of about 150mV per g of acceleration… which is not correlated to the tilt sensitivity as far as I can tell.
  • The accelerometer therefore has a significant “pre-gain” factor applied to it during the offset subtraction process.
  • The gyro’s complementary filter has a gain equal to the designed time constant of 3.14 rad/s (0.5 Hz).
  • The PI stage’s integrator does not need the limiting Zener diodes. The reason, which I will explain in the next post, is that it turned out the LMC6484 op amps could not handle the +/- 15v logic rails. The intention was to make sure the integrator never hit the +/-15 volt rails, but with the rails on the actual board being +/-5, I’m satisfied letting it peg itself.
  • The “user inputs” are the knobs on Segfault’s upper control panel.
  • The output zeners are necessary since the maximum and minimum voltages had to stay within the roughly +/- 4.8 volt swing of the PWM oscillator (to be shown below). Otherwise, it would hit 100% duty cycle on at least one half of the H-bridge, resulting in bootstrap failure and a faceplant.
  • Oh, one more thing:

Sparkfun, why the TITS did you guys put a HIGH PASS FILTER on the LPY503AL breakout board? For very low frequencies, that’s an accelerometer, not a gyro. Is that why Segfault has never worked before? Seriously, why is that on there? Because the datasheet said “optional”? Sup dawg, I put an accelerometer on your accelerometer so you can do what exactly?! Dear Robot Jesus, how many people have you screwed over with that move?

Update: The new LPY5xxAL breakout board does away with the high pass filter! Yay!

Anyway, I removed the Rs and Cs associated with that high pass filter, and it’s been absolutely golden ever since. By the way, I have a few LPY503AL boards – they’re not bad, just… there’s this extra zero in the transfer function.

The locked antiphase H-bridge motor drivers remained… kind of the same. I’ll explain in the implementation and testing post later, but I had to Little Blue Wire a few parts of the circuit due to my own derpiness.

conclusion

Wow, how did this turn into a technical paper?

Anyway, that’s about all the theory I care to go over. Segfault is currently running with a 0.5Hz complementary filter and a PI compensator that is primarily integral response dominated (high I, comparatively low P).

Next post: how is seggfault formed