1. 四旋翼的动力学模型

这里不加推导地给出四旋翼的动力学模型：
m [ x ¨ y ¨ z ¨ ] = [ ∑ i = 1 4 F i ( cos ⁡ φ sin ⁡ θ cos ⁡ ψ + sin ⁡ φ sin ⁡ ψ ) ∑ i = 1 4 F i ( cos ⁡ φ sin ⁡ θ sin ⁡ ψ − sin ⁡ φ cos ⁡ ψ ) ∑ i = 1 4 F i cos ⁡ φ cos ⁡ θ − m g ] (1) m \left[ \begin{matrix} \ddot x \\ \ddot y \\ \ddot z \end{matrix} \right] = \left[ \begin{matrix} \sum_{i=1}^4 F_i \left( \cos \varphi \sin \theta \cos \psi + \sin \varphi \sin \psi \right) \\ \sum_{i=1}^4 F_i \left( \cos \varphi \sin \theta \sin \psi - \sin \varphi \cos \psi \right) \\ \sum_{i=1}^4 F_i \cos \varphi \cos \theta - mg \end{matrix} \right] \tag{1} m x¨y¨z¨ = ∑i=14Fi(cosφsinθcosψ+sinφsinψ)∑i=14Fi(cosφsinθsinψ−sinφcosψ)∑i=14Ficosφcosθ−mg (1) [ J x p ˙ J y q ˙ J z r ˙ ] = [ l ( F 2 − F 2 ) − ( J z − J y ) q r l ( F 1 − F 3 ) − ( J x − J z ) p r M D 1 − M D 2 + M D 3 − M D 4 − ( J y − J x ) p q ] (2) \left[ \begin{matrix} J_x \dot p \\ J_y \dot q \\ J_z \dot r \end{matrix} \right] = \left[ \begin{matrix} 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{matrix} \right] \tag{2} Jxp˙Jyq˙Jzr˙ = l(F2−F2)−(Jz−Jy)qrl(F1−F3)−(Jx−Jz)prMD1−MD2+MD3−MD4−(Jy−Jx)pq (2) [ φ ˙ θ ˙ ψ ˙ ] = [ 1 sin ⁡ φ tan ⁡ θ cos ⁡ φ tan ⁡ θ 0 cos ⁡ φ − sin ⁡ φ 0 sin ⁡ φ cos ⁡ θ cos ⁡ φ cos ⁡ θ ] [ p q r ] \left[ \begin{matrix} \dot \varphi \\ \dot \theta \\ \dot \psi \end{matrix} \right] = \left[ \begin{matrix} 1 & \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0 & \cos \varphi & - \sin \varphi \\ 0 & \frac{\sin \varphi}{\cos \theta} & \frac{\cos \varphi}{\cos \theta} \end{matrix} \right] \left[ \begin{matrix} p \\ q \\ r \end{matrix} \right] φ˙θ˙ψ˙ = 100sinφtanθcosφcosθsinφcosφtanθ−sinφcosθcosφ pqr 其中 φ , θ , ψ \varphi, \theta, \psi φ,θ,ψ均为机体欧拉角， p , q , r p,q,r p,q,r为欧拉角的角速度。（关于更加详细的四旋翼建模过程可以参考笔者博客Backstepping反步法控制四旋翼无人机（一））

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

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

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

设 Q ˉ = [ Q Q ⋱ Q F ] , R ˉ = [ R R ⋱ R ] \bar Q = \left[ \begin{matrix} Q & \quad & \quad & \quad & \quad \\ \quad & Q & \quad & \quad & \quad \\ \quad & \quad & \ddots & \quad & \quad \\ \quad & \quad & \quad & Q & \quad \\ \quad & \quad & \quad & \quad & F \end{matrix} \right], \quad \bar R = \left[ \begin{matrix} R & \quad & \quad & \quad \\ \quad & R & \quad & \quad \\ \quad & \quad & \ddots & \quad \\ \quad & \quad & \quad & R \end{matrix} \right] Qˉ= QQ⋱QF ,Rˉ= RR⋱R M = [ I A A 2 ⋮ A N ] , C = [ 0 0 ⋯ 0 B 0 ⋯ 0 A B B ⋯ 0 ⋮ ⋮ ⋱ ⋮ A N − 1 B A N − 2 B ⋯ B ] M = \left[ \begin{matrix} I \\ A \\ A^2 \\ \vdots \\ A^N \end{matrix} \right], \quad C = \left[ \begin{matrix} 0 & 0 & \cdots & 0 \\ B & 0 & \cdots & 0 \\ AB & B & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ A^{N-1}B & A^{N-2}B & \cdots & B \end{matrix} \right] M= IAA2⋮AN ,C= 0BAB⋮AN−1B00B⋮AN−2B⋯⋯⋯⋱⋯000⋮B G = M T Q ˉ , E = M T Q ˉ C , H = C T Q ˉ C + R ˉ G = M^T \bar Q, \quad E = M^T \bar Q C,\quad H = C^T \bar Q C + \bar R G=MTQˉ,E=MTQˉC,H=CTQˉC+Rˉ那么性能指标可以写为
J = x k T G x k + 2 x k T E U k + U k T H U k (3) J = x_k^T G x_k + 2 x_k^T E U_k +U_k^THU_k \tag{3} J=xkTGxk+2xkTEUk+UkTHUk(3)该式即为二次规划的一般形式。二次规划的一般形式为
min ⁡ ( z T Q z + c T z ) (4) \min \left( z^T Q z + c^T z \right) \tag{4} min(zTQz+cTz)(4)(3)式中的 U U U即对应(4)式中的 z z z。

3. 非线性模型预测控制

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

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

具体计算公式如下：
X ˙ = f i ( x i , u j ) \dot X = f_i \left(x_i, u_j \right) X˙=fi(xi,uj) A = [ ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ⋯ ∂ f 1 ∂ x n ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ⋯ ∂ f 2 ∂ x n ⋮ ⋮ ⋱ ⋮ ∂ f n ∂ x 1 ∂ f n ∂ x 2 ⋯ ∂ f n ∂ x n ] x i = x r A = \left[ \begin{matrix} \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} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_n}{\partial x_1} & \frac{\partial f_n}{\partial x_2} & \cdots & \frac{\partial f_n}{\partial x_n} \end{matrix} \right] _{x_i = x_r} A= ∂x1∂f1∂x1∂f2⋮∂x1∂fn∂x2∂f1∂x2∂f2⋮∂x2∂fn⋯⋯⋱⋯∂xn∂f1∂xn∂f2⋮∂xn∂fn xi=xr B = [ ∂ f 1 ∂ u 1 ∂ f 1 ∂ u 2 ⋯ ∂ f 1 ∂ u p ∂ f 2 ∂ u 1 ∂ f 2 ∂ u 2 ⋯ ∂ f 2 ∂ u p ⋮ ⋮ ⋱ ⋮ ∂ f n ∂ u 1 ∂ f n ∂ u 2 ⋯ ∂ f n ∂ u p ] u i = u r B = \left[ \begin{matrix} \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} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_n}{\partial u_1} & \frac{\partial f_n}{\partial u_2} & \cdots & \frac{\partial f_n}{\partial u_p} \end{matrix} \right] _{u_i = u_r} B= ∂u1∂f1∂u1∂f2⋮∂u1∂fn∂u2∂f1∂u2∂f2⋮∂u2∂fn⋯⋯⋱⋯∂up∂f1∂up∂f2⋮∂up∂fn ui=ur值得注意的是， A , B A,B A,B中的 x i , u i x_i, u_i xi,ui应当代入期望值 x r , u r x_r, u_r xr,ur。

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

设状态量为
x 1 = φ x 2 = x ˙ 1 = φ ˙ x 3 = θ x 4 = x ˙ 3 = θ ˙ x 5 = ψ x 6 = x ˙ 5 = ψ ˙ x 7 = x x 8 = x ˙ 7 = x ˙ x 9 = y x 10 = x ˙ 9 = y ˙ x 11 = z x 12 = x ˙ 11 = z ˙ \begin{aligned} x_1 &= \varphi \\ x_2 &= \dot x_1 = \dot \varphi \\ 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{aligned} x1x2x3x4x5x6x7x8x9x10x11x12=φ=x˙1=φ˙=θ=x˙3=θ˙=ψ=x˙5=ψ˙=x=x˙7=x˙=y=x˙9=y˙=z=x˙11=z˙对(1)(2)求导有
X ˙ = f i + g U \dot X = f_i + g U X˙=fi+gU A = [ 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 J y − J z J x x 6 0 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 J z − J x J y x 6 0 0 0 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 J x − J y J z x 4 0 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 cos ⁡ x 1 sin ⁡ x 5 − sin ⁡ x 1 sin ⁡ x 3 cos ⁡ x 5 m U 1 0 cos ⁡ x 1 cos ⁡ x 3 cos ⁡ x 5 m U 1 0 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 − sin ⁡ x 1 sin ⁡ x 3 sin ⁡ x 5 − cos ⁡ x 1 cos ⁡ x 5 m U 1 0 cos ⁡ x 1 cos ⁡ x 3 sin ⁡ x 5 m U 1 0 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 − sin ⁡ x 1 cos ⁡ x 3 m U 1 0 − cos ⁡ x 1 sin ⁡ x 3 m U 1 0 0 0 0 0 0 0 0 0 ] A = \left[ \begin{matrix} 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{matrix} \right] A= 0000000mcosx1sinx5−sinx1sinx3cosx5U10m−sinx1sinx3sinx5−cosx1cosx5U10m−sinx1cosx3U1100JyJz−Jxx60JzJx−Jyx40000000000000mcosx1cosx3cosx5U10mcosx1cosx3sinx5U10m−cosx1sinx3U10JxJy−Jzx6100JzJx−Jyx20000000000000msinx1cosx5−cosx1sinx3sinx5U10mcosx1sinx3cosx5+sinx1sinx5U1000JxJy−Jzx40JyJz−Jxx210000000000000000000000000100000000000000000000000001000000000000000000000000010 B = [ 0 0 0 0 0 l J x 0 0 0 0 0 0 0 0 l J y 0 0 0 0 0 0 0 0 1 J z 0 0 0 0 cos ⁡ x 1 sin ⁡ x 3 cos ⁡ x 5 + sin ⁡ x 1 sin ⁡ x 5 m 0 0 0 0 0 0 0 cos ⁡ x 1 sin ⁡ x 3 sin ⁡ x 5 − sin ⁡ x 1 cos ⁡ x 5 m 0 0 0 0 0 0 0 cos ⁡ x 1 cos ⁡ x 3 m 0 0 0 ] B = \left[ \begin{matrix} 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{matrix} \right] B= 0000000mcosx1sinx3cosx5+sinx1sinx50mcosx1sinx3sinx5−sinx1cosx50mcosx1cosx30Jxl0000000000000Jyl0000000000000Jz1000000 可见 n = 12 , p = 4 n=12, p=4 n=12,p=4。

代入期望值
x 1 r = φ r = 0.02 x 2 r = φ ˙ r = 0 x 3 r = θ r = 0.01 x 4 r = θ ˙ r = 0 x 5 r = ψ r = 0.1 x 6 r = ψ ˙ r = 0 x 7 r = x r = 5 x 8 r = x ˙ r = 0 x 9 r = y r = 3 x 10 r = y ˙ r = 0 x 11 r = z r = − 2 x 12 r = z ˙ r = 2 \begin{aligned} x_{1r} &= \varphi_r = 0.02 \\ x_{2r} &= \dot \varphi_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{aligned} x1rx2rx3rx4rx5rx6rx7rx8rx9rx10rx11rx12r=φr=0.02=φ˙r=0=θr=0.01=θ˙r=0=ψr=0.1=ψ˙r=0=xr=5=x˙r=0=yr=3=y˙r=0=zr=−2=z˙r=2以及其他常量
m = 1 g = 9.8 l = 0.3 J x = 2.4 × 1 0 − 2 J y = 1.4 × 1 0 − 2 J z = 1.4 × 1 0 − 2 \begin{aligned} 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{aligned} mglJxJyJz=1=9.8=0.3=2.4×10−2=1.4×10−2=1.4×10−2共迭代 k = 20 k=20 k=20步，迭代步长 N = 5 N = 5 N=5。
则计算得到
A = [ 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 ] A = \left[ \begin{matrix} 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{matrix} \right] A= 00000000.09960−0.99480−0.0210000000000000000000.994800.09980−0.0100100000000000000000.018900.011900000010000000000000000000000000100000000000000000000000001000000000000000000000000010 B = [ 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 ] B = \left[ \begin{matrix} 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{matrix} \right] B= 00000000.011900.011900.9998012.5000000000000021.4286000000000000071.4286000000
得到状态变量如下图所示。

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

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

四旋翼无人机反步法控制器设计（含simulink仿真）
反步法设计: 1.建立四旋翼无人机非线性模型: 2.位置环虚拟控制律及反步控制器设计: 3. 姿态解算 4.姿态环虚拟控制律及反步控制器设计: 5.搭建simulink,仿真结果如下:
四旋翼无人机动力学模型及控制
四旋翼无人机动力学模型及控制 I: 欧拉角与旋转矩阵 Overview 欧拉角与旋转矩阵 Body Frame Angular Velocity and [ ϕ ˙ , θ ˙ , ψ ˙ ] T [ ...
无人车系统（十一）：轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】
前面介绍的PID,pure pursuit方法,Stanley方法都只是利用当前的系统误差来设计控制器.人们对这些控制器的设计过程中都利用了构建模型对无人车未来状态的估计(或者说利用模型估计未来的运动 ...
模型预测控制_模型预测控制(MPC)算法之一MAC算法
引言随着自动驾驶技术以及机器人控制技术的不断发展及逐渐火热,模型预测控制(MPC)算法作为一种先进的控制算法,其应用范围与领域得到了进一步拓展与延伸.目前提出的模型预测控制算法主要有基于非参数模型的 ...
阿木p200四旋翼配置经验笔记
本文初次写于2021/06/09 我做编队试验用的阿木P200四旋翼,空循环uwb定位下完成了机载计算机控制的编队飞行以及二维码引导降落. 记录一下对到手的P200飞机仍需要更改调试的配置: 硬件加装 ...
Apollo代码学习(六)—模型预测控制(MPC)
Apollo代码学习-模型预测控制前言模型预测控制预测模型线性化单车模型滚动优化反馈矫正总结前言非专业选手,此篇博文内容基于书本和网络资源整理,可能理解的较为狭隘,起点较低,就事论 ...
四旋翼无人机Matlab建模
本文主要分享一下四旋翼无人机的建模过程,然后在Matlab的simulink模块搭建起四旋翼无人机的模型,本篇文章主要参考了康日晖的<四旋翼无人机建模>与南京邮电大学周帆同学的硕士毕业论文 ...
先进非线性控制方法 INDI 快速部署到PX4用于四旋翼控制（part1）
关于INDI的前言: 增量型非线性控制方法是一种2010年以后才流行起来的非线性控制方法,目前主要用于设计先进飞行控制律.这种方法的典型代表是增量型动态逆(incremental nonlinear ...
【控制control】机器人运动控制器----基于模型预测控制MPC方法
系列文章目录提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理文章目录系列文章目录前言一.模型预测控制(MPC)的介绍及构成 1.介绍 2.构成二.模型 ...

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

目录

1. 四旋翼的动力学模型

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

3. 非线性模型预测控制

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

最新文章

热门文章