基于模型预测控制的无人机轨迹跟踪原理详解

整体理解

MPC的核心思想

**模型预测控制（MPC)**的核心想法是：

预测未来：用系统的"数学模型"在计算机里预测未来一段时间系统会怎么动
优化控制：在这段预测时间内解一个"最优控制问题"（通常是二次规划），找到一串控制量序列，使得：
- 轨迹尽量跟随参考轨迹（误差最小）
- 控制量不要太大、变化不要太猛
- 同时满足各种约束（位置、速度、姿态、推力上下限等）
滚动执行：真正执行时，只用这串控制中的第一个控制量，系统往前走一步，再重新预测、再优化，周而复始。这叫滚动优化

本章结构

建立无人机的连续时间动力学模型 → (4-1)～(4-15)
离散化并构造带扰动的状态空间模型 + 扰动观测器 → (4-16)～(4-22)
在此基础上建立MPC代价函数和轨迹跟踪问题 → (4-23)、(4-24)、(4-25)
进一步考虑扰动不确定性，做鲁棒模型预测控制 → (4-26)～(4-40)
最后给一个非线性安全距离优化的小例子 → (4-41)

1. 无人机运动学模型（4.1节）

(4-1) 位置向量

p = [p_x\;\;p_y\;\;p_z]^T \tag{4-1}

含义：用三维向量 $p$ 来表示无人机在惯性坐标系（一般是地球固定坐标系）中的位置，三个分量分别是 x、y、z 方向的位置。

目的：定义后面状态向量的一部分。MPC要控制的核心就是"位置跟踪"，所以 $p$ 是最重要的状态之一。

(4-2) 姿态旋转矩阵 $R$

R= \begin{bmatrix} \cos\varphi\cos\theta & \cos\varphi\sin\theta\sin\psi-\sin\varphi\cos\psi & \cos\varphi\sin\theta\cos\psi+\sin\varphi\sin\psi\\ \sin\varphi\cos\theta & \sin\varphi\sin\theta\sin\psi+\cos\varphi\cos\psi & \sin\varphi\sin\theta\cos\psi-\cos\varphi\sin\psi\\ -\sin\theta & \cos\theta\sin\psi & \cos\theta\cos\psi \end{bmatrix} \tag{4-2}

符号说明：

$\varphi$ ：滚转角（roll）
$\theta$ ：俯仰角（pitch）
$\psi$ ：偏航角（yaw）

含义： $R$ 是机体坐标系 → 惯性坐标系的旋转矩阵。

目的：把"机体坐标系里的推力"转换成"世界坐标系里的加速度方向"，这是建立动力学方程的关键。没有 $R$ ，无法从姿态推算位置如何变化。

(4-3) 速度定义

\dot p(t) = v(t) \tag{4-3}

含义：位置 $p$ 对时间的一阶导数就是速度 $v$ 。

(4-4) 加速度定义

\dot v(t) = a(t) \tag{4-4}

含义：速度对时间的一阶导数是加速度 $a$ 。

(4-5) 无人机动力学（含推力、重力、阻力和外扰）

\dot v(t) = a(t) = R(\varphi,\theta,\psi) \begin{bmatrix}0\\0\\T\end{bmatrix} + \begin{bmatrix}0\\0\\-g\end{bmatrix} + \begin{bmatrix} f_x & 0 & 0\\ 0 & f_y & 0\\ 0 & 0 & f_z \end{bmatrix}v(t) + d(t) \tag{4-5}

四个部分：

$R[0,0,T]^T$ ：推力 $T$ 作用在机体 z 轴，经过旋转矩阵 $R$ 转换到惯性系，形成某个方向的加速度。
$[0,0,-g]^T$ ：重力导致向下加速度 $-g$ 。
$\text{diag}(f_x,f_y,f_z)v$ ：气动力/阻力近似为与速度成比例的项（线性阻尼）， $f_x,f_y,f_z$ 是阻尼系数。
$d(t)$ ：未建模的外界扰动（风、建模误差等）。

目的：这是无人机在惯性系下的完整加速度方程。后续所有线性化、离散化、MPC预测都基于它。

(4-6)、(4-7) 底层姿态控制一阶近似

\dot\varphi(t) = \frac{1}{\tau_\varphi}\big(K_\varphi \varphi_d(t) - \varphi(t)\big) \tag{4-6}

\dot\theta(t) = \frac{1}{\tau_\theta}\big(K_\theta \theta_d(t) - \theta(t)\big) \tag{4-7}

符号：

$\varphi_d(t),\theta_d(t)$ ：姿态内环给定（控制输入）
$\tau_\varphi,\tau_\theta$ ：时间常数
$K_\varphi,K_\theta$ ：增益

含义：把姿态内环控制器近似为一阶惯性环节：姿态会以一定时间常数跟随命令角度。

为什么要这一层近似？

实际无人机有一个高速姿态控制器（比如PID），位置外环不需要知道它很细的细节，只需一个近似动力学即可。
这样可以把"姿态命令 $\varphi_d,\theta_d$ "直接看成MPC控制输入的一部分，而姿态真实值 $\varphi,\theta$ 则变成状态。

(4-8) 连续时间线性化状态方程

\dot x(t) = A_c x(t) + B_c u(t) + B_{d,c}d(t) \tag{4-8}

含义：把前面那一堆非线性方程在悬停附近进行线性化后，统一写成矩阵形式的一阶线性系统。

目的：MPC一般基于线性或线性化模型进行优化。这个公式就是"给MPC用的数学模型"的连续时间版本。

(4-9) 状态向量定义

x = [p^T\;\; v^T\;\; \varphi\;\; \theta]^T \tag{4-9}

组成：

位置 $p$ ：3维
速度 $v$ ：3维
姿态（只选滚转、俯仰） $\varphi,\theta$ ：2维

合计 8维状态。

目的：后面矩阵 $A_c,B_c$ 的尺寸、物理含义都基于它。

(4-10) 控制输入向量

u = [\varphi_d\;\; \theta_d\;\; T]^T \tag{4-10}

含义：MPC输出的控制量是姿态命令 + 推力命令。

目的：把"MPC要决定什么"明确下来。后面的优化变量就是 $u_k$ 序列。

(4-11) 外部扰动向量

d = [d_x\;\; d_y\;\; d_z]^T \tag{4-11}

含义：三个方向上的等效扰动力（或加速度）。

目的：后面要设计扰动观测器，并在MPC里对扰动进行补偿，所以要把扰动显式建模成一个向量。

(4-12) 连续时间状态矩阵 $A_c$

A_c = \begin{bmatrix} 0_{3\times3} & I_{3\times3} & 0_{3\times2}\\ 0_{3\times3} & -\text{diag}(f_x,f_y,f_z) & G\\ 0_{2\times3} & 0_{2\times3} & -\text{diag}(1/\tau_\varphi,1/\tau_\theta) \end{bmatrix} \tag{4-12}

块解释：

左上 $0, I$ ： $\dot p = v$
中间行： $\dot v$ 受速度阻尼（对角负的 $f_x,f_y,f_z$ ）和姿态（通过 $G$ ，本质是重力耦合）影响
最后一行块： $\dot\varphi,\dot\theta$ 服从一阶环节

目的：把连续动力学的线性化结果明确写成矩阵形式，为指数矩阵离散化（4-17）做准备。

(4-13) 输入矩阵 $B_c$

B_c = \begin{bmatrix} 0_{6\times2} & 0_{6\times1}\\ 0_{1\times2} & 1\\ \text{diag}\left(\frac{K_\varphi}{\tau_\varphi},\frac{K_\theta}{\tau_\theta}\right) & 0_{2\times1} \end{bmatrix} \tag{4-13}

含义：

上面6行是位置和速度，对控制输入的直接影响（这里线性化后主要是推力对加速度的影响，姿态命令对位置没有直接一阶影响）
最下面块表示姿态命令 $\varphi_d,\theta_d$ 通过一阶环节影响姿态状态 $\varphi,\theta$

目的：告诉你"每个控制量通过哪条通道作用到哪些状态上"。

(4-14) 扰动输入矩阵 $B_{d,c}$

B_{d,c} = \begin{bmatrix} 0_{3\times3}\\ I_{3\times3}\\ 0_{2\times3} \end{bmatrix} \tag{4-14}

含义：扰动只直接影响速度的导数（加速度），不直接作用在位置和姿态上。

目的：明确扰动在状态方程中的作用位置，方便后续设计扰动观测器与扰动补偿MPC。

(4-15) 重力矩阵 $G$

G = \begin{bmatrix} g & 0\\ 0 & -g\\ 0 & 0 \end{bmatrix} \tag{4-15}

含义：在悬停附近线性化后，姿态（特别是滚转、俯仰的小角度变动）会在 x、y 方向上产生近似 $g\cdot\theta,\;g\cdot\varphi$ 形式的加速度耦合，这被写进矩阵 $G$ 。

目的：把"姿态倾斜→水平方向的重力分量"这一重要耦合关系放进线性模型里，让MPC能正确预测侧向运动。

2. 扰动观测器模型（4.2节）

(4-16) 状态离散化

x_{k+1} = A x_k + B u_k + B_d d_k \tag{4-16}

含义：采样周期为 $\Delta t$ 时的离散时间状态方程：下一时刻的状态等于当前状态、当前控制和当前扰动的线性组合。

目的：MPC通常在数字计算机上实现，必须是离散时间模型。后面所有"第k步、第k+1步"预测都是基于它。

(4-17) 连续到离散的变换

A = e^{A_c\Delta t},\quad B = \int_0^{\Delta t} e^{A_c\tau}d\tau B_c,\quad B_d = \int_0^{\Delta t} e^{A_c\tau}d\tau B_{d,c} \tag{4-17}

含义：用矩阵指数和积分把连续时间 $(A_c,B_c,B_{d,c})$ 精确转换到离散时间 $(A,B,B_d)$ 。

目的：确保离散模型在采样点上准确等价于连续模型，不是随便差分近似。

(4-18) 输出方程

y_k = C x_k \tag{4-18}

含义：传感器测到的量 $y_k$ 是某些状态的线性组合。比如只测位置，就让 $C=[I_{3\times3}\;0]$ 。

目的：为状态估计和扰动估计提供测量方程；MPC也经常用输出误差来定义轨迹跟踪误差。

(4-19) 增广状态引入扰动

\begin{bmatrix} x_{k+1}\\ d_{k+1} \end{bmatrix} = \begin{bmatrix} A & B_d\\ 0 & I \end{bmatrix} \begin{bmatrix} x_k\\ d_k \end{bmatrix} + \begin{bmatrix} B\\ 0 \end{bmatrix}u_k \tag{4-19}

含义：

假设扰动在短时间内变化很慢或近似常值，于是写成 $d_{k+1}=d_k$ （矩阵里是单位阵 $I$ ）
把 $x$ 和 $d$ 合成一个增广状态向量

目的：如果只用 $x$ 做状态估计，很难区分"模型偏差"和"真实状态"。把扰动当成额外状态，就可以通过观测器直接估计扰动大小，为后面做补偿用。

(4-20) 增广输出方程

y_k = [C\;\;0]\begin{bmatrix}x_k\\ d_k\end{bmatrix} \tag{4-20}

含义：传感器仍然只量到 $x$ 的线性组合，看不到扰动，因此输出矩阵对扰动部分是0。

目的：把原来的输出方程扩展到增广状态空间中，方便统一设计观测器。

(4-21) 线性状态/扰动观测器

\begin{bmatrix} \hat x_{k+1}\\ \hat d_{k+1} \end{bmatrix} = \begin{bmatrix} A & B_d\\ 0 & I \end{bmatrix} \begin{bmatrix} \hat x_k\\ \hat d_k \end{bmatrix} + \begin{bmatrix} B\\ 0 \end{bmatrix}u_k + \begin{bmatrix} L_x\\ L_d \end{bmatrix} \big(y_k - C\hat x_k\big) \tag{4-21}

含义：这就是"增广状态的Luenberger观测器"：

前半部分是用模型预测下一步的 $\hat x,\hat d$
最后一项用"测量残差" $y_k-C\hat x_k$ 进行修正
$L_x,L_d$ 是观测器增益，通过极点配置让误差收敛

目的：让我们不仅能估计状态 $\hat x_k$ ，还能估计扰动 $\hat d_k$ 。后面MPC计算参考输入时会用到 $\hat d_k$ 进行稳态补偿。

(4-22) 含扰动补偿的稳态参考求解

\begin{bmatrix} x_{\text{ref},k}\\ u_{\text{ref},k} \end{bmatrix} = \begin{bmatrix} A - I & B\\ C & 0 \end{bmatrix}^{-1} \begin{bmatrix} - B_d\hat d_k\\ \gamma_k \end{bmatrix} \tag{4-22}

符号：

$\gamma_k$ ：期望输出参考轨迹（比如期望位置）
$x_{\text{ref},k},u_{\text{ref},k}$ ：在稳态时能产生该输出、并抵消当前扰动估计 $\hat d_k$ 的理想状态和输入

推导思想：

稳态要求： $x_{k+1}=x_k = x_{\text{ref}}$ ；由 (4-16) 得到 $(A-I)x_{\text{ref}} + Bu_{\text{ref}} + B_d\hat d = 0$
同时输出要等于参考： $C x_{\text{ref}} = \gamma_k$

把两式合成一个线性方程组，就得到上面矩阵形式。

目的（很关键）：对于带扰动的系统，如果你只用"零参考输入"或简单PID，很可能存在稳态误差。利用 $\hat d_k$ 求出能抵消扰动的理想稳态输入 $u_{\text{ref},k}$ ，然后MPC只需控制"偏离这个理想稳态"的部分，就能实现零稳态误差跟踪。

3. 模型预测控制（4.3节）

(4-23) 经典MPC代价函数

MPC通过最小化一个“代价函数”来决策。这个函数把“我们想要的结果”量化成数值，越小越好。

\min_{u_k} J = F(x_{k+N}) + \sum_{i=0}^{N-1} \Big( \|x_{k+i} - x_{\text{ref},i}\|_Q^2 + \|u_{k+i} - u_{\text{ref},i}\|_R^2 + \|u_{k+i} - u_{k+i-1}\|_{R_\Delta}^2 \Big) \tag{4-23}

每一项的作用：

$\|x_{k+i} - x_{\text{ref},i}\|_Q^2$ ：

预测状态与参考轨迹的偏差，让无人机尽量靠近参考轨迹
Q越大，对误差越敏感

$\|u_{k+i} - u_{\text{ref},i}\|_R^2$ ：

惩罚控制量大小，防止推力/姿态命令太大
R越大，越限制控制量

$\|u_{k+i} - u_{k+i-1}\|_{R_\Delta}^2$ ：惩罚控制量变化太快，让控制更平滑、减少执行器冲击。RΔ越大，控制越平滑
终端项 $F(x_{k+N}) = \|x_{k+N}-x_{\text{ref},N}\|_P^2$ ：在预测终点也要求误差小，有助于保证闭环稳定性

Q, R, RΔ, P 是什么？
这些都是权重矩阵，通常取对称正定。数值越大，对应误差越"不被允许"。

目的：把"我们想要的控制行为"定量化成一个最小化问题，而且由于是二次型 + 线性模型，这个问题可以转化为带约束二次规划（QP），方便数值求解。

(4-24) 针对6自由度无人机的具体代价函数

\begin{aligned} J = &\sum_{i=0}^{N-1} \big(x_{k+i}-x_{\text{ref},i}\big)^T Q \big(x_{k+i}-x_{\text{ref},i}\big) + (u_{k+i}-u_{\text{ref},i})^T R_u (u_{k+i}-u_{\text{ref},i}) \\ &+ (u_{k+i}-u_{k+i-1})^T R_\Delta (u_{k+i}-u_{k+i-1}) + (x_{k+N}-x_{\text{ref},N})^T P (x_{k+N}-x_{\text{ref},N}) \end{aligned} \tag{4-24}

与 (4-23) 的区别：

明确把"控制增量" $(u_{k+i}-u_{k+i-1})$ 单独作为惩罚项
把终端状态误差 $(x_{k+N}-x_{\text{ref},N})$ 的二次项单独写出

目的：针对6自由度无人机，把一般的MPC代价函数落实到具体状态分量和控制分量上，方便后面实现和调参。

优化过程：

预测未来N步的状态（基于模型）
计算这N步的代价J
找到使J最小的控制序列 $u_k, u_{k+1}, ..., u_{k+N-1}$
只执行第一步 $u_k$
下一时刻重新预测和优化（滚动优化）

(4-25) 根据扰动估计修正控制输入

u_k^{\text{corr}} = u_k^* + \Delta u_k \tag{4-25}

含义：

$u_k^*$ ：由基础MPC优化得到的控制输入（没考虑扰动补偿或考虑有限）
$\Delta u_k$ ：根据扰动估计结果计算的修正量
$u_k^{\text{corr}}$ ：最终真正送给执行器的控制量

目的：把"基于模型预测得到的开环最优控制"和"针对实时估计扰动的补偿"叠加起来，从而在存在风扰、建模误差时仍能保持精确轨迹跟踪。

4. 鲁棒模型预测控制（4.3.3节）

前面的MPC里，我们假设扰动是"已知（通过观测器估计）"的。但在现实中，扰动本身也有不确定性、高频噪声等。鲁棒MPC的目标是：

扰动观测器只能估计“平均扰动”，无法处理突然变化或高频噪声，鲁棒MPC考虑扰动的不确定性，保证最坏情况下的性能。这样系统在复杂环境中更可靠。

(4-26) 误差动态模型

\Delta x_{k+1} = A\Delta x_k + B\Delta u_k + G w_k \tag{4-26}

含义：

$\Delta x_k = x_k - x_{\text{ref},k}$ ：状态误差
$\Delta u_k = u_k - u_{\text{ref},k}$ ：控制误差（对基准输入的偏差）
$w_k$ ：归一化的扰动序列，经 $G$ 映射到状态误差

目的：把问题统一转到"误差空间"，因为轨迹跟踪关心的是误差是否在界内，而不是绝对值。同时单独提一个 $w_k$ 来刻画扰动不确定性，方便做最坏情况优化。

(4-27)、(4-28) 扰动约束集合

w_k \in W \tag{4-27}

W_\infty = \{ w \mid \|w\|_\infty \le 1\} \tag{4-28}

含义：扰动 $w_k$ 被假设落在一个有界集合中，典型地是无穷范数球（每个分量绝对值不超过1）。

目的："最坏情况"要有个范围，否则扰动可以无限大问题就无解。有了这个集合，可以在优化中明确考虑所有可能的扰动。

(4-29) 鲁棒MPC的目标

\min_{\Delta u_k,\;L,\;\tau} \max_{w} J \tag{4-29}

含义：对所有"允许的扰动序列" $w$ ，我们想让"最坏情况下"的代价 $J$ 尽量小；同时 $\tau$ 是一个输出误差上界。

目的：这是一个典型的"min–max优化问题"，用于描述鲁棒控制：

外界扰动想让误差最大（max）
控制器设计者想让这个"最大可能误差"尽量小（min）

我们想最小化外界想达到的最大值

(4-30) 误差控制序列分解

\Delta U = L W + \Delta V,\quad \Delta V \equiv (\Delta v_{k+1|k}^T,\dots,\Delta v_{k+N-1|k}^T)^T \tag{4-30}

含义：

$\Delta U$ ：整个预测时域上所有步长的控制误差序列堆成的大向量
$W$ ：对应的扰动序列堆成的大向量
$L$ ：反馈增益矩阵，告诉你在每个时刻如何利用"之前发生的扰动"进行反馈补偿
$\Delta V$ ：在没有扰动时的"基准开环控制序列"

目的：不仅要设计一个开环控制序列（ $\Delta V$ ），还要设计一个对未来扰动的反馈律 $L$ ，这样即使扰动发生也能通过反馈迅速纠正。

(4-31) 反馈矩阵 $L$ 的下三角结构

L= \begin{bmatrix} 0 & 0 & \cdots & 0\\ L_{10} & 0 & \cdots & 0\\ L_{20} & L_{21} & \cdots & 0\\ \vdots & \vdots & \ddots & \vdots\\ L_{(N-1)0} & \cdots & L_{(N-1)(N-2)} & 0 \end{bmatrix} \tag{4-31}

含义： $L$ 是一个严格下三角矩阵，也就是：

第k步的控制只能依赖"之前已知的扰动"
不能用未来扰动（因果性约束）

目的：保证控制律是物理可实现的（不能提前知道未来的扰动）。

(4-32) 误差状态预测方程（向量形式）

\Delta X = A_\text{p} \Delta x_{k|k} + B_\text{p}\Delta V + (G_\text{p}+B_\text{p}L)W \tag{4-32}

含义：把N步的误差演化统一写成一次矩阵乘法：

$\Delta X$ ：从第1步到第N步的误差状态序列堆成的大向量
$A_\text{p}\Delta x_{k|k}$ ：由当前误差状态传播的影响
$B_\text{p}\Delta V$ ：由基准控制序列带来的影响
$(G_\text{p}+B_\text{p}L)W$ ：扰动及其反馈引起的额外误差

目的：这样可以在优化中直接对整个预测时域进行约束和优化，而不用一行一行地写递推式。

(4-33) 预测矩阵 $A_\text{p}$

A_\text{p} = \begin{bmatrix} I & A^T & \cdots & (A^{N-1})^T \end{bmatrix}^T \tag{4-33}

含义：一步、两步、…、N步的状态传播矩阵堆叠起来，用来把当前误差 $\Delta x_{k|k}$ 传播到未来每一步。

目的：这是标准MPC里构造"预测矩阵"的做法，便于直接写出未来N步的状态向量。

(4-34)、(4-35) 控制与扰动预测矩阵 $B_\text{p},G_\text{p}$

B_\text{p} = \begin{bmatrix} 0 & 0 & \cdots & 0\\ B & 0 & \cdots & 0\\ AB & B & \cdots & 0\\ \vdots & \vdots & \ddots & \vdots\\ A^{N-2}B & A^{N-3}B & \cdots & B \end{bmatrix} \tag{4-34}

G_\text{p} = \begin{bmatrix} 0 & 0 & \cdots & 0\\ G & 0 & \cdots & 0\\ AG & G & \cdots & 0\\ \vdots & \vdots & \ddots & \vdots\\ A^{N-2}G & A^{N-3}G & \cdots & G \end{bmatrix} \tag{4-35}

含义：

$B_\text{p}$ ：每一个未来时刻的状态都可能受之前多步控制的影响（因为动力学有记忆），这些影响以 $A^{\cdot}B$ 的形式堆成这个下三角矩阵
$G_\text{p}$ ：同理，描述每一步扰动对后续状态的连锁影响

目的：统一写出"控制序列、扰动序列如何影响未来每一步状态"，为后面的输出误差计算与约束提供基础。

(4-36) 输出预测

\Delta Y = C_\text{p}\Delta X,\quad C_\text{p} = \text{diag}(C) \tag{4-36}

含义：对每一步，输出误差是 $\Delta y_k=C\Delta x_k$ ，堆起来就是 $\Delta Y = C_\text{p}\Delta X$ ，其中 $C_\text{p}$ 是把单步 $C$ 放在对角线上的大块对角矩阵。

目的：从状态误差序列得到输出误差序列，方便我们在目标函数里直接约束"输出偏离参考"的大小。

(4-37) 定义 $H = G_\text{p} + B_\text{p}L$

H = G_\text{p} + B_\text{p}L \tag{4-37}

含义：把"扰动经过系统自身"和"扰动经过我们设计的反馈律 $L$ "的综合影响写成一个矩阵 $H$ 。

目的：这样 (4-32) 里关于 $W$ 的项就变成简单的 $H W$ ，便于后面的范数界估计和线性约束推导。

(4-38) 最坏情况下的输出误差上界优化

\min_{\Delta V,L,\tau}\max_{w}\|\Delta Y\|_\infty \le \tau \tag{4-38}

含义：

$\|\Delta Y\|_\infty$ 是所有输出分量、所有时刻中误差绝对值的最大值
我们希望在最坏扰动下，这个最大误差依然不超过 $\tau$ ，并且让 $\tau$ 尽可能小

目的：这是鲁棒MPC的核心设计目标：不是只让"平均意义下"的误差小，而是保证"无论扰动怎么折腾，只要在集合 $W$ 里，误差都不会太大"。

(4-39) 把无穷范数条件转成线性不等式

\begin{cases} C_\text{p}(A_\text{p}\Delta x_{k|k} + B_\text{p}\Delta V) + |C_\text{p}H|\mathbf{1} \le \tau\mathbf{1}\\ -C_\text{p}(A_\text{p}\Delta x_{k|k} + B_\text{p}\Delta V) + |C_\text{p}H|\mathbf{1} \le \tau\mathbf{1} \end{cases} \tag{4-39}

推导思路（直觉版）：

$\Delta Y = C_\text{p}(A_\text{p}\Delta x_{k|k} + B_\text{p}\Delta V + H W)$
因为 $\|W\|_\infty\le1$ ，最坏情况时每个分量都能取 $\pm1$
用绝对值和三角不等式，可以得到 $|\Delta Y| \le |C_\text{p}(A_\text{p}\Delta x_{k|k} + B_\text{p}\Delta V)| + |C_\text{p}H|\mathbf{1}$
要求所有分量都 ≤ $\tau$ 就得到上下两组不等式

目的：把"最坏情况下的无穷范数界"转化为线性不等式约束，从而整个鲁棒MPC问题可以转成线性规划或线性约束二次规划求解。

(4-40) 再加入状态/输入约束

\begin{bmatrix} E_x(A_\text{p}\Delta x_{k|k} + B_\text{p}V)\\ E_u V \end{bmatrix} + \Omega\cdot\mathbf{1} \le \begin{bmatrix} F_x\\ F_u \end{bmatrix} \tag{4-40}

含义：

$E_x,F_x$ ：定义状态误差（或状态本身）必须满足的线性约束，如：
- 高度不能超过上限
- 位置不能超过安全边界等
$E_u,F_u$ ：输入（推力、姿态）大小和变化率的约束
$\Omega$ ：考虑扰动不确定性时加入的安全裕度


展开代码
状态约束 + 安全裕度 ≤ 上界
输入约束 + 安全裕度 ≤ 上界

目的：让鲁棒MPC不仅控制误差，还严格保证物理约束（安全边界、执行器饱和），即使扰动以最坏方式出现，状态和输入仍满足物理约束（如高度上限、推力上限）。

5. 非线性平滑跟踪优化（4.4节）

最后一节是个几何上保证安全距离的例子，不是标准MPC结构，但和MPC结合可以得到安全轨迹。

(4-41) 安全距离优化的位置参考

P^* = \begin{cases} P - \begin{bmatrix} \cos\theta & 0 & 0\\ 0 & \sin\theta & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} d_x\\ d_y\\ d_z \end{bmatrix}, & -90^\circ<\theta\le 90^\circ\\[1em] P + \begin{bmatrix} \cos\theta & 0 & 0\\ 0 & \sin\theta & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} d_x\\ d_y\\ d_z \end{bmatrix}, & 90^\circ<\theta\le 270^\circ \end{cases} \tag{4-41}

含义：

$P$ ：目标（比如被跟踪飞机）的位置
$P^*$ ：对无人机而言的期望跟踪位置，不是贴在目标上，而是离目标一个安全距离 $d=(d_x,d_y,d_z)$
矩阵里用 $\cos\theta,\sin\theta$ 把安全距离向量按照目标航向角 $\theta$ 旋转，保证这个安全点始终在目标附近的某个固定相对位置
上下两种情况根据 $\theta$ 的范围选用，让几何关系保持连续、不发生跳跃

目的：保证在任何目标姿态变化下，无人机都始终距离目标一个安全距离，为MPC提供一个已经考虑安全间隔的参考轨迹位置 $P^*$ 。然后MPC只需要跟踪 $P^*$ ，就能在保持安全距离的前提下实现精确轨迹跟随。

总结

整套思路串联图

物理建模
(4-1)～(4-5) 把无人机的位姿、推力、重力、阻力和外扰写成连续时间非线性模型
线性化与状态空间
(4-6)～(4-15) 用一阶近似和线性化，把系统变成 $\dot x = A_c x + B_c u + B_{d,c}d$
离散化
(4-16)～(4-17) 得到数字实现需要的 $x_{k+1} = A x_k + B u_k + B_d d_k$
扰动观测器
估计当前扰动，计算能抵消扰动的稳态参考
构造MPC代价函数
定义优化目标（跟踪误差、控制平滑性等）：(4-23)、(4-24) 用状态误差 + 控制量 + 控制增量 + 终端误差组成二次型目标，求解最优控制序列
加入扰动不确定性，做鲁棒MPC
(4-26)～(4-40) 在误差空间建立带扰动的预测矩阵，采用min–max形式，转化为线性不等式约束，保证最坏扰动下的误差和约束满足
额外的几何安全优化
(4-41) 用简单几何关系为目标轨迹生成一个始终保持安全距离的虚拟参考点 $P^*$ ，为MPC提供安全的参考轨迹

关键概念回顾

滚动优化（Receding Horizon）：每次只执行第一个控制量，然后重新预测和优化
扰动观测器（Disturbance Observer）：把扰动当成状态来估计，实现前馈补偿
鲁棒MPC（Robust MPC）：考虑扰动不确定性，保证最坏情况下的性能
预测矩阵：把多步预测统一写成矩阵形式，便于优化求解
安全距离优化：在MPC参考轨迹生成阶段就考虑几何安全约束

目录