MPC 学习笔记（第二期）

MPC 学习笔记（第二期）

这次主要是针对 MPC 结合固定翼无人机控制的方法进行学习，主要是分析 MPC 要如何与固定翼无人机的控制模型相结合，学习的对象是

Nonlinear MPC for fixed-wing UAV trajectory tracking: Implementation and flight experiments

是使用 NMPC 方法，进行固定翼无人机轨迹跟踪控制的一篇论文。

论文总体内容总结

这篇论文的主旨是：

在已有低层姿态稳定与空速与高度控制器的基础上，构造一个用于固定
翼 UAV 横侧向路径跟踪的高层非线性模型预测控制器（NMPC），并将低层滚转闭环动态通过系统辨识纳入预测模型，最终完成仿真和实飞验证。

论文摘要部分解析

论文先从无人机应用场景切入，包括三维建模、配送、灾害救援、农作物监测和基础设施巡检。然后强调，相比旋翼机，固定翼在续航和速度方面更适合 mapping/sensing；而小型、手抛式固定翼又有部署简单、系统复杂度低的优势。最后落到一个问题：高级控制方法很多仍停留在仿真阶段，需要真实飞行验证。

论文随后开始缩小问题范围，从“固定翼控制”缩小到“基于优化的 trajectory tracking”，再进一步锁定到 NMPC。论文随后回顾了几类已有工作：

#mermaid-svg-VNKK31R6XRKJcVop{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-VNKK31R6XRKJcVop .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-VNKK31R6XRKJcVop .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-VNKK31R6XRKJcVop .error-icon{fill:#552222;}#mermaid-svg-VNKK31R6XRKJcVop .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-VNKK31R6XRKJcVop .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-VNKK31R6XRKJcVop .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-VNKK31R6XRKJcVop .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-VNKK31R6XRKJcVop .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-VNKK31R6XRKJcVop .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-VNKK31R6XRKJcVop .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-VNKK31R6XRKJcVop .marker{fill:#333333;stroke:#333333;}#mermaid-svg-VNKK31R6XRKJcVop .marker.cross{stroke:#333333;}#mermaid-svg-VNKK31R6XRKJcVop svg{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-VNKK31R6XRKJcVop p{margin:0;}#mermaid-svg-VNKK31R6XRKJcVop .label{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;color:#333;}#mermaid-svg-VNKK31R6XRKJcVop .cluster-label text{fill:#333;}#mermaid-svg-VNKK31R6XRKJcVop .cluster-label span{color:#333;}#mermaid-svg-VNKK31R6XRKJcVop .cluster-label span p{background-color:transparent;}#mermaid-svg-VNKK31R6XRKJcVop .label text,#mermaid-svg-VNKK31R6XRKJcVop span{fill:#333;color:#333;}#mermaid-svg-VNKK31R6XRKJcVop .node rect,#mermaid-svg-VNKK31R6XRKJcVop .node circle,#mermaid-svg-VNKK31R6XRKJcVop .node ellipse,#mermaid-svg-VNKK31R6XRKJcVop .node polygon,#mermaid-svg-VNKK31R6XRKJcVop .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-VNKK31R6XRKJcVop .rough-node .label text,#mermaid-svg-VNKK31R6XRKJcVop .node .label text,#mermaid-svg-VNKK31R6XRKJcVop .image-shape .label,#mermaid-svg-VNKK31R6XRKJcVop .icon-shape .label{text-anchor:middle;}#mermaid-svg-VNKK31R6XRKJcVop .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-VNKK31R6XRKJcVop .rough-node .label,#mermaid-svg-VNKK31R6XRKJcVop .node .label,#mermaid-svg-VNKK31R6XRKJcVop .image-shape .label,#mermaid-svg-VNKK31R6XRKJcVop .icon-shape .label{text-align:center;}#mermaid-svg-VNKK31R6XRKJcVop .node.clickable{cursor:pointer;}#mermaid-svg-VNKK31R6XRKJcVop .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-VNKK31R6XRKJcVop .arrowheadPath{fill:#333333;}#mermaid-svg-VNKK31R6XRKJcVop .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-VNKK31R6XRKJcVop .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-VNKK31R6XRKJcVop .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-VNKK31R6XRKJcVop .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-VNKK31R6XRKJcVop .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-VNKK31R6XRKJcVop .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-VNKK31R6XRKJcVop .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-VNKK31R6XRKJcVop .cluster text{fill:#333;}#mermaid-svg-VNKK31R6XRKJcVop .cluster span{color:#333;}#mermaid-svg-VNKK31R6XRKJcVop div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-VNKK31R6XRKJcVop .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-VNKK31R6XRKJcVop rect.text{fill:none;stroke-width:0;}#mermaid-svg-VNKK31R6XRKJcVop .icon-shape,#mermaid-svg-VNKK31R6XRKJcVop .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-VNKK31R6XRKJcVop .icon-shape p,#mermaid-svg-VNKK31R6XRKJcVop .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-VNKK31R6XRKJcVop .icon-shape .label rect,#mermaid-svg-VNKK31R6XRKJcVop .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-VNKK31R6XRKJcVop .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-VNKK31R6XRKJcVop .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-VNKK31R6XRKJcVop :root{–mermaid-font-family:\”trebuchet ms\”,verdana,arial,sans-serif;}#mermaid-svg-VNKK31R6XRKJcVop .stage1>*{fill:#E3F0FF!important;stroke:#4B78B8!important;stroke-width:2px!important;color:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage1 span{fill:#E3F0FF!important;stroke:#4B78B8!important;stroke-width:2px!important;color:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage1 tspan{fill:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage2>*{fill:#DFF3EC!important;stroke:#458B78!important;stroke-width:2px!important;color:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage2 span{fill:#DFF3EC!important;stroke:#458B78!important;stroke-width:2px!important;color:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage2 tspan{fill:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage3>*{fill:#FFF1DA!important;stroke:#D28B31!important;stroke-width:2px!important;color:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage3 span{fill:#FFF1DA!important;stroke:#D28B31!important;stroke-width:2px!important;color:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage3 tspan{fill:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage4>*{fill:#E7F4DF!important;stroke:#659E55!important;stroke-width:2px!important;color:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage4 span{fill:#E7F4DF!important;stroke:#659E55!important;stroke-width:2px!important;color:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage4 tspan{fill:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage5>*{fill:#F7E0E0!important;stroke:#B45C5C!important;stroke-width:2px!important;color:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage5 span{fill:#F7E0E0!important;stroke:#B45C5C!important;stroke-width:2px!important;color:#111!important;}#mermaid-svg-VNKK31R6XRKJcVop .stage5 tspan{fill:#111!important;}

二维路径跟踪

多段路径跟踪&避障

自适应预测面

风力感知路径跟随

三维MPC

文章认为，NMPC 虽然很灵活，但其非凸优化问题计算量大，因此早期很难真正在线部署。随后引用 Quirynen 等人的快速积分和自动代码生成方法，以及 ACADO Toolkit，说明到当时为止，实时 NMPC 已经开始变得可行。

作者的想法是，以前的数学工具不支持 NMPC 的实时计算，而且以前的建模也会导致 MPC 的计算量极大，而现在有了

$$\text{fast integration + code generation + ACADO}$$

就可以把算法真正放到实物的固定翼无人机上了。此外，作者指出，已经有很多不同的 NMPC 方法了，但是他们只建模高层控制量，然后期望底层控制量会完美跟踪高层的控制指令，也就是说，让实际倾角 $\mu$ 等于期望倾角${\mu }_r$

$$\mu ={\mu }_r.$$

但实际的飞机不是这样的，期望倾角要经过低层姿态控制器和 飞机本体 才能影响实际倾角 $\mu$。如果把低层闭环动态也加入 NMPC 的预测模型，那么预测就会更加真实。这就是这一篇论文的核心创新点：

$$高层运动学+已辨识的底层闭环动态.$$

那么，既然要把 底层动态 放到 NMPC 中，为什么不直接用传统飞机系统辨识呢?

这是因为传统的 开环参数辨识 太麻烦了，就比如下面的：

通过 副翼偏角 ${\delta }_a$ 辨识 滚转角速度 $p$，或者 副翼偏角 辨识 滚转角
$\varphi$

$${\delta }_a\to \varphi ,{\delta }_a\to p.$$

这样的气动辨识通常需要大量飞行试验、专门设施或者复杂激励，在小型 UAV 上并不方便。因此作者转向 闭环系统辨识 这类方法。

也就是研究期望滚转直接到实际滚转这个黑盒或者或灰盒的系统中的参数辨识问题

$${\varphi }_r\to \varphi$$

也就是说研究：

高层给低层控制器一个滚转参考后，实际飞机如何响应？

因此，本文的工作可以拆成 3 个部分：

论文主要创新点详解

本博客在这一部分，会对原文中的内容进行讲解和推导

固定翼横侧向控制若从执行器开始建模，典型因果链为：

副翼偏角 -> 滚转角速度 -> 滚转角 -> 地面航迹角 -> 北向和东向位置

$${\delta }_a⟶p⟶\varphi ⟶\dot{\chi }⟶(n,e),$$

其中 ${\delta }_a$ 为副翼偏角，$p$ 为滚转角速度，$\varphi$ 为滚转角，$\chi$ 为地面航迹角，$(n,e)$ 为北向和东向位置。

若高层 NMPC 直接输出 ${\delta }_a$，预测模型通常需要显式包含

原论文采用已有的 Pixhawk 低层姿态闭环，把高层控制量选择为滚转/倾斜角参考：

$$\boxed{u={\mu }_r}.$$

于是实际控制结构为

$$\text{期望路径}\to \boxed{\text{NMPC}}\to {\mu }_r\to \boxed{\text{低层姿态环}}\to \text{飞机本体}.$$

即预测模型描述的是“高层命令经过底层闭环之后的飞机响应”，而不是完全开环的气动模型。

论文使用 Techpod 小型固定翼 UAV：翼展约$2.6m$、质量约$2.65kg$、标称空速约$14m/s$。Pixhawk 执行 EKF、姿态稳定和 TECS；计算量更大的 NMPC 在 ODROID-U3 上运行，并通过 ROS 与飞控通信[1]。

Techpod 小型固定翼论文原图

低层姿态控制可抽象为

$${\mu }_r\to C_{\mathrm{att}}\to {\delta }_a\to \text{aircraft}\to \mu .$$

NMPC 不需要知道上述每一层的全部内部细节，只需要建立

$$\boxed{{\mu }_r\to \mu }$$

的足够准确的低阶闭环模型。

二维固定翼运动学模型

(1) 坐标与变量

定义惯性平面坐标

$$\mathbf{p}=\left[\begin{array}{c}n\\ e\end{array}\right],$$

其中 $n$ 表示 North，$e$ 表示 East。

定义：

• $V$：空速；
• $\xi$：相对于空气质量的航向/速度方向；
• $\chi$：相对于地面的航迹角（course angle）；
• $\mu$：bank angle；
• $\mathbf{w}={\left[\begin{array}{cc}{w}_n& w_e\end{array}\right]}^T$：水平风速。

论文采用的二维运动学模型为

$$\begin{array}{rl}\dot{n}& =V\cos \xi +w_n\\ \dot{e}& =V\sin \xi +w_e\\ \dot{\xi }& =\frac{g\tan \mu }{V}\end{array}$$

(2) 协调转弯公式的完整推导

原文中式（3）来自定高、协调转弯假设。

在稳定水平协调转弯中，升力 $L$ 随飞机一起倾斜 $\mu$。垂向力平衡：
$$L\cos \mu =mg.$$
水平向心力：
$$L\sin \mu =\frac{m{V}^2}{R_a},$$
其中 $R_a$ 是相对于空气质量运动的转弯半径。两式相除：
$$\tan \mu =\frac{V^2}{g{R}_a}.$$
另一方面，
$$\dot{\xi }=\frac{V}{R_a}.$$
代入 $R_a=V/\dot{\xi }$：
$$\tan \mu =\frac{V\dot{\xi }}{g},$$
最终得到
$$\boxed{\dot{\xi }=\frac{g\tan \mu }{V}}.$$

因此最基本的横向控制因果链为
$$\boxed{\mu \to \dot{\xi }\to \xi \to (n,e)}.$$

（3）最小空气转弯半径

由协调转弯关系：
$$\boxed{R_a=\frac{V^2}{g\tan |\mu |}}.$$
若
$$|\mu |\le {\mu }_{\max },$$
则
$$\boxed{R_{a,\min }=\frac{V^2}{g\tan {\mu }_{\max }}}.$$
这解释了为什么固定翼路径跟踪必须考虑可行曲率：给定速度越大、最大 bank 越小，可跟踪圆弧的最小半径越大。

有风和无风时的轨迹跟踪控制区别

空速向量
$${\mathbf{v}}_a=V\left[\begin{array}{c}\cos \xi \\ \sin \xi \end{array}\right]$$
地速向量
$${\mathbf{v}}_g={\mathbf{v}}_a+\mathbf{w}=\left[\begin{array}{c}V\cos \xi +w_n\\ V\sin \xi +w_e\end{array}\right]$$
因此
$$V_g=\sqrt{(V\cos \xi +w_n{)}^2+(V\sin \xi +w_e{)}^2},$$
且
$$\boxed{\chi =atan2\left(V\sin \xi +w_e,V\cos \xi +w_n\right)}$$

无风时 $\chi =\xi$；有侧风时一般 $\chi \ne \xi$。

例如希望地面航迹沿正北方向，则需 $\dot{e}=0$，因此
$$V\sin \xi +w_e=0$$
以及
$$\boxed{\sin \xi =-\frac{w_e}{V}}.$$
即飞机需要向迎风侧偏置航向，才能保持期望的跟踪轨迹。

为什么要采用非线性的模型

原文中式（1）和（3）中包含
$$\sin \xi ,\cos \xi ,\tan \mu .$$
因此未来状态一般不能全局写成固定矩阵关系
$$\mathbf{Z}=\mathbf{Gx}+\mathbf{HU}$$
这正是本文采用 NMPC 的直接原因之一。

2.4 低层控制的闭环辨识

传统开放环辨识可能研究
$${\delta }_a\to p\text{或}{\delta }_a\to \varphi .$$
论文需要的是
$$\boxed{{\mu }_r\to \mu },$$
因为高层 NMPC 直接操纵的是 bank reference，而非副翼。

这种设置的优势在于：

• 利用已稳定的底层闭环动态，便于低阶建模；
• 避免完整气动辨识，减少飞行试验和模型维数。

论文使用 2-1-1 类型阶跃组合进行闭环辨识，在不同滚转幅值下采集多组数据，并比较多个阶次的 ARX 模型。最终综合验证集拟合效果、模型复杂度和 NMPC 在线计算负担，选择二阶模型。

离散 ARX 模型的一般形式为
$$A(q^{-1})y_k=B(q^{-1})u_k+{\epsilon }_k,$$
其中
$$A(q^{-1})=1+a_1{q}^{-1}+\cdots +a_{n_a}{q}^{-n_a},$$
$$B(q^{-1})=b_1{q}^{-1}+\cdots +b_{n_b}{q}^{-n_b}.$$

二阶滚转闭环模型

论文选择
$$\boxed{G_{\mu }(s)=\frac{\mu (s)}{{\mu }_r(s)}=\frac{b_0}{s^2+a_{1}s+a_0}}.$$

于是

$$(s^2+a_{1}s+a_0)\mu (s)=b_0{\mu }_r(s),$$

得到

$$\boxed{\ddot{\mu }=b_0{\mu }_r-a_1\dot{\mu }-a_0\mu }.$$

令
$$q_{\mu }=\dot{\mu },$$
则
$$\left[\begin{array}{c}\dot{\mu }\\ {\dot{q}}_{\mu }\end{array}\right]=\left[\begin{array}{c}{q}_{\mu }\\ -a_0\mu -a_1{q}_{\mu }+b_0{\mu }_r\end{array}\right].$$

若分母写成标准形式
$$s^2+2\zeta {\omega }_{n}s+{\omega }_n^2,$$
则
$${\omega }_n=\sqrt{a_0},\zeta =\frac{a_1}{2\sqrt{a_0}}.$$

2.6 论文的完整 NMPC 预测模型

忽略路径切换状态时，定义
$${\mathbf{x}}_p={\left[\begin{array}{ccccc}n& e& \mu & \xi & q_{\mu }\end{array}\right]}^T,u={\mu }_r$$
完整低阶预测模型为
x˙p=[Vcos⁡ξ+wnVsin⁡ξ+weqμgtan⁡μVb0u−a1qμ−a0μ]
\\boxed{
\\dot{\\mathbf{x}}_p
=\\begin{bmatrix}
V\\cos\\xi + w_n \\\\
V\\sin\\xi + w_e \\\\
q_\\mu \\\\
\\frac{g\\tan\\mu}{V} \\\\
b_0u – a_1q_\\mu – a_0\\mu
\\end{bmatrix}
}
x˙p​=​Vcosξ+wn​Vsinξ+we​qμ​Vgtanμ​b0​u−a1​qμ​−a0​μ​​​
在线参数可以写成
$$\mathbf{\theta }={\left[\begin{array}{ccc}V& w_n& w_e\end{array}\right]}^T$$
论文在单次预测 horizon 内把当前空速和风估计视为常值在线参数 [1]。

完整状态还包含辅助切换状态：
$$\boxed{\mathbf{x}={\left[\begin{array}{cccccc}n& e& \mu & \xi & q_{\mu }& x_{\mathrm{sw}}\end{array}\right]}^T}$$

2.7 路径跟踪控制推导

（1）一般路径误差

二维向量
$$\mathbf{a}=\left[\begin{array}{c}{a}_n\\ a_e\end{array}\right],\mathbf{b}=\left[\begin{array}{c}{b}_n\\ b_e\end{array}\right]$$
的标量叉积定义为
$$\mathbf{a}\times \mathbf{b}=a_n{b}_e-a_e{b}_n.$$

设 UAV 位置为 $\mathbf{p}$，路径上距离 UAV 最近的点为 $\mathbf{d}$，该点处的单位切向量为 ${\bar{\mathbf{T}}}_d$。论文定义
$$\boxed{e_t=(\mathbf{d}-\mathbf{p})\times {\bar{\mathbf{T}}}_d}.$$
参考航迹角
$$\boxed{{\chi }_d=atan2\left({\bar{T}}_{d,e},{\bar{T}}_{d,n}\right)}$$
方向误差
$$\boxed{e_{\chi }=\mathrm{wrap}\left({\chi }_d-\chi \right)}$$

实际实现必须做 angle wrap，可写成
$$e_{\chi }=atan2\left(\sin ({\chi }_d-\chi ),\cos ({\chi }_d-\chi )\right)$$

仅有 $e_t=0$ 只能说明飞机瞬时位于路径上；飞机仍可能垂直穿过路径。因此还需要使 $e_{\chi }\to 0$。

（2）直线路径最近点

直线路径由 $\mathbf{a},\mathbf{b}\in {\mathbb{R}}^2$ 定义。单位切向量
$$\bar{\mathbf{T}}=\frac{\mathbf{b}-\mathbf{a}}{\Vert \mathbf{b}-\mathbf{a}{\Vert }_2}.$$
无限直线投影参数
$$s={\bar{\mathbf{T}}}^T\left(\mathbf{p}-\mathbf{a}\right)$$
最近点
$$\boxed{\mathbf{d}=\mathbf{a}+s\bar{\mathbf{T}}}.$$

若为有限线段，则
$$s_c=\min \left(\max (s,0),\Vert \mathbf{b}-\mathbf{a}{\Vert }_2\right),$$
并使用
$$\mathbf{d}=\mathbf{a}+s_c\bar{\mathbf{T}}.$$

（3）圆弧路径最近点

圆心 $\mathbf{c}$、半径 $R$：
$$\mathbf{r}=\mathbf{p}-\mathbf{c},\rho =\Vert \mathbf{r}{\Vert }_2,\hat{\mathbf{r}}=\frac{\mathbf{r}}{\rho }.$$
圆上径向最近点
$$\boxed{\mathbf{d}=\mathbf{c}+R\hat{\mathbf{r}}}.$$

定义
$$\mathbf{J}=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right],\sigma =\{\begin{array}{ll}+1,& \text{逆时针}\\ -1,& \text{顺时针}\end{array}$$
则
$$\boxed{{\bar{\mathbf{T}}}_d=\sigma \mathbf{J}\hat{\mathbf{r}}}.$$

原论文指出直线和圆的最近点具有简单解析形式，因此省略具体计算。本节公式是复现所需的标准几何补全。

2.8 Dubins 路径与预测时域内部切换

固定翼不能原地转向，因此 Dubins path 常由 line 与 arc 拼接。原论文把
$$P_{\mathrm{cur}},P_{\mathrm{next}}$$
都作为在线参数传入 NMPC。

可表示为
$$P_{\mathrm{line}}=\{\text{type},\mathbf{a},\mathbf{b}\},$$
$$P_{\mathrm{arc}}=\{\text{type},\mathbf{c},R,\text{dir},{\xi }_0,\Delta \xi \}.$$

若飞机将在 horizon 内由当前直线进入圆弧，而整个 horizon 仍然对当前直线计算误差，则后续 reference 是错误的。因此论文引入
$$x_{\mathrm{sw}}$$
使预测模型可以在 horizon 内部切换到下一段路径。

一个便于复现的连续逻辑表示为
$${\dot{x}}_{\mathrm{sw}}=\{\begin{array}{ll}0,& \text{尚未满足切换条件},\\ \alpha ,& \text{满足切换条件或已进入切换状态},\end{array}$$
其中 $\alpha >0$ 仅用于使辅助状态越过阈值；真实飞机完成路径段切换后，再重置 $x_{\mathrm{sw}}$。

该式是对论文 switch-state 思想的复现友好的一种表示，不声称是原文式(6)的逐字重排。核心思想是：reference segment 本身也要在预测时域内正确演化。

2.9 连续时间 NMPC 最优控制问题

论文使用
$$\boxed{\mathbf{y}={\left[\begin{array}{ccccc}{e}_t& e_{\chi }& \mu & q_{\mu }& {\mu }_r\end{array}\right]}^T}$$

连续时间 OCP：
$$\begin{array}{rl}\underset{\mathbf{X},\mathbf{U}}{\min }J=& {\int }_0^T[(\mathbf{y}-{\mathbf{y}}_{\mathrm{ref}}{)}^T\mathbf{Q}(\mathbf{y}-{\mathbf{y}}_{\mathrm{ref}})\\ & +(u-u_{\mathrm{ref}}{)}^T\mathbf{R}(u-u_{\mathrm{ref}})]\mathrm{d}t\\ & +(\mathbf{y}(T)-{\mathbf{y}}_{\mathrm{ref}}(T){)}^T\mathbf{P}(\mathbf{y}(T)-{\mathbf{y}}_{\mathrm{ref}}(T))\end{array}$$
subject to
$$\begin{array}{rl}\dot{\mathbf{x}}& =f(\mathbf{x},u)\\ \mathbf{y}& =h(\mathbf{x},u)\\ u(t)& \in \mathcal{U}\\ \mathbf{x}(0)& =\hat{\mathbf{x}}(t_0)\end{array}$$
控制约束
$$\boxed{{\mu }_{r,\min }\le {\mu }_r(t)\le {\mu }_{r,\max }}.$$

若
$$\mathbf{Q}=\mathrm{diag}(q_t,q_{\chi },q_{\mu },q_q,q_{\Delta u}),$$
则各项分别对应横向误差、航迹角误差、bank 大小、bank rate 和跨迭代 control-horizon deviation 的代价。

跨 NMPC 迭代的控制计划一致性惩罚

记第 $k$ 次 NMPC 求解的第 $i$ 个节点控制为
$$u_i^{(k)},$$
上一轮同一节点为
$$u_i^{(k-1)}.$$
论文的思想是惩罚
$$\boxed{\Delta u_i^{(k)}=u_i^{(k)}-u_i^{(k-1)}}.$$

这不同于传统输入率惩罚
$$u_i-u_{i-1}.$$

前者要求“连续两次重规划不要突然推翻近期计划”，后者要求“同一条未来计划内部平滑”。

离散实现可写为
$$J_{\mathrm{cons}}=\sum _{i=0}^{N-1}{\rho }_i\Vert u_i^{(k)}-u_i^{(k-1)}{\Vert }_{Q_{\Delta u}}^2.$$

论文指出权重应在 horizon 前部较大、后部减弱。一个便于复现、但不声称为原文唯一实现的例子为
$${\rho }_i={\left(\frac{N-1-i}{N-1}\right)}^2.$$

从连续时间 OCP 到离散 NMPC

设预测网格间隔 $\Delta t$，节点数 $N$：
$$\boxed{T=N\Delta t}.$$

论文仿真中使用
$$\Delta t=0.1\mathrm{s},N=40\text{或}80$$
即$4\mathrm{s}$和$8\mathrm{s}$预测时长；控制器每$0.05\mathrm{s}$重算一次。因此预测网格间隔与闭环控制更新周期不必相同。

若
$$\dot{\mathbf{x}}=f(\mathbf{x},u),$$

区间内输入保持不变，则

$$\begin{array}{rl}{\mathbf{k}}_1& =f({\mathbf{x}}_i,u_i)\\ {\mathbf{k}}_2& =f\left({\mathbf{x}}_i+\frac{\Delta t}{2}{\mathbf{k}}_1,u_i\right)\\ {\mathbf{k}}_3& =f\left({\mathbf{x}}_i+\frac{\Delta t}{2}{\mathbf{k}}_2,u_i\right)\\ {\mathbf{k}}_4& =f\left({\mathbf{x}}_i+\Delta t{\mathbf{k}}_3,u_i\right)\end{array}$$

以及

$$\boxed{{\mathbf{x}}_{i+1}=F_d({\mathbf{x}}_i,u_i)={\mathbf{x}}_i+\frac{\Delta t}{6}\left({\mathbf{k}}_1+2{\mathbf{k}}_2+2{\mathbf{k}}_3+{\mathbf{k}}_4\right)}$$

Direct Multiple Shooting 的完整数学形式

在 direct multiple shooting 中，状态和控制都进入决策向量：
$$\mathbf{z}={\left[\begin{array}{cccccc}{\mathbf{x}}_0^T& \cdots & {\mathbf{x}}_N^T& u_0& \cdots & u_{N-1}\end{array}\right]}^T$$

每个区间独立积分，并施加连续性约束
$$\boxed{{\mathbf{c}}_i(\mathbf{z})={\mathbf{x}}_{i+1}-F_d({\mathbf{x}}_i,u_i)=0}.$$
初始条件
$${\mathbf{x}}_0=\hat{\mathbf{x}}(t_k),$$
输入约束
$$u_{\min }\le u_i\le u_{\max }.$$

离散代价
$$\begin{array}{rl}{J}_d=& \sum _{i=0}^{N-1}\left[{\Vert {\mathbf{y}}_i-{\mathbf{y}}_{\mathrm{ref},i}\Vert }_{\mathbf{Q}}^2+{\Vert u_i-u_{\mathrm{ref},i}\Vert }_{\mathbf{R}}^2\right]\\ & +{\Vert {\mathbf{y}}_N-{\mathbf{y}}_{\mathrm{ref},N}\Vert }_{\mathbf{P}}^2+J_{\mathrm{cons}}\end{array}$$

最终得到 NLP：
$$\boxed{\begin{array}{rl}\underset{\mathbf{z}}{\min }& J_d(\mathbf{z})\\ \text{s.t.}& {\mathbf{c}}_i(\mathbf{z})=0,\\ & {\mathbf{x}}_0=\hat{\mathbf{x}}(t_k),\\ & u_{\min }\le u_i\le u_{\max }.\end{array}}$$

在此基础上，线性系统
$${\mathbf{x}}_{k+1}=\mathbf{A}{\mathbf{x}}_k+\mathbf{B}{\mathbf{u}}_k$$
可以堆叠为
$$\mathbf{Z}=\mathbf{G}{\mathbf{x}}_k+\mathbf{HU},$$
从而把二次代价化成
KaTeX parse error: Undefined control sequence: \\trans at position 40: …nst}+\\mathbf{f}\\̲t̲r̲a̲n̲s̲ ̲\\mathbf{U}
+\\fr…
无约束时
$${\mathbf{U}}^{\star }=-{\mathbf{H}}_Q^{-1}\mathbf{f}.$$

本文中
$${\mathbf{x}}_{i+1}=F_d({\mathbf{x}}_i,u_i)$$
包含 $\sin \xi ,\cos \xi ,\tan \mu$，同时路径误差还依赖最近点几何和 KaTeX parse error: Undefined control sequence: \\atanTwo at position 1: \\̲a̲t̲a̲n̲T̲w̲o̲。因此 $J(\mathbf{U})$ 不是全局二次函数，也不存在对整个工作域固定的 $\mathbf{G},\mathbf{H}$。这就是必须使用非线性优化器的根本原因。

论文使用 SQP、qpOASES 以及 Gauss–Newton real-time iteration 思想~\\cite{stastny2017,houska2011,qpoases}。

在当前轨迹 $({\bar{\mathbf{x}}}_i,{\bar{u}}_i)$ 附近，
$$F_d({\mathbf{x}}_i,u_i)\approx F_d({\bar{\mathbf{x}}}_i,{\bar{u}}_i)+{\mathbf{A}}_i\Delta {\mathbf{x}}_i+{\mathbf{B}}_i\Delta u_i,$$
其中
Ai=∂Fd∂x∣(xˉi,uˉi),Bi=∂Fd∂u∣(xˉi,uˉi).
\\mathbf{A}_i=
\\left.\\frac{\\partial F_d}{\\partial \\mathbf{x}}\\right|_{(\\bar{\\mathbf{x}}_i,\\bar u_i)},
\\quad
\\mathbf{B}_i=
\\left.\\frac{\\partial F_d}{\\partial u}\\right|_{(\\bar{\\mathbf{x}}_i,\\bar u_i)}.
Ai​=∂x∂Fd​​​(xˉi​,uˉi​)​,Bi​=∂u∂Fd​​​(xˉi​,uˉi​)​.

连续性约束线性化：
$$\Delta {\mathbf{x}}_{i+1}-{\mathbf{A}}_i\Delta {\mathbf{x}}_i-{\mathbf{B}}_i\Delta u_i=-{\bar{\mathbf{c}}}_i.$$

若代价写为最小二乘
$$J(\mathbf{z})=\frac{1}{2}\Vert \mathbf{r}(\mathbf{z}){\Vert }_{\mathbf{W}}^2,$$
则
$$\nabla J={\mathbf{J}}_r^T\mathbf{Wr}$$
精确 Hessian：
$${\nabla }^{2}J={\mathbf{J}}_r^T\mathbf{W}{\mathbf{J}}_r+\sum _j{r}_j{\nabla }^2{r}_j$$
Gauss–Newton 近似忽略第二项：
$$\boxed{{\mathbf{H}}_{\mathrm{GN}}\approx {\mathbf{J}}_r^T\mathbf{W}{\mathbf{J}}_r}$$

因此每轮 SQP 解局部 QP：
$$\boxed{\begin{array}{rl}\underset{\Delta \mathbf{z}}{\min }& \frac{1}{2}\Delta {\mathbf{z}}^T{\mathbf{H}}_{\mathrm{GN}}\Delta \mathbf{z}+{\mathbf{g}}^T\Delta \mathbf{z}\\ \text{s.t.}& {\mathbf{A}}_c\Delta \mathbf{z}={\mathbf{b}}_c\\ & \mathbf{l}\le \mathbf{C}\Delta \mathbf{z}\le \mathbf{u}\end{array}}$$
求得 $\Delta {\mathbf{z}}^{\star }$ 后更新
$${\mathbf{z}}^{(j+1)}={\mathbf{z}}^{(j)}+{\alpha }_j\Delta {\mathbf{z}}^{\star }.$$
RTI 的核心是在每个控制周期只做有限的在线 SQP 更新并利用上一时刻解 warm start，从而满足实时要求。

预测时域长度的理论解释

最大 bank angle 下，最大航向变化率为：

$$|\dot{\xi }{|}_{\max }=\frac{g\tan {\mu }_{\max }}{V}$$

完成

$$90^{\circ }=\frac{\pi }{2}$$

航向变化所需的近似时间为：

$$\boxed{T_{90}=\frac{\pi V}{2g\tan {\mu }_{\max }}}$$

若预测网格时间间隔为 $\Delta t$，则为了让预测时域至少能够覆盖这一转弯过程，需要满足：

$$\boxed{N\ge \lceil \frac{T_{90}}{\Delta t}\rceil }$$

论文中说明，$N=40$ 被选作能够覆盖约一次 $90^{\circ }$ 最大 bank 转弯的最低 prediction horizon 量级（Stastny et al., 2017）。

较长 prediction horizon 的主要意义在于提前发现未来可能出现的轨迹误差，从而提前采取控制动作。但是，无论预测时域多长，都无法突破飞机自身的最小转弯半径、最大 bank angle 等物理可行性约束。

仿真设置与结果

论文示例仿真使用如下权重：

$${\mathbf{Q}}_{\mathrm{diag}}=>\left[\begin{array}{ccccc}0.01& 1& 0.1& 0.01& 100\end{array}\right]$$

$$R=10$$

$${\mathbf{P}}_{\mathrm{diag}}=>\left[\begin{array}{ccccc}0.1& 10& 0& 0.01& 0\end{array}\right]$$

prediction discretization interval 为：

$$\Delta t=0.1\mathrm{s}$$

控制器每

$$0.05\mathrm{s}$$

更新一次。

论文比较：

$$N=40\text{和}N=80$$

两种 prediction horizon。

空速设置为：

$$V=14m/s$$

在圆轨迹强风实验中，East 方向风速为：

$$w_e=-10m/s$$

主要现象包括：

实飞设置、实时性和结论边界

实飞实验主要采用：

$$N=40$$

$$\Delta t=0.1\mathrm{s}$$

NMPC 每

$$0.05\mathrm{s}$$

重新求解一次。

实飞阶段的权重设置为：

$${\mathbf{Q}}_{\mathrm{diag}}={\mathbf{P}}_{\mathrm{diag}}=\left[\begin{array}{ccccc}0.01& 10& 0.1& 0.01& 100\end{array}\right]$$

并且：

$$R=10$$

—— Stastny et al., 2017

ODROID-U3 上整个 NMPC ROS node 的计算时间如下：

控制周期为：

$$T_c=50\mathrm{ms}$$

而论文报告的最大计算时间约为：

$$T_{\mathrm{solve},\max }=15.1\mathrm{ms}$$

因此：

$$15.1\mathrm{ms}<50\mathrm{ms}$$

说明该 NMPC 实现在实验平台上具有一定实时计算余量。

论文进行了 box/loiter 以及任意 Dubins segment 序列的实飞实验。

路径收敛以后，稳态位置误差约控制在：

$$|e_t|\lesssim 1\mathrm{m}$$

需要严格区分以下几点：

论文方法的主要优点与局限

优点

局限

整篇论文的一条核心因果链

整套控制系统最核心的因果关系可以表示为：

$$\boxed{{\mu }_r\to q_{\mu }\to \mu \to \dot{\xi }\to \xi \to (n,e)\to (e_t,e_{\chi })\to J}$$

其中：

• ${\mu }_r$：NMPC 输出的 bank-angle reference
• $q_{\mu }=\dot{\mu }$：bank angle rate
• $\mu$：实际 bank angle
• $\dot{\xi }$：航向角变化率
• $\xi$：air-relative heading
• $(n,e)$：飞机位置
• $(e_t,e_{\chi })$：路径误差
• $J$：NMPC optimization cost

NMPC 在每个控制时刻求解未来控制序列：

$$\mathbf{U}={\left[\begin{array}{cccc}{\mu }_{r,0}& {\mu }_{r,1}& \cdots & {\mu }_{r,N-1}\end{array}\right]}^T$$

使预测时域内的综合代价最小。

但是实际只执行控制序列的第一项：

$$\boxed{{\mu }_r(t_k)={\mu }_{r,0}^{\star }}$$

然后获得新的状态估计，并重新求解下一次 NMPC 优化问题。

这就是 receding horizon principle。

最小复现模型

为了复现论文最核心的 NMPC，可以使用最小状态：

$$\mathbf{x}={\left[\begin{array}{ccccc}n& e& \mu & \xi & q_{\mu }\end{array}\right]}^T$$

控制输入：

$$u={\mu }_r$$

其中：

$$q_{\mu }=\dot{\mu }$$

完整预测模型为：

x˙=[Vcos⁡ξ+wnVsin⁡ξ+weqμgtan⁡μVb0u−a1qμ−a0μ]
\\boxed{
\\dot{\\mathbf{x}}
=\\begin{bmatrix}
V\\cos\\xi+w_n
\\\\
V\\sin\\xi+w_e
\\\\
q_\\mu
\\\\
\\dfrac{g\\tan\\mu}{V}
\\\\
b_0u-a_1q_\\mu-a_0\\mu
\\end{bmatrix}
}
x˙=​Vcosξ+wn​Vsinξ+we​qμ​Vgtanμ​b0​u−a1​qμ​−a0​μ​​​

直线或者圆轨迹跟踪时，可以定义 NMPC 输出：

$$\mathbf{y}={\left[\begin{array}{ccccc}{e}_t& e_{\chi }& \mu & q_{\mu }& u\end{array}\right]}^T$$

最小离散代价函数可以写成：

$$J=\sum _{i=0}^{N-1}\left(q_t{e}_{t,i}^2+q_{\chi }{e}_{\chi ,i}^2+q_{\mu }{\mu }_i^2+q_q{q}_{\mu ,i}^2+r{u}_i^2\right)+J_N$$

其中：

$$q_t{e}_{t,i}^2$$

用于惩罚横向路径误差；

$$q_{\chi }{e}_{\chi ,i}^2$$

用于惩罚航迹方向误差；

$$q_{\mu }{\mu }_i^2$$

用于避免过大的 bank angle；

$$q_q{q}_{\mu ,i}^2$$

用于避免过快的滚转运动；

$$r{u}_i^2$$

用于惩罚控制输入。

推荐复现顺序

与线性 MPC 的对应关系

因此可以理解为：

$$\boxed{\text{Linear MPC}\subset \text{NMPC numerical solution framework}}$$

NMPC 的 SQP 求解过程中，每一步局部线性化后，本质上仍会得到一个类似线性 MPC 的 QP。

对 eVTOL / Tiltrotor MPC 的迁移启示

这篇论文最值得迁移到 eVTOL 的并不是它的二维 fixed-wing 模型，而是下面这个思想：

$$\boxed{\text{先确定 NMPC 所在的控制层级，再建立足够准确但尽量低阶的预测模型}}$$

例如，如果 eVTOL 的高层 NMPC 输出：

$$\mathbf{u}={\left[\begin{array}{ccc}{\varphi }_r& {\theta }_r& T_r\end{array}\right]}^T$$

那么可以建立或者辨识：

$$\text{reference command}\to \text{closed-loop vehicle response}$$

而不是直接建立每一个执行器到飞机状态之间的完整高阶模型。

如果进一步使用 actuator-level NMPC，则控制输入可能变为：

$$\mathbf{u}={\left[\begin{array}{ccccccc}{T}_1& \cdots & T_m& {\delta }_{\mathrm{tilt}}& {\delta }_a& {\delta }_e& {\delta }_r\end{array}\right]}^T$$

此时 NMPC 的控制自由度明显增加，但同时：

$$\text{model complexity}$$

$$\text{constraint complexity}$$

和：

$$\text{online computational cost}$$

都会显著增加。

在 transition flight 中还需要考虑：

$$C_L(\alpha )$$

$$C_D(\alpha )$$

$$V_a^2$$

$$\mathbf{R}(q)\mathbf{T}$$

以及：

$${\delta }_{\mathrm{tilt}}$$

等强非线性因素。

因此：

$$\boxed{\text{control-augmented reduced-order modeling}}$$

对于 eVTOL NMPC 可能比普通固定翼更加重要。

附录

实现时的角度处理

对于任何角度误差，都建议将其限制在：

$$[-\pi ,\pi )$$

范围内。

可以定义：

$${\mathrm{wrap}}_{[-\pi ,\pi )}(\theta )=atan2\left(\sin \theta ,\cos \theta \right)$$

因此 course error 可以写成：

$$\boxed{e_{\chi }=atan2\left(\sin ({\chi }_d-\chi ),\cos ({\chi }_d-\chi )\right)}$$

这样可以避免当：

$${\chi }_d\approx -\pi$$

而：

$$\chi \approx +\pi$$

时产生接近：

$$2\pi$$

的虚假角度误差。

一个 NMPC 控制周期的伪代码

Given previous optimal horizon U_prev

for each control instant k:

1. Read state estimate:

x_hat = [n, e, mu, xi, q_mu]

2. Read online parameters:

V
wind
P_current
P_next

3. Warm-start NLP using previous solution

4. Solve:

minimize J(X, U)

subject to:

X(i+1) = Fd(X(i), U(i))

mu_r_min <= U(i) <= mu_r_max

where:

y = [e_t, e_chi, mu, q_mu, mu_r]

5. Apply only the first control:

mu_r_cmd = U_star(1)

6. Store current optimal horizon:

U_prev = U_star

7. Shift horizon

8. Receive new state estimate

9. Repeat

符号表

论文结论的严谨表述

该工作展示了一种面向小型固定翼 UAV 的高层横侧向 NMPC 架构。

其核心思想是使用简化二维运动学模型与闭环系统辨识得到的低阶滚转动态共同构成 control-augmented prediction model。

控制器同时显式考虑风估计、bank-angle reference 约束，以及 prediction horizon 内的 Dubins path switching。

仿真结果说明，在强风路径跟踪问题中，较长 prediction horizon 可以更早预测未来路径偏离，并提前采取控制动作。

实飞实验则验证了该 NMPC 在小型机载计算平台上的实时运行能力以及路径跟踪可行性。

需要注意的是，该论文没有为具体实现提供严格的闭环稳定性证明，同时也没有通过强风实飞证明 NMPC 全面优于传统的 $L_1$ guidance。

参考文献
[1] Kang Y, Hedrick J. Design of nonlinear model predictive controller for a small fixed-wing unmanned aerial vehicle[C]//AIAA Guidance, Navigation, and Control Conference and Exhibit. 2006: 6685.
[2] Kang Y, Hedrick J K. Linear tracking for a fixed-wing UAV using nonlinear model predictive control[J]. IEEE Transactions on Control Systems Technology, 2009, 17(5): 1202-1210.
[3] Rucco A, Aguiar A P, Pereira F L, et al. A predictive path-following approach for fixed-wing unmanned aerial vehicles in presence of wind disturbances[C]//Robot 2015: Second Iberian Robotics Conference: Advances in Robotics, Volume 1. Cham: Springer International Publishing, 2015: 623-634.
[4] Gavilan F, Vazquez R, Camacho E F. A High-level model predictive control guidance law for unmanned aerial vehicles[C]//2015 European Control Conference (ECC). IEEE, 2015: 1362-1369.