Can you explain to me iLQR in a step by step manner? I have a very basic background in…
- User
Can you explain to me iLQR in a step by step manner? I have a very basic background in control theory. First explain the big picture, then go into the mathematics.
Assistant
Okay, let's break down Iterative Linear Quadratic Regulator (iLQR). It's a powerful algorithm for finding (locally) optimal control sequences for non-linear systems.
The Big Picture: Throwing a Ball into a Hoop Blindfolded (Almost)
Imagine you're trying to throw a ball into a hoop, but the ball's flight is affected by complex wind patterns (our non-linear system), and you want to use the least amount of effort (minimize cost).
-
Your First Guess (Initial Trajectory): You make an initial guess of how to throw the ball – a sequence of arm movements (control inputs). You "simulate" this throw in your head (or a simple model) and see where the ball goes (the state trajectory). It probably misses the hoop.
-
Making it Simpler (Linearization & Quadratic Approximation): The true physics of the ball's flight (non-linear dynamics) and the "badness" of missing/effort (cost function) are complicated. So, around the path your guessed throw took, you approximate:
- Dynamics: "If I slightly change my arm movement here, how will the ball's position change there?" You assume these small changes have simple, linear relationships. This is like saying, "If I throw 1% harder, it'll go 1 meter further," ignoring complex aerodynamics for now.
- Cost: You approximate the "badness" as a simple quadratic (bowl-shaped) function. "Being 1cm off is a bit bad, 2cm off is four times as bad."
-
Solving the Simpler Problem (LQR Backward Pass): Now you have a linear system and a quadratic cost around your guessed trajectory. This is exactly what a standard LQR (Linear Quadratic Regulator) can solve optimally!
- The LQR works backward from the hoop (target):
- "To be at the hoop at the end, what should my ball's state be just before?"
- "To get to that state, what control should I have applied in the previous moment, and what state should I have been in?"
- This backward pass figures out the best corrections to your control inputs at each point in time, and also how sensitive the future cost is to being slightly off-track (this is the "Value function"). It gives you two things for each time step:
- A feedforward correction (
k): "Change your original guess for arm movement at this time by this much." - A feedback gain (
K): "If the ball is slightly off course at this time, adjust your arm movement by this much based on how far off it is."
- A feedforward correction (
- The LQR works backward from the hoop (target):
-
Trying the Improved Throw (Forward Pass with Line Search): You now have a recipe for a better sequence of arm movements (original guess +
kcorrections).- You don't just blindly apply the full correction. You might try applying, say, 90% of it, or 50% (this is called a "line search").
- You simulate this new, improved throw. Does it get closer to the hoop? Does it use less effort?
-
Repeat! (Iteration):
- If the new throw is better (lower cost), you accept it. This new throw becomes your new best guess.
- You then repeat the whole process: linearize around this new trajectory, solve the LQR backward, find new corrections, try them out forward.
- Each iteration, you (hopefully) get closer and closer to the optimal way of throwing the ball.
- You stop when the improvements become very small.
In essence, iLQR iteratively refines a guess for control inputs by repeatedly:
- Simulating the system with the current guess (Forward Pass).
- Creating a local linear-quadratic approximation of the problem around this simulated trajectory.
- Solving this local LQ problem to find a better control sequence (Backward Pass).
- Updating the control sequence and checking if it improved (New Forward Pass with Line Search).
The Mathematics Step-by-Step
Let's define our problem more formally.
1. Problem Setup:
-
System Dynamics (Non-linear, Discrete-Time):
x_{k+1} = f(x_k, u_k)wherex_kis the state vector at time stepk, andu_kis the control input vector at time stepk. We have a time horizonN. So,k = 0, 1, ..., N-1. -
Cost Function: We want to minimize a total cost
J, which is a sum of running costslat each time step and a final costl_fat the terminal state:J(X, U) = l_f(x_N) + sum_{k=0}^{N-1} l(x_k, u_k)whereX = (x_0, x_1, ..., x_N)is the state trajectory andU = (u_0, u_1, ..., u_{N-1})is the control sequence. The functionslandl_fcan be non-linear.
The iLQR Algorithm:
Step 0: Initialization
- Choose an initial sequence of control inputs
U_bar = (u_bar_0, u_bar_1, ..., u_bar_{N-1}). This can be all zeros, random, or from a previous solution. - Set an initial regularization parameter
mu > 0(more on this later).
ITERATE:
Step 1: Forward Pass (Rollout / Simulation)
- Given the current control sequence
U_barand initial statex_0, simulate the system forward to get the nominal state trajectoryX_bar = (x_bar_0, x_bar_1, ..., x_bar_N):x_bar_{k+1} = f(x_bar_k, u_bar_k)fork = 0, ..., N-1. - Calculate the total cost
J(X_bar, U_bar)for this nominal trajectory.
Step 2: Backward Pass (Linearization and LQR-like Riccati equations) This is where we find the corrections
dU = (du_0, ..., du_{N-1})and feedback gainsK_k. We work backward fromk = Ntok = 0.-
a) Linearize Dynamics: Around each point
(x_bar_k, u_bar_k)of the nominal trajectory, linearizef:dx_{k+1} approx A_k dx_k + B_k du_kwheredx_k = x_k - x_bar_kanddu_k = u_k - u_bar_k. The matricesA_kandB_kare Jacobians:A_k = df/dx |_(x_bar_k, u_bar_k)(how state changes with state)B_k = df/du |_(x_bar_k, u_bar_k)(how state changes with control) -
b) Quadraticize Cost: Approximate the running cost
l(x_k, u_k)and final costl_f(x_N)as quadratic functions around the nominal trajectory. For the running costl(x_k, u_k):l(x_k, u_k) approx l(x_bar_k, u_bar_k) + l_x^T dx_k + l_u^T du_k + 0.5 dx_k^T l_{xx} dx_k + 0.5 du_k^T l_{uu} du_k + du_k^T l_{ux} dx_kwhere:l_x = dl/dx |_(x_bar_k, u_bar_k)(gradient of cost w.r.t state)l_u = dl/du |_(x_bar_k, u_bar_k)(gradient of cost w.r.t control)l_{xx} = d^2l/dx^2 |_(x_bar_k, u_bar_k)(Hessian of cost w.r.t state)l_{uu} = d^2l/du^2 |_(x_bar_k, u_bar_k)(Hessian of cost w.r.t control)l_{ux} = d^2l/(dudx) |_(x_bar_k, u_bar_k)(Hessian of cost w.r.t control and state) Similar expansions are done forl_f(x_N)givingl_{f,x}andl_{f,xx}. -
c) Backward Riccati-like Equations: Define
V_k(dx_k)as the (quadratic approximation of the) optimal cost-to-go from statex_bar_k + dx_kat timekto the end of the horizon.V_k(dx_k) = 0.5 dx_k^T V_{xx,k} dx_k + V_x_k^T dx_k + (constant term)We only need to trackV_x(vector) andV_{xx}(matrix).-
Initialization (at
k=N):V_x_N = l_{f,x}(evaluated atx_bar_N)V_{xx,N} = l_{f,xx}(evaluated atx_bar_N) -
Iterate backwards from
k = N-1down to0: At each stepk, we want to finddu_kthat minimizes the current cost increment plus the future cost-to-goV_{k+1}. Define the "Q-function" for this step:Q(dx_k, du_k) = l(x_k, u_k) + V_{k+1}(dx_{k+1})Substituting the quadratic expansions forlandV_{k+1}, and the linear expansion fordx_{k+1}:Q_x = l_x + A_k^T V_{x, k+1}Q_u = l_u + B_k^T V_{x, k+1}Q_{xx} = l_{xx} + A_k^T V_{xx, k+1} A_kQ_{uu} = l_{uu} + B_k^T V_{xx, k+1} B_kQ_{ux} = l_{ux} + B_k^T V_{xx, k+1} A_k(sometimes writtenQ_{xu}^T)Regularization: To ensure
Q_{uu}is positive definite (and thus invertible, meaning the local quadratic problem has a minimum), we add regularization:Q_{uu_reg} = Q_{uu} + mu * I(whereIis the identity matrix andmuis the regularization parameter)Now, find the optimal control correction
du_k^*by settingdQ/d(du_k) = 0:Q_u + Q_{uu_reg} du_k + Q_{ux} dx_k = 0This gives us the control policy for the correction:du_k^* = -Q_{uu_reg}^{-1} (Q_u + Q_{ux} dx_k)This can be written as:du_k^* = k_k + K_k dx_kwhere:k_k = -Q_{uu_reg}^{-1} Q_u(the feedforward correction term, a vector)K_k = -Q_{uu_reg}^{-1} Q_{ux}(the feedback gain matrix) Storek_kandK_kfor eachk.Finally, update
V_x_kandV_{xx_k}using the fact that we'll applydu_k^*:V_{x,k} = Q_x + K_k^T Q_{uu_reg} k_k + K_k^T Q_u + Q_{ux}^T k_k(Simpler:V_{x,k} = Q_x - K_k^T Q_{uu_reg} K_k Q_xleads toV_{x,k} = Q_x + K_k^T Q_u + Q_{ux}^T k_kafter substitutingK_k). The standard forms are:V_{x,k} = Q_x + K_k^T Q_u + Q_{ux}^T k_k + K_k^T Q_{uu_reg} k_kV_{xx,k} = Q_{xx} + K_k^T Q_{uu_reg} K_k + K_k^T Q_{ux} + Q_{ux}^T K_k(There are slightly different but equivalent formulations. A common one is:V_{x,k} = Q_x + K_k^T Q_uV_{xx,k} = Q_{xx} + K_k^T Q_{ux}) Let's use the widely cited updates:V_{x,k} = Q_x + K_k^T Q_u + Q_{ux}^T k_k + K_k^T Q_{uu_reg} k_kV_{xx,k} = Q_{xx} + K_k^T Q_{ux} + Q_{ux}^T K_k + K_k^T Q_{uu_reg} K_k(Actually, simpler forms are derived by substitutingdu_k^*into the expansion ofV_k(dx_k) = Q(dx_k, du_k^*). This gives:V_{x,k} = Q_x + K_k^T Q_uV_{xx,k} = Q_{xx} + K_k^T Q_{ux}These are coefficients of the quadraticV_k(dx_k). Thek_kterms contribute to the constant part ofV_k, which affects expected cost reduction but not the gradients/Hessians for the next step).
-
Step 3: Forward Pass with Line Search (Updating Controls) Now we have the correction terms
k_kand gain matricesK_k. We generate a new trajectory.- Initialize
x_0^{new} = x_bar_0(initial state is fixed). - Choose a step size
alpha(where0 < alpha <= 1), typically start withalpha = 1. - For
k = 0, ..., N-1:dx_k^{new} = x_k^{new} - x_bar_ku_k^{new} = u_bar_k + alpha * k_k + K_k * dx_k^{new}x_{k+1}^{new} = f(x_k^{new}, u_k^{new})
- This gives a new trajectory
X_newandU_new. - Calculate the new cost
J(X_new, U_new).
Step 4: Acceptance and Regularization Update
- If
J(X_new, U_new) < J(X_bar, U_bar)(cost improved):- Accept the update:
X_bar = X_new,U_bar = U_new. - Decrease regularization:
mu = mu / factor(e.g.,factor = 2). This makes the next step more like a Gauss-Newton step (trusts the quadratic model more). - Go to Step 5 (Convergence Check).
- Accept the update:
- Else (cost did not improve or increased):
- Discard
X_new, U_new. - Decrease
alpha(e.g.,alpha = alpha / 2). - If
alphabecomes very small (e.g.,< 1e-4):- The quadratic approximation was poor. Increase regularization significantly:
mu = mu * factor_large(e.g.,factor_large = 10). This makes the step more like gradient descent (trusts the quadratic model less). - Go back to Step 2 (Backward Pass) with the original
X_bar, U_barbut the new, largermu.
- The quadratic approximation was poor. Increase regularization significantly:
- Else (alpha is still reasonably large):
- Go back to Step 3 (Forward Pass with Line Search) with the smaller
alpha.
- Go back to Step 3 (Forward Pass with Line Search) with the smaller
- Discard
Step 5: Convergence Check
- If ( (previous cost - current cost) / previous cost < tolerance ) OR (norm of all
k_kterms is very small) OR (max iterations reached):- Terminate. The current
U_baris the (locally) optimal control sequence.
- Terminate. The current
- Else:
- Go back to Step 1 (Forward Pass) to re-simulate with the newly accepted
U_barand then re-linearize. (Some variants re-use the linearization from the previousX_bar, U_barifalphawas 1 and cost improved, but full re-linearization is standard for iLQR).
- Go back to Step 1 (Forward Pass) to re-simulate with the newly accepted
Key Details and Considerations:
- Regularization (
mu): This is crucial. It ensuresQ_{uu}is positive definite, making the problem solvable. It also helps control the step size. The adjustment ofmuis similar to the Levenberg-Marquardt algorithm. - iLQR vs DDP (Differential Dynamic Programming):
- iLQR only uses first-order expansion of dynamics (
A_k, B_k). - DDP uses second-order expansion of dynamics (i.e., Hessians of
f). This can lead to faster convergence if these Hessians are cheap to compute and well-behaved, but iLQR is often preferred for its simplicity, especially iffis complex. Iffis linear, iLQR and DDP are identical.
- iLQR only uses first-order expansion of dynamics (
- Constraints: iLQR in its basic form doesn't handle state or control constraints (e.g.,
u_min <= u_k <= u_max). These are often incorporated using penalty terms in the cost function or by using more advanced methods like augmented Lagrangian or interior-point methods. - Computational Cost: The main costs are:
- Forward simulation:
O(N). - Computing Jacobians/Hessians: Depends on complexity of
fandl. - Backward pass: Dominated by matrix multiplications and inversions, roughly
O(N * (dim(x)^3 + dim(u)^3)).
- Forward simulation:
iLQR is a workhorse algorithm in robotics and trajectory optimization due to its relatively good convergence properties for many non-linear problems.
-
Other models, same or similar prompt
Responses were produced in different, uncontrolled contexts (system prompts, settings, prior turns). This is not a benchmark.