最優控制LQR求解

LQR

Problem

xt+1=Axk+Buk

xk:state at time tuk:input at time t

It assumes a quadratic cost function :

J=∑n=0N−1 ( xTnQxn+uTnRun ) + xNPxN

with Q,R,P 正定

這裡讨論的求解lqr是，在有模型的限制下面，以及初始條件 x0 , 求解使性能名額最小的u 以及 x

求解過程，疊代求解

先定義cost-to-go function

Ji(xi)=∑n=iN−1(xTnQxn+uTnRun)+xTNPxN

可以了解為從第i時刻開始以初始條件為 xi 到最後産生的性能名額

推導的思想采用動态規劃的思想，就是如果想要 J 最小，我們可以通過疊代求解cost-to-go function的最小值來實作。

那麼可以很簡單的推導為：

JN(xN)=xTNPxNJN−1(xN−1)=xTN−1QxN−1+uTN−1RuN−1+JN(xN)

這裡再把模型的限制 xt+1=Axk+Buk 帶入可以得到

JN−1(xN−1)=xTN−1QxN−1+uTN−1RuN−1+JN(AxN−1+BuN−1)           (1)

為了推導友善，這裡将（1）中的 xN−1,uN−1 替換成 x,u

那麼：

JN−1(x)=xTQx+uTRu+(Ax+Bu)TP(Ax+Bu)           (2)

将（2）對u求梯度然後令其為0可以得到：

∇u{(2)}=2Ru+2BTP(Ax+Bu)=0

u=−(R+BTPB)−1BTPAx          (3)

将（3）帶入（2）， 令 (R+BTPB)−1BTPA=k ， 即 u=−kx

JN−1(x)=xTP^xP^=Q+kTRk+(A−Bk)TP(A−Bk)

這裡可以看到求解後發現， JN−1 最後也可以寫成 xTPx 的形式，隻是P要更新，是以可以疊代的像後面求解，而且結果都是統一的形式

求解結果

loop for i = N-1 : i>=0 : i–

k=(R+BTPB)−1BTPAui=−kxP=Q+kTRk+(A−Bk)TP(A−Bk)

這樣疊代完畢後會得到 {u0,u1,...,uN−1}