Desktop Inverted Pendulum Part 2: Control

With the mechanical bits out of the way, it's time to go through the process of controlling the inverted pendulum.  Brace yourself for babble about transfer functions and other controls stuff.  Video at the end.

First on the agenda is getting the brushless motor to actually produce torque.  While I'd already done 99% of the work required, I still had to redo the current d/q axis current loops.

After some decoupling the motors d and q axes look like independent RL circuits, and are thus easily tamed with PI controllers.  Since the control loops are running on a microcontroller, it makes sense to design them directly in discrete time, rather than in continuous time (although designing in continuous time, assuming you sample fast enough for it to not make a difference, and converting to discrete time can be valid too).  To do so, first find the zero-order hold equivalent of the RL circuit - basically, how the dynamics of the circuit appear to the microcontroller sampling at a fixed rate.

Remembering good ol' 2.004, the transfer function of a series RL circuit from voltage to current is $$\frac{I(s)}{V(s)} = \frac{1}{(Ls + R)}$$ which has a frequency response like this:

Now let's convert that to discrete time.  Some amount of algebra (or numerically in one line of Matlab using c2d) later, you get:
$$\frac{I(z)}{V(z)}=\frac{\frac{1}{R}\Big(1-e^{\frac{-R\dot{}Ts}{L}}\Big)}{z -e^{\frac{-R\dot{}Ts}{L}}}$$
 Where R and L are the same resistance and inductance as before, and Ts is the sample period.  The frequency response looks a lot like the continuous time version, except that the phase (and magnitude, but to a lesser extent) gets kind of wacky towards π radians/sample.

Okay, now to design a discrete current loop.  I like to write a PI controller in the form
$$\frac{U(z)}{E(z)}=k\Big(1 + \frac{ki}{z-1}\Big)$$
which has a pole at z=1, a zero at z=1-ki, and a high-frequency gain of k.  This way, changing k only changes the loop gain, and changing ki only changes the zero location, unlike the typical parallel PI expression.

Set if we set the zero of the controller on top of the pole of the RL system, then the controller * RL just looks like an integrator.  This can be done by setting $$ ki = 1-e^{\frac{-R\dot{}Ts}{L}}$$

Now just to choose a loop gain that achieves whatever crossover frequency (ωc) you want.  Let's say something like π/8 radians/sample, to be conservative, because, to quote a professor, "There are always higher-order poles".  This should be reasonably conservative and give us ~78 degrees of phase margin, as you can tell from the pole-zero map of the return ratio (in this case the circuit times the controller, which is just an integrator):

From the same geometry, you can see that crossing over at ωc = π/2 gives you 45 degrees of phase margin, etc.

We can calculate approximately the loop gain required by doing some more geometry on the bode plot of the return ratio.   We want the loop to cross over (magnitude = 1, or 0 dB if you prefer dB for some reason) at  π/8 radians/sample.  We also know that, at the frequency of the controller zero and circuit pole, the plot has a magnitude of k/R.  Since the slope of the magnitude plot is -1 (on a log/log scale), we can easily calculate the k required to join those two points with a line of the correct slope:

So k should be ...
$$k=r\Bigg(\frac{\omega_{c}}{1-e^{\frac{-R\dot{}Ts}{L}}}\Bigg)$$
This won't be quite exact, because the slope of the magnitude plot isn't perfectly -1 between those two points, but it gets you pretty darn close.

Here's some MATLAB plots verifying the frequency and step response of the current controller designed this way.

Hey, I just worked that all out symbolically.... that means that if my motor controllers could measure resistance and inductance on their own, they could auto-tune their current loops....(also note this isn't specific to current control of RL circuits.  It can just as easily apply to any 1st-order kind of system, like velocity control of a motor (inertia -> inductance, damping -> resistance), etc)


With torque (well, current, but close enough) control out of the way, next step is stabilizing the pendulum upright.  While there's always the "I heard PID controllers were good so lets throw PID at it and plug in arbitrary constants until it works" strategy, which people on the internet seem to like, that's not very interesting.  There are lots of reasonable approaches, but the one I've mostly been playing with is full state feedback with LQR-chosen gains.  While I've taken a class on state space modeling and controls, I had never actually implemented such a controller in hardware, on a microcontroller.

Like most good controls problems, the first step is coming up with a model of the system to design the controller around.  For the near-upright position, the furuta pendulum system can be greatly simplified from the extremely unpleasant full equations of motion.  Rather than doing the proper thing and linearizing those about the pendulum-up position, I was lazy and converted my rotary inverted pendulum into a cart-pole, and linearized those equations about vertical.  Except that has been done for me dozens of times.

The quantities that need to be converted to go from the rotary inverted pendulum to cart-pole:

The only trick is that care needs to be taken doing this conversion- both going from physical device to the model, and from controller gains back to the physical device when implementing the controller.  In the model, the units are meters, kilograms, and Newtons, whereas on the device they are radians, kilogram-meters^2, and Newton-meters respectively.  This model is only valid for small rotations on both axes, but it seems to be good enough to stabilize the system well.

The upright-controller, for reference states of zero, looks like this: $$ \tau = 0\dot{}\theta - 0.1\dot{}\dot{\theta} + 2.52\dot{}\phi + 0.15\dot{}\dot{\phi} $$ where  tau is torque, theta is the angle of the base, and phi is the angle of the pendulum (from upright)

Something to note:  I put no weight on the location of the pendulum (i.e. the x-position of the cart in the cart-pole system), so the gain in front of theta is zero.  This was mostly an aesthetic choice - I thought it would look nice if the pendulum slowly coasted to a stop after being disturbed.

Positions are taken straight from the sensors.  Base velocity (theta dot) is measured with this trick, and pendulum velocity is the (heavily filtered) derivative of angle, since I can't time edges on the Allegro A1335 position sensor used for pendulum angle.

Here's the disturbance response (i.e. me flicking the pendulum) of pendulum angle with two different controllers, as compared to the model used to come up with the controller gains.

To actually get the pendulum to the upright position, I (roughly) used an energy-shaping controller.  Check out the 6.832 notes here for a more detailed description of how that works.  The basic idea is to apply a torque to the pendulum only when it adds energy to the pendulum, and only do so while the pendulum has less than its upright energy)  In other words, the sign of the torque applied to the pendulum should be the same as the sign of the angular velocity of the pendulum, so that torque times angular velocity (i.e. power) is positive.   By doing the opposite, you can remove energy from the system, if you want to swing down from upright, for example.  At the end of the day, my energy shaping controller looks like:
$$\tau{} = K\dot{}cos^4(\theta{})\dot{}\dot{\theta{}}\dot{}(9.81(1-cos(\theta{}) )-.0075\dot{\theta{}}^2)$$
Tau is torque, theta is the pendulum angle.  K is a constant found through experimentation.  The 9.81-.... term at the end is proportional to the additional energy needed to bring the pendulum upright.  The 4th power cosine term has the effect of skewing the controller output towards the bottom of the swing, when the pendulum velocity is the greatest.  This term was actually sort of an accident.  By some miracle, the swing-up controller worked the first time I implemented it, without that term.  But when I later went to write a linear swing-down controller, I realized that, at the bottom of the pendulum's swing, my angle measurement was rolling over.  The angle jump of 2 pi caused a spike in velocity which gave a "kick" to the energy-shaping controller.  Once I fixed that problem, the gain of the controller was too low to do anything, so I cranked up the gain.  I found this new controller to not be very effective, and would mostly cause the motor to spin around a bunch.  So I added the 4th power cosine term to add back the "kick" back to the bottom and that fixed it.

Here's what the angle and torque command during swing up look like, before it kicks into the linear controller.  This is actual measured data:

Finally, there's a simple state machine which chooses the controller to use at any given time - swing up, swing down, or stabilize up.

Here's a video showing the different control states: