概述
本文介绍离散时间有限范围内的LQR(Linear Quadratic Regulator)算法求解过程.
LQR问题背景
对于一个离散时间系统:
xt+1=Axt+But,x0=xinit(1)
其中,A∈Rn×n,B∈Rn×m
关于最优问题,就在于如何选择合适的u0,u1,...,使得状态量x0,x1,...足够小,因此得到好的调节和控制;或者使得u0,u1,...足够小,以使用更少的能量。这两个量通常相互制约,如果采用更大的输入u,就会驱使状态量x更快达到0。采用线性二次调节原理可以解决这个问题。
LQR代价函数
为了表示控制系统达到稳定控制所付出的代价,定义如下二次型代价函数:
J(U)=τ=0∑N−1(xτTQxτ+uτTRuτ)+xNTQfxN(2)
其中函数参数U=(u0,u1,..,uN),并且矩阵Q,Qf,R为正定矩阵,及
Q=QT≥0,Qf=QfT≥0,R=RT>0
Q |
Qf |
R |
给定状态代价矩阵 |
最终状态代价矩阵 |
输入代价矩阵 |
- N:时间范围(考虑N=∞)
- xτTQxτ:衡量状态偏差
- uτTRuτ:衡量输入大小
- xNTQfxN:衡量最终状态偏差
- Q,R:分别设定状态偏差和输入的相对权重
- R>0:意味着任何非零输入都增加J的代价
因此,关于LQR问题就是找出使得代价函数J(U)最小的一组控制输入(u0,u1,...,uN−1)lqr。
求解LQR方法
本文主要介绍两种求解LQR的方法,分别为最小二乘法和动态规划算法。
最小二乘法
根据公式(1)可知,x0是X=(x0,...,xN)的线性函数,并且U=(u0,...,uN−1),可以得出如下关系:
x1x2⋮xn=Ax0+Bu0=Ax1+Bu1=AxN−1+BuN−1(3)
将上述公式(3)逐个带入得
x1x2⋮xn=Ax0+Bu0=A2x0+ABu0+Bu1=ANx0+AN−1Bu0+AN−2Bu1+⋯+BuN−1(4)
整理得
⎣⎢⎢⎢⎡x0x1⋮xN⎦⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎡0BAB⋮AN−1B…0B⋮AN−2B…0……B⎦⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎡u0u1⋮uN−1⎦⎥⎥⎥⎤+⎣⎢⎢⎢⎡IA⋮AN⎦⎥⎥⎥⎤x0(5)
其中
G=⎣⎢⎢⎢⎢⎢⎡0BAB⋮AN−1B…0B⋮AN−2B…0……B⎦⎥⎥⎥⎥⎥⎤,H=⎣⎢⎢⎢⎡IA⋮AN⎦⎥⎥⎥⎤
等式(5)可以进一步表示为
X=GU+Hx0(6)
其中,G∈RNn×Nm,H∈RNn×n。
从而等式(2)所表示得代价函数可以表示为
J(U)=∥disg(Q1/2,…,Q1/2,Qf1/2)(GU+Hx0)∥2+∥diag(R1/2,…,R1/2)U∥2(7)
这就转化成一个求解最小二乘法的问题,其问题大小为N(n+m)×Nm。
动态规划法(Dynamic Programming)
动态规划算法是解决多阶段决策过程最优化的一种有效的数学方法。
值函数
首先定义一个值函数Vt:Rn→R,其中t=(0,…,N):
Vt(z)=ut,…,uN−1min(τ=t∑N−1(xτTQxτ+uτtRuτ)+xNTQfxN)(8)
如果设置xt=z,根据公式(1)的关系,xτ+1=Axτ+Buτ,并且τ=t,…,N。
- Vt(z)可以表示在t时刻,从状态z开始的LQR最小代价值
- V0(x0)表示在0时刻,从状态x0开始的LQR最小代价值
Vt可以表示为二次型的形式,即VT(z)=zTPtz,其中Pt=PtT≥0。当t=N时,代价值函数为:
VN(z)=zTQfz(9)
因此PN=Qf。
根据动态规划原理,等式(8)可以写成如下递归关系式:
Vt(z)=wmin(zTQz+wTRw+Vt+1(Az+Bw))(10)
其中,
- zTQz+wTRw:如果ut=w,则代表t时刻产生的代价值;
- Vt+1(Az+Bw):代表从t+1时刻开始,引起的最小代价值;
提取等式(10)中与w无关的选项得
Vt(z)=zTQz+wmin(wTRw+Vt+1(Az+Bw))(11)
等式(11)描述了Vt(z)与Vt+1(z)之间的递归关系。
求极值
假设Vt+1=zTPt+1z,并且Pt+1=Pt+1T≥0,等式(11)可以进一步转化为Pt+1的形式:
Vt(z)=zTQz+wmin(wTRw+(Az+Bw)TPt+1(Az+Bw))(12)
为了求最小值,对w求导,导数为零的点即为最值点。
2wTR+2(Az+Bw)TPt+1B=0(13)
推导等式(13),求取w:
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)
将等式(15)带入等式(12)得
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)
上述公式化简过程中,由于Pt+1=Pt+1T,R=RT,所以((R+BTPt+1B)−1)T=(R+BTPt+1B)−1。
由等式(17)可知
Pt=Q+ATPt+1A−ATPt+1B(R+BTPt+1B)−1BTPt+1A(18)
求解过程
关于LQR的求解过程,可以采用动态规划算法,依据上述公式(20)的递归关系,反向递推,求出满足一定条件的最小代价值。
- 确定迭代范围N
- 设置迭代初始值PN=Qf
- 循环迭代,t=N,…,1
Pt−1=Q+ATPt+1A−ATPt+1B(R+BTPt+1B)−1BTPt+1A
- 则反馈系数Kt=−(R+BTPt+1B)−1BTPt+1A,对于时间t=0,…,N−1
- 优化的控制量utlqr=Ktxt