多旋翼混控器
从零开始的完全指南

Chapter One什么是混控器？

在我们打开代码之前，先把一个问题想清楚：当你推动遥控器的摇杆，让一架四旋翼向前飞的时候， 飞控究竟在背后做了什么？

飞控系统的任务可以粗略分成三层。最上层，遥控器或导航模块 给出飞手想要的东西——比如"我想要前倾 10 度"。中间层，姿态控制器 根据当前姿态和目标姿态，计算出需要的角加速度与推力： \(\dot{p}_{sp}, \dot{q}_{sp}, \dot{r}_{sp}, T_{sp}\) （即 roll、pitch、yaw 方向的角加速度，和垂直推力）。

可问题是——飞机上并没有"roll 电机"或"pitch 电机"。 只有四个（或六个、八个）均匀分布的电机，每个只会转。要让飞机产生向前的俯仰力矩， 必须让后面的电机转快一点、前面的慢一点；要产生偏航，需要改变顺/逆时针电机对的转速比例。 把一组抽象的"力矩 + 推力"需求，翻译成 N 个电机的具体油门量——这就是混控器要做的事。

混控器的输入永远是 4 维的——roll、pitch、yaw、thrust。 输出是 N 维的——N 是电机数量（四旋翼 N=4，六旋翼 N=6，八旋翼 N=8）。 所以它的本质是一个 4 → N 的映射函数。

Chapter Two物理基础：电机怎么产生力矩

在聊数学之前，先把物理搞清楚。以一架最常见的四旋翼 X 型为例， 俯视时四个电机分布在正方形的四个角上。每个电机除了向下推空气产生向上的推力， 它本身的旋转还会产生反扭矩——就像直升机如果不装尾桨，机身会反着转。

力矩是怎么来的？

• Roll（横滚）：让右侧电机（M1、M4）转慢一点、左侧（M2、M3）转快一点， 左侧推力更大，飞机就向右滚。
• Pitch（俯仰）：让前侧电机（M1、M3）转慢、后侧（M2、M4）转快， 飞机就向前俯。
• Yaw（偏航）：注意 M1、M2 是 CCW（逆时针），M3、M4 是 CW（顺时针）。 让一对同向电机整体转快，另一对转慢，它们的反扭矩不再平衡，机身就会绕 z 轴转。
• Thrust（推力）：四个电机一起加减，飞机升降。

Chapter Three控制分配矩阵 P

把上一节的观察写成数学。定义：

\[ \mathbf{m} = \begin{bmatrix} \dot{p} \\ \dot{q} \\ \dot{r} \\ T \end{bmatrix} \quad\text{（roll/pitch/yaw 角加速度 + 推力，归一化到 [-1,1]/[0,1]）} \] \[ \mathbf{u} = \begin{bmatrix} u_1 \\ u_2 \\ \vdots \\ u_N \end{bmatrix} \quad\text{（每个电机的输出，归一化到 [0,1]）} \]

那么混控的核心关系就是一个线性变换：

\[ \boxed{\; \mathbf{u} = P \cdot \mathbf{m} \;} \]

P 是一个 N×4 的矩阵，每一行对应一个电机，每一列对应一个控制轴。 第 i 行第 j 列的元素 \(P_{ij}\) 表示：第 j 个控制轴变化 1 个单位时，第 i 个电机该变化多少。

直接上代码里的例子——四旋翼 X 型的 P 矩阵：

# quad_x
P1 = np.matrix([
    [-0.71,  0.71,  1.,   1.  ],  # M1  front-right, CCW
    [ 0.71, -0.71,  1.,   1.  ],  # M2  back-left,   CCW
    [ 0.71,  0.71, -1.,   1.  ],  # M3  front-left,  CW
    [-0.71, -0.71, -1.,   1.  ]]) # M4  back-right,  CW
#          roll    pitch    yaw  thrust

逐列解读

反方向：伪逆矩阵 B

如果我给出四个电机的转速 \(\mathbf{u}\)，想反推它们合起来产生了什么 \(\mathbf{m}\)，需要用 P 的伪逆：

\[ \mathbf{m} = B \cdot \mathbf{u}, \qquad B = P^+ \]

代码里就是：

B = np.linalg.pinv(P)

这一步在混控器里其实不是必须的——P 已经足够把指令变成电机输出了。 但 B 在测试和调试时很有用，我们可以验证"混完之后真正分配出去的是什么"： m_new = B * u_new_sat。

Chapter Four不同机型的 P 矩阵

代码里定义了五种机型的 P 矩阵。我们看一遍，你会发现它们有共同结构：

一个很重要的性质

无论哪个机型，thrust 那一列永远全是 1（代码里所有 P 矩阵的最后一列都是 1）。 这说明推力指令对每个电机的贡献是相同的——直觉上也对，推力是所有电机"同增同减"的结果。

这个性质在后面非常关键：当我们需要"在不改变 roll/pitch/yaw 的前提下调整电机输出"时， thrust 列就是那个完美的"去饱和方向"——因为沿着 [1,1,1,1] 方向移动，所有电机都加/减同一个数， 姿态力矩完全不变。

Chapter Five饱和问题：为什么事情会变复杂

如果所有指令都合理、永远没有极端情况，那混控器只需要一行代码：u = P * m， 故事就结束了。可惜现实不是这样。

电机的 PWM 输出有物理边界——它不能倒着转，也不能转到无限快。在归一化之后，约束是：

\[ 0 \leq u_i \leq 1 \quad \text{对所有 } i \]

可是当姿态控制器要求非常激烈的机动时，算出来的 \(\mathbf{u}\) 很可能超出这个范围。 比如推力已经很大（比如 0.9），再叠加一个猛烈的 roll 指令，某个电机可能被要求输出 1.15—— 电机不会长出新的翅膀，它只能给你 1.0。这就是"饱和"。

一个具体的例子

假设我们在 quad_x 上，给 \(\mathbf{m}_{sp} = [0.2,\ 0,\ 0,\ 0.9]^T\)—— 0.9 的推力加上一点 roll。直接算：

\[ \mathbf{u} = P \cdot \mathbf{m}_{sp} = \begin{bmatrix} -0.71\times 0.2 + 0.9 \\ 0.71\times 0.2 + 0.9 \\ 0.71\times 0.2 + 0.9 \\ -0.71\times 0.2 + 0.9 \end{bmatrix} = \begin{bmatrix} 0.758 \\ 1.042 \\ 1.042 \\ 0.758 \end{bmatrix} \]

M2 和 M3 的要求都是 1.042——已经超上限了！ 如果直接简单地把它们钉在 1.0，那么 M2、M3 实际输出比 M1、M4 多的只有 0.242， 而我们想要的 roll 需要的差值是 0.284。roll 就做不到了—— 姿态控制丢失。这对稳定性是灾难性的。

去饱和的直觉：找一个"不痛不痒"的方向

对上面这个例子——如果我们把所有四个电机同时减去 0.042，会怎样？

\[ \mathbf{u}' = \begin{bmatrix} 0.716 \\ 1.000 \\ 1.000 \\ 0.716 \end{bmatrix} \]

现在 M2、M3 恰好在上限，没有超。四个电机的相对差值完全不变，roll 完美保留。 代价是什么？总推力从 0.9 变成了 0.858——稍微降了一点。

我们沿着推力方向（也就是 P 的第 4 列 [1,1,1,1]）微调了输出，牺牲推力来换取姿态。 这就是去饱和算法的核心思想。

Chapter Six核心算法一：compute_desaturation_gain

算法要做的事：给定一个可能饱和的电机输出向量 \(\mathbf{u}\)，和一个"去饱和向量" \(\mathbf{d}\) （也就是我们愿意沿着它调整的方向），找一个标量增益 \(k\)，使得

\[ \mathbf{u}' = \mathbf{u} + k \cdot \mathbf{d} \]

尽可能地不饱和。

数学推导

对每个电机 \(i\)，要让它回到 \([u_{min}, u_{max}]\) 内，需要：

\[ u_{min} \leq u_i + k \cdot d_i \leq u_{max} \]

分开看两个不等式。定义：

• 下越界量：\(\Delta u^-_i = u_{min} - u_i\)（若 \(u_i < u_{min}\)，这是正数，表示"需要往上抬这么多"）
• 上越界量：\(\Delta u^+_i = u_{max} - u_i\)（若 \(u_i > u_{max}\)，这是负数，表示"需要往下压这么多"）

对应代码：

d_u_sat_plus  = u_max - u   # 上限还剩多少
d_u_sat_minus = u_min - u   # 下限还差多少

然后，对每个电机，计算把它刚好拉回上限或刚好拉回下限所需的 \(k\) 值：

\[ k_i^+ = \frac{u_{max} - u_i}{d_i} \quad (\text{if } d_i \ne 0), \qquad k_i^- = \frac{u_{min} - u_i}{d_i} \]

但代码很聪明，只记录真正有意义的 \(k\)：

if d_u_sat_minus[i] > 0.0:      # 只有真的越了下限才记
    k[2*i]   = d_u_sat_minus[i] / desaturation_vector[i]
if d_u_sat_plus[i]  < 0.0:      # 只有真的越了上限才记
    k[2*i+1] = d_u_sat_plus[i]  / desaturation_vector[i]

其他情况 k 保持为 0（数组初始化时就是零）。为什么？ 因为我们不希望"虽然没有饱和的电机，但为了让它更靠近中间"而强行去动——那样反而会把没饱和的电机推向饱和。

合成最终 k

k_min = min(k)
k_max = max(k)
k = k_min + k_max   # ← 最关键的一行
return k

这一步非常微妙。让我来解释：

• k_min 是所有（负的）k 中最小的——它对应"最严重的上饱和"那个电机， 把它拉下来所需的最负的 k。
• k_max 是所有（正的）k 中最大的——它对应"最严重的下饱和"那个电机， 把它抬上来所需的最正的 k。
• k = k_min + k_max：两者相加，目的是把饱和分摊到两侧—— 让最大的那个电机下降的同时，最小的那个电机也同等程度地上升，使剩余饱和对称分布。

Chapter Seven核心算法二：minimize_sat

上一节的 \(k\) 只是一次尝试。如果饱和范围比输出范围还大（max(u) − min(u) > u_max − u_min）， 一次调整不够。minimize_sat 做了两步：

def minimize_sat(u, u_min, u_max, desaturation_vector):
    k_1 = compute_desaturation_gain(u, u_min, u_max, desaturation_vector)
    u_1 = u + k_1 * desaturation_vector   # 第一次尝试

    k_2 = compute_desaturation_gain(u_1, u_min, u_max, desaturation_vector)
    k_opt = k_1 + 0.5 * k_2             # 再加半个修正

    u_prime = u + k_opt * desaturation_vector
    return u_prime

1. 第一次：用 \(k_1\) 调整，把饱和尽量对称化。 如果饱和可以完全消除，\(k_1\) 之后就不饱和了，第二次算出的 \(k_2=0\)。
2. 第二次：如果还有残余饱和（说明饱和范围太宽、一次去不完）， 再加 0.5 * k_2 做平衡——在剩下的上下越界之间取一个中点。

这样，minimize_sat 就成了整个混控器的核心工具：给定一个"愿意牺牲的方向"， 它能以最优方式调整输出、把饱和降到最低。

Chapter Eight三种混控模式的对比

混控器面对的不是单一饱和场景，而是"当事情无法两全时，我优先保什么"的哲学问题。 PX4 提供三种"哲学"：

normal_mode — 保守派

代码思路是一连串"如果还饱和，就牺牲下一项"：

def normal_mode(m_sp, P, u_min, u_max):
    # 1. 先算 roll+pitch 产生的输出（yaw 暂不管）
    m_sp_no_yaw = m_sp.copy()
    m_sp_no_yaw[2, 0] = 0.0
    u = P * m_sp_no_yaw

    # 2. 尝试用 thrust 列去饱和，但只允许推力下降
    u_T = P[:, 3]
    u_prime = minimize_sat(u, u_min, u_max, u_T)
    if (u_prime > u).any():
        u_prime = u                     # ← 若要求推力升高则撤回

    # 3. 如果还饱和，就减 roll/pitch 的幅度
    u_p_dot = P[:, 0]                 # roll 方向
    u_p2 = minimize_sat(u_prime, u_min, u_max, u_p_dot)
    u_q_dot = P[:, 1]                 # pitch 方向
    u_p3 = minimize_sat(u_p2, u_min, u_max, u_q_dot)

    # 4. 最后把 yaw 拼回去
    u_final = mix_yaw(m_sp, u_p3, P, u_min, u_max)
    return (u, u_final)

核心约束在第 2 步那个 if (u_prime > u).any()—— 只要发现去饱和让任何一个电机输出变大（等价于推力上升），就整体作废，不动。 这是"不偷油门"的保证。

代价是：如果飞手给了很低油门，但同时要求很大的 roll，推力又不允许升高， 那飞机就会降到离地面更近——姿态会失控，"airmode off" 飞低油门容易掉就是这个原因。

airmode_rp — 中间派

def airmode_rp(m_sp, P, u_min, u_max):
    m_sp_no_yaw = m_sp.copy()
    m_sp_no_yaw[2, 0] = 0.0
    u = P * m_sp_no_yaw

    u_T = P[:, 3]
    u_prime = minimize_sat(u, u_min, u_max, u_T)   # ← 允许推力双向
    u_final = mix_yaw(m_sp, u_prime, P, u_min, u_max)
    return (u, u_final)

和 normal_mode 几乎一样，但去掉了那个 if 判断—— 这意味着为了保住 roll/pitch，推力可以自由地上升或下降。 低油门 + 大 roll 也不怕了，因为算法会自动把油门加一点来给 roll 留空间。

这就是"Airmode" 这个名字的由来：即使飞手给的油门是 0，空中（air）姿态控制仍然能生效。

airmode_rpy — 激进派

def airmode_rpy(m_sp, P, u_min, u_max):
    u = P * m_sp                                   # ← yaw 直接一起混

    u_T = P[:, 3]
    u_prime = minimize_sat(u, u_min, u_max, u_T)   # 用 thrust 去饱和

    # 若还饱和，用 yaw 做最后一次去饱和（而非像其他模式丢掉 yaw）
    u_T = P[:, 2]
    u_prime_yaw = minimize_sat(u_prime, u_min, u_max, u_T)

    return (u, u_prime_yaw)

和 rp 的区别是：yaw 一开始就和 roll/pitch 一起被算进 \(\mathbf{u}\)。 只有在饱和消不掉时，才牺牲 yaw。适合穿越机之类需要每一根轴都跟手的场景。

Chapter NineYaw 的特殊待遇：mix_yaw

为什么 yaw 值得单独讲？因为相比 roll/pitch，yaw 的响应极其容易让电机饱和。

原因：roll/pitch 靠的是两个电机反向变化（一个加、一个减），对总推力影响小； 而 yaw 靠的是一组电机（CCW 那对）整体加速，另一组（CW）整体减速—— 整半数的电机要同时增加输出。在油门已经很高时，这几乎一定撞上限。

def mix_yaw(m_sp, u, P, u_min, u_max):
    # 只取出 yaw 分量单独混
    m_sp_yaw_only = np.matlib.zeros(m_sp.size).T
    m_sp_yaw_only[2, 0] = m_sp[2, 0]
    u_p = u + P * m_sp_yaw_only

    # 第一步：允许上限多出 0.15（即最多推到 1.15），沿 yaw 方向去饱和
    u_r_dot = P[:, 2]
    u_pp = minimize_sat(u_p, u_min, u_max + 0.15, u_r_dot)

    # 第二步：用 thrust 方向把超限的电机拉回 [0, u_max]
    u_T = P[:, 3]
    u_ppp = minimize_sat(u_pp, 0, u_max, u_T)

    # 只允许降推力，不允许升
    if (u_ppp > u_pp).any():
        u_ppp = u_pp
    return u_ppp

最后那个 if (u_ppp > u_pp).any() 保证：即使 yaw 要求需要推力升高， 也不允许——宁可降低 yaw 幅度，也不偷飞手的油门。这和 normal_mode 的哲学一致。

Chapter Ten完整流程：一个具体案例走一遍

设定：四旋翼 X 型 (quad_x)，airmode_rp 模式，输入 \(\mathbf{m}_{sp} = [0.2,\ 0,\ 0.1,\ 0.9]^T\) （轻微 roll + 轻微 yaw + 高油门）。我们一步步走。

1. 剥离 yaw，先算无 yaw 的输出

   \(\mathbf{m}_{sp}' = [0.2,\ 0,\ 0,\ 0.9]^T\)

   \(\mathbf{u} = P \cdot \mathbf{m}_{sp}' = [0.758,\ 1.042,\ 1.042,\ 0.758]^T\)

   M2、M3 越了上限 0.042。

2. 用 thrust 方向（[1,1,1,1]）做 minimize_sat

   上边界还剩：\([0.242, -0.042, -0.042, 0.242]\)
   下边界要抬：\([-0.758, -1.042, -1.042, -0.758]\)（全都是负数，全部不记）

   只有 M2、M3 上饱和，\(k = k_{min} + k_{max} = -0.042 + 0 = -0.042\)

   第一次调整：\(\mathbf{u}_1 = \mathbf{u} + (-0.042)\cdot[1,1,1,1] = [0.716,\ 1.000,\ 1.000,\ 0.716]\)

   已经没有饱和了，第二次 \(k_2 = 0\)，不需要修正。

   \(\mathbf{u}_{prime} = [0.716,\ 1.000,\ 1.000,\ 0.716]\)

3. 进入 mix_yaw

   yaw 对应的 P 列是 [1, 1, −1, −1]，乘以 0.1：yaw 贡献 = [0.1, 0.1, −0.1, −0.1]

   叠加：\(\mathbf{u}_p = [0.816,\ 1.100,\ 0.900,\ 0.616]\)

   M2 越过 1.0 达到 1.100。mix_yaw 先允许到 1.15 做一次 yaw 方向去饱和——1.100 < 1.15，未触发。

   然后用 thrust 方向把 M2 拉回 [0, 1]：

   \(k = -0.100\)，\(\mathbf{u}_{ppp} = [0.716,\ 1.000,\ 0.800,\ 0.516]\)

4. 检查推力是否被偷偷抬高

   \(\mathbf{u}_{ppp} \leq \mathbf{u}_{pp}\)，推力只降不升，通过。

5. 输出 saturation clamp

   把任何还在 [0, 1] 外的再强制 clip——本例中都在范围内，无需操作。

   最终 \(\mathbf{u}_{final} = [0.716,\ 1.000,\ 0.800,\ 0.516]^T\)

6. 验证：实际分配到了什么 m？

   \(\mathbf{m}_{new} = B \cdot \mathbf{u}_{final}\) —— 会发现 roll 和 yaw 基本保留， thrust 比原本 0.9 略低了一点（~0.758）。这正是 airmode_rp 的预期行为： 用一点推力的损失换取姿态的精确。

Chapter Eleven交互式演示

拖动滑块改变期望的 roll / pitch / yaw / thrust，切换模式，观察 quad_x 四个电机的实际输出。 红色斜纹表示该电机触发了饱和（输出被 clip 到 0 或 1）。

建议试试的情况：油门拉到 1.0，然后加一点 roll/yaw，看看 none 和 rp/rpy 模式的差别； 或者油门 0.0 加 roll，感受 airmode 的救命效果。

Chapter Twelve小结与扩展

走到这里，我们回顾混控器的全貌：

1. 混控器是从 4 维指令（roll/pitch/yaw/thrust）到 N 维电机输出的映射。
2. 核心工具是控制分配矩阵 P，每个机型有自己的 P。
3. 理想情况下 \(\mathbf{u} = P \cdot \mathbf{m}\)，但现实中电机有上下限，会饱和。
4. 饱和时不能简单 clip，否则控制目标丢失。
5. compute_desaturation_gain + minimize_sat 提供了"沿某方向最小饱和调整"的数学工具。
6. 不同模式（none / rp / rpy）通过选择愿意牺牲的方向体现不同的控制哲学。
7. Yaw 有单独的 mix_yaw，因为它最容易让电机饱和，有特殊的"允许 +0.15"策略。

更深层的问题

• 为什么 PX4 用这种启发式方法而不是解优化问题？ 因为每个控制周期（250Hz / 1kHz）都要跑一次混控，标准 QP 求解器太慢。 这套代码本质上是对"约束最小二乘"的一个极快的近似解。
• 这套算法是最优的吗？ 在合理假设下（P 秩为 4、去饱和向量方向合理），它能在单个饱和轴上给出全局最优解。 对多轴同时饱和情况，它是一个很好的次优解。PX4 一直在权衡"简单快"和"最优"。
• 扩展到非多旋翼呢？ 对固定翼、VTOL、多轴飞行器，PX4 有另一套基于 ControlAllocation 类的更通用的框架，但核心思想是一样的：先 \(\mathbf{u} = P \mathbf{m}\)， 再用伪逆 + 饱和处理。建议读完这份教程后去看 src/modules/control_allocator/ 里的代码。