无人机|LQR控制算法及其无人机控制中的应用仿真
前言
LQR全称Linear Quadratic Regulator（线性二次调节器），顾名思义用于解决形如
x ˙ = A x + B u y = C x + D u \begin{aligned}\dot{x}&=Ax+Bu\\y&=Cx+Du\end{aligned} x˙y=Ax+Bu=Cx+Du
线性时不变系统的一种线性控制方法，是最优控制方法的一种。
LQR通过全状态反馈将不同状态加权求和得到最优控制量，本文所讨论的是无限时间LQR问题，即可以保证系统是渐进稳定的，不考虑收敛时间。该方法主要思想是构造以状态量以及控制量相关的二次代价函数，通过最小化该代价函数寻找成本最低的解。
LQR基本原理
考虑形如
x ˙ = A x + B u y = C x + D u \begin{aligned}\dot{x}&=Ax+Bu\\y&=Cx+Du\end{aligned} x˙y=Ax+Bu=Cx+Du的系统
其中控制量满足
u = − K x u=-Kx u=−Kx
K为反馈矩阵
考虑无穷时间内的代价函数，由于系统无稳态误差，故时间趋于无穷时终端约束项为0
构造代价函数为
m i n J = 1 2 ∫ 0 ∞ ( x T Q x + u T R u ) d t , Q = Q T , R = R T , Q ≥ 0 , R > 0 min J=\frac1{2}\int_0^\infty(x^TQx+u^TRu)dt,Q=Q^T,R=R^T,Q\geq0,R>0 minJ=21∫0∞(xTQx+uTRu)dt,Q=QT,R=RT,Q≥0,R>0
一般地，Q和R均为正定对角阵。
求解该代价函数最小时所对应的K矩阵，即可求得最优控制量。
matlab中可以直接使用工具包求解，如
K=lqr（A，B，Q，R）
公式推导
利用拉格朗日乘子法构造增广泛函
J ， = ∫ 0 ∞ ( 1 2 ( x T Q x + u T R u ) + λ T ( A x + B u − x ˙ ) ) d t J^{，}=\int_0^\infty(\frac1{2}(x^TQx+u^TRu)+\lambda^T(Ax+Bu-\dot{x}))dt J，=∫0∞(21(xTQx+uTRu)+λT(Ax+Bu−x˙))dt
定义纯量函数，及哈密尔顿函数
H ( x , u , λ , t ) = 1 2 ( x T Q x + u T R u ) + λ T ( A x + B u ) H(x,u,\lambda,t)=\frac1{2}(x^TQx+u^TRu)+\lambda^T(Ax+Bu) H(x,u,λ,t)=21(xTQx+uTRu)+λT(Ax+Bu)
则有
由变分法可得取极值时应满足控制方程
∂ H ∂ u = 0 \frac{\partial H}{\partial u}=0 ∂u∂H=0
则有
∂ H ∂ u = R u + B T λ = 0 \frac{\partial H}{\partial u}=Ru+B^T\lambda=0 ∂u∂H=Ru+BTλ=0
得
u ∗ = − R − 1 B T λ u^*=-R^{-1}B^T\lambda u∗=−R−1BTλ
又u应为关于x得线性表达，且由上式可得此时u为 λ \lambda λ的线性表达，故 λ \lambda λ也应为x的线性表达。
设
λ = P x \lambda=Px λ=Px
则有
u ∗ = − R − 1 B T P x u^*=-R^{-1}B^TPx u∗=−R−1BTPx
又根据正则方程
∂ H ∂ x + λ ˙ = 0 \frac{\partial H}{\partial x}+\dot\lambda=0 ∂x∂H+λ˙=0
∂ H ∂ λ = x ˙ \frac{\partial H}{\partial \lambda}=\dot x ∂λ∂H=x˙
得
λ ˙ = − ∂ H ∂ x = − Q x − A T λ = − Q x − A T P x \dot\lambda = -\frac{\partial H}{\partial x} = -Qx-A^T\lambda=-Qx-A^TPx λ˙=−∂x∂H=−Qx−ATλ=−Qx−ATPx
x ˙ = ∂ H ∂ λ = A x − B R − 1 B T P x \dot x=\frac{\partial H}{\partial \lambda}=Ax-BR^{-1}B^TPx x˙=∂λ∂H=Ax−BR−1BTPx
又对 λ = P x \lambda=Px λ=Px两边求导，得
λ ˙ = P ˙ x + P x ˙ \dot\lambda=\dot Px+P\dot x λ˙=P˙x+Px˙
P为常数矩阵时，则有
− Q x − A T P x = P A x − P B R − 1 B T P x -Qx-A^TPx = PAx-PBR^{-1}B^TPx −Qx−ATPx=PAx−PBR−1BTPx
又x为非零矩阵，则有
P A + A T P − P B R − 1 B T P + Q = 0 PA+A^TP-PBR^{-1}B^TP+Q=0 PA+ATP−PBR−1BTP+Q=0
即为riccati方程
求解该方程可得P
由此可解得 u ∗ = − R − 1 B T P x u^*=-R^{-1}B^TPx u∗=−R−1BTPx
仿真
在上篇基础上进行控制器修改
TODO