多旋翼飞行器动力混控算法解析与实践-嵌云网-嵌入式AI开发资源站

多旋翼飞行器动力混控算法解析与实践

徐小疼

1. 多旋翼动力混控的核心挑战

作为ArduPilot飞控系统的核心算法之一，多旋翼动力混控（Motor Mixing）承担着将飞行器的姿态控制指令转化为各个电机推力输出的关键任务。这个看似简单的"翻译"过程，实际上需要解决几个关键矛盾：

首先，飞行器在三维空间中的运动控制需求（滚转、俯仰、偏航）必须通过有限的电机推力变化来实现。以最常见的四旋翼为例，四个电机需要同时满足：

维持基础升力（对抗重力）
产生滚转力矩（左右倾斜）
产生俯仰力矩（前后倾斜）
产生偏航力矩（绕垂直轴旋转）

其次，电机的推力输出存在物理限制——最小不能低于停转阈值（通常不是严格的0），最大不能超过电机/电调的能力上限。这意味着混控算法必须在有限的"推力预算"内，智能地分配各个控制维度的需求。

2. 混控算法的整体架构

2.1 输入输出接口

AP_MotorsMatrix::output_armed_stabilizing()作为混控核心函数，其输入主要来自两个部分：

姿态控制器输出的角速率指令（_roll_in, _pitch_in, _yaw_in）
油门指令（get_throttle()）

输出则是每个电机的归一化推力指令（_thrust_rpyt_out[i]），范围通常在[0,1]之间，最终会被转换为具体的PWM信号。

2.2 处理流程概览

该函数的处理流程可以概括为以下几个关键阶段：

输入补偿：对原始指令进行电压、气压补偿，确保在不同环境下控制响应一致
指令归一化：将所有输入转换到统一的"推力域"
约束处理：确保指令在物理可实现范围内
推力分配：根据机架几何特性计算各电机推力
饱和管理：处理指令冲突时的优先级和缩放
失效检测：监控电机状态，必要时启动容错机制

3. 关键实现细节解析

3.1 环境补偿机制

在实际飞行中，电池电压下降和空气密度变化都会显著影响电机的推力输出。为此，算法首先进行补偿计算：

cpp复制const float compensation_gain = thr_lin.get_compensation_gain();
const float roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
const float pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
float yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
float throttle_thrust = get_throttle() * compensation_gain;

这里的compensation_gain是一个动态调整的系数，它基于当前电压和气压计算得出，使得相同的控制指令在不同环境下产生的推力效果保持一致。这种补偿对于保持飞行品质至关重要，特别是在电池电量较低时。

3.2 推力分配原理

推力分配的核心在于理解"混控矩阵"的概念。对于不同的机架类型（如X型四旋翼、六旋翼、八旋翼等），每个电机对于各个控制维度的贡献是不同的。这些关系通过一组预设的系数来定义：

cpp复制_thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];

其中_roll_factor[i]和_pitch_factor[i]就是描述第i个电机对滚转和俯仰控制贡献的系数。这些系数在初始化时根据机架类型自动计算设置。

以典型的X型四旋翼为例：

右前方电机（电机1）的滚转系数为负（向右滚转需要减小该电机推力）
左前方电机（电机2）的滚转系数为正
俯仰系数的分配也遵循类似逻辑

3.3 偏航控制的空间管理

偏航控制（Yaw）在多旋翼中是通过电机扭矩的反作用力实现的，这需要电机之间产生差动推力。算法采用了一种智能的空间管理策略：

cpp复制if (!is_zero(_yaw_factor[i]) && (!_thrust_boost || i != _motor_lost_index)) {
    const float thrust_rp_best_throttle = throttle_thrust_best_rpy + _thrust_rpyt_out[i];
    float motor_room = is_positive(yaw_thrust * _yaw_factor[i]) ?
                       (1.0 - thrust_rp_best_throttle) :
                       thrust_rp_best_throttle;
    const float motor_yaw_allowed = MAX(motor_room, 0.0)/fabsf(_yaw_factor[i]);
    yaw_allowed = MIN(yaw_allowed, motor_yaw_allowed);
}

这段代码计算了在当前姿态指令下，每个电机还能为偏航控制提供多少"余量"。它考虑了两种可能的情况：

偏航指令需要增加电机推力时，余量为1.0减去当前推力
偏航指令需要减小电机推力时，余量就是当前推力值

最终取所有电机中的最小值作为全局偏航余量（yaw_allowed），确保不会有过大的偏航指令导致任何电机饱和。

3.4 饱和处理与优先级策略

当控制需求超过物理限制时，算法需要做出合理的取舍。ArduPilot采用的策略可以概括为"姿态优先，油门次之"：

cpp复制if (rpy_high - rpy_low > 1.0f) {
    rpy_scale = 1.0f / (rpy_high - rpy_low);
}
if (throttle_avg_max + rpy_low < 0) {
    rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low);
}

这里计算了一个缩放系数rpy_scale，用于在必要时等比缩小所有的姿态指令，确保：

所有电机的推力变化范围不超过[0,1]
即使按照最大油门设置，也不会导致任何电机推力低于最小值

这种处理方式虽然会导致姿态响应暂时达不到预期，但保证了飞行器的基本稳定性，避免失控。

4. 失效电机处理机制

对于具有冗余设计的机架（如六旋翼、八旋翼），ArduPilot实现了完善的电机失效处理策略。当检测到某个电机失效时：

启动推力增强模式（_thrust_boost），允许剩余电机暂时超负荷工作
重新计算混控矩阵，调整各电机的贡献系数
平滑过渡到新的工作点，避免突然的推力变化

cpp复制if (_thrust_boost) {
    // 特殊处理失效电机的情况
    if (i == _motor_lost_index) {
        rpy_high = (rpy_high + 1.0f) * 0.5f;
    }
}

这种机制显著提高了飞行器的可靠性，在某些情况下甚至允许飞行器在一个电机完全失效的情况下继续完成任务。

5. 工程实践与调参建议

5.1 关键参数解析

MOT_YAW_HEADROOM：设置偏航控制的最小余量（单位：‰），默认值通常为50（即5%）。在需要更敏捷的偏航响应时可以适当增大，但会减少其他控制维度的可用范围。
MOT_THST_HOVER：悬停时的油门百分比。这个参数帮助算法更好地估计可用推力范围，对于自动飞行模式特别重要。
**ATC_THR_MIX_**系列参数：控制姿态与油门之间的混合比例，影响在推力不足时的行为倾向。

5.2 常见问题排查

*频繁出现限幅（limit.标志被置位）
- 检查电池状态，可能是电压不足导致可用推力下降
- 检查桨叶是否损坏或电机是否过热
- 考虑降低姿态控制器的增益或最大角速率设定
偏航响应迟钝
- 增大MOT_YAW_HEADROOM
- 检查偏航混控系数是否正确
- 确保没有电机出现响应延迟
油门响应非线性
- 校准电调端点
- 检查MOT_THST_HOVER设置是否合理
- 验证电压补偿是否正常工作

6. 性能优化技巧

计算效率优化：
- 预先计算并存储混控矩阵，避免实时计算
- 使用快速平方根和三角函数近似算法
- 合理使用定点数运算（在低端处理器上）
控制品质优化：
- 实现动态余量分配，根据飞行模式调整优先级
- 加入指令滤波，避免过于激进的变化
- 实现自适应补偿，针对不同负载特性自动调整
调试辅助：
- 添加详细的实时状态日志
- 实现混控效果的可视化工具
- 设计自动化测试框架

在实际项目中，我们曾经通过优化混控矩阵的计算精度，将一款六旋翼的悬停功耗降低了约8%；另一个案例中，通过调整偏航余量分配策略，显著改善了重型八旋翼在强风中的航向保持能力。

多旋翼动力混控算法作为飞行控制的核心环节，其设计和实现质量直接决定了飞行器的性能和可靠性。理解这些底层原理不仅有助于故障诊断和参数调整，也为自定义飞行器设计和特殊控制需求提供了基础。