最优算法-LQR-离散时间有限边界

概述

本文介绍离散时间有限范围内的LQR(Linear Quadratic Regulator)算法求解过程.

LQR问题背景

对于一个离散时间系统：
$$x_{t+1}=A{x}_t+B{u}_t,x_0=x_{init}\text{\hspace{1em}(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 _{\tau =0}^{N-1}(x_{\tau }^{T}Q{x}_{\tau }+u_{\tau }^{T}R{u}_{\tau })+x_N^T{Q}_f{x}_N\text{\hspace{1em}(2)}$$
其中函数参数$U=(u_0,u_1,..,u_N)$，并且矩阵$Q,Q_f,R$为正定矩阵，及
$$\begin{array}{clc}Q=Q^T\ge 0,& Q_f=Q_f^T\ge 0,& R=R^T>0\end{array}$$

$Q$	$Q_f$	$R$
给定状态代价矩阵	最终状态代价矩阵	输入代价矩阵

• $N$：时间范围(考虑$N=\infty$)
• $x_{\tau }^{T}Q{x}_{\tau }$：衡量状态偏差
• $u_{\tau }^{T}R{u}_{\tau }$：衡量输入大小
• $x_N^T{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& =A{x}_0+B{u}_0\\ x_2& =A{x}_1+B{u}_1\\ ⋮& \\ x_n& =A{x}_{N-1}+B{u}_{N-1}\end{array}\text{\hspace{1em}(3)}$$
将上述公式(3)逐个带入得
$$\begin{array}{cl}{x}_1& =A{x}_0+B{u}_0\\ x_2& =A^2{x}_0+AB{u}_0+B{u}_1\\ ⋮& \\ x_n& =A^N{x}_0+A^{N-1}B{u}_0+A^{N-2}B{u}_1+\cdots +B{u}_{N-1}\end{array}\text{\hspace{1em}(4)}$$
整理得
$$\left[\begin{array}{c}{x}_0\\ x_1\\ ⋮\\ x_N\end{array}\right]=\left[\begin{array}{clcc}0& \dots & & \\ B& 0& \dots & \\ AB& B& 0& \dots \\ ⋮& ⋮& & \\ A^{N-1}B& A^{N-2}B& \dots & B\end{array}\right]\left[\begin{array}{c}{u}_0\\ u_1\\ ⋮\\ u_{N-1}\end{array}\right]+\left[\begin{array}{c}I\\ A\\ ⋮\\ A^N\end{array}\right]x_0\text{\hspace{1em}(5)}$$
其中
$$G=\left[\begin{array}{clcc}0& \dots & & \\ B& 0& \dots & \\ AB& B& 0& \dots \\ ⋮& ⋮& & \\ A^{N-1}B& A^{N-2}B& \dots & B\end{array}\right],H=\left[\begin{array}{c}I\\ A\\ ⋮\\ A^N\end{array}\right]$$

等式(5)可以进一步表示为
$$X=GU+H{x}_0\text{\hspace{1em}(6)}$$
其中，$G\in R^{Nn\times Nm}$，$H\in R^{Nn\times n}$。

从而等式(2)所表示得代价函数可以表示为
$$J(U)=\parallel diag(Q^{1/2},\dots ,Q^{1/2},Q_f^{1/2})(GU+H{x}_0){\parallel }^2+\parallel diag(R^{1/2},\dots ,R^{1/2})U{\parallel }^2\text{\hspace{1em}(7)}$$
这就转化成一个求解最小二乘法的问题，其问题大小为$N(n+m)\times Nm$。

动态规划法(Dynamic Programming)

动态规划算法是解决多阶段决策过程最优化的一种有效的数学方法。

值函数

首先定义一个值函数$V_t:R^n\to R$,其中$t=(0,\dots ,N)$：
$$V_t(z)=\underset{u_t,\dots ,u_{N-1}}{\min }(\sum _{\tau =t}^{N-1}(x_{\tau }^{T}Q{x}_{\tau }+u_{\tau }^{t}R{u}_{\tau })+x_N^T{Q}_f{x}_N)\text{\hspace{1em}(8)}$$
如果设置$x_t=z$，根据公式(1)的关系，$x_{\tau +1}=A{x}_{\tau }+B{u}_{\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^T{P}_{t}z,其中P_t=P_t^T\ge 0$。当$t=N$时，代价值函数为：
$$V_N(z)=z^T{Q}_{f}z\text{\hspace{1em}(9)}$$
因此$P_N=Q_f$。

根据动态规划原理，等式(8)可以写成如下递归关系式：
$$V_t(z)=\underset{w}{\min }(z^{T}Qz+w^{T}Rw+V_{t+1}(Az+Bw))\text{\hspace{1em}(10)}$$
其中，

• $z^{T}Qz+w^{T}Rw$：如果$u_t=w$,则代表$t$时刻产生的代价值；
• $V_{t+1}(Az+Bw)$：代表从$t+1$时刻开始，引起的最小代价值；

提取等式(10)中与$w$无关的选项得
$$V_t(z)=z^{T}Qz+\underset{w}{\min }(w^{T}Rw+V_{t+1}(Az+Bw))\text{\hspace{1em}(11)}$$
等式(11)描述了$V_t(z)$与$V_{t+1}(z)$之间的递归关系。

求极值

假设$V_{t+1}=z^T{P}_{t+1}z$,并且$P_{t+1}=P_{t+1}^T\ge 0$,等式(11)可以进一步转化为$P_{t+1}$的形式：
$$V_t(z)=z^{T}Qz+\underset{w}{\min }(w^{T}Rw+(Az+Bw{)}^T{P}_{t+1}(Az+Bw))\text{\hspace{1em}(12)}$$
为了求最小值，对$w$求导，导数为零的点即为最值点。
$$2w^{T}R+2(Az+Bw{)}^T{P}_{t+1}B=0\text{\hspace{1em}(13)}$$
推导等式(13)，求取$w$:
$$\begin{array}{clc}{w}^{T}R+z^T{A}^T{P}_{t+1}B+w^T{B}^T{P}_{t+1}B& =0& \\ w^T(R+B^T{P}_{t+1}B)& =-z^T{A}^T{P}_{t+1}B& \text{(合并同类项并移项)}\\ (R+B^T{P}_{t+1}B{)}^{T}w& =-B^T{P}_{t+1}^{T}Az& \text{(转置)}\\ (R+B^T{P}_{t+1}B)w& =-B^T{P}_{t+1}Az& (P_{t+1}=P_{t+1}^T,R=R^T)\\ w& =-(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t+1}Az& \text{(矩阵求逆)}\end{array}\text{\hspace{1em}(14)}$$
由等式(14)可知，最优输入为
$$w^{\ast }=-(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t+1}Az\text{\hspace{1em}(15)}$$
将等式(15)带入等式(12)得
$$V_t(z)=z^{T}Qz+w^{\ast T}R{w}^{\ast }+(Az+B{w}^{\ast }{)}^T{P}_{t+1}(Az+B{w}^{\ast })\text{\hspace{1em}(16)}$$
对等式(16)化简得
$$\begin{array}{cl}{V}_t(z)& =z^{T}Qz+w^{\ast T}R{w}^{\ast }+(Az+B{w}^{\ast }{)}^T{P}_{t+1}(Az+B{w}^{\ast })\\ & =z^{T}Qz+w^{\ast T}R{w}^{\ast }+z^T{A}^T{P}_{t+1}Az+2z^T{A}^T{P}_{t+1}B{w}^{\ast }+w^{\ast T}{B}^T{P}_{t+1}B{w}^{\ast }\\ & =z^{T}Qz+z^T{A}^T{P}_{t+1}Az+w^{\ast T}(R+B^T{P}_{t+1}B)w^{\ast }+2z^T{A}^T{P}_{t+1}B{w}^{\ast }\\ & =z^{T}Qz+z^T{A}^T{P}_{t+1}Az\\ & +z^T{A}^T{P}_{t+1}B(R+B^T{P}_{t+1}B{)}^{-1}(R+B^T{P}_{t+1}B)(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t+1}Az\\ & -2z^T{A}^T{P}_{t+1}B(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t+1}Az\\ & =z^{T}Qz+z^T{A}^T{P}_{t+1}Az\\ & +z^T{A}^T{P}_{t+1}B(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t+1}Az\\ & -2z^T{A}^T{P}_{t+1}B(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t+1}Az\\ & =z^{T}Qz+z^T{A}^T{P}_{t+1}Az-z^T{A}^T{P}_{t+1}B(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t+1}Az\\ & =z^T(Q+A^T{P}_{t+1}A-A^T{P}_{t+1}B(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t+1}A)z\\ & =z^T{P}_{t}z\end{array}\text{\hspace{1em}(17)}$$
上述公式化简过程中，由于$P_{t+1}=P_{t+1}^T,R=R^T$，所以$((R+B^T{P}_{t+1}B{)}^{-1}{)}^T=(R+B^T{P}_{t+1}B{)}^{-1}$。

由等式(17)可知
$$P_t=Q+A^T{P}_{t+1}A-A^T{P}_{t+1}B(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t+1}A\text{\hspace{1em}(18)}$$

求解过程

关于LQR的求解过程，可以采用动态规划算法，依据上述公式(20)的递归关系，反向递推，求出满足一定条件的最小代价值。

1. 确定迭代范围$N$
2. 设置迭代初始值$P_N=Q_f$
3. 循环迭代，$t=N,\dots ,1$

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

4. 则反馈系数$K_t=-(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t+1}A$，对于时间$t=0,\dots ,N-1$
5. 优化的控制量$u_t^{lqr}=K_t{x}_t$