Triple Inverted Pendulum

Why this problem

An inverted pendulum is the textbook example of an unstable, underactuated system: gravity wants it to fall, and you have fewer actuators than degrees of freedom you need to control. Stack three of them on a single cart and the difficulty compounds — you now have to hold four coupled bodies (the cart plus three links) stable while pushing on only one of them. The system is open-loop unstable, highly nonlinear, and extremely sensitive: a few milliseconds of control delay or a slightly mistuned gain and the whole stack diverges.

I picked it because it is the cleanest distillation of the control challenge at the heart of legged and humanoid robotics. A walking robot is, in effect, an inverted pendulum balancing on a small base of support — it stays upright by continuously sensing how it is falling and reacting through the one set of actuators it has contact with the ground through. If you can stabilize a triple pendulum, you understand the math and the failure modes behind balance control.

The system & its dynamics

The plant is a cart that translates horizontally along a track, carrying three rigid links connected in series by free (unactuated) pin joints. The only control input is the horizontal force u applied to the cart. That gives four generalized coordinates:

• x — cart position
• θ₁, θ₂, θ₃ — the angle of each link from vertical

I derived the equations of motion using the Lagrangian method — writing the total kinetic and potential energy of the cart-and-links system in terms of the generalized coordinates, then applying the Euler–Lagrange equations. The result is a set of coupled, nonlinear second-order differential equations of the form M(q)·q̈ + C(q, q̇)·q̇ + G(q) = F·u, where M is the configuration-dependent mass/inertia matrix, C captures Coriolis and centrifugal coupling between the links, and G is the gravity vector. Because the inertia matrix is dense, every link's motion is coupled to every other — there is no clean way to control one joint in isolation.

Linearization & state-space model

LQR is a linear control method, so the nonlinear dynamics have to be linearized about the operating point I care about: all three links balanced straight up (θ₁ = θ₂ = θ₃ = 0). I took the Jacobian of the equations of motion about that upright equilibrium to get a linear state-space model:

ẋ = A·x + B·u, with state x = [x, θ₁, θ₂, θ₃, ẋ, θ̇₁, θ̇₂, θ̇₃]ᵀ

That is an 8-state model: four positions and their four velocities. Before designing anything, I checked the controllability matrix — confirming that the single cart force can, in principle, drive all eight states. It can (the system is fully controllable about the upright point), which is what makes the seemingly impossible task of balancing three links with one actuator actually solvable. The linear model is only valid in a small neighborhood of vertical, which becomes one of the central design constraints later.

Controller design — LQR

With a controllable linear model, I designed a Linear–Quadratic Regulator. LQR computes the full-state-feedback gain K (so that u = −K·x) that minimizes a quadratic cost functional:

J = ∫₀^∞ (xᵀQx + uᵀRu) dt

Here Q penalizes deviation of the states from upright (how much I "care" about each angle and position error) and R penalizes control effort (how hard I let the cart push). Solving the associated algebraic Riccati equation yields the optimal K. The beauty of LQR for a system this coupled is that it produces a single gain matrix that accounts for all the cross-coupling at once — it doesn't try to control each link independently, it weighs the whole state together and produces one coordinated cart command.

Tuning Q and R

The interesting engineering is in choosing Q and R — there is no single "correct" answer, only trade-offs:

• Heavier weighting on the upper link. The topmost pendulum is the most volatile — small errors there grow fastest — so its angle gets a larger penalty in Q than the cart position.
• Aggressive vs. gentle. Pushing R down makes the controller react harder and recover faster, but demands large, fast cart forces and shrinks the region where the linear model still holds. Raising R gives smoother, more realistic actuation at the cost of slower recovery. I tuned for the fastest recovery that still kept the cart within a believable force and travel envelope.
• Position vs. balance. Penalizing cart position keeps the cart from drifting down the track, but competes with balancing effort — too much and the controller sacrifices stability to chase position. I biased the weighting toward keeping the links upright first.

Simulation & testing

I implemented the full nonlinear plant and the LQR controller in MATLAB/Simulink and ran the feedback loop against the nonlinear dynamics — not the linearized model used for design — so the test is honest about where the linear controller starts to break down. I evaluated it two ways:

• Initial-condition recovery: releasing the stack from a small offset from vertical and watching the controller drive all three links back to upright.
• Disturbance rejection: injecting an impulse (a "shove") mid-balance and measuring whether the controller catches and re-stabilizes the stack.

The animation above is the simulated cart-and-links responding under the LQR law.

Results

• The controller holds all three links upright and rejects small-to-moderate disturbances, coordinating a single cart force to recover the whole stack.
• It demonstrates the core result cleanly: one actuator stabilizing four coupled bodies, with the gain matrix automatically handling the cross-coupling.
• As expected for a linear controller on a strongly nonlinear plant, there is a finite "basin" — disturbances large enough to push the links far from vertical leave the region where the linearization is valid, and the system can no longer recover. Mapping that boundary is part of what the project teaches.

Challenges & what they taught me

• Fast, divergent dynamics. A triple pendulum falls quickly and the upper link's error grows fastest — the controller has very little time to react, which puts real demands on loop rate and gain accuracy.
• The linearization boundary. The single biggest lesson: a linear controller is only as good as the region where the linear model holds. Knowing where that region ends — and that it shrinks as you tune more aggressively — is the difference between a controller that looks good in a demo and one that survives the real world.
• Coupling is unavoidable. You cannot decompose this into three independent problems; the math forces you to treat the whole system at once, which is exactly why full-state methods like LQR exist.

Where I'd take it next

• Swing-up + balance: an energy-based controller to bring the links from hanging-down to near-vertical, then hand off to the LQR for the balance phase.
• Model predictive control (MPC): to respect actuator force/travel limits explicitly and extend the recoverable region beyond what a fixed linear gain allows.
• State estimation: a Kalman filter to run the controller from noisy, partial measurements instead of perfect full-state feedback — the realistic hardware case.

Skills & Tools