# Vertical Lego RobotMay 11, 2007 6:16 AM   Subscribe

Help my robot remain upright.

So I'm trying to make a self-balancing robot from Lego. This is not an unsolved problem by any means, even in my chosen medium. However, I seem to be too dumb to figure out how to do it on my own and need a little assistance. Also, I'm adding a wrinkle that may screw the whole thing up.

First of all, I want the entire solution to be pure Lego, which eliminates things like ultrasonic sensors (this is RIS, not NXT), HiTechnic acceleration sensors, etc. But detecting tilt using a stock rotation sensor (geared at 1:25) isn't hard, so that's fine.

The next thing is reacting the robot when it detects the tilt. This is the basic inverted pendulum on a cart problem. Long story short (see below), I need some understanding of the physics of the situation. First step: free body diagram. But I'm confused because the torque due to gravity and the torque due to cart acceleration would seem to be happening on different axes (the cart has no moment arm around the axis where the inverted pendulum attaches...). So I googled around and the diagrams I found didn't even mention angular anything at all, just linear forces. Wha?

So that's my first question: In the classic inverted pendulum cart problem, how do you formulate the equations that describe the situation?

Now, here's the wrinkle: Instead of reasonably measuring the tilt of the pendulum at the point where it connects to the cart, I've done something crazy. I've hung a second pendulum from the inverted pendulum and am measuring that. The idea is that the second pendulum will remain vertical and the angle will be the same. However, I've now realized that when the cart is accelerating this secondary pendulum won't be hanging straight down. It may in fact be lined up with the inverted pendulum, making the cart think it is balanced.

This is the real reason I need the mathematical model. I need to know if my cockamamie1 setup is at all feasible or if the dual pendulum concept is self-defeating. Maybe someone can even provide a direct proof that this won't work.

1The cockamamieness actually has a purpose. I ultimately want to balance the robot in two dimensions on top of a single spherical "wheel". Because I need to measure both pitch and roll, I don't think I can measure at the axis, which is how I came up with the pendulum tilt sensor concept. I realize that this would be a hard problem even without limiting myself to Lego and even if I weren't an idiot.
posted by DU to Science & Nature (18 answers total) 1 user marked this as a favorite

I found this paper that talks about PID control of an inverted pendulum using PIC software. PICs are outside of your scope, but the appendix does have a FBD and discusses derivation of equations.

I'm having trouble envisioning where your second pendulum hangs from the inverted one. Any chance of posting an image/sketch? Are you solving a double inverted pendulum problem, or single inverted pendulum + single regular pendulum?

Pendulum starts to sound like a really funny word if you type and think it enough.
posted by olinerd at 6:46 AM on May 11, 2007

There are two ways to formulate the equations of motion for an inverted pendulum - the Easy Way and the Hard Way. The Easy way is using conservation of energy to form the lagrangian, then differentiate to get the mathematical equations. That method is summarized pretty succinctly here.

The Hard Way is to attempt a force balance. It's not impossible, but it's definitely difficult. First, you have to realize that there are two different coordinate frames to solve this in - cartesian (left and right) and polar (the rotation of the pendulum about it's attachment point). I can't find any write-ups online that attempt this method, although I know they exist, and I threw away all my nonlinear dynamics course notes a long time ago.
posted by muddgirl at 6:56 AM on May 11, 2007

Hey, good link (after I created an account). I'll have to study it to figure out how it's working, but it could be just the thing.

It's not a double inverted pendulum, it's the single inverted + single regular one. Basically, instead of measuring the title where the inverted pendulum meets the cart, I want to measure it where the regular pendulum meets the inverted one. I was imagining it statically and in that case it's the exact same measurement. Dynamically it's not, though, so I bet this isn't going to work.
posted by DU at 7:00 AM on May 11, 2007

The Hard Way is to attempt a force balance.

Then what's this all about? (The fact that that guy is also doing Lego is a coincidence, though the fact that he fails at it may not be irrelevant. I have found similar derivations, such as here.) I thought I just wasn't understanding the free body diagrams, but maybe they really don't make sense and they are just pretending to derive the equations?
posted by DU at 7:05 AM on May 11, 2007

DU - I have no idea what the guy in the first link is doing. Ignore that one.

But the dude in the second link derives the correct equations, at a cursory glance. What's your problem with that derivation?
posted by muddgirl at 7:14 AM on May 11, 2007

The problem is that I need a little more handholding than that. He pulls the equation for N out of thin air, AFAICT (and modulo my admitted dunderheadedness). I need a Derivation for Dummies. The PDF from olinerd might be good, though I have to study it and I'm theoretically working right now.
posted by DU at 7:22 AM on May 11, 2007

DU: in that equation, the sum of the forces = mass * acceleration. All he's doing is rearranging the equation (the sum of all the forces, including N) and setting N = to mass*acc - all the other forces except N. Was that your confusion?
posted by olinerd at 7:27 AM on May 11, 2007

No, I mean like...in the second term, what is mass times length times angular acceleration? Length times an angular acceleration is a torque (IIRC), times a mass is....not a force. And then how do you square an angular velocity to get an angular acceleration in the next term? And why is l the length used in the first place?

I feel like such a moron asking these questions. I could swear I learned it right and yet I couldn't find it in my textbook.
posted by DU at 7:33 AM on May 11, 2007

DU, look at the wikipedia link that muddgirl posted earlier; it derives the same equation in a more step-by-step manner...
posted by olinerd at 7:46 AM on May 11, 2007

Sorry to sound defeatist, but if these links are the stupefyingly obvious explanations then this problem is WAY too hard for me.
posted by DU at 7:59 AM on May 11, 2007

What's your background? These were problem set problems in my junior year dynamics class -- if you're not an engineer, I can see how they're not really intuitive. Inverted pendulums are hard.

The important thing is really less the physics and more your ability to manipulate numbers. What can you substitute for something else, how can you rearrange the equations and plug them in to other equations, blah blah blah ... that's the intuition you need for this problem. That, and a healthy appreciation for partial differential equations.

Though the one other thing I did get out of this class and the subsequent Controls class is that while this is all very important for a starting point, when it comes down to it, control is really a matter of trial-and-error. If you find this stuff too intimidating, get as far as you can with as simplified linearized equations as you can, and then just play around with it. My guess is you can get close via that method.
posted by olinerd at 8:04 AM on May 11, 2007

(DU, looking at your posting history, it seems like you have a lot of experience with robotics/programming/other engineering-related things. So sorry if my assumption that you are a non-engineer sounded patronizing. It was not intended to be. I just know that for me, it's only the very recent intense pounding into my head of problem sets and equations that make your problem looks solvable; if you either never had that or you're further removed from it, I can see how the problem looks wicked crazy.)
posted by olinerd at 8:15 AM on May 11, 2007

Software engineer with the equivalent of 1 or 2 semesters of basic physics.

I was intending to ignore the physics for the most part and just kind of tweak a PID algorithm until it worked. The only reason I care about the mathematical model is I need to know if my harebrained second pendulum scheme is even possible. I suspect it isn't and I need to come up with a mechanical solution to obtaining both pitch and roll from a universal axle.

Actually, now that I said "universal axle" out loud, I may know how it could be done.
posted by DU at 8:15 AM on May 11, 2007

So I googled around and the diagrams I found didn't even mention angular anything at all, just linear forces. Wha?

Here are a couple of pages from an old lab - Pendulum on Cart. Notice the instantaneous z dimension, tangent to the motion of the pendulum mass.. I find it a lot more intuitive than the wikipedia page.

I need to know if my cockamamie1 setup is at all feasible or if the dual pendulum concept is self-defeating. Maybe someone can even provide a direct proof that this won't work.

Using a second pendulum is worth a try, but how you implement it really depends on what the entire system is supposed to do. As long as the cart has a steady state zero acceleration, I think you just make the tilt sensor's time constant as fast as possible (make it short), and ignore its dynamics - may not work, you'll have to experiment. If you want the cart to move around while balancing the pendulum, you'll have to factor in the coupled dynamics. It should still be controllable/observable (ie, you have enough information to keep the pendulum balanced), so still a useful sensor, but it makes the problem that much more complicated.

I realize that this would be a hard problem even without limiting myself to Lego

Yes, well.. It seems like a very challenging project :)

What is the smallest angle that you can detect? I don't mean the resolution on your measurement, mind, but how much rotation has to happen before you begin to get a response - because of gear backlash, there will be a dead zone.
posted by Chuckles at 11:06 AM on May 11, 2007

(make it short)

Heh, that is ambiguous.. Make the length short :P
posted by Chuckles at 12:20 PM on May 11, 2007

Two notes, DU:

It's not a double inverted pendulum, it's the single inverted + single regular one.

'Inverted pendulum' and 'regular pendulum' are the same thing. They differ only in the angle of the pendulum. This setup is called a double pendulum, and it is a chaotic system, meaning that small changes in the initial conditions can lead to exponentially greater separations in state at a later time.

Length times an angular acceleration is a torque (IIRC), times a mass is....not a force. And then how do you square an angular velocity to get an angular acceleration in the next term?

The units of angular velocity are radians/sec, and the units of angular acceleration are radians/sec2. Radians are dimensionless, and so you can (dimensionally) get angular acceleration by squaring angular velocity. (It's not how you'd calculate it, though.) Length times angular acceleration, dimensionally, is acceleration (since radians are dimensionless.) Mass times acceleration is force. (This is Newton's second law.)

I strongly suspect the second pendulum will not work--its angle is coupled to the first pendulum in a way that will not easily give the information to determine tilt. I'll see if I can't post an explanation in a bit.
posted by Upton O'Good at 3:38 PM on May 11, 2007

The traditional way to invert a pendulum is to consider first its dynamics. At a 'spherical cow' level, a pendulum can be modeled as a massless rigid rod with a point mass on the end. (We'll ignore frictional/damping forces, the fact the mass isn't concentrated at a single point, etc.) The only external force that will act on the pendulum is gravity, but the rod and the point that it rotates about will provide whatever forces are necessary to keep the mass at the same distance from the center. (These are called forces of constraint.)

We'll create a coordinate system, now, centered at the point this whole thing is rotating around. We'll call the position of the point mass in the x-direction x, and the position of the point mass in the y-direction y. We can also say that the length of the pendulum rod is L, and the angle that the pendulum makes with the positive y-axis is θ. We'll let the point mass be m. The setup will then look like this, with θ = 0 being an inverted pendulum:

y-axis

^
|
^| θ  o
||_  /
y || \/ L
|| /
v|/
< ---------------> x-axis
|< --->
|  x
|
|
|
|
v

I'll use what's called a Lagrangian formulation--it's another way of approaching classical mechanics which avoids dealing with the constraint forces. You can skip this derivation, if you like--I'll go directly to the dynamics below.

First, the system only has one real degree of freedom--the angle the pendulum takes. We can write the x- and y- coordinates and their derivatives in terms of θ:

x = L sinθ, x' = L θ' cosθ
y = L cosθ, y' = -L θ sinθ

where ' designates a time derivative (d/dt).

The Lagrangian is a value equal to the kinetic energy (T) minus the potential energy (V). For this system, T = m L2 θ'2/2, and V = m g L cosθ, where g is the acceleration due to gravity. L = T-V, and we can write the Euler-Lagrange equations (which determine the motion of the system) as:

d/dt(∂L/∂θ')-∂L/∂θ = 0
implies
mLθ'- mgLsinθ = 0
or
θ' - g/Lsinθ = 0
which is the equation of motion for the pendulum.

To make use of this, we define the vector x as [x1, x2] = [θ,θ']. Then x' = [x1', x2'] = [θ',θ'], and we can write the dynamics of the pendulum as two first order equations:

x1' = x2
x2' = g/Lsin x1

It is possible to solve these exactly, but that's not quite what you'd want to do if you're going to be doing PID control. For your case, you probably just want to make it stable locally about the upright position, and for that you'd want to linearize the equation--that is, to approximate the nonlinear function at a specific point by a linear function. We'll do that by taking the Jacobian, which is the vector equivalent to the first derivative in a multidimensional Taylor series, and evaluating it at the point about which we want to linearize. In your case, θ = 0. (Sorry if some of this is unfamiliar.)

If we do that we'll get the following set of linear equations, which will be valid close to θ = 0:

x1' = x2
x2' = g/Lx1

These equations are linear, and we can do all kinds of interesting control things to them. As Chuckles mentioned above, controllability and observability are important. 'Controllability' means that given any initial position xinitial, we can move the system to a position xfinal in an arbitrary amount of time. 'Observability' means that given any state at any time, we can reconstruct the initial state xinitial. From your description, I assume you'll be controlling with a torque and observing the angle; in this case, the system is both controllable and observable.

More importantly, you can also calculate the transfer function of the system, and plot it with a Bode plot--this should help you get a starting location for your PID controller, and you can tune it from there. To learn more about control systems, you could buy any number of textbooks, but I might suggest the Wikibooks Control Systems book--you can spend money later if you want a more permanent or in-depth reference.

This is all a bit approximate, but hopefully covers the basics of what you'll need mathematically. I personally don't like discarding the physics and just playing with the PID--it's important to know why things happen as they do.

Hope this helps.
posted by Upton O'Good at 5:21 PM on May 11, 2007

Since you like the crazy-awesome approach, consider building a weighted spinning top into the secondary (sensor) pendulum, thus gyroscopically stablising it, and so making the rotation readings much more useful. There are lego bricks with metal weights in them (I think they're ballast from train sets), and and older style of 9V motor with higher RPM than the modern ones, so less external gearing would be involved, as I imagine the primary challenge would be building the gyro small enough. Ebay and Bricklink make it easy to get esoteric Lego bricks :-)
posted by -harlequin- at 10:09 PM on May 11, 2007

« Older War can be fun!   |   Why does my router hate you so much? Newer »