LQR
久闻其名，不知其详。虽然算法不难，但是网上搜了很多资料都没有很系统的讲解，主要是没有很详细的用例介绍，这门课程有代码例子，可以很好的理解入门，具体链接在github的LQR.ipynb里 LQR 这个文件非常重要，建议打开后结合本文一起看。
数学描述及其含义
LQR本质就是带约束的QP的优化问题，控制的目标体现在目标函数上，约束条件体现在系统的模型以及初始条件上，常见的动力模型（离散化）可以表示为 x k + 1 = A k x k + B k u k x_{k+1} = A_k x_k + B_k u_k x k + 1 = A k x k + B k u k
min x k , u k 1 2 x N T L x N + ∑ k = 0 N − 1 1 2 x k T Q x k + 1 2 u k T R u k s.t. x k + 1 = A k x k + B k u k , x 0 = x i n i t , x N = x d e s , k = 0 , … , N − 1. \begin{equation}
\begin{aligned}
\min_{x_k, u_k} & \ \frac{1}{2} x_N^TL x_N + \sum_{k=0}^{N-1} \frac{1}{2}x_k^TQ x_k + \frac{1}{2}u_k^TR u_k \\
\text{s.t. } \quad & x_{k+1} = A_k x_{k} + B_k u_k, \\
& x_0 = x_{init}, \\
& x_N = x_{des}, \\
& k = 0, \dots, N-1.
\end{aligned}
\end{equation}
x k , u k min s.t. 2 1 x N T L x N + k = 0 ∑ N − 1 2 1 x k T Q x k + 2 1 u k T R u k x k + 1 = A k x k + B k u k , x 0 = x ini t , x N = x d es , k = 0 , … , N − 1. 从上式中我们知道，我们期望在N步后达到目标状态x N x_N x N x 0 x_0 x 0 u k u_k u k R k k R_{kk} R kk u K u_K u K
这个目标函数整体要表达意思就是选择一系列x k , u k {x_k,u_k} x k , u k x N x_N x N u k u_k u k x k x_k x k
QP优化问题
因为这个LQR本质就是优化问题，要求解上述问题，我们需要先知道标准的QP问题长什么样，如下
min s 1 2 s T P s + q T s s.t. C s = d , \begin{equation}
\begin{aligned}
\min_{s} \quad &\frac{1}{2}s^{T}Ps + q^{T}s \\
\text{s.t. } \quad & Cs= d,
\end{aligned}
\end{equation} s min s.t. 2 1 s T P s + q T s C s = d , 根据KKT条件，我们可以得到标准QP问题的解
L ( s , u , λ ) = 1 2 s T P s + q T s + λ T ( C s − d ) , ∇ x L ( s , u , λ ) = P s + q + C T λ = 0 , ∇ λ L ( s , u , λ ) = C s − d = 0 , \begin{aligned}
\mathcal{L}(s, u, \lambda) &= \frac{1}{2}s^{T}Ps + q^{T}s + \lambda^{T}(Cs - d), \\
\nabla_x \mathcal{L}(s, u, \lambda) &= Ps + q + C^{T}\lambda = 0, \\
\nabla_\lambda \mathcal{L}(s, u, \lambda) &= Cs - d = 0,
\end{aligned} L ( s , u , λ ) ∇ x L ( s , u , λ ) ∇ λ L ( s , u , λ ) = 2 1 s T P s + q T s + λ T ( C s − d ) , = P s + q + C T λ = 0 , = C s − d = 0 , 也就是:
[ P C T C 0 ] [ s λ ] = [ − q d ] \begin{bmatrix} P & C^{T} \\
C & 0
\end{bmatrix}
\begin{bmatrix} s \\
\lambda
\end{bmatrix}
=
\begin{bmatrix} -q \\
d
\end{bmatrix} [ P C C T 0 ] [ s λ ] = [ − q d ] 很明显这是一个AX=b的形式，可以求解析解也可以求迭代解。
问题转化
既然已经知道LQR问题的数学描述，也知道了标准QP的形式，那么我们只需要把LQR转化为标准形式就可以轻松求解了。转化后的问题形式如下,这里的变量为s = [ x 0 , x 1 , . . . , x N , u 0 , . . . , u k − 1 ] s = [{x_0,x_1,...,x_N,u_0,... ,u_{k-1}}] s = [ x 0 , x 1 , ... , x N , u 0 , ... , u k − 1 ]
P = [ Q 0 ⋯ 0 0 ⋯ 0 ⋮ ⋱ ⋮ ⋮ ⋱ ⋮ 0 ⋯ L 0 ⋯ 0 0 ⋯ 0 R 0 ⋯ 0 ⋮ ⋱ ⋮ ⋮ ⋱ ⋮ 0 ⋯ 0 0 ⋯ R N − 1 ] , q = 0 C = [ A 0 − I 0 ⋯ 0 0 B 0 0 ⋯ 0 0 A 1 − I ⋯ 0 0 0 B 1 ⋯ 0 ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋱ ⋮ 0 0 0 ⋯ A N − 1 − I 0 0 ⋯ B N − 1 I 0 0 ⋯ 0 0 0 0 ⋯ 0 0 0 0 ⋯ 0 I 0 0 ⋯ 0 ] , d = [ 0 0 ⋮ 0 x init x des ] \begin{gathered}
{P}=\left[\begin{array}{ccc|ccc}
Q_0 & \cdots & 0 & 0 & \cdots & 0 \\
\vdots & \ddots & \vdots & \vdots & \ddots & \vdots \\
0 & \cdots & L & 0 & \cdots & 0 \\
\hline 0 & \cdots & 0 & R_0 & \cdots & 0 \\
\vdots & \ddots & \vdots & \vdots & \ddots & \vdots \\
0 & \cdots & 0 & 0 & \cdots & R_{N-1}
\end{array}\right], \quad {q}=0 \\
{C}=\left[\begin{array}{cccccc|cccc}
A_0 & -I & 0 & \cdots & 0 & 0 & B_0 & 0 & \cdots & 0 \\
0 & A_1 & -I & \cdots & 0 & 0 & 0 & B_1 & \cdots & 0 \\
\vdots & \vdots & \vdots & & \vdots & \vdots & \vdots & \vdots & \ddots & \vdots \\
0 & 0 & 0 & \cdots & A_{N-1} & -I & 0 & 0 & \cdots & B_{N-1} \\
\hline I & 0 & 0 & \cdots & 0 & 0 & 0 & 0 & \cdots & 0 \\
0 & 0 & 0 & \cdots & 0 & I & 0 & 0 & \cdots & 0
\end{array}\right], \quad {d}=\left[\begin{array}{c}
0 \\
0 \\
\vdots \\
0 \\
\hline x^{\text {init }} \\
x^{\text {des }}
\end{array}\right]
\end{gathered} P = ⎣ ⎡ Q 0 ⋮ 0 0 ⋮ 0 ⋯ ⋱ ⋯ ⋯ ⋱ ⋯ 0 ⋮ L 0 ⋮ 0 0 ⋮ 0 R 0 ⋮ 0 ⋯ ⋱ ⋯ ⋯ ⋱ ⋯ 0 ⋮ 0 0 ⋮ R N − 1 ⎦ ⎤ , q = 0 C = ⎣ ⎡ A 0 0 ⋮ 0 I 0 − I A 1 ⋮ 0 0 0 0 − I ⋮ 0 0 0 ⋯ ⋯ ⋯ ⋯ ⋯ 0 0 ⋮ A N − 1 0 0 0 0 ⋮ − I 0 I B 0 0 ⋮ 0 0 0 0 B 1 ⋮ 0 0 0 ⋯ ⋯ ⋱ ⋯ ⋯ ⋯ 0 0 ⋮ B N − 1 0 0 ⎦ ⎤ , d = ⎣ ⎡ 0 0 ⋮ 0 x init x des ⎦ ⎤ 至此，问题的数学部分已经全部结束了，是不是特别简单？接下来就是要求解了。其实有两种方法求解，第一种是写成AX=b的形式后，直接用最小二乘法求解，另一种就是cvxpy库了。
cvxpy库求解
对于python，我们需要用到一个cvxpy库是个专门求解最优化问题的库，让我想起自己本科毕设也用到了这个东西，时间过得还蛮快的。cvxxpy库使用非常简单，具体的tutorial详见cvxpy
其实核心就是下面代码的最后三行。 具体见代码里的注释
m = 15
n = 10
p = 5
np.random.seed(1)
P = np.random.randn(n, n)
P = P.T @ P
q = np.random.randn(n)
G = np.random.randn(m, n)
h = G @ np.random.randn(n)
A = np.random.randn(p, n)
b = np.random.randn(p)
x = cp.Variable(n)
prob = cp.Problem(cp.Minimize((1 /2 )*cp.quad_form(x, P) + q.T @ x),
[G @ x <= h,A @ x == b] )
prob.solve()
LQR离散化
看上面的代码觉得LQR也还算容易，但是现在有一个问题，我们的状态方程以及求解全部都是离散方程，但是现实生活中时间是连续的呀！！！ 一个汽车在运行时总不能是跳跃的吧，现实世界可没有绯红之王（doge）。
所以我们需要把连续的线性系统离散化。
先看一下连续的线性时不变系统怎么表示
d x d t = A x + B u y = C x + D u \frac{dx}
{dt} = Ax + Bu \\
y = Cx + Du d t d x = A x + B u y = C x + D u A (系统矩阵), B (控制矩阵), C (感知矩阵), and D (直接加和) 是常量矩阵
流程图可以表示如下
上式是最简单的常微分方程，我们可以知道x(t)的解是
x ( t ) = e A t x ( 0 ) + ∫ 0 t e A ( t − τ ) B u ( τ ) d τ . x(t) = e^{At}x(0) + ∫_0^t e^{A(t−τ )}Bu(τ )dτ. x ( t ) = e A t x ( 0 ) + ∫ 0 t e A ( t − τ ) B u ( τ ) d τ . 要证明也比较简单，两边同时微分即可
现在开始离散化。
离散化一般是怎么做的呢？答案是采样，既然要采样，我们肯定要确定一个采样率，也就是dt，确定了之后，采样要保证什么？当然是每个采样点的值一定要与原系统的值一样，否则怎么敢说离散化呢？ 所以就有了如下公式
x t k + 1 = x ( ( k + 1 ) T ) = e A T x ( k T ) + ( ∫ 0 T e A ν d ν ) B u ( k T ) = e A T x t k + ( ∫ 0 T e A ν d ν ) B u t k = A d x t k + B d u t k x_{t_{k+1}} = x ((k + 1)T ) \\
= e^{AT} x(kT ) +
(∫ ^T _0 e^{Aν} dν ) Bu(kT )\\
=e^{AT} x_{t_k} +
(∫ ^T _0 e^{Aν} dν )
Bu_{t_k} \\
=A_dx_{tk} + B_du_{t_k} x t k + 1 = x (( k + 1 ) T ) = e A T x ( k T ) + ( ∫ 0 T e A ν d ν ) B u ( k T ) = e A T x t k + ( ∫ 0 T e A ν d ν ) B u t k = A d x t k + B d u t k 所以离散化的系统方程和控制方程变为
A d = e A T B d = ( ∫ 0 T e A ν d ν ) B A_d = e^{AT}\\
B_d =
(∫ ^T _0 e^{Aν} dν ) B A d = e A T B d = ( ∫ 0 T e A ν d ν ) B 而矩阵的幂，我们可以用泰勒公式展开，得到下面这个非常著名的式子
e x p ( A ) = e A = I + A + 1 2 ! A 2 + 1 3 ! A 3 + 1 4 ! A 4 + ⋅ ⋅ ⋅ exp(A) = e^A = I + A + \frac{1}
{2!}A^2 + \frac{1} {3!}A^3 + \frac{1} {4!}A^4 + · · · e x p ( A ) = e A = I + A + 2 ! 1 A 2 + 3 ! 1 A 3 + 4 ! 1 A 4 + ⋅⋅⋅ 我们一般到二阶就可以了。
这样我们就成功把系统离散化了。
以上就是标准的离散化方法，但是通用的一个比较简单的离散化方法为欧拉法,基于x ˙ ≈ x k + 1 − x k T \dot{x} ≈ \frac{x_{k+1}−x_{k}}{T} x ˙ ≈ T x k + 1 − x k
x k + 1 = ( I + T A ) x k + T B u k . x_{k+1} = (I + T A)x_{k} + T Bu_{k}. x k + 1 = ( I + T A ) x k + TB u k . 全状态反馈控制
到这里就结束了！！！ 真的吗？我不信。
我们现在在做的是什么？是控制啊 阿sir！LQR虽然说是一个很像优化问题的控制算法，但是归根结底还是控制啊，那么控制最重要的三件事是什么？
反馈！反馈！还是TM的反馈！
可是看一看我们的数学方程，哪里体现反馈了？没有啊。如果没有反馈我们在求出一系列值之后就放任自由了，可是机器是不可能表现的和理论计算完全一致的，这样一来，随着时间的推移误差会越来越大，最后的结果就是个灾难！！！而且如果不考虑当前运行状态，控制变量u随便乱输入的话，也许能达到能量最小，但是控制的量一定不会平滑，如果考虑到车辆上，一路上就会相当颠簸。如图
为了解决这个问题，我们需要给输入一个约束，让它变得尽量的合理，也就是引入状态反馈，如下图是全状态反馈的结构图
所以，我们需要改变一下我们的数学方程，我们的输入量需要反映系统的状态，那就用最简单的线性表示好了，令u k = K x k u_k=Kx_k u k = K x k
control库求解
如果只是简单的应用，直接使用python的control库即可
K, S, E = lqr(A, B, Q, R)
K (2D array (or matrix) ) – State feedback gainsS (2D array (or matrix) ) – Solution to Riccati equationE (1D array ) – Eigenvalues of the closed loop system
注意，这里u = -K x
基于之前的值求解
但是除了这个方法外，还有另一种比较直观或者取巧的办法。
如果我们已经有了一组测量出来的[ u k , x k ] [{u_k,x_k}] [ u k , x k ] U = K X U=KX U = K X
X : = [ x 0 , x 1 , x 2 , ⋯ , x N − 1 , x N ] ∈ R n × ( N + 1 ) . X:= \left[x_0, x_1, x_2,\cdots, x_{N-1}, x_N \right] \in \mathbb{R}^{n \times (N+1)}. X := [ x 0 , x 1 , x 2 , ⋯ , x N − 1 , x N ] ∈ R n × ( N + 1 ) .
U : = [ u 0 , u 1 , u 2 , ⋯ , u N − 1 , u N ] ∈ R m × ( N + 1 ) . U:= [ u_0, u_1, u_2, \cdots, u_{N-1}, u_N ] \in \mathbb{R}^{m \times (N+1)}. U := [ u 0 , u 1 , u 2 , ⋯ , u N − 1 , u N ] ∈ R m × ( N + 1 ) .
U = K X . U = K X. U = K X .
如果我们考虑K的行
K = : [ K 1 K 2 ⋮ K m ] , K i ∈ R 1 × n K =: \begin{bmatrix}K_1 \\ K_2 \\ \vdots \\ K_m \end{bmatrix}, K_i \in \mathbb{R}^{1\times n} K =: ⎣ ⎡ K 1 K 2 ⋮ K m ⎦ ⎤ , K i ∈ R 1 × n U i = K i X , i = 1 , 2 , ⋯ , m . U_i = K_i X, i = 1, 2, \cdots, m. U i = K i X , i = 1 , 2 , ⋯ , m . K i ^ = arg min K i ∥ U i T − X T K i T ∥ 2 \begin{equation}
\hat{K_i} = \argmin_{K_i} \|U^{T}_i - X^{T}K^{T}_i\|^2
\end{equation} K i ^ = K i arg min ∥ U i T − X T K i T ∥ 2 通过求导
∂ ∥ U i T − X T K i T ∥ 2 ∂ K i T = ∂ ∂ K i T ( K i X X T K i T − 2 U i X T K i T ) = 2 ( X X T K i T − X U i T ) = 0. \begin{equation}
\begin{aligned}
&\frac{\partial \|U^{T}_i - X^{T}K^{T}_i\|^2}{\partial K^{T}_i} \\
= &\frac{\partial}{\partial K^T_i} (K_iXX^{T}K^T_i - 2U_iX^{T}K^{T}_i) \\
= & 2 (XX^{T}K^T_i - XU_i^T)
= 0.
\end{aligned}
\end{equation} = = ∂ K i T ∂ ∥ U i T − X T K i T ∥ 2 ∂ K i T ∂ ( K i X X T K i T − 2 U i X T K i T ) 2 ( X X T K i T − X U i T ) = 0. 最终我们有
K ^ i T = ( X X T ) − 1 X U i T , \hat{K}^{T}_i = (XX^{T})^{-1}XU^{T}_i, K ^ i T = ( X X T ) − 1 X U i T , 把他们堆叠起来
K ^ T = ( X X T ) − 1 X U T , K ^ = U X T ( X T X ) − 1 . \hat{K}^T = (XX^{T})^{-1}XU^{T}, \ \ \ \hat{K} = UX^{T}(X^{T}X)^{-1}. K ^ T = ( X X T ) − 1 X U T , K ^ = U X T ( X T X ) − 1 . 这就是最后的答案。
所以系统状态方程可以变为
x k + 1 = A x k + B u k = ( A + B K ) x k x_{k+1}=Ax_k+Bu_k=(A+BK)x_k x k + 1 = A x k + B u k = ( A + B K ) x k 那么到这一步结束了吗？还没有咧！
观测状态预测
似乎之前的公式已经很完备了，也没有改进的地方了，确实，理论上是的。但是，是不是总觉得少了点什么？看一看我们的系统方程
d x d t = A x + B u y = C x + D u \frac{dx}
{dt} = Ax + Bu \\
y = Cx + Du d t d x = A x + B u y = C x + D u 我们的观测函数好像没有用唉，它难道没有用吗?
要时刻切记，我们解决的是现实世界的问题。 在之前的问题中，我们的确使用到了全状态反馈，而且默认使用的都是理论计算出的状态变量且我们能够观测到所有的量。但是现实世界中，观测的状态是有误差的，不可能完全表示实际状态的，而且哪里就那么容易全状态反馈了呢？我们并不总是能观测到所有的状态的啊！！！
所以，我们需要根据观测到的量推测出当前状态下实际的状态，具体来说是利用下面的公式
min J m e a s + λ J p r o c s . t . x t + 1 = A t x t + B t u t , t = 1 , . . . , T − 1 \min J_{meas} + λJ_proc \\
s.t. x_{t+1} = A_t x_t + B_t u_t, t = 1, . . . , T − 1 min J m e a s + λ J p roc s . t . x t + 1 = A t x t + B t u t , t = 1 , ... , T − 1 J m e a s = ∑ i N ∣ ∣ C i x i − y i ∣ ∣ 2 J p r o c = ∑ i N ∣ ∣ u ∣ ∣ 2 J_{meas} = \sum_i^N ||C_ix_i-y_i||^2\\
J_{proc}= \sum_i^N ||u||^2 J m e a s = i ∑ N ∣∣ C i x i − y i ∣ ∣ 2 J p roc = i ∑ N ∣∣ u ∣ ∣ 2 转写成s的形式
min ∣ ∣ A ˉ s − b ∣ ∣ s . t . C ˉ s = d s = [ x 1 , . . . , x n , u 1 , . . , u n − 1 ] \min ||\bar{A}s-b||\\
s.t. \bar{C}s=d\\
s=[{x_1,...,x_n,u_1,..,u_{n-1}}] min ∣∣ A ˉ s − b ∣∣ s . t . C ˉ s = d s = [ x 1 , ... , x n , u 1 , .. , u n − 1 ] 很明显，这可以用最小二乘法求解，求出来后的状态变量是根据观测得到的，结合之前的u=Kx的公式，就可以不断求解了。
可控性
这下总该结束了吧？不然！！！ 因为还有最后一个量我们其实并没有考虑那就是步骤N,我们该怎么选择N呢？其实想想也知道，N不是随意的，不然我当前位置与目标位置为1000km,然后我的N选择1，难道我就能一步开过去吗？显然是不现实的。所以N的选择也是有讲究的。
我们先看最后的状态X N X_N X N
x N = A N x 0 + A N − 1 B u 0 + A N − 2 B u 0 + ⋯ B u N − 1 = A N x 0 + [ A N − 1 B , A N − 2 B , ⋯ , B ] [ u N − 1 u N − 2 ⋮ u 0 ] \begin{align}
x_N &= A^{N}x_0 + A^{N-1}Bu_0 + A^{N-2}Bu_0 + \cdots Bu_{N-1} \\
&= A^{N}x_0 + \left[A^{N-1}B, A^{N-2}B, \cdots, B\right]\begin{bmatrix}u_{N-1} \\ u_{N-2}\\ \vdots \\ u_0 \end{bmatrix}
\end{align} x N = A N x 0 + A N − 1 B u 0 + A N − 2 B u 0 + ⋯ B u N − 1 = A N x 0 + [ A N − 1 B , A N − 2 B , ⋯ , B ] ⎣ ⎡ u N − 1 u N − 2 ⋮ u 0 ⎦ ⎤ C N : = [ A N − 1 B , A N − 2 B , ⋯ , B ] . \mathcal{C}_N := \left[A^{N-1}B, A^{N-2}B, \cdots, B\right]. C N := [ A N − 1 B , A N − 2 B , ⋯ , B ] .
所以这个公式表示的就是目标状态与控制量之间的关系，目标状态是由
如果C N C_N C N
状态变量在U张开的空间中，控制变量是V T V^T V T
r a n k ( C ) = n rank(C)=n r ank ( C ) = n n是X的维度
也就是说N的选择要使得C的秩为n，只有这样这个系统才是可控的。
结语
到这里，真的结束了，梳理了LQR涉及的绝大部分基础知识，如果看到这里全都掌握了，那么就可以去看LQR的变种以及自己应用了。
当然，还可以继续深挖，比如A,B对系统零极点的影响，如A的奇异值与极点的关系，对系统稳定性的影响等。
