无人机|LQR控制算法及其无人机控制中的应用仿真

前言

LQR全称Linear Quadratic Regulator（线性二次调节器），顾名思义用于解决形如
$$\begin{array}{rl}\dot{x}& =Ax+Bu\\ y& =Cx+Du\end{array}$$
线性时不变系统的一种线性控制方法，是最优控制方法的一种。
LQR通过全状态反馈将不同状态加权求和得到最优控制量，本文所讨论的是无限时间LQR问题，即可以保证系统是渐进稳定的，不考虑收敛时间。该方法主要思想是构造以状态量以及控制量相关的二次代价函数，通过最小化该代价函数寻找成本最低的解。

LQR基本原理

考虑形如
$$\begin{array}{rl}\dot{x}& =Ax+Bu\\ y& =Cx+Du\end{array}$$的系统
其中控制量满足
$$u=-Kx$$
K为反馈矩阵
考虑无穷时间内的代价函数，由于系统无稳态误差，故时间趋于无穷时终端约束项为0
构造代价函数为
$$minJ=\frac{1}{2}{\int }_0^{\infty }(x^{T}Qx+u^{T}Ru)dt,Q=Q^T,R=R^T,Q\ge 0,R>0$$
一般地，Q和R均为正定对角阵。
求解该代价函数最小时所对应的K矩阵，即可求得最优控制量。
matlab中可以直接使用工具包求解，如

K=lqr（A，B，Q，R）

公式推导

利用拉格朗日乘子法构造增广泛函
$$J^{，}={\int }_0^{\infty }(\frac{1}{2}(x^{T}Qx+u^{T}Ru)+{\lambda }^T(Ax+Bu-\dot{x}))dt$$
定义纯量函数，及哈密尔顿函数
$$H(x,u,\lambda ,t)=\frac{1}{2}(x^{T}Qx+u^{T}Ru)+{\lambda }^T(Ax+Bu)$$
则有

由变分法可得取极值时应满足控制方程
$$\frac{\partial H}{\partial u}=0$$
则有
$$\frac{\partial H}{\partial u}=Ru+B^T\lambda =0$$
得
$$u^{\ast }=-R^{-1}{B}^T\lambda$$
又u应为关于x得线性表达，且由上式可得此时u为$\lambda$的线性表达，故$\lambda$也应为x的线性表达。
设
$$\lambda =Px$$
则有
$$u^{\ast }=-R^{-1}{B}^{T}Px$$
又根据正则方程
$$\frac{\partial H}{\partial x}+\dot{\lambda }=0$$
$$\frac{\partial H}{\partial \lambda }=\dot{x}$$
得
$$\dot{\lambda }=-\frac{\partial H}{\partial x}=-Qx-A^T\lambda =-Qx-A^{T}Px$$
$$\dot{x}=\frac{\partial H}{\partial \lambda }=Ax-B{R}^{-1}{B}^{T}Px$$
又对$\lambda =Px$两边求导，得
$$\dot{\lambda }=\dot{P}x+P\dot{x}$$
P为常数矩阵时，则有
$$-Qx-A^{T}Px=PAx-PB{R}^{-1}{B}^{T}Px$$
又x为非零矩阵，则有
$$PA+A^{T}P-PB{R}^{-1}{B}^{T}P+Q=0$$
即为riccati方程
求解该方程可得P
由此可解得$u^{\ast }=-R^{-1}{B}^{T}Px$

仿真

在上篇基础上进行控制器修改
TODO