LQR控制

在开始LQR控制之前需要先知道二次型的概念。简单地说，二次型就是一个二次多项式，或者更准确地说是多项式的矩阵表示。我们考虑一个这样的\(n\)元多项式（在所有项中，如果\(i=j\)则称为平方项，\(i\neq j\)则称为交叉项）：

\[ f(x_1,x_2,...x_n)=\sum\limits^n_{i=1}\sum\limits^n_{j=1}a_{ij}x_ix_j\quad(a_{ij}=a_{ji}) \]

（最后那个条件是因为交叉项对称，\(x_ix_j=x_jx_i\)，对应系数也得一致）

那么我们可以把每个\(a_{ij}\)组成一个对称矩阵\(A\)，此时就有：

\[ \begin{aligned} &x^TAx\\ &=(x_1,x_2,...x_n)A(x_1,x_2,...x_n)^T\\ &=(x_1,x_2,...x_n)(x_1\vec{a_1},x_2\vec{a_2},...x_n\vec{a_n})\\ &=x_1\sum\limits^n_{i=1}x_ia_{1i}+x_2\sum\limits^n_{i=1}x_ia_{2i}+...+x_n\sum\limits^n_{i=1}x_ia_{ni}\\ &=\sum\limits^n_{i=1}\sum\limits^n_{j=1}a_{ij}x_ix_j\\ &=f(x_1,x_2,...x_n) \end{aligned} \]

也就是我们可以断定多项式表达和矩阵表达形式是等价的。这里先把这个命题证一遍的用意是，以下的论述中看到\(x^TAx\)这种二次型的矩阵表达需要意识到这其实是一个多项式。矩阵A决定\(x^TAx\)的正负。如果\(x^TAx>0\)，称A是正定的；\(x^TAx\geq0\)，称A是半正定的；\(x^TAx<0\)，称A是负定的。

LQR控制的本质是一种model-based最优控制，它也是一种反馈控制，换句话讲就是它致力于找到一个所谓的K矩阵，来让\(u=-Kx\)。我们首先考察连续无限时域LQR，也就是考察系统在任意久的时间中的演化动态，与之相对的是有限时域LQR（更关注系统在指定时间内怎样达成我们期望的某些状态，此时代价函数中会对应的引入终端代价项，后面会展开讲解有限时域LQR）。无限时域LQR的原始形式可以表述为一个这样的（等式约束）优化问题：

\[ \begin{aligned} \min_{u(t)} \quad & J = \int_{0}^{\infty} \left[ x(t)^T Q x(t) + u(t)^T R u(t) \right] \,\mathrm{d}t \\ \text{subject to} \quad & \dot{x}(t) = A x(t) + B u(t), \quad x(0) = x_0 \\ & Q \succeq 0, \quad R \succ 0 \end{aligned} \]

（来源Wikipedia）

从这个表述中可以看出以下几个特征，首先是LQR控制针对的系统必须是LTI系统（假如非线性，就必须考虑线性化），同时矩阵Q必须是半正定的（保证状态惩罚非负，确保代价函数有下界），且R必须是正定的（确保如果\(u\)非零则代价一定为正，避免算出一个较大但代价为零的\(u\)）

LQR最典型的特征就是代价函数使用二次型。LQR的代价函数包含状态代价\(x^TQx\)和控制代价\(u^TRu\)，这可以保证系统状态（如平步站立状态）和控制量（如电机力矩，如果没有控制代价，LQR可能算出一个超出电机执行能力的力矩）满足要求。代价函数使用二次型，本质上反应的是“广义能量”的概念，这一点可以观察经典物理中自然存在的能量，如动能\(\dfrac{1}{2}mv^2\)是速度的二次型，弹性势能\(\dfrac{1}{2}kx^2\)是位移的二次型，实际上LQR的思想本质上就是把状态误差和控制量转化为广义上的“能量消耗”，再通过最小化能量消耗达成最优控制。

接下来考虑怎么求解这个优化问题。这就涉及另一个必要的预备知识，动态规划。简单来说，动态规划是一种广泛使用的解决问题的方法，应用领域非常广，如果中学时代打过OI，会对它非常熟悉，但是这里讲的动态规划和算法层面的动态规划仍然不是一个东西，这里不做过多展开。LQR的优化问题求解主要使用的就是动态规划的思路。

动态规划的思想是从Bellman最优性原理出发的，这里先贴一下Bellman的原文

An optimal policy has the property that whatever the initial state and initial decision are, the remaining decisions must constitute an optimal policy with regard to the state resulting from the first decision. (See Bellman, 1957, Chap. III.3.)

“一个最优策略具有这样的性质：无论初始状态和初始决策如何，剩余决策必须构成以第一个决策导致的状态为初始状态的最优策略。”

直观理解就是：假设从A点出发到C点，经过B点。如果A→B→C是最优路径，那么B→C也必须是B到C的最优路径。即使你在A点选择了不同的路径（可能非最优）到B，但是站在B点的你要使整个路径长度最短，后续路径依然要选择B到C的最优路径。

如果从代价函数的视角来看，假如我们在一段特定的时间\([t_0,t_f]\)中间取一个时间点\(t\)，那么如果要使整段时间的代价函数最小，\([t_0, t]\)和\([t, t_f]\)这两段时间内的代价函数各自都必须最小。那么我们可以考虑一个更普遍的代价泛函（注意其中的各个参数不一定是二次型），代表从\(t_0\)到\(t_f\)之间的总消耗，即以下形式的优化问题

\[ \begin{aligned} \min_{u(s)\in\mathcal{U},s \in [t_0,t_f]} & J(x(t), u(t), t) = \int^{t_f}_{t_0}g(x(s),u(s),s)\mathrm{d}s\\ \text{subject to} \quad &\dot x(s)=f(x(s),u(s),s)\\ & u(s)\in\mathcal U \\ & s \in [t_0,t_f] \end{aligned} \]

取\(\delta t\rightarrow 0\)，那么原有的\([t_0, t_f]\)就被划分为\([t_0,t_0+\delta t]\)和\([t_0+\delta t, t_f]\)，此时回忆一下之前的Bellman最优性原理，可以发现，如果我们在\(t+\delta t\)这个时间点，要使\([t_0,t_f]\)内的代价最小，就必须让\([t_0+\delta t, t_f]\)内的代价最小，换句话就是我们要拿到的其实就是\([t_0,t_0+\delta t]\)中已经产生的代价加上\([t_0+\delta t, t_f]\)中未来产生的可能代价的下确界，据此我们可以定义出所谓的「值函数」

\[ V(t_0, x) = \inf _u \quad [\int^{t_0+\delta t}_{t_0}g(x(s), u(s), s)\mathrm{d}s+V(t_0+\delta t,x+f(x(s),u(s),s)\delta t)] \]

此时我们对后半段在点\((t_0,x)\)处进行一阶泰勒展开，保留高阶项（这步是为了消掉上式左侧的\(V(t_0,x)\)）

\[ V(t_0+\delta t,x+f(x(s),u(s),s)\delta t) = V(t_0,x)+\dfrac{\partial V}{\partial t}\delta t+\dfrac{\partial V}{\partial x}f\delta t+o(\delta t) \]

代入原本的值函数表达式，得到

\[ V(t_0, x) = \inf _u \quad [\int^{t_0+\delta t}_{t_0}g(x(s), u(s), s)\mathrm{d}s+V(t_0,x)+\dfrac{\partial V}{\partial t}\delta t+\dfrac{\partial V}{\partial x}f\delta t+o(\delta t)] \]

化简，两边同时除\(\delta t\)

\[ \begin{aligned} 0 &= \inf _u \quad [\int^{t_0+\delta t}_{t_0}g(x(s), u(s), s)\mathrm{d}s+\dfrac{\partial V}{\partial x}f\delta t+o(\delta t)] \\ 0 &= \inf _u \quad [g(x(s), u(s), s)\delta t+\dfrac{\partial V}{\partial t}\delta t+\dfrac{\partial V}{\partial x}f\delta t+o(\delta t)] \\ -\dfrac{\partial V}{\partial t} &= \inf _u \quad [g(x(s), u(s), s)+\dfrac{\partial V}{\partial x}f] \end{aligned} \]

我们最终得到的这个式子就是Hamilton-Jacobi-Bellman方程，也称为HJB方程。括号内的式子也被称为哈密顿量，也就是\(\mathscr{H} = g(x(s), u(s), s)+\dfrac{\partial V}{\partial x}f\)。

之前我们提到过无限时域LQR的优化问题形式，有限时域LQR其实就是在它的基础上将问题范围限定为时间段\([t_0,t_f]\)，再在代价函数中加入终端代价项，也就是它看起来像这样：

\[ \begin{aligned} \min_{u(t)} \quad & J = \dfrac{1}{2}x^T(t_f)Sx(t_f)+\dfrac{1}{2}\int_{t_0}^{t_f} \left[ x(t)^T Q x(t) + u(t)^T R u(t) \right] \,\mathrm{d}t \\ \text{subject to} \quad & \dot{x}(t) = A x(t) + B u(t), \quad x(0) = x_0 \\ & Q \succeq 0, \quad R \succ 0 \end{aligned} \]

根据这个优化问题，我们可以写出HJB方程

\[ -\dfrac{\partial V}{\partial t} = \inf _{u} \quad \left[\dfrac{1}{2}\left( x^T Q x + u^T R u \right) +\dfrac{\partial V}{\partial x}(A x + B u)\right] \]

然后，我们先假设有限时域LQR的值函数是一个二次型（从直观上很容易接受，但是其实需要证明，这里不打算严格证明，后面我们的推导会印证这一点），也就是\(V(t,x)=\dfrac{1}{2}x^TP(t)x\)，其中\(P\)是对称矩阵，拿到值函数之后我们就可以将上面的方程进一步推导

\[ -\dfrac{1}{2}x^T\dot Px = \inf_{u} \quad \left[\dfrac{1}{2}\left( x^T Q x + u^T R u \right)+x^TP\left(A x + B u\right)\right] \]

为了求出右侧的下确界，我们先求哈密顿量的梯度，再令其为零

\[ \begin{aligned} \mathscr{H} &= \dfrac{1}{2}\left( x^T Q x + u^T R u \right)+x^TP\left(A x + B u\right)\\ \dfrac{\partial \mathscr{H}}{\partial u} &= Ru^*+B^TPx=0 \end{aligned} \]

最终解得

\[ u^*=-R^{-1}B^TPx \]

到此为止，其实我们想要的东西已经拿到了，换言之就是我们已经知道了最优的\(K\)矩阵就是\(K=R^{-1}B^TP\)，但是我们仍然需要回答一个「\(P\)矩阵从哪里来」的问题，这个矩阵一开始是我们「假设」出来的，如果对它一无所知，即使拿到了\(K\)矩阵的表达式也求不出最优控制，接下来通过进一步推导来挖掘出它更多的数学特征。

把刚才解出的最优控制\(u^*\)代回HJB方程

\[ \begin{aligned} -\dfrac{1}{2}x^T\dot Px =& \dfrac{1}{2}\left( x^T Q x + u^{*T} R u^* \right)+x^TP\left(A x + B u^*\right)\\ =& \dfrac{1}{2}\left( x^T Q x + (-R^{-1}B^TPx)^T R (-R^{-1}B^TPx) \right)+ \\ & x^TP\left(A x + B (-R^{-1}B^TPx)\right)\\ =& \dfrac{1}{2} x^T \left(Q-PBR^{-1}B^TP+PA+A^TP\right) x\\ \dot P =& Q-PBR^{-1}B^TP+PA+A^TP \end{aligned} \]

我们最后推出来的这个方程称为连续时间代数Riccati方程，通过解这个方程，我们可以得到\(P\)矩阵，进而得到最优控制\(u^*\)。实际上可以证明，对于有限时域LQR问题，存在唯一的对称矩阵\(P\)使得代数Riccati方程在时域\([t_0,t_f]\)上有解（这保证了LQR的值函数是二次型）。