3. 农机作业速度闭环控制实现

速度闭环的控制对象与信号链

农机作业速度闭环的核心，是把“目标速度”与“实际速度”的偏差转化为油门开度指令。作业场景中，土壤阻力、坡度、农具负载会持续扰动车速，因此必须依靠反馈而非固定油门开环控制。典型信号链为：目标速度设定 → 偏差计算 → PID 控制器 → 油门执行器 → 发动机/传动系统 → 实际车速 → 速度传感器反馈。

本例采用离散位置式 PID，控制周期取 100 ms（10 Hz），与常见农机控制器任务周期一致。速度单位统一为 km/h，油门开度归一化为 0~100%。积分项需做限幅（抗积分饱和），输出也需限幅到执行器物理范围。

离散 PID 实现

下面给出可直接运行的 Python 实现（仅依赖标准库），包含积分抗饱和与输出限幅。控制器对象按控制周期调用 update()，返回油门开度百分比。

class SpeedPID:
    def __init__(self, kp, ki, kd, ts=0.1, out_min=0.0, out_max=100.0):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.ts = ts
        self.out_min, self.out_max = out_min, out_max
        self.integral = 0.0
        self.prev_err = 0.0

    def update(self, v_ref, v_fb):
        err = v_ref - v_fb
        # 积分累加（先累加，后限幅）
        self.integral += err * self.ts
        deriv = (err - self.prev_err) / self.ts
        out = self.kp * err + self.ki * self.integral + self.kd * deriv
        # 输出限幅 + 抗积分饱和（回退积分）
        if out > self.out_max:
            out = self.out_max
            self.integral -= err * self.ts
        elif out < self.out_min:
            out = self.out_min
            self.integral -= err * self.ts
        self.prev_err = err
        return out

被控对象用一个简化的一阶惯性模型模拟：油门开度经时间常数 τ 影响车速，并叠加负载扰动。该模型仅用于教学对比，不代表真实发动机标定数据。

def simulate(kp, ki, kd, v_ref=8.0, steps=300, ts=0.1, tau=1.5, gain=0.12, disturb=0.0):
    pid = SpeedPID(kp, ki, kd, ts)
    v = 0.0
    log = []
    for k in range(steps):
        u = pid.update(v_ref, v)
        # 一阶惯性：dv/dt = (gain*u - v)/tau + 扰动
        dv = (gain * u - v) / tau + disturb
        v += dv * ts
        log.append((k * ts, v_ref, v, u))
    return log

不同 PID 参数的跟踪对比

设定目标速度 8 km/h，从 0 起步，运行 30 s。三组参数分别代表欠阻尼、临界、过阻尼倾向。下表为示例仿真得到的定性指标（上升时间、超调、稳态误差），用于说明参数影响规律，实际数值随对象模型变化。

规律：增大 Kp 加快响应但易超调；Ki 消除稳态误差但过大会引起积分饱和与振荡；Kd 抑制超调但对速度反馈噪声敏感，农机振动环境下不宜过大。工程上建议先调 Kp 到临界振荡再回退，再加 Ki 消差，最后小幅加 Kd 抑制超调。