4. 农具悬挂位置伺服控制

悬挂位置伺服的任务拆解

农具悬挂位置伺服的目标是让农具实际耕深跟随驾驶室设定的目标耕深。系统由三部分构成：设定值（目标耕深）、执行器（电液比例换向阀驱动提升油缸）、反馈环节（安装在提升臂或下拉杆上的角度/位移传感器）。控制器比较目标位置与反馈位置得到偏差，经比例-积分环节输出阀芯开度指令，油缸推动悬挂机构运动，直到偏差落入死区。

工程上需要先做两件事：一是把传感器电压标定成耕深（或提升臂角度），二是确定阀的中位死区与最小可控开度。下面给出一个可直接运行的离散位置伺服实现，包含标定、带死区补偿的PI控制、以及不同负载下的跟踪验证。

标定与控制器实现

传感器输出为提升臂角度对应的电压，先做两点标定：把农具放到最低位（耕深 0 mm）记录电压 V0，放到已知耕深 h1（例如 250 mm）记录电压 V1，则耕深换算为线性映射。控制器采用离散 PI，输出为阀开度百分比，并加入中位死区补偿：当计算出的开度绝对值小于死区阈值时，直接置零，避免阀在中位附近抖动。

import numpy as np

class HitchPositionServo:
    def __init__(self, kp=0.9, ki=0.35, dt=0.02,
                 deadband=3.0, u_min=-100.0, u_max=100.0):
        self.kp, self.ki, self.dt = kp, ki, dt
        self.deadband = deadband
        self.u_min, self.u_max = u_min, u_max
        self.integ = 0.0

    def calibrate(self, v0, h0, v1, h1):
        # 电压 -> 耕深 线性标定
        self.k_h = (h1 - h0) / (v1 - v0)
        self.b_h = h0 - self.k_h * v0

    def read_depth(self, voltage):
        return self.k_h * voltage + self.b_h

    def reset(self):
        self.integ = 0.0

    def update(self, h_ref, h_fb):
        e = h_ref - h_fb
        # 积分限幅，防止饱和
        self.integ += e * self.dt
        self.integ = np.clip(self.integ, -200.0, 200.0)
        u = self.kp * e + self.ki * self.integ
        u = np.clip(u, self.u_min, self.u_max)
        # 中位死区补偿
        if abs(u) < self.deadband:
            u = 0.0
        return u

不同负载下的跟踪验证

用一阶加纯延迟的简化模型代替真实液压系统做教学验证：油缸速度与阀开度成正比，负载越大等效增益越低、响应越慢。设定目标耕深从 120 mm 阶跃到 200 mm，分别在轻载（增益 1.0）和重载（增益 0.6）下运行，观察稳态误差与调节时间。

def simulate(load_gain, kp=0.9, ki=0.35, T=6.0, dt=0.02):
    servo = HitchPositionServo(kp=kp, ki=ki, dt=dt)
    servo.calibrate(v0=0.5, h0=0.0, v1=4.5, h1=250.0)
    h = 120.0          # 初始耕深 mm
    h_ref = 200.0      # 目标耕深 mm
    tau = 0.35         # 液压环节时间常数 s
    log = []
    for k in range(int(T / dt)):
        u = servo.update(h_ref, h)
        # 阀开度 -> 油缸速度（含负载增益与一阶惯性）
        v_cmd = load_gain * u * 0.9      # mm/s per % 开度
        h += (v_cmd - (h - 120.0) / tau) * dt
        log.append((k * dt, h, u))
    return np.array(log)

light = simulate(load_gain=1.0)
heavy = simulate(load_gain=0.6)

def metrics(log, h_ref=200.0, band=2.0):
    t, h, u = log[:, 0], log[:, 1], log[:, 2]
    err = np.abs(h - h_ref)
    idx = np.where(err <= band)[0]
    settle = t[idx[0]] if len(idx) else float('nan')
    return h[-1], h_ref - h[-1], settle

print("轻载 终值/稳态误差/调节时间:", metrics(light))
print("重载 终值/稳态误差/调节时间:", metrics(heavy))

预期现象（教学示例，非实测）：轻载下约 1.2 s 内进入 ±2 mm 带，稳态误差接近 0；重载下等效增益下降，调节时间明显变长，若积分增益不足会残留数毫米稳态误差。此时可适当提高 Ki 或对阀开度做负载前馈补偿。若重载下出现持续振荡，说明 Kp 偏大，应降低比例增益并保留死区补偿。