跳到主要內容

[最佳控制] Finite Horizon LQR with Penalty on Final State

考慮 LQR 問題如下

成本函數
\[{V_N}({x_0},u): = \sum\limits_{i = 1}^{N - 1} {\underbrace {l(x\left( i \right),u\left( i \right))}_{{\rm{stage}}\begin{array}{*{20}{c}}
{}
\end{array}{\rm{cost}}}}  + \underbrace {{l_N}({x_N})}_{{\rm{terminal}}\begin{array}{*{20}{c}}
{}
\end{array}{\rm{cost}}}\]其中 \[\left\{ \begin{array}{l}
u: = \{ u(0),u(1),...,u(N - 1)\} \\
l(x,u): = \frac{1}{2}({x^T}Qx + {u^T}Ru)\\
{l_N}(x): = \frac{1}{2}{x^T}{P_f}x
\end{array} \right.\]另外假設 $Q \ge 0$  positive semi-definite 且 $R >0$ positive definite。

我們的目標:找到 $u^*=\{u^*(0),u^*(1),...,u^*(N-1)\}$ 使得
\[\begin{array}{l}
\mathop {\min }\limits_u {V_N}\left( {x\left( 0 \right),u} \right)\\
s.t.\;\;{x^ + } = Ax + Bu
\end{array}\]

現在利用 Backward Dynamic Programming,我們從最後的狀態 $x(N)$ 逐步回推最佳解,亦即觀察
\[{V_N}({x_0},u): = l({x_0},{u_0}) + l({x_1},{u_1}) + ... + \underbrace {l({x_{N - 1}},{u_{N - 1}}) + {l_N}({x_N})}_{{u_{N - 1}}\begin{array}{*{20}{c}}
{}
\end{array}{\rm{affects}}\begin{array}{*{20}{c}}
{}
\end{array}{\rm{only}}\begin{array}{*{20}{c}}
{}
\end{array}{\rm{there!}}}\]由 Optimality Principle 逐步求解的最佳解 必為整體最佳的一環,故此我們先最佳化 下列子問題
\[ \begin{array}{l}
\mathop {\min }\limits_{{u_{N - 1}}} l({x_{N - 1}},{u_{N - 1}}) + {l_N}({x_N})\\
s.t.\begin{array}{*{20}{c}}
{}
\end{array}{x_N} = A{x_{N - 1}} + B{u_{N - 1}}
\end{array}\]現在將拘束條件帶入,我們可得到以下無拘束最佳化問題:
\[\small \begin{array}{l}
\left\{ \begin{array}{l}
\mathop {\min }\limits_{{u_{N - 1}}} l({x_{N - 1}},{u_{N - 1}}) + {l_N}({x_N})\\
s.t.\begin{array}{*{20}{c}}
{}
\end{array}{x_N} = A{x_{N - 1}} + B{u_{N - 1}}
\end{array} \right.\\
 \Rightarrow \mathop {\min }\limits_{{u_{N - 1}}} \frac{1}{2}\left( {x_{N - 1}^TQx_{N - 1}^{} + u_{N - 1}^TRu_{N - 1}^{}} \right) + \frac{1}{2}{\left( {A{x_{N - 1}} + B{u_{N - 1}}} \right)^T}{P_f}\left( {A{x_{N - 1}} + B{u_{N - 1}}} \right)\\
 \Rightarrow \mathop {\min }\limits_{{u_{N - 1}}} \frac{1}{2}\left[ {x_{N - 1}^TQx_{N - 1}^{} + \underbrace {u_{N - 1}^TRu_{N - 1}^{}}_{{V_1}} + \underbrace {{{\left( {A{x_{N - 1}} + B{u_{N - 1}}} \right)}^T}{P_f}\left( {A{x_{N - 1}} + B{u_{N - 1}}} \right)}_{{V_2}}} \right] \ \ \ \ \ (*)
\end{array}\]現在我們可利用下列結果
------------
FACT:
\[\left\{ \begin{array}{l}
{V_1} = \frac{1}{2}{\left( {x - a} \right)^T}\Phi \left( {x - a} \right)\\
{V_2} = \frac{1}{2}{\left( {\Theta x - b} \right)^T}\Gamma \left( {\Theta x - b} \right)
\end{array} \right.\]且 $\Phi $ positive definite 與 $\Theta $ 為 positive semi-definite。則其和可表為\[V = {V_1} + {V_2} = \frac{1}{2}{\left( {x - v} \right)^T}\Omega \left( {x - v} \right) + d\]其中
\[\left\{ \begin{array}{l}
\Omega  = \Phi  + {\Theta ^T}\Gamma \Theta \\
v = {\left( {\Phi  + {\Theta ^T}\Gamma \Theta } \right)^{ - 1}}\left( {\Phi a + {\Theta ^T}\Gamma b} \right)\\
d = V\left( v \right)
\end{array} \right.\]
------------
觀察式 $(*)$,我們可利用上述 FACT 比較係數
\[\Phi  = R;\Theta  = B;\Gamma  = {P_f};v = u_{N - 1}^*;b =  - A{x_{N - 1}};a = 0
\]故在此階段的最佳解 $u_{N-1}^*$ 為
\[u_{N - 1}^* = \underbrace { - {{\left( {R + {B^T}{P_f}B} \right)}^{ - 1}}{B^T}{P_f}A}_{{K_{N - 1}}}{x_{N - 1}} = {K_{N - 1}}{x_{N - 1}}
\]且在此階段的 optimal cost (又稱 optimal cost to go) 為
\[V_{N - 1}^*\left( {{x_{N - 1}}} \right) = d + \frac{1}{2}x_{N - 1}^TQx_{N - 1}^{}
\]其中 $d$ 為前述 FACT 中的常數項,我們計算如下
\[\small \begin{array}{*{20}{l}}
{d = V\left( v \right) = {V_1}\left( v \right) + {V_2}\left( v \right)}\\
{\begin{array}{*{20}{c}}
{}&{}
\end{array} = \frac{1}{2}{{\left( {x - a} \right)}^T}\Phi \left( {x - a} \right) + \frac{1}{2}{{\left( {\Theta x - b} \right)}^T}\Gamma \left( {\Theta x - b} \right)}\\
{\begin{array}{*{20}{c}}
{}&{}
\end{array} = \frac{1}{2}{{\left( {{K_{N - 1}}{x_{N - 1}}} \right)}^T}R\left( {{K_{N - 1}}{x_{N - 1}}} \right) + \frac{1}{2}{{\left( {\left( {A + B{K_{N - 1}}} \right){x_{N - 1}}} \right)}^T}{P_f}\left( {\left( {A + B{K_{N - 1}}} \right){x_{N - 1}}} \right)}\\
{\begin{array}{*{20}{c}}
{}&{}
\end{array} = \frac{1}{2}x_{N - 1}^T\left[ {K_{N - 1}^TR{K_{N - 1}} + {{\left( {A + B{K_{N - 1}}} \right)}^T}{P_f}\left( {A + B{K_{N - 1}}} \right)} \right]{x_{N - 1}}}
\end{array}
\]故 optimal cost to go
\[\small \begin{array}{l}
V_{N - 1}^*\left( {{x_{N - 1}}} \right) = d + \frac{1}{2}x_{N - 1}^TQx_{N - 1}^{}\\
\begin{array}{*{20}{c}}
{}&{}&{}&{}
\end{array} = \frac{1}{2}x_{N - 1}^T\left[ {K_{N - 1}^TR{K_{N - 1}} + {{\left( {A + B{K_{N - 1}}} \right)}^T}{P_f}\left( {A + B{K_{N - 1}}} \right)} \right]{x_{N - 1}} + \frac{1}{2}x_{N - 1}^TQx_{N - 1}^{}\\
\begin{array}{*{20}{c}}
{}&{}&{}&{}
\end{array} = \frac{1}{2}x_{N - 1}^T\left[ {Q + K_{N - 1}^TR{K_{N - 1}} + {{\left( {A + B{K_{N - 1}}} \right)}^T}{P_f}\left( {A + B{K_{N - 1}}} \right)} \right]{x_{N - 1}}
\end{array}\]如果我們把最佳解 $u_{N - 1}^* = {K_{N - 1}}{x_{N - 1}} =  - {\left( {R + {B^T}{P_f}B} \right)^{ - 1}}{B^T}{P_f}A{x_{N - 1}}$ 帶入 optimal cost to go,可得
\[\small \begin{array}{l}
V_{N - 1}^*\left( {{x_{N - 1}}} \right) = \frac{1}{2}x_{N - 1}^T\left[ {Q + K_{N - 1}^TR{K_{N - 1}} + {{\left( {A + B{K_{N - 1}}} \right)}^T}{P_f}\left( {A + B{K_{N - 1}}} \right)} \right]{x_{N - 1}}\\
\begin{array}{*{20}{c}}
{}&{}&{}&{}
\end{array} = \frac{1}{2}x_{N - 1}^T\left[ {Q + {A^T}{P_f}A + K_{N - 1}^T\left( {R + {B^T}{P_f}B} \right){K_{N - 1}} + 2K_{N - 1}^T{B^T}{P_f}A} \right]{x_{N - 1}}\\
\begin{array}{*{20}{c}}
{}&{}&{}&{}
\end{array} = \frac{1}{2}x_{N - 1}^T\underbrace {\left[ {Q + {A^T}{P_f}A - {A^T}{P_f}B{{\left( {R + {B^T}{P_f}B} \right)}^{ - 1}}{B^T}{P_f}A} \right]}_{{M_{N - 1}}}{x_{N - 1}}
\end{array}\]但注意到我們並不曉得 $x_{N-1}$ 故我們需要繼續回頭解 $x_{N-2}$ 亦即 接著求解
\[\begin{array}{l}
\left\{ \begin{array}{l}
\mathop {\min }\limits_{{u_{N - 2}}} l\left( {{x_{N - 2}},{u_{N - 2}}} \right) + V_{N - 1}^*\left( {{x_{N - 1}}} \right)\\
s.t.\begin{array}{*{20}{c}}
{}
\end{array}{x_{N - 1}} = A{x_{N - 2}} + B{u_{N - 2}}
\end{array} \right.\\
 \Rightarrow \left\{ \begin{array}{l}
\mathop {\min }\limits_{{u_{N - 2}}} l\left( {{x_{N - 2}},{u_{N - 2}}} \right) + \frac{1}{2}x_{N - 1}^T{M_{N - 1}}{x_{N - 1}}\\
s.t.\begin{array}{*{20}{c}}
{}
\end{array}{x_{N - 1}} = A{x_{N - 2}} + B{u_{N - 2}}
\end{array} \right.
\end{array}\]但注意到先前我們已經解了
\[\left\{ \begin{array}{l}
\mathop {\min }\limits_{{u_{N - 1}}} l\left( {{x_{N - 1}},{u_{N - 1}}} \right) + \frac{1}{2}x_N^T{P_f}{x_N}\\
s.t.\begin{array}{*{20}{c}}
{}
\end{array}{x_N} = A{x_{N - 1}} + B{u_{N - 1}}
\end{array} \right.\]故讀者可直接比對上述兩者,即可得知只要將 前述我們所推出的解其中的 $P_f$ 矩陣換成 $M_{n-1}$ 即可馬上得到 $N-2$ stage的最佳解 與 optimal cost to go。亦即
\[\left\{ \begin{array}{l}
u_{N - 2}^* = {K_{N - 2}}{x_{N - 2}} =  - {\left( {R + {B^T}{M_{N - 1}}B} \right)^{ - 1}}{B^T}{M_{N - 1}}A{x_{N - 2}}\\
V_{N - 2}^*\left( {{x_{N - 2}}} \right) = \frac{1}{2}x_{N - 2}^T{M_{N - 2}}{x_{N - 2}}\\
{M_{N - 2}} = Q + {A^T}{M_{N - 1}}A - {A^T}{M_{N - 1}}B{\left( {R + {B^T}{M_{N - 1}}B} \right)^{ - 1}}{B^T}{M_{N - 1}}A
\end{array} \right.\]

Summary
透過跌代求解 Riccati equation 進而得到一組控制力序列$u(0),u(1),...u(N-1)$;亦即透過下列跌代式:
\[\begin{array}{l}
for\begin{array}{*{20}{c}}
{}
\end{array}k = N - 1,N - 2,...,0\\
\left\{ \begin{array}{l}
u_{}^*\left( k \right) = K\left( k \right)x\left( k \right),\begin{array}{*{20}{c}}
{}&{}
\end{array}\\
K\left( k \right) =  - {\left( {{B^T}M\left( {k + 1} \right)B + R} \right)^{ - 1}}{B^T}M\left( {k + 1} \right)A,
\end{array} \right.\\
\\
for\begin{array}{*{20}{c}}
{}
\end{array}k = N,N - 1,...,0\\
\left\{ \begin{array}{l}
M\left( {k - 1} \right): = Q + {A^T}M\left( k \right)A - {A^T}M\left( k \right)B{\left( {{B^T}M\left( k \right)B + R} \right)^{ - 1}}{B^T}M\left( k \right)A\\
M\left( N \right): = {P_f}\\
V_{}^*\left( k \right) = \frac{1}{2}{x^T}\left( k \right)M\left( {k + 1} \right)x\left( k \right)
\end{array} \right.
\end{array}\]

但注意到最佳解並不一定保證系統穩定 (Kalman (1960b, p.113) 指出系統的 optimality 並不保證 stability,亦即 系統採用 optimal control law 並不保證系統閉迴路穩定。),現在我們將透過以下例子顯示 儘管 $Q >0, R>0$ 且 $N \ge 1$ 所求得的最佳控制力並不保證系統閉迴路穩定。

Example: Finite Horizon LQ control
考慮離散系統 $x(k+1) = Ax(k) + B u(k)$ 且 $y(k) = Cx(k)$;其中
\[A = \left[ {\begin{array}{*{20}{c}}
{4/3}&{ - 2/3}\\
1&0
\end{array}} \right];B = \left[ {\begin{array}{*{20}{c}}
1\\
0
\end{array}} \right];C = \left[ {\begin{array}{*{20}{c}}
{ - 2/3}&1
\end{array}} \right]\]現在考慮  $N=5$ 與 $N=7$ ,試設計 有限維度 LQ 控制器時的最佳解。
Solution
上述系統對應的 轉移函數如下
\[G\left( z \right) = \frac{{ - 2/3z + 1}}{{{z^2} - 4/3z + 2/3}}\]注意到此系統的 開迴路 零點 (zero) 落在 $z = 3/2 = 1.5$;亦即具有 不穩定 零點 ( 因為離散系統穩定範圍落在單位圓 $|z| \le 1$ 之內。) 現在我們利用 LQ 控制器,令參數矩陣 $R:=0.001$ 且 $Q := C^TC = P_f$ 且 $Q \ge 0$ 為半正定矩陣;此時若我們額外再加入一點小擾動
\[\begin{array}{l}
Q: = {C^T}C + 0.001I\\
\begin{array}{*{20}{c}}
{}&{}
\end{array} = \left[ {\begin{array}{*{20}{c}}
{ - 2/3}\\
1
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{ - 2/3}&1
\end{array}} \right] + \left[ {\begin{array}{*{20}{c}}
{0.001}&0\\
0&{0.001}
\end{array}} \right]\\
\begin{array}{*{20}{c}}
{}&{}
\end{array} = \left[ {\begin{array}{*{20}{c}}
{4/9}&{ - 2/3}\\
{ - 2/3}&1
\end{array}} \right] + \left[ {\begin{array}{*{20}{c}}
{0.001}&0\\
0&{0.001}
\end{array}} \right]\\
\begin{array}{*{20}{c}}
{}&{}
\end{array} = \left[ {\begin{array}{*{20}{c}}
{4/9 + 0.001}&{ - 2/3}\\
{ - 2/3}&{1.001}
\end{array}} \right]
\end{array}\]接著我們逐步跌代求解最佳控制力序列 $u(4),u(3),u(2),u(1),u(0)$ 如下:
$k = N = 5$,我們有
\[\begin{array}{l}
 \Rightarrow \left\{ \begin{array}{l}
u_{}^*\left( 4 \right) = K\left( 4 \right)x\left( 4 \right),\begin{array}{*{20}{c}}
{}&{}
\end{array}\\
K\left( 4 \right) = \left[ {\begin{array}{*{20}{c}}
{0.1629}&{0.6652}
\end{array}} \right]\\
M\left( 5 \right) = Q\\
V_{}^*\left( 5 \right) = \frac{1}{2}{x^T}\left( 5 \right)Qx\left( 5 \right)
\end{array} \right. \Rightarrow \left\{ \begin{array}{l}
u_{}^*\left( 3 \right) = K\left( 3 \right)x\left( 3 \right),\begin{array}{*{20}{c}}
{}&{}
\end{array}\\
K\left( 3 \right) = \left[ {\begin{array}{*{20}{c}}
{0.1518}&{0.6652}
\end{array}} \right],\\
M\left( 4 \right): = \left[ {\begin{array}{*{20}{c}}
{0.4489}&{ - 0.666}\\
{ - 0.666}&{1.0014}
\end{array}} \right]\\
V_{}^*\left( 4 \right) = \frac{1}{2}{x^T}\left( 4 \right)M\left( 4 \right)x\left( 4 \right)
\end{array} \right.\\
 \Rightarrow \left\{ \begin{array}{l}
u_{}^*\left( 2 \right) = K\left( 2 \right)x\left( 2 \right),\begin{array}{*{20}{c}}
{}&{}
\end{array}\\
K\left( 2 \right) = \left[ {\begin{array}{*{20}{c}}
{0.1257}&{0.6652}
\end{array}} \right]\\
M\left( 3 \right): = \left[ {\begin{array}{*{20}{c}}
{0.4568}&{ - 0.666}\\
{ - 0.666}&{1.0014}
\end{array}} \right]\\
V_{}^*\left( 3 \right) = \frac{1}{2}{x^T}\left( 3 \right)M\left( 3 \right)x\left( 3 \right),
\end{array} \right. \Rightarrow \left\{ \begin{array}{l}
u_{}^*\left( 1 \right) = K\left( 1 \right)x\left( 1 \right),\begin{array}{*{20}{c}}
{}&{}
\end{array}\\
K\left( 1 \right) = \left[ {\begin{array}{*{20}{c}}
{0.0724}&{0.6653}
\end{array}} \right],\\
M\left( 2 \right): = \left[ {\begin{array}{*{20}{c}}
{0.4741}&{ - 0.666}\\
{ - 0.666}&{1.0014}
\end{array}} \right]\\
V_{}^*\left( 2 \right) = \frac{1}{2}{x^T}\left( 2 \right)M\left( 2 \right)x\left( 2 \right)
\end{array} \right.\\
 \Rightarrow \left\{ \begin{array}{l}
u_{}^*\left( 0 \right) = K\left( 0 \right)x\left( 0 \right),\begin{array}{*{20}{c}}
{}&{}
\end{array}\\
K\left( 0 \right) = \left[ {\begin{array}{*{20}{c}}
{ - 0.0256}&{0.6653}
\end{array}} \right],\\
M\left( 1 \right): = \left[ {\begin{array}{*{20}{c}}
{0.5098}&{ - 0.666}\\
{ - 0.666}&{1.0014}
\end{array}} \right]\\
V_{}^*\left( 1 \right) = \frac{1}{2}{x^T}\left( 1 \right)M\left( 1 \right)x\left( 1 \right)
\end{array} \right.
\end{array}\]注意到若我們計算閉迴路系統的極點可得
$$eig(A+ BK_{N=5}(0)) = \{1.307, 0.001\}$$ 亦即 使用此最佳控制力會導致閉迴路系統不穩定 $(z = 1.307 \ge 1)$ 但讀者可自行試驗若改考慮 $N=7$時候,閉迴路極點會變成
\[
eig(A+ BK_{N=7}(0)) = \{0.989, 0.001\}
\]亦即系統被穩定化。若我們考慮 $N=\infty$ 時可以得到
\[
eig(A+ BK_{N=\infty}(0)) = \{0.664, 0.001\}
\]此結果稱作 infinite horizon control law。會在之後再行介紹。

留言

這個網誌中的熱門文章

[數學分析] 什麼是若且唯若 "if and only if"

數學上的 if and only if  ( 此文不討論邏輯學中的 if and only if,只討論數學上的 if and only if。) 中文翻譯叫做  若且唯若 (or 當且僅當) , 記得當初剛接觸這個詞彙的時候,我是完全不明白到底是甚麼意思,查了翻譯也是愛莫能助,畢竟有翻跟沒翻一樣,都是有看沒有懂。 在數學上如果看到 if and only if  這類的句子,其實是表示一種 雙條件句 ,通常可以直接將其視為" 定義(Definition)" 待之,今天要分享的是這樣的一個句子如何用比較直觀的方法去看他 假設我們現在有 兩個邏輯陳述句 A 與  B. 注意到,在此我們不必考慮這兩個陳述句到底是什麼,想表達什麼,或者到底是否為真(true),這些都不重要。只要知道是兩個陳述即可。 現在,考慮新的陳述:  "A if and only if B" 好了,現在主角登場,我們可以怎麼看待這個句子呢? 事實上我們可以很直覺的把這句子拆成兩部分看待,也就是 "( A if B ) and ( A only if B )" 那麼先針對第一個部分  A if B  來看, 其實這句就是說  if B then A, 更直白一點就是 "if B is true, then A is also true".  在數學上等價可以寫為 "B implies A" .  或者更常用一個箭頭符號來表示 "B $\Rightarrow$  A"  現在針對第二個部分  A only if B 此句意指  "If B is not true, then A is also not true". 所以如果已知 A is true,  那麼按照上句不難推得 B is also true 也就是說  A only if B  等價為 "If A is true then B is also true". 同樣,也可以寫作   "A implies B"   或者用箭頭表示  "A   $\Rightarrow$     B".

[數學分析] 淺談各種基本範數 (Norm)

這次要介紹的是數學上一個重要的概念: Norm: 一般翻譯成 範數 (在英語中 norm 有規範的意思,比如我們說normalization就是把某種東西/物品/事件 做 正規化,也就是加上規範使其正常化),不過個人認為其實翻譯成 範數 也是看不懂的...這邊建議把 Norm 想成長度就好 (事實上norm是長度的抽象推廣), 也許讀者會認為好端端的長度不用,為何又要發明一個 norm 來自討苦吃?? 既抽象又艱澀。 事實上想法是這樣的: 比如說現在想要比較兩個數字 $3$ , $5$ 之間的大小,則我們可以馬上知道 $ 3 < 5 $;同樣的,如果再考慮小數與無理數如 $1.8753$ 與 $\pi$,我們仍然可以比較大小 $1.8753 < \pi = 3.1415...$ 故可以發現我們有辦法對 "純量" 做明確的比大小,WHY? 因為前述例子中 $3$, $5$, $1.8753$ or $\pi$ 其各自的大小有辦法被 "measure "! 但是如果是現在考慮的是一組數字 我們如何去measure 其大小呢?? 比如說 \[x:=[1, -2, 0.1, 0 ]^T \]上式的大小該是多少? 是 $1$? $-2$? $0.1$??? 再者如果更過分一點,我們考慮一個矩陣 \[A = \left[ {\begin{array}{*{20}{c}} 1&2\\ 3&4 \end{array}} \right] \],想要知道這個矩陣的大小又該怎麼辦?? 是 $1$ ? $2$ 還是 $4$ ?..其實現階段我們說不清楚。 也正是如此,可以發現我們確實需要新的 "長度" 的定義來幫助我們如何去 measure 矩陣/向量/甚至是函數的大小。 故此,我們首先定義甚麼是Norm,(也就是把 "長度" or "大小" 的本質抽離出來) ================== Definition: Norm 考慮 $V$ 為一個向量空間(Vector space),則我們說  Norm 為一個函數 $||\cdot|| : V \rightarrow \mathbb{R}$ 且滿足下列性質