1. 基于PID的机器人关节速度控制

任务目标

实现一个离散PID控制器，驱动机器人单关节模型跟踪期望速度曲线。通过调节比例、积分、微分增益，观察并分析跟踪性能（超调、稳态误差、响应时间）的变化。

机器人关节模型

采用一阶惯性模型近似关节速度响应：

τ = J·(dv/dt) + b·v

其中J为转动惯量（0.5 kg·m²），b为粘性摩擦系数（1.0 N·m·s/rad），τ为控制力矩，v为关节速度。使用前向欧拉法离散化，采样周期dt = 0.01s。

完整实现代码

import numpy as np
import matplotlib.pyplot as plt

# ---------- 机器人关节模型 ----------
class JointModel:
    def __init__(self, J=0.5, b=1.0, dt=0.01):
        self.J = J
        self.b = b
        self.dt = dt
        self.v = 0.0

    def step(self, torque):
        # 前向欧拉离散：v_{k+1} = v_k + (dt/J)·(τ - b·v_k)
        dv = (self.dt / self.J) * (torque - self.b * self.v)
        self.v += dv
        return self.v

# ---------- PID 控制器 ----------
class PIDController:
    def __init__(self, Kp, Ki, Kd, dt, output_limit=50.0):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.dt = dt
        self.limit = output_limit
        self.integral = 0.0
        self.prev_error = 0.0

    def compute(self, target, current):
        error = target - current
        self.integral += error * self.dt
        derivative = (error - self.prev_error) / self.dt
        self.prev_error = error

        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        # 力矩限幅
        return np.clip(output, -self.limit, self.limit)

    def reset(self):
        self.integral = 0.0
        self.prev_error = 0.0

# ---------- 仿真 ----------
def simulate(Kp, Ki, Kd, target_profile, dt=0.01, total_time=5.0):
    model = JointModel(dt=dt)
    pid = PIDController(Kp, Ki, Kd, dt)
    steps = int(total_time / dt)
    time = np.linspace(0, total_time, steps)
    v_actual = []
    v_target = []

    for k in range(steps):
        # 获取当前时刻期望速度（阶跃或正弦）
        if target_profile == 'step':
            target = 2.0 if time[k] < 2.0 else 1.0   # 2s时从2降为1
        elif target_profile == 'sine':
            target = 1.0 + 0.5 * np.sin(2 * np.pi * 0.5 * time[k])
        else:
            raise ValueError('Unknown profile')

        torque = pid.compute(target, model.v)
        actual = model.step(torque)
        v_actual.append(actual)
        v_target.append(target)

    return time, np.array(v_target), np.array(v_actual)

# ---------- 运行与绘图 ----------
if __name__ == '__main__':
    # 参数组1：基准
    t, v_ref, v_out = simulate(20.0, 5.0, 0.5, 'step')
    # 参数组2：高Kp
    t2, v_ref2, v_out2 = simulate(60.0, 5.0, 0.5, 'step')
    # 参数组3：高Ki
    t3, v_ref3, v_out3 = simulate(20.0, 30.0, 0.5, 'step')

    plt.figure(figsize=(10, 5))
    plt.plot(t, v_ref, 'k--', linewidth=2, label='Reference')
    plt.plot(t, v_out, 'b-', label='Kp=20, Ki=5, Kd=0.5')
    plt.plot(t2, v_out2, 'r-', label='Kp=60, Ki=5, Kd=0.5')
    plt.plot(t3, v_out3, 'g-', label='Kp=20, Ki=30, Kd=0.5')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity (rad/s)')
    plt.legend()
    plt.grid(True)
    plt.title('PID Joint Velocity Tracking - Step Response')
    plt.savefig('pid_step_comparison.png', dpi=100)
    plt.show()

    # 正弦跟踪测试
    t4, v_ref4, v_out4 = simulate(25.0, 8.0, 0.3, 'sine')
    plt.figure(figsize=(10, 4))
    plt.plot(t4, v_ref4, 'k--', label='Reference')
    plt.plot(t4, v_out4, 'm-', label='PID Tracking')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity (rad/s)')
    plt.legend()
    plt.grid(True)
    plt.title('PID Sine Wave Tracking')
    plt.savefig('pid_sine_tracking.png', dpi=100)
    plt.show()

运行方式

pip install numpy matplotlib
python pid_velocity_control.py

验证方法与判定标准

• 阶跃响应观察：程序输出pid_step_comparison.png，对比三条曲线。基准参数（Kp=20）应无稳态误差，约0.5s内到达目标速度；高Kp（60）曲线应出现明显超调（峰值超过目标值）；高Ki（30）曲线应出现振荡或更长的调节时间。
• 正弦跟踪观察：pid_sine_tracking.png中，PID输出曲线应基本跟随参考正弦，存在小幅相位滞后（约0.1~0.3s），幅值略有衰减。
• 定量判定：可在代码中增加以下计算来量化性能——

# 在simulate返回后添加：
def compute_metrics(t, ref, out, t_start=2.0):
    mask = t >= t_start
    steady_err = np.mean(np.abs(ref[mask] - out[mask]))
    overshoot = 0.0
    # 找第一次到达目标后的最大值
    idx = np.argmax(out[t < 2.0])  # 阶跃上升段
    overshoot = max(0, (out[idx] - 2.0) / 2.0 * 100)
    return steady_err, overshoot

# 示例调用（取消注释即可使用）：
# err1, ov1 = compute_metrics(t, v_ref, v_out)
# print(f'Baseline: steady_err={err1:.4f}, overshoot={ov1:.1f}%')

判定通过条件：基准参数稳态误差<0.05 rad/s，高Kp组超调>10%，高Ki组出现振荡（速度曲线过零至少一次）。

参数影响分析要点

常见问题

• 积分饱和：当误差持续较大时，积分项累积过大导致输出饱和。可在PID类中增加积分限幅（如self.integral = np.clip(self.integral, -10, 10)）。
• 微分噪声：实际系统中速度测量含噪声时，微分项会放大噪声。可对测量值先做低通滤波，或使用“测量微分”代替“误差微分”。
• 采样周期影响：dt增大时，相同PID参数下系统稳定性下降。若修改dt，需重新整定参数。