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

文章目录

概述
LQR问题背景
LQR代价函数
求解LQR方法
- 最小二乘法
- 动态规划法(Dynamic Programming)
- - 值函数
  - 求极值
  - 求解过程

概述

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

LQR问题背景

对于一个离散时间系统：
xt+1=Axt+But,x0=xinit(1)x_{t+1}=Ax_t + Bu_t,x_0=x_{init}\tag{1} xt+1=Axt+But,x0=xinit(1)
其中，A∈Rn×nA\in R^{n\times n}A∈Rn×n，B∈Rn×mB\in R^{n\times m}B∈Rn×m

关于最优问题，就在于如何选择合适的u0,u1,...u_0,u_1,...u0,u1,...，使得状态量x0,x1,...x_0,x_1,...x0,x1,...足够小，因此得到好的调节和控制；或者使得u0,u1,...u_0,u_1,...u0,u1,...足够小，以使用更少的能量。这两个量通常相互制约，如果采用更大的输入uuu，就会驱使状态量xxx更快达到0。采用线性二次调节原理可以解决这个问题。

LQR代价函数

为了表示控制系统达到稳定控制所付出的代价，定义如下二次型代价函数：
J(U)=∑τ=0N−1(xτTQxτ+uτTRuτ)+xNTQfxN(2)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} J(U)=τ=0∑N−1(xτTQxτ+uτTRuτ)+xNTQfxN(2)
其中函数参数U=(u0,u1,..,uN)U = (u_0,u_1,..,u_N)U=(u0,u1,..,uN)，并且矩阵Q,Qf,RQ,Q_f,RQ,Qf,R为正定矩阵，及
Q=QT≥0,Qf=QfT≥0,R=RT>0\begin{array}{cl} Q=Q^{T}\geq0,&Q_f=Q_{f}^{T}\geq0,&R=R^{T}>0 \end{array} Q=QT≥0,Qf=QfT≥0,R=RT>0

QQQ	QfQ_fQf	RRR
给定状态代价矩阵	最终状态代价矩阵	输入代价矩阵

NNN：时间范围(考虑N=∞N = \inftyN=∞)
xτTQxτx^{T}_{\tau} Q x_{\tau}xτTQxτ：衡量状态偏差
uτTRuτu^{T}_{\tau} R u_{\tau}uτTRuτ：衡量输入大小
xNTQfxNx^{T}_{N} Q_{f} x_{N}xNTQfxN：衡量最终状态偏差
QQQ，RRR：分别设定状态偏差和输入的相对权重
R>0R>0R>0：意味着任何非零输入都增加JJJ的代价

因此，关于LQR问题就是找出使得代价函数J(U)J(U)J(U)最小的一组控制输入(u0,u1,...,uN−1)lqr(u_0,u_1,...,u_{N-1})_{lqr}(u0,u1,...,uN−1)lqr。

求解LQR方法

本文主要介绍两种求解LQR的方法，分别为最小二乘法和动态规划算法。

最小二乘法

根据公式(1)可知，x0x_0x0是X=(x0,...,xN)X = (x_0,...,x_N)X=(x0,...,xN)的线性函数，并且U=(u0,...,uN−1)U = (u_0,...,u_{N-1})U=(u0,...,uN−1)，可以得出如下关系：
x1=Ax0+Bu0x2=Ax1+Bu1⋮xn=AxN−1+BuN−1(3)\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} x1x2⋮xn=Ax0+Bu0=Ax1+Bu1=AxN−1+BuN−1(3)
将上述公式(3)逐个带入得
x1=Ax0+Bu0x2=A2x0+ABu0+Bu1⋮xn=ANx0+AN−1Bu0+AN−2Bu1+⋯+BuN−1(4)\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} x1x2⋮xn=Ax0+Bu0=A2x0+ABu0+Bu1=ANx0+AN−1Bu0+AN−2Bu1+⋯+BuN−1(4)
整理得
[x0x1⋮xN]=[0…B0…ABB0…⋮⋮AN−1BAN−2B…B][u0u1⋮uN−1]+[IA⋮AN]x0(5)\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} ⎣⎢⎢⎢⎡x0x1⋮xN⎦⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎡0BAB⋮AN−1B…0B⋮AN−2B…0……B⎦⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎡u0u1⋮uN−1⎦⎥⎥⎥⎤+⎣⎢⎢⎢⎡IA⋮AN⎦⎥⎥⎥⎤x0(5)
其中
G=[0…B0…ABB0…⋮⋮AN−1BAN−2B…B],H=[IA⋮AN]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] G=⎣⎢⎢⎢⎢⎢⎡0BAB⋮AN−1B…0B⋮AN−2B…0……B⎦⎥⎥⎥⎥⎥⎤,H=⎣⎢⎢⎢⎡IA⋮AN⎦⎥⎥⎥⎤

等式(5)可以进一步表示为
X=GU+Hx0(6)X= GU + Hx_0 \tag{6} X=GU+Hx0(6)
其中，G∈RNn×NmG\in R^{Nn\times Nm}G∈RNn×Nm，H∈RNn×nH\in R^{Nn\times n}H∈RNn×n。

从而等式(2)所表示得代价函数可以表示为
J(U)=∥diag(Q1/2,…,Q1/2,Qf1/2)(GU+Hx0)∥2+∥diag(R1/2,…,R1/2)U∥2(7)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} J(U)=∥diag(Q1/2,…,Q1/2,Qf1/2)(GU+Hx0)∥2+∥diag(R1/2,…,R1/2)U∥2(7)
这就转化成一个求解最小二乘法的问题，其问题大小为N(n+m)×NmN(n + m)\times NmN(n+m)×Nm。

动态规划法(Dynamic Programming)

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

值函数

首先定义一个值函数Vt:Rn→RV_t:R^n \to RVt:Rn→R,其中t=(0,…,N)t=(0,\dots,N)t=(0,…,N)：
Vt(z)=min⁡ut,…,uN−1(∑τ=tN−1(xτTQxτ+uτtRuτ)+xNTQfxN)(8)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} Vt(z)=ut,…,uN−1min(τ=t∑N−1(xτTQxτ+uτtRuτ)+xNTQfxN)(8)
如果设置xt=zx_t = zxt=z，根据公式(1)的关系，xτ+1=Axτ+Buτx_{\tau+1} = Ax_{\tau} + Bu_{\tau}xτ+1=Axτ+Buτ,并且τ=t,…,N\tau=t,\dots,Nτ=t,…,N。

Vt(z)V_t(z)Vt(z)可以表示在ttt时刻，从状态zzz开始的LQR最小代价值
V0(x0)V_0(x_0)V0(x0)表示在0时刻，从状态x0x_0x0开始的LQR最小代价值

VtV_tVt可以表示为二次型的形式，即VT(z)=zTPtz,其中Pt=PtT≥0V_T(z)=z^TP_tz,其中P_t=P_t^T \geq 0VT(z)=zTPtz,其中Pt=PtT≥0。当t=Nt=Nt=N时，代价值函数为：
VN(z)=zTQfz(9)V_N(z) = z^TQ_f z \tag{9} VN(z)=zTQfz(9)
因此PN=QfP_N = Q_fPN=Qf。

根据动态规划原理，等式(8)可以写成如下递归关系式：
Vt(z)=min⁡w(zTQz+wTRw+Vt+1(Az+Bw))(10)V_t(z)=\min_w\bigl(z^TQz + w^TRw + V_{t+1}(Az+Bw)\bigr)\tag{10} Vt(z)=wmin(zTQz+wTRw+Vt+1(Az+Bw))(10)
其中，

zTQz+wTRwz^TQz + w^TRwzTQz+wTRw：如果ut=wu_t = wut=w,则代表ttt时刻产生的代价值；
Vt+1(Az+Bw)V_{t+1}(Az+Bw)Vt+1(Az+Bw)：代表从t+1t+1t+1时刻开始，引起的最小代价值；

提取等式(10)中与www无关的选项得
Vt(z)=zTQz+min⁡w(wTRw+Vt+1(Az+Bw))(11)V_t(z)=z^TQz + \min_w\bigl(w^TRw + V_{t+1}(Az+Bw)\bigr)\tag{11} Vt(z)=zTQz+wmin(wTRw+Vt+1(Az+Bw))(11)
等式(11)描述了Vt(z)V_t(z)Vt(z)与Vt+1(z)V_{t+1}(z)Vt+1(z)之间的递归关系。

求极值

假设Vt+1=zTPt+1zV_{t+1}= z^TP_{t+1}zVt+1=zTPt+1z,并且Pt+1=Pt+1T≥0P_{t+1}=P^{T}_{t+1} \geq0Pt+1=Pt+1T≥0,等式(11)可以进一步转化为Pt+1P_{t+1}Pt+1的形式：
Vt(z)=zTQz+min⁡w(wTRw+(Az+Bw)TPt+1(Az+Bw))(12)V_t(z)=z^TQz + \min_w\bigl(w^TRw + (Az+Bw)^TP_{t+1}(Az+Bw)\bigr)\tag{12} Vt(z)=zTQz+wmin(wTRw+(Az+Bw)TPt+1(Az+Bw))(12)
为了求最小值，对www求导，导数为零的点即为最值点。
2wTR+2(Az+Bw)TPt+1B=0(13)2w^TR + 2(Az+Bw)^TP_{t+1}B = 0 \tag{13} 2wTR+2(Az+Bw)TPt+1B=0(13)
推导等式(13)，求取www:
wTR+zTATPt+1B+wTBTPt+1B=0wT(R+BTPt+1B)=−zTATPt+1B(合并同类项并移项)(R+BTPt+1B)Tw=−BTPt+1TAz(转置)(R+BTPt+1B)w=−BTPt+1Az(Pt+1=Pt+1T,R=RT)w=−(R+BTPt+1B)−1BTPt+1Az(矩阵求逆)(14)\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} wTR+zTATPt+1B+wTBTPt+1BwT(R+BTPt+1B)(R+BTPt+1B)Tw(R+BTPt+1B)ww=0=−zTATPt+1B=−BTPt+1TAz=−BTPt+1Az=−(R+BTPt+1B)−1BTPt+1Az(合并同类项并移项)(转置)(Pt+1=Pt+1T,R=RT)(矩阵求逆)(14)
由等式(14)可知，最优输入为
w∗=−(R+BTPt+1B)−1BTPt+1Az(15)w^* =-(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}Az \tag{15} w∗=−(R+BTPt+1B)−1BTPt+1Az(15)
将等式(15)带入等式(12)得
Vt(z)=zTQz+w∗TRw∗+(Az+Bw∗)TPt+1(Az+Bw∗)(16)V_t(z)=z^TQz + w^{*T}Rw^* + (Az+Bw^*)^TP_{t+1}(Az+Bw^*)\tag{16} Vt(z)=zTQz+w∗TRw∗+(Az+Bw∗)TPt+1(Az+Bw∗)(16)
对等式(16)化简得
Vt(z)=zTQz+w∗TRw∗+(Az+Bw∗)TPt+1(Az+Bw∗)=zTQz+w∗TRw∗+zTATPt+1Az+2zTATPt+1Bw∗+w∗TBTPt+1Bw∗=zTQz+zTATPt+1Az+w∗T(R+BTPt+1B)w∗+2zTATPt+1Bw∗=zTQz+zTATPt+1Az+zTATPt+1B(R+BTPt+1B)−1(R+BTPt+1B)(R+BTPt+1B)−1BTPt+1Az−2zTATPt+1B(R+BTPt+1B)−1BTPt+1Az=zTQz+zTATPt+1Az+zTATPt+1B(R+BTPt+1B)−1BTPt+1Az−2zTATPt+1B(R+BTPt+1B)−1BTPt+1Az=zTQz+zTATPt+1Az−zTATPt+1B(R+BTPt+1B)−1BTPt+1Az=zT(Q+ATPt+1A−ATPt+1B(R+BTPt+1B)−1BTPt+1A)z=zTPtz(17)\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} Vt(z)=zTQz+w∗TRw∗+(Az+Bw∗)TPt+1(Az+Bw∗)=zTQz+w∗TRw∗+zTATPt+1Az+2zTATPt+1Bw∗+w∗TBTPt+1Bw∗=zTQz+zTATPt+1Az+w∗T(R+BTPt+1B)w∗+2zTATPt+1Bw∗=zTQz+zTATPt+1Az+zTATPt+1B(R+BTPt+1B)−1(R+BTPt+1B)(R+BTPt+1B)−1BTPt+1Az−2zTATPt+1B(R+BTPt+1B)−1BTPt+1Az=zTQz+zTATPt+1Az+zTATPt+1B(R+BTPt+1B)−1BTPt+1Az−2zTATPt+1B(R+BTPt+1B)−1BTPt+1Az=zTQz+zTATPt+1Az−zTATPt+1B(R+BTPt+1B)−1BTPt+1Az=zT(Q+ATPt+1A−ATPt+1B(R+BTPt+1B)−1BTPt+1A)z=zTPtz(17)
上述公式化简过程中，由于Pt+1=Pt+1T,R=RTP_{t+1}=P^{T}_{t+1},R=R^TPt+1=Pt+1T,R=RT，所以((R+BTPt+1B)−1)T=(R+BTPt+1B)−1\bigl((R + B^TP_{t+1}B)^{-1}\bigr)^T = (R + B^TP_{t+1}B)^{-1}((R+BTPt+1B)−1)T=(R+BTPt+1B)−1。

由等式(17)可知
Pt=Q+ATPt+1A−ATPt+1B(R+BTPt+1B)−1BTPt+1A(18)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} Pt=Q+ATPt+1A−ATPt+1B(R+BTPt+1B)−1BTPt+1A(18)

求解过程

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

确定迭代范围NNN
设置迭代初始值PN=QfP_N=Q_fPN=Qf
循环迭代，t=N,…,1t = N,\dots,1t=N,…,1

Pt−1=Q+ATPt+1A−ATPt+1B(R+BTPt+1B)−1BTPt+1AP_{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 Pt−1=Q+ATPt+1A−ATPt+1B(R+BTPt+1B)−1BTPt+1A

则反馈系数Kt=−(R+BTPt+1B)−1BTPt+1AK_t = -(R + B^TP_{t+1}B)^{-1}B^TP_{t+1}AKt=−(R+BTPt+1B)−1BTPt+1A，对于时间t=0,…,N−1t=0,\dots,N-1t=0,…,N−1
优化的控制量utlqr=Ktxtu_t^{lqr}=K_tx_tutlqr=Ktxt