最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（中）

1. Introduction of Optimal Control (OC)

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

1.1 System dynamics

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

1.2 Optimal control problem, cost function and constraints formulation

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

1.3 Control policy

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

1.4 Bellman’s Principle of Optimality (BPO) and Dynamic Programming (DP)

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

1.5 Hamiltonian-Jacobian-Bellman

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

1.6 Pontryagin’s Maximum Principle (PMP)

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

2. Linear Quadratic Regulator (LQR)

Linear Quadratic Regulator is a type of optimal control method that finds a feedback controller for a linear system that minimizes a quadratic cost function. The cost function is often defined as a sum of the deviations of the state and the control input from their desired values, in quadratic form. The optimal controller (regulator) has the form $\mathbf{u}=-K\mathbf{x}$, where $K$ is a matrix that depends on the system dynamics and the cost function parameters.

The history of LQR dates back to the 1950s and 1960s, when researchers such as Richard Bellman, Rudolf Kalman and John Zames developed the theory of dynamic programming, optimal control and linear systems. The LQR problem was one of the first and most fundamental problems that could be solved by these methods. The LQR algorithm was also extended to handle stochastic disturbances and imperfect measurements, resulting in the linear-quadratic-Gaussian (LQG) problem.

LQR is widely used in engineering applications because it is easy to implement (given acceptable state identification and state estimation), robust, and efficient. It is also a popular benchmark for other control algorithms, such as reinforcement learning.

The practical implementation of LQR involves solving a matrix equation called the algebraic Riccati equation (ARE), which gives the optimal feedback gain matrix K. The ARE can be derived by applying the Hamilton-Jacobi-Bellman equation to the LQR problem, or by using variational methods. The ARE can be solved numerically by various algorithms, such as Schur-decomposition, Newton iteration or Riccati recursion.

Firstly, define the OCP of LQR as follows:

$$\begin{array}{rl}\underset{{\mathbf{u}}_{1:N-1}\in \mathcal{U}}{\min }\mathbf{J}({\mathbf{x}}_1,{\mathbf{u}}_{1,...,N-1})& =\frac{1}{2}\sum _{k=1}^{N-1}{\mathbf{x}}_k^T{Q}_k{\mathbf{x}}_k+{\mathbf{u}}_k^T{R}_k{\mathbf{u}}_k+\frac{1}{2}{\mathbf{x}}_N^T{Q}_N{\mathbf{x}}_N\\ s.t.{\mathbf{x}}_{k+1}& =A_k{\mathbf{x}}_k+B_k{\mathbf{u}}_k,k=1,...,N-1\end{array}\text{\hspace{1em}(25)}$$

with $Q⪰0$ positive defined, $R\succ 0$ positive semi-definite, and initial state ${\mathbf{x}}_1$ given. The cost function is quadratic in both state and control input. The state equation is linear. The control input is unconstrained. The terminal state is penalized.

We will derive LQR feedback gain with several approaches below.

2.1 LQR with indirect shooting based on PMP

Define the Hamiltonian as follows:

$$\begin{array}{rl}H(\mathbf{x},\mathbf{u},\mathbf{\lambda })& =\frac{1}{2}{\mathbf{x}}^{T}Q\mathbf{x}+\frac{1}{2}{\mathbf{u}}^{T}R\mathbf{u}+{\mathbf{\lambda }}^T(A\mathbf{x}+B\mathbf{u})\\ & =\frac{1}{2}{\mathbf{x}}^{T}Q\mathbf{x}+\frac{1}{2}{\mathbf{u}}^{T}R\mathbf{u}+{\mathbf{\lambda }}^{T}A\mathbf{x}+{\mathbf{\lambda }}^{T}B\mathbf{u}\end{array}\text{\hspace{1em}(26)}$$

with PMP, we have
$${\mathbf{x}}_{k+1}={\nabla }_{\lambda }H=A{\mathbf{x}}_k+B{\mathbf{u}}_k\text{\hspace{1em}(27a)}$$
$${\lambda }_k={\nabla }_{\mathbf{x}}H=Q{\mathbf{x}}_k+A^T{\mathbf{\lambda }}_{k+1}\text{\hspace{1em}(27b)}$$
with terminal condition ${\mathbf{\lambda }}_N=Q{\mathbf{x}}_N.$ (Please note that we are assuming linear time-invariant system here.)

In order to find optimal control ${\mathbf{u}}^{\ast }$, we can set the derivative of Hamiltonian with respect to $\mathbf{u}$ to zero:
$$\frac{\partial H}{\partial \mathbf{u}}=R{\mathbf{u}}_k+{\mathbf{\lambda }}_{k+1}^{T}B=0\text{\hspace{1em}(28)}$$
from which, we have:
$${\mathbf{u}}_k^{\ast }=-R^{-1}{B}^T{\mathbf{\lambda }}_{k+1}\text{\hspace{1em}(29)}$$

Then the general procedure for solving LQR problem with indirect shooting is as follows:

1. Start with an initial guess of ${\mathbf{u}}_{1:N-1}.$
2. Roll-out with the initial state ${\mathbf{x}}_1$ and the control sequence ${\mathbf{u}}_{1:N-1}$ to get ${\mathbf{x}}_{1:N}.$
3. Solve the terminal condition ${\mathbf{\lambda }}_N=Q{\mathbf{x}}_N$ and Eq.27 to get ${\mathbf{\lambda }}_{1:N-1}$ and get $\Delta \mathbf{u}$ with Eq.29.
4. Roll out with a line-search on $\Delta \mathbf{u}.$
5. Repeat step 3 and 4 until convergence.

2.2 LQR as Quadratic Programming

The LQR problem can also be formulated as a quadratic programming problem, which can then be solved by various algorithms, such as active-set method, interior-point method, and so on.

To start with, we can re-organize the state and control sequence stacked as follows:
$$\mathbf{z}:={\left[\begin{array}{cccccccc}{\mathbf{u}}_1& {\mathbf{x}}_2& \cdots & {\mathbf{u}}_2& {\mathbf{x}}_3& \cdots & {\mathbf{u}}_{N-1}& {\mathbf{x}}_N\end{array}\right]}^T\text{\hspace{1em}(30)}$$

and correspondingly the cost function weighting matrix as follows:
$$\mathbf{H}:=\left[\begin{array}{cccccc}{\mathbf{R}}_1& & & & & \\ & {\mathbf{Q}}_2& & & & \\ & & {\mathbf{R}}_2& & & \\ & & & {\mathbf{Q}}_3& & \\ & & & & \ddots & \\ & & & & & {\mathbf{Q}}_N\end{array}\right]\text{\hspace{1em}(31)}$$

(please be noted that we are omitting ${\mathbf{Q}}_1$ cause we assume that the initial state is fixed and no longer a decision variable.)

Then the cost function can be written as:
$$J(\mathbf{z})=\frac{1}{2}{\mathbf{z}}^T\mathbf{H}\mathbf{z}\text{\hspace{1em}(32)}$$

Plug the dynamics into the stacked state and control sequence, we have:
$$\underset{\text{C matrix}}{\underbrace{\left[\begin{array}{ccccccc}{B}_1& -I& 0& \cdots & & & \\ & A& B& -I& 0& \cdots & \\ & & A& B& -I& 0& \cdots \\ & & & \ddots & \ddots & \ddots & \\ & & & & A& B& -I\end{array}\right]}}\underset{\text{z}}{\underbrace{\left[\begin{array}{c}{\mathbf{u}}_1\\ {\mathbf{x}}_2\\ {\mathbf{u}}_2\\ ⋮\\ {\mathbf{x}}_N\end{array}\right]}}=\underset{\text{D matrix}}{\underbrace{\left[\begin{array}{c}-\mathbf{A}{\mathbf{x}}_1\\ 0\\ 0\\ ⋮\\ 0\end{array}\right]}}\text{\hspace{1em}(33)}$$

which can serve as the equality constraint for the quadratic programming problem.

To be more specific, the quadratic programming problem for LQR can be formulated as follows:

$$\begin{gathered} \underset{\mathbf{z}}{\mathrm{minimize}}\frac{1}{2}{\mathbf{z}}^T\mathbf{H}\mathbf{z} \\ s.t.\mathbf{C}\mathbf{z}=\mathbf{D}\text{\hspace{1em}(34)} \end{gathered}$$

Then the Lagrangian of the QP will be:
$$L(\mathbf{z},\mathbf{\lambda })=\frac{1}{2}{\mathbf{z}}^T\mathbf{H}\mathbf{z}+{\mathbf{\lambda }}^T(\mathbf{C}\mathbf{z}-\mathbf{D})\text{\hspace{1em}(35)}$$

whose KKT condition is:
$$\begin{array}{rl}\frac{\partial L}{\partial \mathbf{z}}& =\mathbf{H}\mathbf{z}+{\mathbf{C}}^T\mathbf{\lambda }=0\\ \frac{\partial L}{\partial \mathbf{\lambda }}& =\mathbf{C}\mathbf{z}-\mathbf{D}=0\end{array}\text{\hspace{1em}(36)}$$

The KKT condition Eq.36 can be organized as a linear system:
$$\left[\begin{array}{cc}H& C^T\\ C& 0\end{array}\right]\left[\begin{array}{c}z\\ \lambda \end{array}\right]=\left[\begin{array}{c}0\\ D\end{array}\right]\text{\hspace{1em}(37)}$$
which can be solved analytically.

Beyond the analytically solution of the KKT condition, we can take a closer look at Eq.36, which is pretty sparse and has a block-tridiagonal structure. This structure can be exploited to solve the KKT condition efficiently.

To get started, we first expose the block-tridiagonal structure of the KKT condition Eq.36 as follows:
$$\begin{array}{c}\#1\\ \#2\\ \#3\\ \#4\\ \#5\\ \#6\\ \#7\\ \#8\\ \#9\end{array}\left[\begin{array}{ccccccccc}\mathbf{R}& & & & & & {\mathbf{B}}^T& & \\ & \mathbf{Q}& & & & & -\mathbf{I}& {\mathbf{A}}^T& \\ & & \mathbf{R}& & & & & {\mathbf{B}}^T& \\ & & & \mathbf{Q}& & & & -\mathbf{I}& A^T\\ & & & & \mathbf{R}& & & & {\mathbf{B}}^T\\ & & & & & {\mathbf{Q}}_4& & & -\mathbf{I}\\ \mathbf{B}& -\mathbf{I}& & & & & 0& \cdots & 0\\ & \mathbf{A}& \mathbf{B}& -\mathbf{I}& & & ⋮& \ddots & ⋮\\ & & & \mathbf{A}& \mathbf{B}& -\mathbf{I}& 0& \cdots & 0\end{array}\right]\left[\begin{array}{c}{\mathbf{u}}_1\\ {\mathbf{x}}_2\\ {\mathbf{u}}_2\\ {\mathbf{x}}_3\\ {\mathbf{u}}_3\\ {\mathbf{x}}_4\\ {\mathbf{\lambda }}_2\\ {\mathbf{\lambda }}_3\\ {\mathbf{\lambda }}_4\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\\ 0\\ -{\mathbf{Ax}}_1\\ 0\\ 0\end{array}\right]\text{\hspace{1em}(38)}$$
(here we are just using four time steps as an example, the structure is the same for any number of time steps)

Then we can solve the KKT condition Eq.36 by forward substitution and backward substitution.

Start from line #6, we have
$${\mathbf{Q}}_4{\mathbf{x}}_4-\mathbf{I}{\mathbf{\lambda }}_4=0⟹{\mathbf{\lambda }}_4={\mathbf{Q}}_4{\mathbf{x}}_4\text{\hspace{1em}(39)}$$

plugging ${\mathbf{\lambda }}_4$ back into #5 and working backwards,
$$R{u}_3+B^T{\lambda }_4=0⟹R{u}_3+B^T{Q}_4{x}_4=0\text{\hspace{1em}(40)}$$
plugging system dynamics $x_4=\mathbf{A}{x}_3+B{u}_3$ will get,
$$R{u}_3+B^T{Q}_4(A{x}_3+B{u}_3)=0\text{\hspace{1em}(41)}$$

$$⟹u_3=\underset{{\mathrm{K}}_3}{\underbrace{-(R+B^T{Q}_{4}B{)}^{-1}{B}^T{Q}_{4}A}}{x}_3\text{\hspace{1em}(42)}$$
which is exactly the most common form of feedback gain we can find for LQR in textbook.

Repeat for line #4 and plug ${\mathbf{\lambda }}_4$,

$$\mathbf{Q}{\mathbf{x}}_3-\mathbf{I}{\mathbf{\lambda }}_3+{\mathbf{A}}^T{\mathbf{\lambda }}_4=0⟹\mathbf{Q}{\mathbf{x}}_3-\mathbf{I}{\mathbf{\lambda }}_3+{\mathbf{A}}^T{\mathbf{Q}}_4{\mathbf{x}}_4=0\text{\hspace{1em}(43)}$$
again, plugging the system dynamics $x_4=A{x}_3+B{u}_3$ leads to
$$Q{x}_3-I{\lambda }_3+A^T{Q}_4(A{x}_3+B{u}_3)=0\text{\hspace{1em}(44)}$$
since we have solved $u_3=-K_3{x}_3$ in the previous step, we can solve ${\mathbf{\lambda }}_3$ as,

$$\begin{gathered} \mathbf{Q}{\mathbf{x}}_3-\mathbf{I}{\lambda }_3+{\mathbf{A}}^T{Q}_4(\mathbf{A}{\mathbf{x}}_3-\mathbf{B}{\mathbf{K}}_3{\mathbf{x}}_3)=0 \\ ⟹{\mathbf{\lambda }}_3=\underset{{\mathbf{P}}_3}{\underbrace{\left[\mathbf{Q}+{\mathbf{A}}^T{\mathbf{Q}}_4(\mathbf{A}-\mathbf{B}{\mathbf{K}}_3)\right]{\mathbf{x}}_3}}\text{\hspace{1em}(45)} \end{gathered}$$

Now we have a recursion for $\mathbf{K}$ and $\mathbf{P}$ :

1. ${\mathbf{P}}_N={\mathbf{Q}}_N$
2. ${\mathbf{K}}_k=-(\mathbf{R}+{\mathbf{B}}^T{\mathbf{P}}_{k+1}\mathbf{B}){\mathbf{B}}^T{\mathbf{P}}_{k+1}\mathbf{A}$
3. ${\mathbf{P}}_k=\mathbf{Q}+{\mathbf{A}}^T{\mathbf{P}}_{k+1}(\mathbf{A}-\mathbf{B}{\mathbf{K}}_k)$

which is called the algebraic Riccati Equation/Recursion (ARE).

We can then solve the LQR QP problem by doing a backward Riccati recursion followed by a forward roll-out to compute ${\mathbf{x}}_{1:N}$ and ${\mathbf{u}}_{1:N-1}$ from the initial state. This is part of the reason why people love LQR, which gives nice and efficient closed-form solutions.

The Linear Quadratic Regulator has several extensions, e.g., the infinite-horizon LQR, the stochastic LQR, etc., which are currently out of the scope of this tutorial.

2.3 LQR as Dynamic Programming

Besides indirect shooting and the Riccati recursion, LQR can also be solved with DP formulation. The DP formulation is more general and can be applied to non-linear systems. Recall the general procedure of DP in 1.4, we iterate over time and solve the value function recursively. For LQR, the value function at terminal time is defined as
$$\begin{array}{rl}{\mathbf{V}}_N(\mathbf{x})=Terminalcost& =\frac{1}{2}{\mathbf{x}}_N^T{\mathbf{Q}}_N{\mathbf{x}}_N\\ & =\frac{1}{2}{\mathbf{x}}_N^T{\mathbf{P}}_N{\mathbf{x}}_N\end{array}\text{\hspace{1em}(46)}$$

and working backwards,
$$\begin{array}{rl}{\mathbf{V}}_{N-1}(\mathbf{x})& =\underset{{\mathbf{u}}_{N-1}}{\min }\frac{1}{2}{\mathbf{x}}_{N-1}^T{\mathbf{Q}}_k{\mathbf{x}}_{N-1}+\frac{1}{2}{\mathbf{u}}_{N-1}^T\mathbf{R}{\mathbf{u}}_{N-1}+{\mathbf{V}}_N\\ & =\underset{{\mathbf{u}}_{N-1}}{\min }\frac{1}{2}{\mathbf{x}}_{N-1}^T{\mathbf{Q}}_k{\mathbf{x}}_{N-1}+\frac{1}{2}{\mathbf{u}}_{N-1}^T\mathbf{R}{\mathbf{u}}_{N-1}+\frac{1}{2}{\mathbf{x}}_N^T{\mathbf{P}}_N{\mathbf{x}}_N\\ & =\underset{{\mathbf{u}}_{N-1}}{\min }\frac{1}{2}{\mathbf{x}}_{N-1}^T{\mathbf{Q}}_k{\mathbf{x}}_{N-1}+\frac{1}{2}{\mathbf{u}}_{N-1}^T\mathbf{R}{\mathbf{u}}_{N-1}+\\ & \frac{1}{2}(\mathbf{A}{\mathbf{x}}_{N-1}+\mathbf{B}{\mathbf{u}}_{N-1}{)}^T{\mathbf{P}}_N(\mathbf{A}{\mathbf{x}}_{N-1}+\mathbf{B}{\mathbf{u}}_{N-1})\end{array}\text{\hspace{1em}(47)}$$

in order to find the optimal ${\mathbf{u}}_{N-1}^{\ast }$,we set
$$\frac{\partial {\mathbf{V}}_{N-1}}{\partial {\mathbf{u}}_{N-1}}=0\text{\hspace{1em}(48)}$$

which leads to

$$\begin{gathered} \mathbf{R}{\mathbf{u}}_{N-1}+{\mathbf{B}}^T{\mathbf{P}}_N(\mathbf{A}{\mathbf{x}}_{N-1}+\mathbf{B}{\mathbf{u}}_{N-1})=0 \\ ⟹{\mathbf{u}}_{N-1}^{\ast }=\underset{{\mathrm{K}}_{\mathrm{N}-1}}{\underbrace{-(\mathbf{R}+{\mathbf{B}}^T{\mathbf{P}}_N\mathbf{B}{)}^{-1}\mathbf{B}{\mathbf{P}}_N\mathbf{A}}}{\mathbf{x}}_{N-1}\text{\hspace{1em}(49)} \end{gathered}$$

which is the feedback law for LQR, and to iterate for $\mathbf{P}$, we need to plug the optimal ${\mathbf{u}}_{N-1}^{\ast }$ back to the value function ${\mathbf{V}}_{N-1}$, which leads to
$${\mathbf{V}}_{N-1}(\mathbf{x})=\frac{1}{2}{\mathbf{x}}_{N-1}^T\underset{{\mathrm{P}}_{\mathrm{N}-1}}{\underbrace{\left[\mathbf{Q}+{\mathbf{K}}_{N-1}^T\mathbf{R}{\mathbf{K}}_{N-1}+(\mathbf{A}-{\mathbf{BK}}_{N-1}{)}^T{\mathbf{P}}_N(\mathbf{A}-\mathbf{B}{\mathbf{K}}_{N-1})\right]}}{\mathbf{x}}_{N-1}\text{\hspace{1em}(50)}$$
Now with Eq. 46, 49 and 50 we have a recursion for solving the LQR problem with general DP algorithm,

1. Initialize ${\mathbf{P}}_N={\mathbf{Q}}_N$;
2. For $k=N-1,\cdots ,1$:
3. ${\mathbf{K}}_k=-(\mathbf{R}+{\mathbf{B}}^T{\mathbf{P}}_{k+1}\mathbf{B}{)}^{-1}{\mathbf{B}}^T{\mathbf{P}}_{k+1}\mathbf{A}$
4. ${\mathbf{P}}_k=\mathbf{Q}+{\mathbf{K}}_k^T\mathbf{R}{\mathbf{K}}_k+(\mathbf{A}-\mathbf{B}{\mathbf{K}}_k{)}^T{\mathbf{P}}_{k+1}(\mathbf{A}-\mathbf{B}{\mathbf{K}}_k)$

Compared to the Riccati recursion, the DP formulation is more computationally expensive, which will explode as the system dimension grows, known as the Bellman’s Curse of Dimensionality.

Still, this is the linear case for DP, while for non-linear case, we need to solve the Hamilton-Jacobi-Bellman equation, which is even more computationally expensive.

3. Differential Dynamic Programming (DDP)

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（下）.

3.1 Local Approximation

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（下）.

3.2 Backward Pass

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（下）.

3.3 Line Search

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（下）.

3.4 Forward Pass

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（下）.