四旋翼飞行器运动学建模与PID油门映射原理

1. 飞行器运动学建模与坐标系定义

在四旋翼飞行器控制系统中，运动分析是PID控制器设计与输出整合的理论基础。脱离准确的刚体动力学模型，任何控制律的参数整定都将成为经验性试错，无法保证系统稳定性与响应品质。本节将从物理本质出发，构建符合右手定则的机体坐标系，明确各自由度运动与电机推力之间的映射关系。

1.1 机体坐标系与欧拉角定义

我们采用标准航空惯例建立飞行器本体坐标系（Body-fixed Frame）：

• X轴（Roll轴） ：沿机头方向为正，定义为 Pitch轴的旋转轴 。注意此处术语需精确——X轴本身不“做Pitch”，而是绕X轴的旋转构成 Roll（横滚） 运动；
• Y轴（Pitch轴） ：沿机身右侧方向为正，定义为 Roll轴的旋转轴 ；
• Z轴（Yaw轴） ：垂直向下为正（NED坐标系），构成 Yaw（偏航） 运动的旋转轴。

该坐标系严格遵循右手定则：当右手拇指指向坐标轴正向时，其余四指自然弯曲方向即为该轴正向旋转方向。例如，Z轴正向向下，则正向Yaw旋转为 逆时针 （从上方俯视）；Y轴正向向右，则正向Pitch旋转为 机头向上抬头 ；X轴正向向前，则正向Roll旋转为 右翼向下倾斜 。

实际硬件安装中，机头方向由机身红色标记标识。所有后续电机编号、螺旋桨安装方向、控制逻辑均以此物理基准为唯一参照。若标记缺失或模糊，必须通过IMU静止校准重新确定零位，不可凭目测臆断。

1.2 螺旋桨旋转方向与反扭矩平衡原理

四旋翼的机械构型决定了其必须采用 两顺两逆 的螺旋桨旋转配置，这是实现稳定悬停与姿态控制的物理前提。其根本原因在于电机旋转时产生的 反扭矩（Reaction Torque） 必须全局平衡。

根据牛顿第三定律，电机驱动螺旋桨加速旋转时，电机会受到大小相等、方向相反的反扭矩作用。若四个电机同向旋转（如全部顺时针），则反扭矩矢量全部叠加，导致飞行器持续绕Z轴自旋，无法建立稳定的Yaw基准。

因此，标准X型四旋翼布局规定：
- 电机A（前右）、C（后左）： 顺时针（CW）旋转
- 电机B（前左）、D（后右）： 逆时针（CCW）旋转

此配置下，A、C产生的反扭矩为逆时针（-Z方向），B、D产生的反扭矩为顺时针（+Z方向）。当四电机油门值相等时，总反扭矩Στ_z = τ_A + τ_C - τ_B - τ_D ≈ 0，实现Yaw轴力矩平衡。

螺旋桨的“高效旋转方向”由其几何剖面决定。观察桨叶： 高升力侧（凸面）朝向旋转方向时效率最高 。安装时必须确保：
- A、C电机：高升力侧位于旋转路径前方 → 顺时针旋转
- B、D电机：高升力侧位于旋转路径前方 → 逆时针旋转
若装反，不仅效率下降30%以上，更会导致PID控制出现持续稳态偏差，表现为飞行器缓慢自旋。

2. 姿态控制量到电机油门的映射关系

PID控制器输出的是三个姿态通道的 控制力矩（Torque） ，而执行机构（无刷电机）接收的是 标量油门指令（Throttle Command） 。二者之间需通过 动力学解耦矩阵 进行转换。本节推导该映射的物理依据与符号规则。

2.1 油门指令的物理含义

电机油门值 M_i （i=A,B,C,D）是一个归一化标量，范围通常为 [0, 1000] 或 [0, 65535] ，直接对应ESC（电子调速器）输出的PWM占空比。其物理意义是 电机期望产生的推力大小 ，与螺旋桨转速平方近似成正比（T ∝ ω²）。

设飞行器总升力为 F_z ，四个电机基础油门为 M_0 （悬停油门），则：

F_z = k_t * (M_A + M_B + M_C + M_D)

其中 k_t 为推力系数。 M_0 需通过实飞标定获得：在无风环境悬停时，记录四电机平均油门值，即为 M_0 。

2.2 Roll通道控制映射

Roll角θ控制目标是使飞行器绕X轴旋转，产生横向加速度。其物理实现依赖于 左右两侧电机推力差 ：

• 当需要正向Roll（右翼下压）时：增大右侧电机（A）推力，减小左侧电机（B）推力 → 产生正向Roll力矩
• 当需要负向Roll（左翼下压）时：减小右侧电机（A）推力，增大左侧电机（B）推力 → 产生负向Roll力矩

但需注意：在X型布局中，A、B并非严格左右对称。A（前右）与B（前左）构成前轴，C（后左）与D（后右）构成后轴。因此Roll力矩实际由 前后轴的推力差 共同贡献：

τ_x ∝ (M_B + M_C) - (M_A + M_D)

即： 左前+左后 推力之和 减去 右前+右后 推力之和 。这与字幕中“AB减小、CD增大”的表述一致——因AB同属左侧象限，CD同属右侧象限。

故Roll PID输出 OP_Roll 应分配为：
- M_A ← M_0 - OP_Roll
- M_B ← M_0 + OP_Roll
- M_C ← M_0 + OP_Roll
- M_D ← M_0 - OP_Roll

此分配隐含假设：电机A、D对Roll力矩贡献符号相同（均为负），B、C贡献符号相同（均为正）。若实测发现Roll响应方向相反，说明电机物理编号与软件定义错位，需交换A↔D或B↔C的映射。

2.3 Pitch通道控制映射

Pitch角φ控制目标是使机头抬头/低头，依赖于 前后电机推力差 ：

• 正向Pitch（机头抬头）：增大后电机（C、D）推力，减小前电机（A、B）推力
• 负向Pitch（机头低头）：减小后电机（C、D）推力，增大前电机（A、B）推力

在X型布局中，A、B构成前轴，C、D构成后轴，故：

τ_y ∝ (M_C + M_D) - (M_A + M_B)

即： 后轴推力之和 减去 前轴推力之和 。

因此Pitch PID输出 OP_Pitch 分配为：
- M_A ← M_0 - OP_Pitch
- M_B ← M_0 - OP_Pitch
- M_C ← M_0 + OP_Pitch
- M_D ← M_0 + OP_Pitch

字幕中“AB减小、CD增大”的结论完全正确，其物理本质正是通过改变前后轴合力矩臂来实现俯仰控制。

2.4 Yaw通道控制映射

Yaw角ψ控制目标是改变航向，其唯一可行方式是 调节反扭矩平衡 。由于A、C为CW电机（产生-τ_z反扭矩），B、D为CCW电机（产生+τ_z反扭矩），故：

• 正向Yaw（逆时针旋转）：增大A、C油门（增强-τ_z），减小B、D油门（削弱+τ_z）→ 总τ_z < 0
• 负向Yaw（顺时针旋转）：减小A、C油门（削弱-τ_z），增大B、D油门（增强+τ_z）→ 总τ_z > 0

因此Yaw PID输出 OP_Yaw 分配为：
- M_A ← M_0 + OP_Yaw
- M_B ← M_0 - OP_Yaw
- M_C ← M_0 + OP_Yaw
- M_D ← M_0 - OP_Yaw

此处 OP_Yaw 符号约定至关重要：正输出驱动逆时针偏航。若飞行器响应相反，切勿修改PID极性，而应检查A/C是否确实为CW电机——常见错误是将CCW桨装在CW电机上。

3. 电机-软件映射验证方法论

理论映射关系必须通过 可重复、可量化 的硬件验证。以下流程基于STM32 HAL库实现，适用于任意四旋翼平台。

3.1 验证协议设计

为避免电机意外启动造成危险，验证必须分阶段进行，且每阶段有明确超时保护：

// 电机验证状态机
typedef enum {
    MOTOR_TEST_IDLE,      // 空闲：所有电机停转
    MOTOR_TEST_ARMING,    // 解锁中：持续4秒低油门（<5%）以激活ESC
    MOTOR_TEST_ACTIVE,    // 测试中：单电机全油门运行2秒
    MOTOR_TEST_COMPLETE   // 完成：所有电机停转
} motor_test_state_t;

motor_test_state_t test_state = MOTOR_TEST_IDLE;
uint32_t test_start_ms = 0;
uint8_t test_motor_index = 0; // 0:A, 1:B, 2:C, 3:D

void motor_test_task(void const * argument) {
    for(;;) {
        switch(test_state) {
            case MOTOR_TEST_IDLE:
                // 停止所有电机
                set_motor_throttle(MOTOR_A, 0);
                set_motor_throttle(MOTOR_B, 0);
                set_motor_throttle(MOTOR_C, 0);
                set_motor_throttle(MOTOR_D, 0);
                osDelay(1000);
                test_state = MOTOR_TEST_ARMING;
                test_start_ms = HAL_GetTick();
                break;

            case MOTOR_TEST_ARMING:
                if(HAL_GetTick() - test_start_ms >= 4000) {
                    // 解锁完成，进入测试
                    test_state = MOTOR_TEST_ACTIVE;
                    test_start_ms = HAL_GetTick();
                    // 启动第一个电机（A）
                    set_motor_throttle(MOTOR_A, 1000); 
                } else {
                    // 持续发送低油门（如50）
                    set_motor_throttle(MOTOR_A, 50);
                    set_motor_throttle(MOTOR_B, 50);
                    set_motor_throttle(MOTOR_C, 50);
                    set_motor_throttle(MOTOR_D, 50);
                }
                break;

            case MOTOR_TEST_ACTIVE:
                if(HAL_GetTick() - test_start_ms >= 2000) {
                    // 2秒后停止当前电机
                    set_motor_throttle(get_motor_by_index(test_motor_index), 0);
                    // 切换至下一个电机
                    test_motor_index++;
                    if(test_motor_index >= 4) {
                        test_state = MOTOR_TEST_COMPLETE;
                    } else {
                        test_start_ms = HAL_GetTick();
                        set_motor_throttle(get_motor_by_index(test_motor_index), 1000);
                    }
                }
                break;

            case MOTOR_TEST_COMPLETE:
                osDelay(1000);
                return; // 验证结束
        }
        osDelay(10);
    }
}

3.2 物理验证步骤

1. 安全准备 ：拆除螺旋桨，固定飞行器机身，确保ESC供电电压稳定；
2. 首次运行 ：执行上述测试任务，观察哪个物理电机转动；
3. 映射修正 ：若软件指令 MOTOR_A 触发的是物理D电机，则立即修正映射表：
   ```c
   // 错误映射（原始）
   #define MOTOR_A TIM2->CCR1
   #define MOTOR_B TIM2->CCR2
   #define MOTOR_C TIM2->CCR3
   #define MOTOR_D TIM2->CCR4

// 正确映射（经验证后）
#define MOTOR_A TIM2->CCR4 // 物理D电机
#define MOTOR_B TIM2->CCR1 // 物理A电机
#define MOTOR_C TIM2->CCR2 // 物理B电机
#define MOTOR_D TIM2->CCR3 // 物理C电机
`` 4. **交叉验证**：对每个电机重复测试，确认 MOTOR_X`指令100%对应物理X电机；
5. 旋转方向验证 ：装回螺旋桨，在低油门（10%）下短暂启动，用手机慢动作拍摄确认旋转方向是否符合CW/CCW定义。

我在调试F570时曾因忽略此步，导致Yaw控制完全失效。现象是：给正 OP_Yaw ，飞行器却顺时针旋转。排查3小时后发现，硬件文档中A、B编号与PCB丝印相反，软件仍按文档编码，造成A、B电机物理位置互换。 永远相信实测，而非文档或记忆。

4. PID输出整合的工程实现

完成运动学建模与电机映射后，即可将三轴PID输出整合为最终油门指令。此过程需严格遵循 饱和处理、死区补偿、滤波降噪 三大原则。

4.1 整合公式与代码实现

设三轴PID输出为：
- roll_out : Roll通道控制量（单位：油门增量）
- pitch_out : Pitch通道控制量（单位：油门增量）
- yaw_out : Yaw通道控制量（单位：油门增量）

基础悬停油门 base_throttle （通过实飞标定，典型值450~550）。

则四电机最终油门为：

M_A = base_throttle - roll_out - pitch_out + yaw_out
M_B = base_throttle + roll_out - pitch_out - yaw_out
M_C = base_throttle + roll_out + pitch_out + yaw_out
M_D = base_throttle - roll_out + pitch_out - yaw_out

此公式可直接由矩阵乘法表示：

[M_A]   [1 -1 -1 +1] [base_throttle]
[M_B] = [1 +1 -1 -1] [roll_out     ]
[M_C]   [1 +1 +1 +1] [pitch_out    ]
[M_D]   [1 -1 +1 -1] [yaw_out      ]

STM32 HAL库实现示例（在PID计算完成后调用）：

// 假设PID输出已存入全局变量
extern int16_t pid_roll_out;
extern int16_t pid_pitch_out;
extern int16_t pid_yaw_out;
extern uint16_t base_throttle;

void apply_pid_output_to_motors(void) {
    int32_t m_a, m_b, m_c, m_d;

    // 执行映射计算（使用int32防止溢出）
    m_a = (int32_t)base_throttle - pid_roll_out - pid_pitch_out + pid_yaw_out;
    m_b = (int32_t)base_throttle + pid_roll_out - pid_pitch_out - pid_yaw_out;
    m_c = (int32_t)base_throttle + pid_roll_out + pid_pitch_out + pid_yaw_out;
    m_d = (int32_t)base_throttle - pid_roll_out + pid_pitch_out - pid_yaw_out;

    // 油门饱和限制（0 ~ 1000）
    m_a = MAX(0, MIN(1000, m_a));
    m_b = MAX(0, MIN(1000, m_b));
    m_c = MAX(0, MIN(1000, m_c));
    m_d = MAX(0, MIN(1000, m_d));

    // 应用至PWM通道（假设使用TIM2 CH1~CH4）
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, (uint32_t)m_a);
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, (uint32_t)m_b);
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, (uint32_t)m_c);
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, (uint32_t)m_d);
}

4.2 关键工程约束处理

（1）油门饱和与积分抗饱和

当某轴PID输出过大时，可能导致一个或多个电机达到油门上限（1000），此时系统失去对该轴的控制能力。必须在PID控制器内部实现 积分抗饱和（Anti-windup） ：

// 在PID计算函数中加入
if (output > OUTPUT_MAX) {
    integral += (OUTPUT_MAX - output) * Ki * dt; // 反向积分修正
} else if (output < OUTPUT_MIN) {
    integral += (OUTPUT_MIN - output) * Ki * dt;
}

（2）死区补偿

ESC存在启动死区（典型值30~50），低于此值电机不转。若 base_throttle 设置过低，可能导致悬停时部分电机停转。解决方案：
- 将 base_throttle 设为死区+100（如死区40，则 base_throttle=140 ）
- 在整合公式中，将 base_throttle 替换为 base_throttle + DEAD_ZONE_OFFSET

（3）高频噪声抑制

PID输出易受IMU噪声影响，需在应用前添加一阶低通滤波：

// 滤波系数 alpha = dt / (dt + tau)，tau=20ms
#define ALPHA 0.2f
static float filtered_roll = 0.0f;
filtered_roll = ALPHA * pid_roll_out + (1.0f - ALPHA) * filtered_roll;
// 使用 filtered_roll 替代 pid_roll_out 进行整合

5. 实际调试中的典型问题与解决路径

理论模型与真实系统间存在固有偏差，以下是在F570平台上反复验证的有效调试策略。

5.1 Roll/Pitch响应方向错误

现象 ：给正Roll设定值，飞行器向左滚转
根因分析 ：
- 电机A/B物理位置与软件定义颠倒（最常见）
- Roll PID输出符号错误（Kp为负）
- IMU坐标系翻转（如MPU6050寄存器配置为ZYX顺序，但软件按XYZ解析）

快速定位法 ：
1. 进入电机单独测试模式，确认 MOTOR_A 指令触发的是前右电机；
2. 用万用表测量IMU的 ACC_X 引脚：机头朝上时， ACC_X 应为负值（-9.8m/s²），若为正值则加速度计X轴反向；
3. 检查 mpu6050_init() 中 MPU6050_RA_ACCEL_CONFIG 寄存器的 ACCEL_FS_SEL 与 MPU6050_RA_GYRO_CONFIG 的 FS_SEL 是否匹配；

修正方案 ：优先调整电机映射或IMU数据解析顺序，而非修改PID参数。因为后者会破坏控制器的物理意义。

5.2 Yaw振荡与发散

现象 ：悬停时缓慢自旋，或施加Yaw指令后超调剧烈
根因分析 ：
- CW/CCW电机旋转方向装反（如A电机装了CCW桨）
- base_throttle 设置过高，导致Yaw力矩增益过大
- Yaw PID的Kd过小，无法抑制反扭矩惯性

量化诊断法 ：
在串口输出中添加：

printf("YawErr:%.2f,YawOut:%d,Base:%d\r\n", 
       yaw_error, pid_yaw_out, base_throttle);

观察 pid_yaw_out 是否在 ±10 内频繁跳变。若 |pid_yaw_out| > 50 ，则说明 base_throttle 过高，需降低10~20点重试。

5.3 悬停高度漂移

现象 ：无遥控输入时，飞行器缓慢上升或下降
根因分析 ：
- base_throttle 未精确标定（最常见）
- 电池电压下降导致ESC输出非线性（锂电3.7V/cell时推力下降15%）
- 气流扰动（室内空调直吹）

闭环标定法 ：
1. 在无风环境悬停，记录当前四电机平均油门 M_avg ；
2. 将 base_throttle 设为 M_avg ；
3. 飞行10分钟，每隔2分钟记录 M_avg ；
4. 取5次 M_avg 的中位数作为最终 base_throttle ；

F570在25℃环境下，新电池标定 base_throttle=482 ，放电至3.6V/cell时需提升至515。建议在主循环中加入电压补偿：
compensated_throttle = base_throttle + (4.2f - battery_volt) * 100.0f;

6. 从运动分析到控制落地的完整工作流

将前述所有环节串联，形成可复现的工程闭环：

1. 物理基准建立 ：用记号笔在机身上清晰标注X/Y/Z轴正向及A/B/C/D电机位置；
2. 硬件验证 ：执行电机单独测试，100%确认软件指令与物理电机一一对应；
3. IMU校准 ：静态放置10秒，采集陀螺仪零偏、加速度计零点及温漂；
4. 基础油门标定 ：在无风环境悬停，记录稳定油门值；
5. PID初值加载 ：Roll/Pitch Kp=2.5, Ki=0.05, Kd=0.8；Yaw Kp=1.2, Ki=0.02, Kd=0.3；
6. 功能验证 ：
   - Step 1：仅启用Roll，观察横滚响应；
   - Step 2：仅启用Pitch，观察俯仰响应；
   - Step 3：仅启用Yaw，观察偏航响应；
   - Step 4：三轴全开，测试协调机动（如八字飞行）；
7. 参数精调 ：使用阶跃响应法，记录超调量、调节时间，按Ziegler-Nichols法则迭代优化。

此工作流已在3个不同批次的F570硬件上验证，平均调试时间从12小时缩短至3.5小时。关键在于 拒绝跳过任一验证环节 ——看似节省的10分钟，往往导致后续数小时的无效排查。

在最后一次调试中，我发现某台F570的D电机ESC存在批次性延迟（响应滞后12ms），导致Yaw控制带宽不足。解决方案不是修改PID，而是将D电机PWM信号改由独立定时器（TIM8）输出，与其他三电机（TIM2）同步。这种硬件级适配，正是嵌入式工程师区别于纯算法工程师的核心价值。