基础算法-LQR-离散时间有限边界

发表于 更新于 分类于 ， 阅读次数：

概述

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

LQR问题背景

对于一个离散时间系统：

\[ x_{t+1}=Ax_t + Bu_t,x_0=x_{init}\tag{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^{N-1}_{\tau=0}(x^{T}_{\tau}Qx_{\tau} + u^{T}_{\tau}Ru_{\tau})+ x^{T}_{N}Q_{f}x_{N}\tag{2} \]

其中函数参数\(U = (u_0,u_1,..,u_N)\)，并且矩阵\(Q,Q_f,R\)为正定矩阵，及

\[ \begin{array}{cl} Q=Q^{T}\geq0,&Q_f=Q_{f}^{T}\geq0,&R=R^{T}>0 \end{array} \]

\(Q\) \(Q_f\) \(R\)
给定状态代价矩阵 最终状态代价矩阵 输入代价矩阵
  • \(N\)：时间范围(考虑\(N = \infty\))
  • \(Q\)，\(R\)：分别设定状态偏差和输入的相对权重
  • \(R>0\)：意味着任何非零输入都增加\(J\)的代价
  • \(x_{\tau}^{T} {Q} x_{\tau}\)：衡量状态偏差
  • \(u^{T}_{\tau} R u_{\tau}\)：衡量输入大小
  • \(x^{T}_{N} Q_{f} x_{N}\)：衡量最终状态偏差

因此，关于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 &= Ax_0 + Bu_0\\ x_2 &= Ax_1 + Bu_1\\ \vdots\\ x_n &= Ax_{N-1} + Bu_{N-1} \end{array}\tag{3} \]

将上述公式(3)逐个带入得

\[ \begin{array}{cl} x_1 &= Ax_0 + Bu_0\\ x_2 &= A^{2}x_0 + ABu_0 + Bu_1\\ \vdots\\ x_n &= A^{N}x_0 + A^{N-1}Bu_0 + A^{N-2}Bu_1 + \dots+ Bu_{N-1} \end{array} \tag{4} \]

整理得

\[ \left[\begin{array}{cl} x_0\\ x_1\\ \vdots\\ x_N \end{array}\right]= \left[ \begin{array}{cl} 0 & \dots \\ B & 0 & \dots \\ AB & B & 0 & \dots \\ \vdots & \vdots \\ A^{N-1}B & A^{N-2}B & \dots & B \end{array}\right] \left[ \begin{array}{cl} u_0\\ u_1\\ \vdots\\ u_{N-1} \end{array} \right]+ \left[ \begin{array}{cl} I\\ A\\ \vdots\\ A^{N} \end{array} \right]x_0 \tag{5} \]

其中

\[ G=\left[ \begin{array}{cl} 0 & \dots \\ B & 0 & \dots \\ AB & B & 0 & \dots \\ \vdots & \vdots \\ A^{N-1}B & A^{N-2}B & \dots & B \end{array}\right]，H=\left[ \begin{array}{cl} I\\ A\\ \vdots\\ A^{N} \end{array} \right] \]

等式(5)可以进一步表示为

\[ X= GU + Hx_0 \tag{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^{1/2}_{f})(GU+Hx_0)\parallel^2+ \parallel diag(R^{1/2},\dots,R^{1/2})U\parallel^2 \tag{7} \]

这就转化成一个求解最小二乘法的问题，其问题大小为\(N(n + m)\times Nm\)。

动态规划法(Dynamic Programming)

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

值函数

首先定义一个值函数\(V_t:R^n \to R\),其中\(t=(0,\dots,N)\)：

\[ V_t(z)=\min_{u_t,\dots,u_{N-1}}\Bigl(\sum_{\tau=t}^{N-1}(x^T_\tau Qx_\tau + u^t_\tau Ru_\tau) + x_N^TQ_fx_N\Bigr) \tag{8} \]

如果设置\(x_t = z\)，根据公式(1)的关系，\(x_{\tau+1} = Ax_{\tau} + Bu_{\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^TP_tz\), 其中\(P_t=P_t^T \geq 0\)。当\(t=N\)时，代价值函数为：

\[ V_N(z) = z^TQ_f z \tag{9} \]

因此\(P_N = Q_f\)。

根据动态规划原理，等式(8)可以写成如下递归关系式：

\[ V_t(z)=\min_w\bigl(z^TQz + w^TRw + V_{t+1}(Az+Bw)\bigr)\tag{10} \]

其中，

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

提取等式(10)中与\(w\)无关的选项得

\[ V_t(z)=z^TQz + \min_w\bigl(w^TRw + V_{t+1}(Az+Bw)\bigr)\tag{11} \]

等式(11)描述了\(V_t(z)\)与\(V_{t+1}(z)\)之间的递归关系。

最优控制率\(u_t\)可以表示为

\[ u_t^{lqr}=arg\min_w\bigl(w^TRw + V_{t+1}(Az+Bw)\bigr) \]

求极值

假设\(V_{t+1}= z^TP_{t+1}z\),并且\(P_{t+1}=P^{T}_{t+1} \geq0\),等式(13)可以进一步转化为\(P_{t+1}\)的形式：

\[ V_t(z)=z^TQz + \min_w\bigl(w^TRw + (Az+Bw)^TP_{t+1}(Az+Bw)\bigr)\tag{12} \]

为了求最小值，对\(w\)求导，导数为零的点即为最值点。

\[ 2w^TR + 2(Az+Bw)^TP_{t+1}B = 0 \tag{13} \]

推导等式(13)，求取\(w\):

\[ \begin{array}{cl} w^TR + z^{T}A^{T}P_{t+1}B+w^{T}B^{T}P_{t+1}B &= 0\\ w^T(R + B^TP_{t+1}B) &= - z^{T}A^{T}P_{t+1}B &\text{(合并同类项并移项)}\\ (R + B^TP_{t+1}B)^Tw &= -B^TP_{t+1}^{T}Az & \text{(转置)}\\ (R + B^TP_{t+1}B)w &= -B^TP_{t+1}Az &(P_{t+1}=P^{T}_{t+1},R=R^T)\\ w &=-(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az &\text{(矩阵求逆)} \end{array}\tag{14} \]

由等式(14)可知，最优输入为

\[ w^* =-(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az \tag{15} \]

将等式(15)带入等式(12)得

\[ V_t(z)=z^TQz + w^{*T}Rw^* + (Az+Bw^*)^TP_{t+1}(Az+Bw^*)\tag{16} \]

对等式(16)化简得

\[ \begin{array}{cl} V_t(z) &= & z^TQz + w^{*T}Rw^* + (Az+Bw^*)^TP_{t+1}(Az+Bw^*)\\ &= & z^TQz + w^{*T}Rw^* + z^TA^TP_{t+1}Az + 2z^TA^TP_{t+1}Bw^* + w^{*T}B^TP_{t+1}Bw^*\\ &= &z^TQz + z^TA^TP_{t+1}Az + w^{*T}(R+B^TP_{t+1}B)w^* + 2z^TA^TP_{t+1}Bw^*\\ &= &z^TQz + z^TA^TP_{t+1}Az\\ & &+z^TA^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}(R+B^TP_{t+1}B)(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az\\ & &-2z^TA^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az\\ &= &z^TQz + z^TA^TP_{t+1}Az\\ & &+z^TA^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az\\ & &-2z^TA^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az\\ &= &z^TQz + z^TA^TP_{t+1}Az - z^TA^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az\\ &= &z^T(Q + A^TP_{t+1}A - A^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}A)z\\ &= &z^TP_tz \end{array}\tag{17} \]

上述公式化简过程中，由于\(P_{t+1}=P^{T}_{t+1},R=R^T\)，所以\(\bigl((R+B^TP_{t+1}B)^{-1}\bigr)^T = (R + B^TP_{t+1}B)^{-1}\)。

由等式(17)可知

\[ P_t = Q + A^TP_{t+1}A - A^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}A \tag{18} \]

求解过程

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

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

\[ P_{t-1} = Q + A^TP_{t+1}A - A^TP_{t+1}B(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}A \]

  1. 则反馈系数\(K_t = -(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}A\)，对于时间\(t=0,\dots,N-1\)
  2. 优化的控制量\(u_t^{lqr}=K_tx_t\)