【Optimal Control (CMU 16-745)】Lecture 7 The Linear Quadratic Regulator Three Ways

Review:

• Control history
• Deterministic optimal control
• Pontryagin’s principle

Lecture 7 The Linear Quadratic Regulator Three Ways

Overview

• LQR intro
• LQR via shooting
• LQR as a QP
• Riccati recursion

1. LQR problem

$$\begin{array}{rl}\underset{{\mathbf{x}}_{1:N},{\mathbf{u}}_{1:N-1}}{\min }& J=\sum _{k=1}^{N-1}\left(\frac{1}{2}{\mathbf{x}}_k^T{\mathbf{Q}}_k{\mathbf{x}}_k+\frac{1}{2}{\mathbf{u}}_k^T\mathbf{R}{\mathbf{u}}_k\right)+\frac{1}{2}{\mathbf{x}}_N^T{\mathbf{Q}}_N{\mathbf{x}}_N\\ \text{s.t. }& {\mathbf{x}}_{k+1}={\mathbf{A}}_k{\mathbf{x}}_k+{\mathbf{B}}_k{\mathbf{u}}_k,k=1,\cdots ,N-1\\ & {\mathbf{Q}}_k⪰0,\mathbf{R}\succ 0\end{array}$$

$\mathbf{R}$ should be positive definite, otherwise the control input will be infinite. But ${\mathbf{Q}}_k$ can be positive semidefinite.

• Can (locally) approximate many nonlinear problems.
• Computational tractable
• Many extensions (e.g. infinite horizon, stochastic, etc.)
• Time invariant: ${\mathbf{A}}_k=\mathbf{A}$, ${\mathbf{B}}_k=\mathbf{B}$, ${\mathbf{Q}}_k=\mathbf{Q}$, ${\mathbf{R}}_k=\mathbf{R}$, $\forall k$.

2. LQR with indirect shooting

$$\begin{array}{rl}& {\mathbf{x}}_{k+1}={\nabla }_{\lambda }\mathcal{H}({\mathbf{x}}_k,{\mathbf{u}}_k,{\lambda }_{k+1})=\mathbf{A}{\mathbf{x}}_k+\mathbf{B}{\mathbf{u}}_k\\ & {\lambda }_k={\nabla }_{\mathbf{x}}\mathcal{H}({\mathbf{x}}_k,{\mathbf{u}}_k,{\lambda }_{k+1})=\mathbf{Q}{\mathbf{x}}_k+{\mathbf{A}}^{\top }{\lambda }_{k+1}\\ & {\lambda }_N=\frac{\partial {\ell }_F}{\partial {\mathbf{x}}_N}={\mathbf{Q}}_N{\mathbf{x}}_N\\ & {\mathbf{u}}_k=\underset{\tilde{\mathbf{u}}}{\mathrm{argmin}}\mathcal{H}({\mathbf{x}}_k,\tilde{\mathbf{u}},{\lambda }_{k+1})=-{\mathbf{R}}^{-1}{\mathbf{B}}^{\top }{\lambda }_{k+1}\end{array}$$

Note:
$\mathcal{H}\left({\mathbf{x}}_k,\tilde{\mathbf{u}},{\lambda }_{k+1}\right)=\frac{1}{2}{\mathbf{x}}_k^{\top }\mathbf{Q}{\mathbf{x}}_k+\frac{1}{2}{\tilde{\mathbf{u}}}^{\top }\mathbf{R}\tilde{\mathbf{u}}+{\lambda }_{k+1}^{\top }\left(\mathbf{A}{\mathbf{x}}_k+\mathbf{B}\tilde{\mathbf{u}}\right)$
To minimize $\mathcal{H}$, we take derivative w.r.t. $\tilde{\mathbf{u}}$ and set it to zero:
$$\frac{\partial \mathcal{H}}{\partial \tilde{\mathbf{u}}}=\mathbf{R}\tilde{\mathbf{u}}+{\mathbf{B}}^{\top }{\lambda }_{k+1}=0\Rightarrow \tilde{\mathbf{u}}=-{\mathbf{R}}^{-1}{\mathbf{B}}^{\top }{\lambda }_{k+1}$$

(1) Procedure

i. Start with an initial guess ${\mathbf{u}}_{1:N-1}$ trajectory
ii. Simulate (“rollout”) to get ${\mathbf{x}}_{1:N}$
iii. Backward pass to get $\lambda$ and $\Delta \mathbf{u}$
iv. Rollout with line search on $\Delta \mathbf{u}$
v. Go to iii until convergence

(2) Example: double integrator

Dynamics:
$$\dot{\mathbf{x}}=\left[\begin{array}{c}\dot{\mathbf{q}}\\ \ddot{\mathbf{q}}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}\mathbf{q}\\ \dot{\mathbf{q}}\end{array}\right]+\left[\begin{array}{c}0\\ 1\end{array}\right]\mathbf{u}$$

Think of this as a brick sliding on ice (no friction).

Discretize with Euler’s method:
$${\mathbf{x}}_{k+1}=\left[\begin{array}{cc}1& h\\ 0& 1\end{array}\right]{\mathbf{x}}_k+\left[\begin{array}{c}\frac{h^2}{2}\\ h\end{array}\right]{\mathbf{u}}_k=\mathbf{A}{\mathbf{x}}_k+\mathbf{B}{\mathbf{u}}_k$$

Code:

using LinearAlgebra
using PyPlot

# Discrete dynamics
h = 0.1   # time step
A = [1 h; 0 1]
B = [0.5*h*h; h]

n = 2     # number of state
m = 1     # number of controls
Tfinal = 5.0 # final time #try larger values
N = Int(Tfinal/h)+1    # number of time steps
thist = Array(range(0,h*(N-1), step=h));

# Initial conditions
x0 = [1.0; 0] 

# Cost weights
Q = 1.0*I(2)
R = 0.1
Qn = 1.0*I(2)

# Cost function
function J(xhist,uhist)
    cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]
    for k = 1:(N-1)
        cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*uhist[k]'*R*uhist[k]
    end
    return cost
end

# Dynamics
function rollout(xhist, uhist)
    xnew = zeros(size(xhist))
    xnew[:,1] = xhist[:,1]
    for k = 1:(N-1)
        xnew[:,k+1] .= A*xnew[:,k] + B*uhist[k]
    end
    return xnew
end

# Initial guess
xhist = repeat(x0, 1, N)
uhist = zeros(N-1)
Δu = ones(N-1)
λhist = zeros(n,N)
xhist = rollout(xhist, uhist) #initial rollout to get state trajectory

# J(xhist,uhist) #Initial cost

b = 1e-2 #line search tolerance
α = 1.0
iter = 0
while maximum(abs.(Δu[:])) > 1e-2 #terminate when the gradient is small
    
    #Backward pass to compute λ and Δu
    λhist[:,N] .= Qn*xhist[:,N]
    for k = N-1:-1:1
        Δu[k] = -(uhist[k]+R\B'*λhist[:,k+1])
        λhist[:,k] .= Q*xhist[:,k] + A'*λhist[:,k+1]
    end
    
    #Forward pass with line search to compute x
    α = 1.0
    unew = uhist + α.*Δu
    xnew = rollout(xhist, unew)
    while J(xnew, unew) > J(xhist, uhist) - b*α*Δu[:]'*Δu[:]
        α = 0.5*α
        unew = uhist + α.*Δu
        xnew = rollout(xhist, unew)
    end
    uhist .= unew;
    xhist .= xnew;
    iter += 1
end

# Plot x1 vs. x2, u vs. t, x vs. t, etc.
clf()
figure(figsize=(12,3.5))
subplot(1,2,1)
plot(thist, xhist[1,:], label="Position")
plot(thist, xhist[2,:], label="Velocity")
xlabel("Time")
title("State Trajectory")
legend()

subplot(1,2,2)
plot(thist[1:end-1], uhist, label="Control Input")
xlabel("Time")
title("Control Input")
legend()

display(gcf())
savefig("lqr-shooting.png", dpi=300, bbox_inches="tight")

Result:

Set the final time to 10.0s, we get:

The solutions are similar, but a longer time horizon needs more iterations to converge (2415 vs. 664). Its time complexity is linear to the time horizon.

3. LQR as a QP

(1) Redefine some variables

Assume initial state ${\mathbf{x}}_1$ is given (not a decision variable).
Define $\mathbf{z}=\left[\begin{array}{c}{\mathbf{u}}_1\\ {\mathbf{x}}_2\\ {\mathbf{u}}_2\\ {\mathbf{x}}_3\\ ⋮\\ {\mathbf{x}}_N\end{array}\right]$.
Define cost matrix:
$$\mathbf{H}=\mathrm{diag}({\mathbf{R}}_1,{\mathbf{Q}}_2,{\mathbf{R}}_2,{\mathbf{Q}}_3,\cdots ,{\mathbf{Q}}_N),$$ such that
$$J=\frac{1}{2}{\mathbf{z}}^{\top }\mathbf{Hz}$$
Do the same for dynamics:
Define $\mathbf{C}$ and $\mathbf{d}$:
$$\mathbf{C}=\left[\begin{array}{cccccccc}\mathbf{B}& -\mathbf{I}& 0& 0& \cdots & 0& 0& 0\\ 0& \mathbf{A}& \mathbf{B}& -\mathbf{I}& \cdots & 0& 0& 0\\ ⋮& ⋮& ⋮& ⋮& \ddots & ⋮& ⋮& ⋮\\ 0& 0& 0& 0& \cdots & \mathbf{A}& \mathbf{B}& -\mathbf{I}\end{array}\right],\mathbf{d}=\left[\begin{array}{c}-\mathbf{A}{\mathbf{x}}_1\\ 0\\ ⋮\\ 0\end{array}\right]$$

So the dynamics constraint can be written as:
$$\mathbf{Cz}=\mathbf{d}$$

(2) QP formulation and its closed-form solution

Now we can write the LQR problem as a QP:
$$\begin{array}{rl}\underset{\mathbf{z}}{\min }& J=\frac{1}{2}{\mathbf{z}}^{\top }\mathbf{Hz}\\ \text{s.t. }& \mathbf{Cz}=\mathbf{d}\end{array}$$

The Lagrangian is:
$$\mathcal{L}=\frac{1}{2}{\mathbf{z}}^{\top }\mathbf{Hz}+{\lambda }^{\top }(\mathbf{Cz}-\mathbf{d})$$

KKT conditions:
$$\begin{array}{rl}{\nabla }_{\mathbf{z}}\mathcal{L}& =\mathbf{Hz}+{\mathbf{C}}^{\top }\lambda =0\\ {\nabla }_{\lambda }\mathcal{L}& =\mathbf{Cz}-\mathbf{d}=0\end{array}\Rightarrow \left[\begin{array}{cc}\mathbf{H}& {\mathbf{C}}^{\top }\\ \mathbf{C}& 0\end{array}\right]\left[\begin{array}{c}\mathbf{z}\\ \lambda \end{array}\right]=\left[\begin{array}{c}0\\ \mathbf{d}\end{array}\right]$$

We get the exact solution by solving one linear system.

(3) Example: double integrator

Code:

using LinearAlgebra
using PyPlot
using SparseArrays

# Discrete dynamics
h = 0.1   # time step
A = [1 h; 0 1]
B = [0.5*h*h; h]

n = 2     # number of state
m = 1     # number of controls
Tfinal = 100.0 # final time
N = Int(Tfinal/h)+1    # number of time steps
thist = Array(range(0,h*(N-1), step=h));

# Initial conditions
x0 = [1.0; 0]

# Cost weights
Q = sparse(1.0*I(2))
R = sparse(0.1*I(1))
Qn = sparse(1.0*I(2))

#Cost function
function J(xhist,uhist)
    cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]
    for k = 1:(N-1)
        cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*(uhist[k]'*R*uhist[k])[1]
    end
    return cost
end

# Cost
H = blockdiag(R, kron(I(N-2), blockdiag(Q,R)), Qn);

# Constraints
C = kron(I(N-1), [B -I(2)])
for k = 1:N-2
    C[(k*n).+(1:n), (k*(n+m)-n).+(1:n)] .= A
end
d = [-A*x0; zeros(size(C,1)-n)];

# Solve the linear system
y = [H C'; C zeros(size(C,1),size(C,1))]$$zeros(size(H,1)); d]

# Get state history
z = y[1:size(H,1)]   # states and controls [u0,x1,u1,...,xN]
Z = reshape(z,n+m,N-1)
xhist = Z[m+1:end,:]
xhist = [x0 xhist]

# Get control history
uhist = Z[1,:];

# Plot x1 vs. x2, u vs. t, x vs. t, etc.
times = range(0,h*(N-1), step=h)

clf()
figure(figsize=(12,3.5))
subplot(1,2,1)
plot(times, xhist[1,:], label="Position")
plot(times, xhist[2,:], label="Velocity")
xlabel("Time")
title("State Trajectory")
legend()

subplot(1,2,2)
plot(times[1:end-1], uhist, label="Control Input")
xlabel("Time")
title("Control Input")
legend()

display(gcf())
savefig("lqr-qp.png", dpi=300, bbox_inches="tight")

Result:

Compute time is much shorter than shooting method, and a longer time horizon is available.

4. A closer look at the LQR QP

(1) KKT system

The KKT system for LQR is very sparse (lots of zeros) and has a lot of structure.
$$\left[\begin{array}{c}\begin{array}{ccccccccc}\mathbf{R}& & & & & & {\mathbf{B}}^{\top }& & \\ & \mathbf{Q}& & & & & -\mathbf{I}& {\mathbf{A}}^{\top }& \\ & & \mathbf{R}& & & & & {\mathbf{B}}^{\top }& \\ & & & \mathbf{Q}& & & & -\mathbf{I}& {\mathbf{A}}^{\top }\\ & & & & \mathbf{R}& & & & {\mathbf{B}}^{\top }\\ & & & & & {\mathbf{Q}}_N& & & -\mathbf{I}\\ \mathbf{B}& -\mathbf{I}& & & & & & & \\ & \mathbf{A}& \mathbf{B}& -\mathbf{I}& & & & 0& \\ & & & \mathbf{A}& \mathbf{B}& -\mathbf{I}& & & \end{array}\end{array}\right]\left[\begin{array}{c}\begin{array}{c}{\mathbf{u}}_1\\ {\mathbf{x}}_2\\ {\mathbf{u}}_2\\ {\mathbf{x}}_3\\ {\mathbf{u}}_3\\ {\mathbf{x}}_4\\ {\lambda }_2\\ {\lambda }_3\\ {\lambda }_4\end{array}\end{array}\right]=\left[\begin{array}{c}\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\\ 0\\ -\mathbf{A}{\mathbf{x}}_1\\ 0\\ 0\end{array}\end{array}\right]$$

(2) Solving procedure (Riccati recursion)

i. ${\mathbf{Q}}_N{\mathbf{x}}_4-{\lambda }_4=0\Rightarrow {\lambda }_4={\mathbf{Q}}_N{\mathbf{x}}_4$
ii. Substitute ${\lambda }_4$ into the last equation:
$\mathbf{R}{\mathbf{u}}_3+{\mathbf{B}}^{\top }{\lambda }_4=\mathbf{R}{\mathbf{u}}_3+{\mathbf{B}}^{\top }{\mathbf{Q}}_N{\mathbf{x}}_4=0$
Plug in dynamics for ${\mathbf{x}}_4$:
$\mathbf{R}{\mathbf{u}}_3+{\mathbf{B}}^{\top }{\mathbf{Q}}_N\left(\mathbf{A}{\mathbf{x}}_3+\mathbf{B}{\mathbf{u}}_3\right)=0$
$\Rightarrow {\mathbf{u}}_3=-{\left(\mathbf{R}+{\mathbf{B}}^{\top }{\mathbf{Q}}_N\mathbf{B}\right)}^{-1}{\mathbf{B}}^{\top }{\mathbf{Q}}_N\mathbf{A}{\mathbf{x}}_3\triangleq -{\mathbf{K}}_3{\mathbf{x}}_3$
iii. Substitute ${\mathbf{u}}_3$ into the fourth equation:
$\mathbf{Q}{\mathbf{x}}_3-{\lambda }_3+{\mathbf{A}}^{\top }{\lambda }_4=0$
Plug in ${\lambda }_4$: $\mathbf{Q}{\mathbf{x}}_3-{\lambda }_3+{\mathbf{A}}^{\top }{\mathbf{Q}}_N{\mathbf{x}}_4=0$
Plug in dynamics: $\mathbf{Q}{\mathbf{x}}_3-{\lambda }_3+{\mathbf{A}}^{\top }{\mathbf{Q}}_N\left(\mathbf{A}{\mathbf{x}}_3+\mathbf{B}{\mathbf{u}}_3\right)=0$
Plug in ${\mathbf{u}}_3$: $\mathbf{Q}{\mathbf{x}}_3-{\lambda }_3+{\mathbf{A}}^{\top }{\mathbf{Q}}_N\left(\mathbf{A}{\mathbf{x}}_3-\mathbf{B}{\mathbf{K}}_3{\mathbf{x}}_3\right)=0$
$\Rightarrow {\lambda }_3=\left(\mathbf{Q}+{\mathbf{A}}^{\top }{\mathbf{Q}}_N\left(\mathbf{A}-\mathbf{B}{\mathbf{K}}_3\right)\right){\mathbf{x}}_3\triangleq {\mathbf{P}}_3{\mathbf{x}}_2$

Now we have a recursion for ${\mathbf{K}}_k$ and ${\mathbf{P}}_k$:
$$\begin{array}{rl}& {\mathbf{P}}_N={\mathbf{Q}}_N,\\ & {\mathbf{K}}_k={\left(\mathbf{R}+{\mathbf{B}}^{\top }{\mathbf{P}}_{k+1}\mathbf{B}\right)}^{-1}{\mathbf{B}}^{\top }{\mathbf{P}}_{k+1}\mathbf{A},\\ & {\mathbf{P}}_k=\mathbf{Q}+{\mathbf{A}}^{\top }{\mathbf{P}}_{k+1}\left(\mathbf{A}-\mathbf{B}{\mathbf{K}}_k\right)\end{array}$$

• This is called the Riccati recursion/equation. We can solve the QP by doing a backward Riccati recursion followed by a forward rollout to compute ${\mathbf{x}}_{1:N}$ and ${\mathbf{u}}_{1:N-1}$ form initial conditions.

• General (dense) QP has complexity $\mathcal{O}(N^3(n+m{)}^3)$ ($N$ is the time horizon, $n$ is the state dimension, $m$ is the control dimension).

• Riccati recursion has complexity $\mathcal{O}(N(n+m{)}^3)$.

• Even more important: we now have a feedback policy ${\mathbf{u}}_k=-{\mathbf{K}}_k{\mathbf{x}}_k$ instead of an open-loop trajectory.

(3) Example: double integrator

Code:

P = zeros(n,n,N)
K = zeros(m,n,N-1)

P[:,:,N] .= Qn

#Backward Riccati recursion
for k = (N-1):-1:1
    K[:,:,k] .= (R + B'*P[:,:,k+1]*B)\(B'*P[:,:,k+1]*A)
    P[:,:,k] .= Q + A'*P[:,:,k+1]*(A-B*K[:,:,k])
end

#Forward rollout starting at x0
xhist = zeros(n,N)
xhist[:,1] = x0
uhist = zeros(m,N-1)
for k = 1:(N-1)
    uhist[:,k] .= -K[:,:,k]*xhist[:,k]
    xhist[:,k+1] .= A*xhist[:,k] + B*uhist[k]
end

• Plot the control feedback, we find that the feedback matrix $\mathbf{K}$ converges to a constant value.
  

• Using Julia’s dlqr function, we get the feedback matrix, which is the same as the converged value.

• Therefore, when we want to stabilize the system, we can use a constant feedback matrix (treat it as an infinite horizon LQR).