4. PID离散化与实现

在嵌入式飞控系统中，被控对象（如电机、舵面）和执行机构（如PWM驱动的电调、伺服器）本质上是离散时间系统——飞控主控（MCU）以固定周期（如1kHz、500Hz）读取传感器数据、运行控制律、输出控制量。因此，连续域的PID公式必须经过离散化处理才能在MCU上运行。本章将深入讲解位置式PID与增量式PID的数学推导、采样时间的选择依据，并给出可直接落地的C语言实现框架。理解这一章，是后续进行PID参数整定和代码优化的基础。

4.1 连续PID的离散化数学基础

核心原理：从微分方程到差分方程

连续PID控制律的标准形式为：

u(t) = Kp·e(t) + Ki·∫₀ᵗ e(τ)dτ + Kd·de(t)/dt

其中，Kp为比例系数，Ki为积分系数，Kd为微分系数。在MCU中，我们无法直接计算积分和微分，必须将其转化为对误差序列e(k)的代数运算。离散化的核心思想是用数值积分近似连续积分，用差分近似微分。最常用的方法是前向欧拉法（又称矩形法），其近似关系为：

• 积分项：∫₀ᵗ e(τ)dτ ≈ T·Σᵢ₌₀ᵏ e(i)，其中T为采样周期。
• 微分项：de(t)/dt ≈ [e(k) - e(k-1)] / T。

将上述近似代入连续PID公式，得到位置式PID的离散表达式：

u(k) = Kp·e(k) + Ki·T·Σᵢ₌₀ᵏ e(i) + Kd·[e(k) - e(k-1)] / T

为了编程方便，通常将Ki·T合并为新的积分系数Ki'，将Kd/T合并为新的微分系数Kd'，即：

u(k) = Kp·e(k) + Ki'·Σᵢ₌₀ᵏ e(i) + Kd'·[e(k) - e(k-1)]

其中，Ki' = Ki·T，Kd' = Kd/T。这一转换看似简单，但隐含了采样时间T对控制性能的深刻影响——T越小，离散化误差越小，但计算频率和传感器噪声的影响会增大。

位置式PID与增量式PID的对比

位置式PID直接输出控制量的值u(k)，其优点是物理意义明确，适用于执行机构需要位置控制的场景（如舵机角度）。但它的缺点是：一旦积分项累积过大，输出可能饱和，且当系统出现故障时，输出突变可能造成危险。增量式PID则输出控制量的增量Δu(k)：

Δu(k) = u(k) - u(k-1)
       = Kp·[e(k) - e(k-1)] + Ki'·e(k) + Kd'·[e(k) - 2e(k-1) + e(k-2)]

增量式PID的输出是相对变化量，因此具有以下优势：

• 抗积分饱和：即使长时间存在偏差，输出增量也不会无限累积，因为积分项被隐含在增量计算中。
• 故障安全性：当执行机构（如电调）接收增量信号时，即使控制器输出异常，执行机构也只会响应增量变化，不会跳变到极端位置。
• 易于实现手动/自动切换：在飞控中，从手动模式切换到自动模式时，只需将当前输出作为基准，增量式PID不会产生冲击。

案例拆解：四旋翼滚转通道的PID形式选择

以四旋翼的滚转通道为例，飞控输出的是期望滚转力矩，最终通过混控器转换为四个电机的PWM占空比。若采用位置式PID，当飞控上电瞬间，积分项从零开始累积，若此时存在较大初始偏差（如飞机倾斜），积分项会迅速增大，导致电机转速瞬间拉满，可能造成飞机翻转。而采用增量式PID，输出的是每个控制周期内PWM占空比的增量，即使初始偏差很大，第一个周期的输出增量也仅为Kp·e(0)，不会产生极端输出。因此，在飞控的角速度内环和姿态外环中，增量式PID是更安全、更常用的选择。

4.2 采样时间T的选择策略

核心原理：采样时间对系统性能的影响

采样时间T是离散化过程中最重要的参数之一。从香农采样定理出发，采样频率fs必须大于被控对象闭环带宽的2倍（工程上通常取5~10倍）。对于飞控系统，典型的被控对象是电机和螺旋桨，其机械时间常数约为10~50ms，对应的带宽约为20~100Hz。因此，飞控的采样频率通常设置在500Hz~2kHz之间。若T过大，系统会丢失高频动态信息，导致控制滞后；若T过小，MCU的计算负担加重，且传感器噪声会被微分项放大。

判断方法：如何确定合适的采样时间

1. 分析执行机构带宽：查阅电机、舵机的响应带宽，确保fs ≥ 10×f_bandwidth。例如，某舵机带宽为50Hz，则采样频率至少为500Hz。
2. 考虑传感器更新率：IMU（惯性测量单元）的更新率通常为1kHz~8kHz，飞控的PID控制频率应与之匹配或为其整数分频。
3. 评估MCU计算余量：在STM32F4（168MHz）上，运行一次增量式PID约需2~5μs，若控制频率为1kHz，则CPU占用率仅为0.2%~0.5%，余量充足。
4. 实测系统延迟：通过注入阶跃信号，观察系统响应，若出现明显振铃或相位滞后，应适当减小T。

实战场景：多旋翼飞控的采样时间配置

以常见的Pixhawk飞控为例，其默认的PID控制频率为：角速度环400Hz，姿态环250Hz，位置环50Hz。这种多速率设计是合理的——内环需要更高的带宽来抑制扰动，外环对实时性要求较低。若将角速度环频率提升到1kHz，虽然理论上性能更好，但会占用更多CPU时间，且对IMU数据质量要求更高。实际调试中，若发现电机出现高频啸叫，往往是采样频率过高导致微分项放大噪声，此时应适当降低采样频率或增加低通滤波。

4.3 C语言实现框架：从结构体到控制函数

核心原理：面向对象的C语言设计

在嵌入式飞控中，通常需要同时管理多个PID控制器（滚转、俯仰、偏航、高度等）。为了提高代码复用性和可维护性，推荐使用结构体封装PID参数和状态。以下是一个典型的增量式PID结构体定义：

typedef struct {
    float Kp;           // 比例系数
    float Ki;           // 积分系数（已包含T）
    float Kd;           // 微分系数（已包含T）
    float out_limit;    // 输出限幅（值）
    float integral_limit; // 积分限幅（仅位置式需要）
    float err[3];       // 误差历史：err[0]=e(k), err[1]=e(k-1), err[2]=e(k-2)
    float output;       // 当前输出（位置式）或增量（增量式）
} PID_TypeDef;

操作流程：增量式PID的C语言实现

增量式PID的计算函数如下：

float PID_Incremental_Calc(PID_TypeDef *pid, float target, float feedback) {
    // 1. 计算当前误差
    pid->err[2] = pid->err[1];
    pid->err[1] = pid->err[0];
    pid->err[0] = target - feedback;
    
    // 2. 计算增量
    float d_out = pid->Kp * (pid->err[0] - pid->err[1])
                + pid->Ki * pid->err[0]
                + pid->Kd * (pid->err[0] - 2*pid->err[1] + pid->err[2]);
    
    // 3. 增量限幅
    if (d_out > pid->out_limit) d_out = pid->out_limit;
    if (d_out < -pid->out_limit) d_out = -pid->out_limit;
    
    // 4. 累加输出（注意：此处输出的是控制量，需外部保存）
    pid->output += d_out;
    
    // 5. 输出限幅
    if (pid->output > pid->out_limit) pid->output = pid->out_limit;
    if (pid->output < -pid->out_limit) pid->output = -pid->out_limit;
    
    return pid->output;
}

位置式PID的实现则需单独维护积分累加项：

float PID_Positional_Calc(PID_TypeDef *pid, float target, float feedback) {
    float err = target - feedback;
    pid->integral += err;  // 积分累加
    // 积分限幅
    if (pid->integral > pid->integral_limit) pid->integral = pid->integral_limit;
    if (pid->integral < -pid->integral_limit) pid->integral = -pid->integral_limit;
    
    float d_out = pid->Kp * err + pid->Ki * pid->integral + pid->Kd * (err - pid->err[0]);
    pid->err[0] = err;
    
    // 输出限幅
    if (d_out > pid->out_limit) d_out = pid->out_limit;
    if (d_out < -pid->out_limit) d_out = -pid->out_limit;
    return d_out;
}

案例拆解：代码中的抗积分饱和与微分滤波

在实际飞控中，仅靠上述基础实现是不够的。以下是一个增强型实现的关键点：

• 微分滤波：直接使用误差差分会放大高频噪声，通常对微分项进行一阶低通滤波：d_term = (1-α)·d_term_prev + α·Kd·(e(k)-e(k-1))，其中α = T/(T+τ)，τ为滤波时间常数。
• 积分分离：当误差超过阈值时，暂停积分累加，防止积分饱和。例如，当|err| > 50°时，不进行积分。
• 输出平滑：在增量式PID中，对最终输出进行斜坡限幅，避免控制量突变。

4.4 常见误区与总结复盘

误区一：采样时间随意设置

很多初学者直接使用1ms或10ms作为采样周期，而不考虑被控对象的动态特性。这可能导致控制效果不佳，甚至系统不稳定。正确做法是：先估算系统带宽，再确定采样频率，最后通过实验微调。

误区二：位置式PID和增量式PID混用

在同一个控制回路中，不能同时使用两种形式的PID。例如，角速度环使用增量式，而姿态环使用位置式，两者之间的接口需要明确：姿态环输出的是期望角速度（作为角速度环的目标值），角速度环输出的是控制增量。如果混用，会导致控制量计算混乱。

误区三：忽略积分限幅

位置式PID中，积分项无限累积会导致输出饱和，即“积分饱和”现象。在飞控中，这表现为电机长时间满油门，无法响应反向控制。解决方法是：在积分累加后立即进行限幅，或者使用“积分分离”策略。

总结：离散化实现的要点清单

离散化是连接控制理论与嵌入式实现的桥梁。掌握位置式与增量式的区别、采样时间的选择方法以及C语言的工程化实现，是飞控开发的基本功。在实际项目中，建议先在仿真环境中验证离散化算法的正确性，再移植到硬件上，并通过飞行日志反复迭代优化。