最优控制LQR求解

LQR

Problem

$$\large{x_{t+1} = A x_k + B_{u_k} }$$

$$x_k : state\ at \ time \ t \\ u_k:input \ at \ time \ t \\ $$

It assumes a quadratic cost function :

$$\large{J = \sum_{n=0}^{N-1} \ (\ x_n^TQx_n + u_n^TRu_n \ ) \ + \ x_NPx_N}$$

with $Q, R, P$ 正定

这里讨论的求解lqr是，在有模型的约束下面，以及初始条件$x_0$, 求解使性能指标最小的u 以及 x

求解过程，迭代求解

先定义cost-to-go function

$$\large{J_i(x_i) = \sum_{n=i}^{N-1}(x_n^TQx_n + u_n^TRu_n) + x_N^TPx_N}$$

可以理解为从第i时刻开始以初始条件为$x_i$到最后产生的性能指标

推导的思想采用动态规划的思想，就是如果想要$J$最小，我们可以通过迭代求解cost-to-go function的最小值来实现。

那么可以很简单的推导为：

$$\large{J_N(x_N) = x_N^TPx_N} \\ \large{J_{N-1}(x_{N-1}) = x_{N-1}^TQx_{N-1} + u_{N-1}^T R u_{N-1} + J_N(x_N)}$$

这里再把模型的约束$\large{x_{t+1} = A x_k + B_{u_k} }$带入可以得到

$$\large{J_{N-1}(x_{N-1}) = x_{N-1}^TQx_{N-1} + u_{N-1}^T R u_{N-1} + J_N(Ax_{N-1} + Bu_{N-1})} \ \ \ \ \ \ \ \ \ \ \ (1)$$

为了推导方便，这里将（1）中的$x_{N-1} , u_{N-1}$替换成$x, u$

那么：

$$\large{J_{N-1}(x)= x^TQx + u^T R u + (Ax + Bu)^TP(Ax+Bu)} \ \ \ \ \ \ \ \ \ \ \ (2)$$

将（2）对u求梯度然后令其为0可以得到：

$$\large{\nabla_u\{(2)\} = 2Ru + 2B^TP(Ax + Bu)} =0$$

$$\large{ u = -(R + B^TPB)^{-1}B^TPAx \ \ \ \ \ \ \ \ \ \ (3)}$$

将（3）带入（2）， 令$(R + B^TPB)^{-1}B^TPA = k$， 即$u=-kx$

$$\large{J_{N-1}(x) = x^T \hat{P}x} \\ \large{\hat{P} = Q+k^TRk } +(A-Bk)^TP(A-Bk)$$

这里可以看到求解后发现，$J_{N-1}$ 最后也可以写成$x^TPx$的形式，只是P要更新，所以可以迭代的像后面求解，而且结果都是统一的形式

求解结果

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

$$\large{k =( R + B^TPB)^{-1}B^TPA \\ u_i = -k x \\ P = Q+k^TRk +(A-Bk)^TP(A-Bk) }$$

这样迭代完毕后会得到${\{u_0 , u_1 , ... , u_{N-1}\}}$