📚 课程目录第3章 PID位置控制实现

3. PID位置控制实现

任务目标

实现一个用于AGV单轴运动的PID位置控制器，输入目标位置与当前实际位置，输出速度指令，使AGV能够准确、平稳地到达目标点。本案例聚焦于控制器核心逻辑与仿真验证，不涉及具体电机驱动硬件接口。

核心实现

# pid_position_controller.py
import time

class PIDPositionController:
    def __init__(self, kp=2.0, ki=0.1, kd=0.5, dt=0.02, output_limit=1.0, integral_limit=0.5):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.dt = dt
        self.output_limit = output_limit
        self.integral_limit = integral_limit

        self._integral = 0.0
        self._prev_error = 0.0

    def reset(self):
        """复位控制器内部状态（积分项与上次误差）"""
        self._integral = 0.0
        self._prev_error = 0.0

    def compute(self, target_pos, current_pos):
        """
        计算速度指令（单位：m/s）
        :param target_pos: 目标位置（m）
        :param current_pos: 当前实际位置（m）
        :return: 速度指令（m/s），范围被限制在 [-output_limit, output_limit]
        """
        error = target_pos - current_pos

        # 比例项
        p_term = self.kp * error

        # 积分项（带限幅，防止积分饱和）
        self._integral += error * self.dt
        self._integral = max(-self.integral_limit, min(self.integral_limit, self._integral))
        i_term = self.ki * self._integral

        # 微分项（对误差求导，等效于对位置求负导数）
        d_term = self.kd * (error - self._prev_error) / self.dt
        self._prev_error = error

        # 合成输出并限幅
        output = p_term + i_term + d_term
        output = max(-self.output_limit, min(self.output_limit, output))
        return output

# 最小仿真验证闭环
if __name__ == "__main__":
    # 仿真参数
    SIM_DT = 0.02          # 控制周期（s）
    TARGET_POS = 2.0       # 目标位置（m）
    INITIAL_POS = 0.0      # 初始位置（m）
    MAX_SPEED = 0.5        # AGV最大速度（m/s），用于模拟执行器限幅

    # 创建控制器（使用与仿真一致的dt）
    pid = PIDPositionController(kp=2.0, ki=0.1, kd=0.5, dt=SIM_DT, output_limit=MAX_SPEED)

    # 模拟AGV运动学：位置 += 速度 * dt（理想积分模型）
    pos = INITIAL_POS
    sim_time = 0.0
    MAX_SIM_TIME = 10.0    # 最长仿真时间（s）

    print(f"仿真开始：目标位置={TARGET_POS}m，初始位置={INITIAL_POS}m")
    print("时间(s)  位置(m)  速度指令(m/s)  误差(m)")

    while sim_time < MAX_SIM_TIME:
        # 1. 控制器计算速度指令
        vel_cmd = pid.compute(TARGET_POS, pos)

        # 2. 模拟AGV执行（理想速度源，直接积分）
        pos += vel_cmd * SIM_DT

        # 3. 记录与输出（每0.2秒打印一次）
        if int(sim_time / 0.2) != int((sim_time - SIM_DT) / 0.2):
            print(f"{sim_time:6.2f}  {pos:6.3f}  {vel_cmd:8.3f}  {TARGET_POS - pos:6.3f}")

        # 4. 判断是否到达（误差小于1mm视为到位）
        if abs(TARGET_POS - pos) < 0.001:
            print(f"\n到达目标！用时 {sim_time:.2f}s，最终位置 {pos:.4f}m")
            break

        sim_time += SIM_DT
    else:
        print(f"\n仿真超时（{MAX_SIM_TIME}s），最终位置 {pos:.4f}m，误差 {TARGET_POS - pos:.4f}m")

运行与验证

运行方式：直接执行Python脚本：

python pid_position_controller.py

通过条件：观察输出，满足以下两点即判定控制器实现正确：

位置从0m逐渐增大并最终稳定在2.0m（误差<1mm），且无超调或仅有微小超调（<5%）；
速度指令始终被限制在±0.5m/s范围内，且到达目标后速度指令趋近于0。

关键说明

控制器采用位置式PID，输出为速度指令，适用于AGV底层速度闭环的执行器架构。
积分限幅（integral_limit）用于防止积分饱和导致的超调；输出限幅（output_limit）对应AGV实际最大速度。
仿真中采用理想积分模型模拟AGV运动，实际部署时需将vel_cmd发送至电机驱动器，并通过编码器/激光里程计获取current_pos反馈。
若实际系统存在较大惯性或延迟，可适当增大kd抑制超调，或减小kp提高稳定性。