[最佳控制] 離散時間 LQR- Finite Time Horizon

這次要介紹 控制理論中一個重要的結果：
 Discrete Time Linear Quadratic Regulator (LQR) in Finite Time Horizon，
中文翻譯為 離散時間線性二次調節器，我們這邊會針對此問題利用 Dynamic Programming 的方法來逐步求解

================
LQR Problem (Finite Horizon)：
考慮狀態方程：
\[
x(k+1) = A x(k) + B u(k)
\]其中 $x(k) \in \mathbb{R}^n, A\in \mathbb{R}^{n \times n}, B \in \mathbb{R}^{n \times m}, u(k) \in \mathbb{R}^{m \times 1}$
且 考慮 Performance index：
\[
J(u) = \displaystyle \sum_{k=0}^{N-1} x^T(k+1) Q x(k+1) + u^T(k) R u(k)
\] 其中 $Q, R$ 必須滿足 $Q^T = Q, Q \succ 0$， $R^T = R, R \succ 0$。 (亦即 $Q, R$ 必須為 對稱 + 正定 矩陣)

試求出一組最佳控制力序列 $u(N-1), u(N-2),... u(0)$ 使得成本函數 $J(u)$ 最小。
================

Comment:
1. LQR 顧名思義是其具有系統狀態方程為線性 $x(k+1) = Ax(k)+Bu(k)$ 與  Performance Index 中的項都為二次式。
\[
J(u) = \displaystyle \sum_{k=0}^{N-1} x^T(k+1) Q x(k+1) + u^T(k) R u(k)
\]
2. 上述對於矩陣 $Q, R$ 的對稱與正定假設是必須的 (之後需求在求解最佳控制力序列的時候需要求解反矩陣，故需要這些性質。)

3. 上述 LQR in Finite Horizon的問題所求得的最佳控制力序列為 Time Varying 。(此性質會在下面求解的時候再度強調。)

4. 若 Performance index 考慮 $N \rightarrow \infty$，亦即
\[
J(u) = \displaystyle \sum_{k=0}^{\infty} x^T(k+1) Q x(k+1) + u^T(k) R u(k)
\]則我們說此問題為 LQR in Infinite Horizon 或稱 Steady State LQR。這類問題會在之後再做介紹。(此類問題所得到的最佳控制序列將不再是 Time Varying，且只需求解一次 Algebraic Ricatti Equation 即可獲得最佳控制力序列)



Solution of Discrete Time LQR problem in Finite Horizon 
現在我們開始進行求解。

這邊我們使用 Dynamic Programming 方法來求解上述 LQR問題。回憶 Dynamic Programming Equation 的定義：
\[I\left( {x\left( l \right),N - l} \right) = \mathop {\min }\limits_{u\left( l \right) \in {\Omega _l}} \left\{ {J\left( {x\left( l \right),u\left( l \right)} \right) + I\left( {x\left( {l + 1} \right),N - \left( {l + 1} \right)} \right)} \right\}
\] 考慮 Optimal Cost of one-step-to-go ($l=N-1$):
\[\begin{array}{l}
 \Rightarrow I\left( {x\left( {N - 1} \right),1} \right) = \min \left\{ {J\left( {x\left( {N - 1} \right),u\left( {N - 1} \right)} \right) + I\left( {x\left( N \right),0} \right)} \right\}\\
\begin{array}{*{20}{c}}
{}&{}&{}&{}&{}&{}
\end{array} = \min \left\{ {{x^T}(N)Qx(N) + {u^T}(N - 1)Ru(N - 1)} \right\}
\end{array}
\]將系統狀態方程 $x(k+1) = A x(k) + B u(k) $ 代入：
\[\begin{array}{l}
 \Rightarrow I\left( {x\left( {N - 1} \right),1} \right) = \\
\begin{array}{*{20}{c}}
{}&{}&{}&{}
\end{array}\min \left\{ \begin{array}{l}
{\left[ {Ax\left( {N - 1} \right) + Bu\left( {N - 1} \right)} \right]^T}Q\left[ {Ax\left( {N - 1} \right) + Bu\left( {N - 1} \right)} \right]\\
 + {u^T}(N - 1)Ru(N - 1)
\end{array} \right\}\\
 \Rightarrow I\left( {x\left( {N - 1} \right),1} \right) = \min \left\{ \begin{array}{l}
{x^T}\left( {N - 1} \right){A^T}QAx\left( {N - 1} \right)\\
\begin{array}{*{20}{c}}
{}
\end{array} + 2{x^T}\left( {N - 1} \right){A^T}QBu\left( {N - 1} \right)\\
\begin{array}{*{20}{c}}
{}
\end{array} + {u^T}\left( {N - 1} \right){B^T}QBu\left( {N - 1} \right)\\
\begin{array}{*{20}{c}}
{}
\end{array} + {u^T}(N - 1)Ru(N - 1)
\end{array} \right\}
\end{array}
\] 由一階必要條件 FONC： ${\nabla _{u\left( {N - 1} \right)}}I\left( {x\left( {N - 1} \right),1} \right) = 0$ 可知
\[\begin{array}{l}
2{B^T}{Q^T}Ax\left( {N - 1} \right) + 2{B^T}QBu\left( {N - 1} \right) + 2Ru(N - 1) = 0\\
 \Rightarrow \left[ {R + {B^T}QB} \right]u(N - 1) =  - {B^T}{Q^T}Ax\left( {N - 1} \right)\\
 \Rightarrow u^*(N - 1) =  - \underbrace {{{\left[ {R + {B^T}QB} \right]}^{ - 1}}{B^T}QA}_{K\left( {N - 1} \right)}x\left( {N - 1} \right)
\end{array}
\]接著我們把上述求得的 1-step-to-go 的最佳控制力 $u^*(k)$ 代回 Optimal cost of 1-step-to-go，我們得到 (透過一些代數運算)
\[I\left( {x\left( {N - 1} \right),1} \right) = {x^T}\left( {N - 1} \right)\underbrace {{A^T}\left\{ {Q - QB{{\left[ {R + {B^T}QB} \right]}^{ - 1}}^T{B^T}Q} \right\}A}_{: = M\left( {N - 1} \right)}x\left( {N - 1} \right)
\]在做下一步跌代之前我們先給個 comments

Comments :
1.注意到上式中我們求 ${u^*}(N - 1) =  - {\left[ {R + {B^T}QB} \right]^{ - 1}}{B^T}QAx\left( {N - 1} \right)$ 需要計算反矩陣 ${\left[ {R + {B^T}QB} \right]^{ - 1}}$ ，故需檢驗反矩陣是否存在：不過這個問題可以被證明反矩陣確實存在： (因為 $R$ 為對稱正定矩陣，$B^TQB$ 為對稱半正定矩陣，由FACT: 正定矩陣+ 半正定矩陣 = 正定矩陣，且正定矩陣具有 eigenvalue 全為正，故可推知反矩陣存在。)

2. 上式所求得的最佳控制力 ${u^*}(N - 1) =  - {\left[ {R + {B^T}QB} \right]^{ - 1}}{B^T}QAx\left( {N - 1} \right) =  - K\left( {N - 1} \right)x\left( {N - 1} \right)$ 其中的 $K_{N-1}$ 稱作 Optimal Gain Matrix。此控制力具有回授控制形式 Feedback form。 (類似 $u=-Kx$ 的形式)

3. 最佳控制力儘管具有回授控制 (Feedback form)的形式，但在此問題中為時變得 Gain。此性質在下一步跌代會顯現出來。

4. 當我們有了上述第一步跌代的結果，整個 LQR問題就簡單許多，因為之後的跌代只有 $Q$ 矩陣會改變其餘結果均不變。我們直接看下一步跌代便會發現此性質：

Back to computation :
考慮 Optimal Cost of 2-steps-to-go： ($l=N-2$):
\[\begin{array}{l}
I\left( {x\left( {N - 2} \right),2} \right) = \min \left\{ {J\left( {x\left( {N - 2} \right),u\left( {N - 2} \right)} \right) + I\left( {x\left( {N - 1} \right),1} \right)} \right\}\\
 \Rightarrow I\left( {x\left( {N - 2} \right),2} \right)\\
\begin{array}{*{20}{c}}
{}&{}
\end{array} = \min \left\{ \begin{array}{l}
{x^T}(N - 1)Qx(N - 1) + {u^T}(N - 2)Ru(N - 2)\\
\begin{array}{*{20}{c}}
{}&{}&{}&{}
\end{array} + {x^T}\left( {N - 1} \right)M\left( {N - 1} \right)x\left( {N - 1} \right)
\end{array} \right\}\\
 \Rightarrow I\left( {x\left( {N - 2} \right),2} \right)\\
\begin{array}{*{20}{c}}
{}&{}
\end{array} = \min \left\{ {{x^T}(N - 1)\underbrace {\left[ {Q + M\left( {N - 1} \right)} \right]}_{: = \tilde Q}x\left( {N - 1} \right) + {u^T}(N - 2)Ru(N - 2)} \right\}
\end{array}
\]可以發現上式中只有 $Q$ 變成了 $\tilde{Q}$ 其餘參數均固定。故我們可以馬上寫下對應的最佳控制力 $u^*(N-2)$:
\[{u^*}(N - 2) =  - {\left[ {R + {B^T}\tilde QB} \right]^{ - 1}}{B^T}\tilde QAx\left( {N - 2} \right) = K (N-2) x(N-2)
\]其對應的 Optimal cost of 2 steps to go：
\[I\left( {x\left( {N - 2} \right),2} \right) = {x^T}\left( {N - 2} \right)\underbrace {{A^T}\left\{ {\tilde Q - \tilde QB{{\left[ {R + {B^T}\tilde QB} \right]}^{ - 1}}^T{B^T}\tilde Q} \right\}A}_{: = M\left( {N - 2} \right)}x\left( {N - 2} \right)
\]故我們只需要持續重複上述步驟，即可依序解得 $K(N-3), K(N-4),..., K(0)$ 與 $M(N-3), M(N-4),...,M(0)$ 與 $J^*$


Comment:
注意到 $u^*(N-2)$ 的 Gain $K(N-2)$ 與 $u^*(N-1)$ 的 Gain $K(N-1)$ 並不相同，此說明了之前所敘的時變特性(Time Varying Property)