四旋翼的非线性模型预测控制（MPC）

1. 四旋翼的动力学模型

这里不加推导地给出四旋翼的动力学模型：
$$m\left[\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right]=\left[\begin{array}{c}{\sum }_{i=1}^4{F}_i\left(\cos \phi \sin \theta \cos \psi +\sin \phi \sin \psi \right)\\ {\sum }_{i=1}^4{F}_i\left(\cos \phi \sin \theta \sin \psi -\sin \phi \cos \psi \right)\\ {\sum }_{i=1}^4{F}_i\cos \phi \cos \theta -mg\end{array}\right]\text{\hspace{1em}(1)}$$$$\left[\begin{array}{c}{J}_x\dot{p}\\ J_y\dot{q}\\ J_z\dot{r}\end{array}\right]=\left[\begin{array}{c}l\left(F_2-F_2\right)-\left(J_z-J_y\right)qr\\ l\left(F_1-F_3\right)-\left(J_x-J_z\right)pr\\ M_{D1}-M_{D2}+M_{D3}-M_{D4}-\left(J_y-J_x\right)pq\end{array}\right]\text{\hspace{1em}(2)}$$$$\left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \sin \phi \tan \theta & \cos \phi \tan \theta \\ 0& \cos \phi & -\sin \phi \\ 0& \frac{\sin \phi }{\cos \theta }& \frac{\cos \phi }{\cos \theta }\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$$其中$\phi ,\theta ,\psi$均为机体欧拉角，$p,q,r$为欧拉角的角速度。（关于更加详细的四旋翼建模过程可以参考笔者博客Backstepping反步法控制四旋翼无人机（一））

2. 模型预测控制（MPC）基本方程

模型预测控制（Model Predictive Control, MPC）本质上是最优控制的一种，尝试在约束条件下达到最优的系统表现。它通过预测模型在未来一段时间内的表现来进行优化控制，通过不断迭代，最终使系统逐渐优化。

一般地，对于一个系统
$$\begin{gathered} x\left(k+1\right)=AX（k)+BU(k) \\ A\in R^{n\times n},B\in R^{n\times p} \end{gathered}$$建立向量
$$X_k=\left[\begin{array}{c}x(k|k)\\ x(k+1|k)\\ ⋮\\ x(k+N-1|k)\end{array}\right],U_k=\left[\begin{array}{c}u(k|k)\\ u(k+1|k)\\ ⋮\\ u(k+N-1|k)\end{array}\right]$$同时假设输出即为$y=x$，那么误差即为
$$E=y-x=0$$另外，性能指标具有如下形式
$$J=\sum _{i=0}^{N-1}\left[x(k+i|k{)}^{T}Qx(k+i|k)+u(k+i|k{)}^{t}Ru(k+i|k)\right]+x(k+N{)}^{T}Fx(k+N)$$其中$Q,R,F$分别为状态、控制量、静态误差的权重。通过调整三者的相对大小，可以实现对三者的不同权重调整，系统表现也会有所不同。

设$$\bar{Q}=\left[\begin{array}{ccccc}Q& & & & \\ & Q& & & \\ & & \ddots & & \\ & & & Q& \\ & & & & F\end{array}\right],\bar{R}=\left[\begin{array}{cccc}R& & & \\ & R& & \\ & & \ddots & \\ & & & R\end{array}\right]$$$$M=\left[\begin{array}{c}I\\ A\\ A^2\\ ⋮\\ A^N\end{array}\right],C=\left[\begin{array}{cccc}0& 0& \cdots & 0\\ B& 0& \cdots & 0\\ AB& B& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ A^{N-1}B& A^{N-2}B& \cdots & B\end{array}\right]$$$$G=M^T\bar{Q},E=M^T\bar{Q}C,H=C^T\bar{Q}C+\bar{R}$$那么性能指标可以写为
$$J=x_k^{T}G{x}_k+2x_k^{T}E{U}_k+U_k^{T}H{U}_k\text{\hspace{1em}(3)}$$该式即为二次规划的一般形式。二次规划的一般形式为
$$\min \left(z^{T}Qz+c^{T}z\right)\text{\hspace{1em}(4)}$$(3)式中的$U$即对应(4)式中的$z$。

3. 非线性模型预测控制

不难看出，以上给出的方法仅仅适用于线性系统。当系统为非线性时，$A,B$矩阵将很难写出，进而$M,C,G,E,H$矩阵无法计算。这部分针对非线性系统给出了模型预测控制的算法。

很容易看出，非线性模型预测控制的难点就在于如何获取矩阵$A,B$。不妨设系统的期望值为$x_r$，利用泰勒展开将系统的非线性表达式在期望值附近展开，随后利用拉格朗日方程得到矩阵$A,B$即可。

具体计算公式如下：
$$\dot{X}=f_i\left(x_i,u_j\right)$$$$A={\left[\begin{array}{cccc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}& \cdots & \frac{\partial f_1}{\partial x_n}\\ \frac{\partial f_2}{\partial x_1}& \frac{\partial f_2}{\partial x_2}& \cdots & \frac{\partial f_2}{\partial x_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial f_n}{\partial x_1}& \frac{\partial f_n}{\partial x_2}& \cdots & \frac{\partial f_n}{\partial x_n}\end{array}\right]}_{x_i=x_r}$$$$B={\left[\begin{array}{cccc}\frac{\partial f_1}{\partial u_1}& \frac{\partial f_1}{\partial u_2}& \cdots & \frac{\partial f_1}{\partial u_p}\\ \frac{\partial f_2}{\partial u_1}& \frac{\partial f_2}{\partial u_2}& \cdots & \frac{\partial f_2}{\partial u_p}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial f_n}{\partial u_1}& \frac{\partial f_n}{\partial u_2}& \cdots & \frac{\partial f_n}{\partial u_p}\end{array}\right]}_{u_i=u_r}$$值得注意的是，$A,B$中的$x_i,u_i$应当代入期望值$x_r,u_r$。

下面以四旋翼为例进行计算。

设状态量为
$$\begin{array}{rl}{x}_1& =\phi \\ x_2& ={\dot{x}}_1=\dot{\phi }\\ x_3& =\theta \\ x_4& ={\dot{x}}_3=\dot{\theta }\\ x_5& =\psi \\ x_6& ={\dot{x}}_5=\dot{\psi }\\ x_7& =x\\ x_8& ={\dot{x}}_7=\dot{x}\\ x_9& =y\\ x_{10}& ={\dot{x}}_9=\dot{y}\\ x_{11}& =z\\ x_{12}& ={\dot{x}}_{11}=\dot{z}\end{array}$$对(1)(2)求导有
$$\dot{X}=f_i+gU$$$$A=\left[\begin{array}{cccccccccccc}0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& \frac{J_y-J_z}{J_x}{x}_6& 0& \frac{J_y-J_z}{J_x}{x}_4& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& \frac{J_z-J_x}{J_y}{x}_6& 0& 0& 0& \frac{J_z-J_x}{J_y}{x}_2& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\\ 0& \frac{J_x-J_y}{J_z}{x}_4& 0& \frac{J_x-J_y}{J_z}{x}_2& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0\\ \frac{\cos x_1\sin x_5-\sin x_1\sin x_3\cos x_5}{m}{U}_1& 0& \frac{\cos x_1\cos x_3\cos x_5}{m}{U}_1& 0& \frac{\sin x_1\cos x_5-\cos x_1\sin x_3\sin x_5}{m}{U}_1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ \frac{-\sin x_1\sin x_3\sin x_5-\cos x_1\cos x_5}{m}{U}_1& 0& \frac{\cos x_1\cos x_3\sin x_5}{m}{U}_1& 0& \frac{\cos x_1\sin x_3\cos x_5+\sin x_1\sin x_5}{m}{U}_1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\\ \frac{-\sin x_1\cos x_3}{m}{U}_1& 0& \frac{-\cos x_1\sin x_3}{m}{U}_1& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$$$$B=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& \frac{l}{J_x}& 0& 0\\ 0& 0& 0& 0\\ 0& 0& \frac{l}{J_y}& 0\\ 0& 0& 0& 0\\ 0& 0& 0& \frac{1}{J_z}\\ 0& 0& 0& 0\\ \frac{\cos x_1\sin x_3\cos x_5+\sin x_1\sin x_5}{m}& 0& 0& 0\\ 0& 0& 0& 0\\ \frac{\cos x_1\sin x_3\sin x_5-\sin x_1\cos x_5}{m}& 0& 0& 0\\ 0& 0& 0& 0\\ \frac{\cos x_1\cos x_3}{m}& 0& 0& 0\end{array}\right]$$可见$n=12,p=4$。

代入期望值
$$\begin{array}{rl}{x}_{1r}& ={\phi }_r=0.02\\ x_{2r}& ={\dot{\phi }}_r=0\\ x_{3r}& ={\theta }_r=0.01\\ x_{4r}& ={\dot{\theta }}_r=0\\ x_{5r}& ={\psi }_r=0.1\\ x_{6r}& ={\dot{\psi }}_r=0\\ x_{7r}& =x_r=5\\ x_{8r}& ={\dot{x}}_r=0\\ x_{9r}& =y_r=3\\ x_{10r}& ={\dot{y}}_r=0\\ x_{11r}& =z_r=-2\\ x_{12r}& ={\dot{z}}_r=2\end{array}$$以及其他常量
$$\begin{array}{rl}m& =1\\ g& =9.8\\ l& =0.3\\ J_x& =2.4\times 10^{-2}\\ J_y& =1.4\times 10^{-2}\\ J_z& =1.4\times 10^{-2}\end{array}$$共迭代$k=20$步，迭代步长$N=5$。
则计算得到
$$A=\left[\begin{array}{cccccccccccc}0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0\\ 0.0996& 0& 0.9948& 0& 0.0189& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ -0.9948& 0& 0.0998& 0& 0.0119& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\\ -0.02& 0& -0.01& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$$$$B=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 12.5& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 21.4286& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 71.4286\\ 0& 0& 0& 0\\ 0.0119& 0& 0& 0\\ 0& 0& 0& 0\\ 0.0119& 0& 0& 0\\ 0& 0& 0& 0\\ 0.9998& 0& 0& 0\end{array}\right]$$
得到状态变量如下图所示。

注意，图中表示的是6个状态量的误差值。可以看出，6个状态量很快地达到了稳定态，证明了模型预测控制的有效性。