[最佳控制] 離散時間 穩態 LQR 控制問題 (1)

延續前篇，這次要介紹的是 Discrete Time Linear Quadratic Regulator in Infinite Horizon 或稱 Steady State LQR。

================
LQR Problem (Infinite Horizon LQR)：
考慮離散狀態方程：
$$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}$且 $(A,B)$ controllable。
定義 Performance index：
$$J(u) = \displaystyle \sum_{k=0}^{\infty} 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^*$ 使得成本函數 $J(u)$ 最小。
================

Comment:
讀者須注意到 Infinite Horizon 的 LQR問題要求計算 Performance index 為無窮級數和，此解必須保證收斂。以下定理告訴我們何時 此 Performance index 收斂

Lemma
考慮離散系統 $x(k+1) = A x(k) + B u(k)$，若 $(A,B)$ 可控制，且選 $Q, R >0$ 為正定矩陣，則上述 infinite horizon LQR 問題保證 閉迴路系統 狀態收斂到 $0$ 且 cost 為有界。

Proof: omitted. (see J. B. Rawlings and D. Q. Mayne, "Model Predictive Control: Theory and Design, p. 24", 2009)


現在我們可以開始求解 Infinite Horizon LQR問題：
Solution
回憶 Steady State Bellman Equation，為了符號簡便起見，我們寫成 functional equation 形式，
$$I(x) = \displaystyle \min_{u \in \Omega} \{J(x,u) + I(f(x,u)) \}$$ 上式中 $J(x,u)$ 為 Branch cost，亦即 $J(x,u) = x^T Q x + u^T R u$ (並非 $\sum_{k=0}^{\infty} (\cdot)...$)

首先我們猜一組解 $I(x) = x^T P x$ 且矩陣 $P$ 為對稱正定矩陣，亦即滿足 $P^T = P, P \succ 0$。我們之後會找到此 $P$ 應該長甚麼樣子。

將猜測的解代入上述的 Steady State Bellman Equation，故現在我們得到
$$I(x) = \min_{u \in \Omega} \{J(x,u) + I(f(x,u)) \}$$ 注意到 $I(f(x,u) = f(x,u)^T P f(x,u) = (Ax+Bu)^TP(Ax+Bu)$，故我們可得
  $$\begin{array}{l} I(x) = \mathop {\min }\limits_{u \in \Omega } \{ J(x,u) + I(f(x,u))\} \\ \Rightarrow {x^T}Px = \mathop {\min }\limits_u \left\{ {{x^T}Qx + {u^T}Ru + {{\left( {Ax + Bu} \right)}^T}P\left( {Ax + Bu} \right)} \right\}\\ \Rightarrow {x^T}Px = \mathop {\min }\limits_u \left\{ {{x^T}\left( {Q + {A^T}PA} \right)x + 2{x^T}{A^T}PBu + {u^T}Ru + {u^T}{B^T}PBu} \right\} \end{array}$$ 透過一階必要條件 FONC: $\frac{\partial }{{\partial u}} = 0$ 對上式右邊求解
  $$\begin{array}{l} 2{\left( {{x^T}{A^T}PB} \right)^T} + 2Ru + 2{B^T}PBu = 0\\ \Rightarrow {u^*} = - {\left( {R + {B^T}PB} \right)^{ - 1}}{B^T}PAx \end{array}$$ 現在將 $u^*$ 代回 $(*)$  可得 $$\begin{array}{l} {x^T}Px = \mathop {\min }\limits_u \left\{ {{x^T}\left( {Q + {A^T}PA} \right)x + 2{x^T}{A^T}PBu + {u^T}Ru + {u^T}{B^T}PBu} \right\}\\ \Rightarrow {x^T}Px = \left\{ {{x^T}\left\{ {Q + {A^T}PA - {A^T}PB{{\left( {R + {B^T}PB} \right)}^{ - 1}}{B^T}PA} \right\}x} \right\} \end{array}$$ 比較左右兩邊可得到 $P$ 必須滿足下式： $$P = Q + {A^T}PA - {A^T}PB{\left( {R + {B^T}PB} \right)^{ - 1}}{B^T}PA$$ 此式稱為 Discrete Time Algebraic Ricatti Equation (ARE)，一般而言，可利用 MATLA 指令 dare(A,B,Q, R) 求解 P。

由於 $u^* = - {\left( {R + {B^T}PB} \right)^{ - 1}}{B^T}PAx$，其中除了 $P$ 未定之外，其餘所需要的參數都已知且皆與跌代時間無關，故此無窮時間LQR問題得到的 最佳控制力為 Time invariant。

現在我們總結如下：求解無窮時間的LQR問題只要做兩個步驟即可

STEP 1: 求解一次 Algebraic Ricatti Equation 得到 $P$ (利用 MATLAB: dare.m 或者徒手計算)

$$P = Q + {A^T}PA - {A^T}PB{\left( {R + {B^T}PB} \right)^{ - 1}}{B^T}PA$$ STEP2 : 將 $P$ 代入 ${u^*} =  - {\left( {R + {B^T}PB} \right)^{ - 1}}{B^T}PAx$

下面我們看個例子：

Example:
考慮一個離散時間線性系統狀態方程：
$$x_1(k+1) = x_2(k) \\ x_2(k+1) = x_1(k) + u(k)$$ 且考慮 Cost function：
$$J = \sum_{k=0}^{\infty}2x_1^2(k) + 2x_1(k)x_2(k) + x_2^2(k) + 3u^2(k)$$ 且控制力具有如下形式：
$$u(k) = K_1 x_1(k) + K_2 x_2(k)$$ 試求 $K_1, K_2$ 使 上述 Cost function 最小：

Solution
首先定義  $x(k) := [x_1(k), x_2(k)]^T$ ，則我們有
$$x\left( {k + 1} \right) = \underbrace {\left[ {\begin{array}{*{20}{c}} 0&1\\ 1&0 \end{array}} \right]}_A\left[ {\begin{array}{*{20}{c}} {{x_1}(k)}\\ {{x_2}(k)} \end{array}} \right] + \underbrace {\left[ {\begin{array}{*{20}{c}} 0\\ 1 \end{array}} \right]}_Bu(k)$$ 與 Cost function
$$\begin{array}{l} J = \sum\limits_{k = 0}^\infty {(2x_1^2(} k) + 2{x_1}(k){x_2}(k) + x_2^2(k) + 3{u^2}(k))\\ \begin{array}{*{20}{c}} {} \end{array} = x{\left( k \right)^T}\underbrace {\left[ {\begin{array}{*{20}{c}} 2&1\\ 1&1 \end{array}} \right]}_Qx\left( k \right) + \underbrace 3_R{u^2}(k) \end{array}$$ 那麼現在此問題變成 Steady-state LQR problem，故由前述討論可知我們有 Optimal feedback control 為
$$u^*(k) = -(R+B^T P B)^{-1} B^T PA \cdot x(k)$$ 其中 $P$ 滿足 $P=P^T, P \succ 0$ 可由 ARE
$$P= A^TPA - A^T PB (R+ B^TPB)^{-1}B^TPA+Q$$ 利用 MATLAB 指令 dare(A,B,Q,R) 解得 $P = \left[ {\begin{array}{*{20}{c}} {3.7841}&{1.6815}\\ {1.6815}&{4.4022} \end{array}} \right]$ 現在將 $P$ 帶回 $u^*$中
$$u\left( k \right) = - \left[ {\begin{array}{*{20}{c}} {0.5947}&{0.2272} \end{array}} \right]x\left( k \right)$$ i.e., $K_1 = -05947, K_2 =-0.2272$. $\square$