最優算法-LQR-離散時間有限邊界

概述

本文介紹離散時間有限範圍內的LQR(Linear Quadratic Regulator)算法求解過程.

LQR問題背景

對於一個離散時間系統：
$x_{t+1}=Ax_t + Bu_t,x_0=x_{init}\tag{1}$
其中，$A\in R^{n\times n}$，$B\in R^{n\times m}$

關於最優問題，就在於如何選擇合適的$u_0,u_1,...$，使得狀態量$x_0,x_1,...$足夠小，因此得到好的調節和控制；或者使得$u_0,u_1,...$足夠小，以使用更少的能量。這兩個量通常相互制約，如果採用更大的輸入$u$，就會驅使狀態量$x$更快達到0。採用線性二次調節原理可以解決這個問題。

LQR代價函數

爲了表示控制系統達到穩定控制所付出的代價，定義如下二次型代價函數：
$J(U)=\sum^{N-1}_{\tau=0}(x^{T}_{\tau}Qx_{\tau} + u^{T}_{\tau}Ru_{\tau})+ x^{T}_{N}Q_{f}x_{N}\tag{2}$
其中函數參數$U = (u_0,u_1,..,u_N)$，並且矩陣$Q,Q_f,R$爲正定矩陣，及
$\begin{array}{cl} Q=Q^{T}\geq0,&Q_f=Q_{f}^{T}\geq0,&R=R^{T}>0 \end{array}$

$Q$	$Q_f$	$R$
給定狀態代價矩陣	最終狀態代價矩陣	輸入代價矩陣

• $N$：時間範圍(考慮$N = \infty$)
• $x^{T}_{\tau} Q x_{\tau}$：衡量狀態偏差
• $u^{T}_{\tau} R u_{\tau}$：衡量輸入大小
• $x^{T}_{N} Q_{f} x_{N}$：衡量最終狀態偏差
• $Q$，$R$：分別設定狀態偏差和輸入的相對權重
• $R>0$：意味着任何非零輸入都增加$J$的代價

因此，關於LQR問題就是找出使得代價函數$J(U)$最小的一組控制輸入$(u_0,u_1,...,u_{N-1})_{lqr}$。

求解LQR方法

本文主要介紹兩種求解LQR的方法，分別爲最小二乘法和動態規劃算法。

最小二乘法

根據公式(1)可知，$x_0$是$X = (x_0,...,x_N)$的線性函數，並且$U = (u_0,...,u_{N-1})$，可以得出如下關係：
$\begin{array}{cl} x_1 &= Ax_0 + Bu_0\\ x_2 &= Ax_1 + Bu_1\\ \vdots\\ x_n &= Ax_{N-1} + Bu_{N-1} \end{array}\tag{3}$
將上述公式(3)逐個帶入得
$\begin{array}{cl} x_1 &= Ax_0 + Bu_0\\ x_2 &= A^{2}x_0 + ABu_0 + Bu_1\\ \vdots\\ x_n &= A^{N}x_0 + A^{N-1}Bu_0 + A^{N-2}Bu_1 + \dots+ Bu_{N-1} \end{array} \tag{4}$
整理得
$\left[\begin{array}{cl} x_0\\ x_1\\ \vdots\\ x_N \end{array}\right]= \left[ \begin{array}{cl} 0 & \dots \\ B & 0 & \dots \\ AB & B & 0 & \dots \\ \vdots & \vdots \\ A^{N-1}B & A^{N-2}B & \dots & B \end{array}\right] \left[ \begin{array}{cl} u_0\\ u_1\\ \vdots\\ u_{N-1} \end{array} \right]+ \left[ \begin{array}{cl} I\\ A\\ \vdots\\ A^{N} \end{array} \right]x_0 \tag{5}$
其中
$G=\left[ \begin{array}{cl} 0 & \dots \\ B & 0 & \dots \\ AB & B & 0 & \dots \\ \vdots & \vdots \\ A^{N-1}B & A^{N-2}B & \dots & B \end{array}\right],H=\left[ \begin{array}{cl} I\\ A\\ \vdots\\ A^{N} \end{array} \right]$

等式(5)可以進一步表示爲
$X= GU + Hx_0 \tag{6}$
其中，$G\in R^{Nn\times Nm}$，$H\in R^{Nn\times n}$。

從而等式(2)所表示得代價函數可以表示爲
$J(U)=\parallel disg(Q^{1/2},\dots,Q^{1/2},Q^{1/2}_{f})(GU+Hx_0)\parallel^2+ \parallel diag(R^{1/2},\dots,R^{1/2})U\parallel^2 \tag{7}$
這就轉化成一個求解最小二乘法的問題，其問題大小爲$N(n + m)\times Nm$。

動態規劃法(Dynamic Programming)

動態規劃算法是解決多階段決策過程最優化的一種有效的數學方法。

值函數

首先定義一個值函數$V_t:R^n \to R$,其中$t=(0,\dots,N)$：
$V_t(z)=\min_{u_t,\dots,u_{N-1}}\Bigl(\sum_{\tau=t}^{N-1}(x^T_\tau Qx_\tau + u^t_\tau Ru_\tau) + x_N^TQ_fx_N\Bigr) \tag{8}$
如果設置$x_t = z$，根據公式(1)的關係，$x_{\tau+1} = Ax_{\tau} + Bu_{\tau}$,並且$\tau=t,\dots,N$。

• $V_t(z)$可以表示在$t$時刻，從狀態$z$開始的LQR最小代價值
• $V_0(x_0)$表示在0時刻，從狀態$x_0$開始的LQR最小代價值

$V_t$可以表示爲二次型的形式，即$V_T(z)=z^TP_tz,其中P_t=P_t^T \geq 0$。當$t=N$時，代價值函數爲：
$V_N(z) = z^TQ_f z \tag{9}$
因此$P_N = Q_f$。

根據動態規劃原理，等式(8)可以寫成如下遞歸關係式：
$V_t(z)=\min_w\bigl(z^TQz + w^TRw + V_{t+1}(Az+Bw)\bigr)\tag{10}$
其中，

• $z^TQz + w^TRw$：如果$u_t = w$,則代表$t$時刻產生的代價值；
• $V_{t+1}(Az+Bw)$：代表從$t+1$時刻開始，引起的最小代價值；

提取等式(10)中與$w$無關的選項得
$V_t(z)=z^TQz + \min_w\bigl(w^TRw + V_{t+1}(Az+Bw)\bigr)\tag{11}$
等式(11)描述了$V_t(z)$與$V_{t+1}(z)$之間的遞歸關係。

求極值

假設$V_{t+1}= z^TP_{t+1}z$,並且$P_{t+1}=P^{T}_{t+1} \geq0$,等式(11)可以進一步轉化爲$P_{t+1}$的形式：
$V_t(z)=z^TQz + \min_w\bigl(w^TRw + (Az+Bw)^TP_{t+1}(Az+Bw)\bigr)\tag{12}$
爲了求最小值，對$w$求導，導數爲零的點即爲最值點。
$2w^TR + 2(Az+Bw)^TP_{t+1}B = 0 \tag{13}$
推導等式(13)，求取$w$:
$\begin{array}{cl} w^TR + z^{T}A^{T}P_{t+1}B+w^{T}B^{T}P_{t+1}B &= 0\\ w^T(R + B^TP_{t+1}B) &= - z^{T}A^{T}P_{t+1}B &\text{(合併同類項並移項)}\\ (R + B^TP_{t+1}B)^Tw &= -B^TP_{t+1}^{T}Az & \text{(轉置)}\\ (R + B^TP_{t+1}B)w &= -B^TP_{t+1}Az &(P_{t+1}=P^{T}_{t+1},R=R^T)\\ w &=-(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az &\text{(矩陣求逆)} \end{array}\tag{14}$
由等式(14)可知，最優輸入爲
$w^* =-(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az \tag{15}$
將等式(15)帶入等式(12)得
$V_t(z)=z^TQz + w^{*T}Rw^* + (Az+Bw^*)^TP_{t+1}(Az+Bw^*)\tag{16}$
對等式(16)化簡得
$\begin{array}{cl} V_t(z) &= z^TQz + w^{*T}Rw^* + (Az+Bw^*)^TP_{t+1}(Az+Bw^*)\\ &= z^TQz + w^{*T}Rw^* + z^TA^TP_{t+1}Az + 2z^TA^TP_{t+1}Bw^* + w^{*T}B^TP_{t+1}Bw^*\\ & = z^TQz + z^TA^TP_{t+1}Az + w^{*T}(R+B^TP_{t+1}B)w^* + 2z^TA^TP_{t+1}Bw^*\\ & = z^TQz + z^TA^TP_{t+1}Az\\ &+z^TA^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}(R+B^TP_{t+1}B)(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az\\ &-2z^TA^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az\\ &=z^TQz + z^TA^TP_{t+1}Az\\ &+z^TA^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az\\ &-2z^TA^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az\\ &= z^TQz + z^TA^TP_{t+1}Az - z^TA^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az\\ &= z^T(Q + A^TP_{t+1}A - A^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}A)z\\ &= z^TP_tz \end{array}\tag{17}$
上述公式化簡過程中，由於$P_{t+1}=P^{T}_{t+1},R=R^T$，所以$\bigl((R + B^TP_{t+1}B)^{-1}\bigr)^T = (R + B^TP_{t+1}B)^{-1}$。

由等式(17)可知
$P_t = Q + A^TP_{t+1}A - A^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}A \tag{18}$

求解過程

關於LQR的求解過程，可以採用動態規劃算法，依據上述公式(20)的遞歸關係，反向遞推，求出滿足一定條件的最小代價值。

1. 確定迭代範圍$N$
2. 設置迭代初始值$P_N=Q_f$
3. 循環迭代，$t = N,\dots,1$

$P_{t-1} = Q + A^TP_{t+1}A - A^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}A$

4. 則反饋係數$K_t = -(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}A$，對於時間$t=0,\dots,N-1$
5. 優化的控制量$u_t^{lqr}=K_tx_t$