Python PID控制器实现求助：基于时间调度的机器人任务代码审核与逻辑解惑

我这个项目的目标是做一个能在预定时间内完成路线的机器人。我尝试实现了一个PID控制器，还写了Python脚本测试代码。现在我用time.sleep()代替机器人控制逻辑，PID控制器会调整sleep时间来让机器人按时完成指令列表。
有没有人能帮我看看这段代码写得有多糟糕？我完全搞不懂这里面的数学逻辑，已经花了超多时间在上面了，提前谢谢大家！

提问者提供的代码 #

import re
import time
import random
import matplotlib.pyplot as plt
import numpy as np
#from buildhat import MotorPair
probe_distance = 10  # distance of probe from center in mm
average_time = 6  # average time of the robot per function

class PID:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.last_error = 0
        self.integral = 0

    def update(self, error, dt):
        derivative = (error - self.last_error) / dt
        self.integral += error * dt
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.last_error = error
        return output
    

class Move():
    def __init__(self, time_target, file_path, motor_a, motor_b):
        with open(file_path, 'r') as f:
            self.file = f.readlines()
        self.time_target = time_target
        self.motor_a = motor_a
        self.motor_b = motor_b
        self.pid = PID(Kp=1.0, Ki=0.1, Kd=0.05)
        #self.pair = MotorPair(motor_a, motor_b)
        

    def run(self):
        self.start_time = time.time()
        #self.pair.set_default_speed(50) # this is the default speed of the robot
        # The Pid controller will determine the speed of the robot based on the time it is taking to complete the tasks
        m.xp = []
        m.yp = []
        i = 0
        adjustment = 1
        last_time_remaining = self.time_target
        while True:
            try:
                line = self.file[i]
            except IndexError:
                break

            # Execute command
             

            time.sleep((60/len(self.file))-(60/len(self.file)*adjustment))



            line = line.strip().removesuffix('\n').lower()
            fchar = line[0]

            time_remaining = self.time_target - (time.time() - self.start_time )
            target = (self.time_target / len(self.file)) * (i + 1)
            error = (self.time_target - target) - time_remaining # error = target - actual
            dt = time_remaining / last_time_remaining
            last_time_remaining = time_remaining
            adjustment = self.pid.update(error, dt)
            self.yp.append(error)
            self.xp.append(time.time() - self.start_time)
            print(f'Error: {error}, Adjustment: {adjustment}')

            




# 1. Initialize the system
m = Move(60, 'sequence.txt', 'C', 'D')

######################
###  Begin Course  ###
######################

# 2. Run the system
m.run()
print(m.xp)
print(m.yp)
xpoints = np.array(m.xp)
ypoints = np.array(m.yp)
plt.plot(xpoints, ypoints)
plt.show()

嘿，先给你梳理下这段代码里的关键问题，再帮你把PID的时间调度逻辑掰明白～

一、核心逻辑的几个致命问题 #

1. 误差计算完全搞反了

你现在的error = (self.time_target - target) - time_remaining逻辑完全绕晕了，咱们得回到PID的本质：误差是「目标状态 - 实际状态」。

• 对这个任务来说，每完成第i+1个步骤，理论上应该已经用了 target_elapsed = (self.time_target / len(self.file)) * (i + 1) 秒
• 而实际已经用了 actual_elapsed = time.time() - self.start_time 秒
• 正确的误差应该是 error = target_elapsed - actual_elapsed：
  • 如果error为正：实际用时比目标少，进度超前，得调大sleep时间放慢
  • 如果error为负：实际用时比目标多，进度落后，得调小sleep时间赶进度

你现在的误差计算会让PID的调整方向完全错乱，这是最核心的问题。

2. DT（时间差）的计算完全错误

PID的导数项依赖两次更新之间的真实时间间隔，你现在用dt = time_remaining / last_time_remaining，这根本不是时间差，而是剩余时间的比例，会直接导致导数项彻底失真，PID完全没法正常工作。
正确的做法是记录每次PID计算的时间，直接算两次的差值：

# 在run方法开头初始化
last_update_time = time.time()
# 循环里计算DT
current_time = time.time()
dt = current_time - last_update_time
last_update_time = current_time

3. Sleep时间的计算逻辑混乱还容易出问题

你现在的time.sleep((60/len(self.file))-(60/len(self.file)*adjustment))不仅绕，还很容易算出负数sleep时间（比如adjustment大于1的时候），直接报错。
应该换成更直白的逻辑：

• 先算每个步骤的基准sleep时间：base_sleep = self.time_target / len(self.file)
• 用PID的调整量去修正基准时间：actual_sleep = base_sleep + adjustment
• 加个边界限制防止异常：actual_sleep = max(0.1, actual_sleep)（不能小于0.1秒，避免无意义的极短sleep）

4. 积分项没有抗饱和（Integral Windup）

当PID的输出被限制（比如sleep时间不能小于0），积分项会一直累积错误，导致后续出现严重超调。你得给积分项加个范围限制，比如在PID的update方法里：

self.integral = max(min(self.integral, 10), -10)  # 把积分值限制在-10到10之间，数值可以根据需求调

5. 实例变量初始化不规范

你在run方法里直接用m.xp = []，但xp和yp是Move类的属性，应该在__init__方法里初始化self.xp = []和self.yp = []，不要直接操作外部实例。

二、修正后的核心代码示例 #

我把关键逻辑改好了，你可以直接参考：

import time
import matplotlib.pyplot as plt
import numpy as np

class PID:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.last_error = 0
        self.integral = 0
        self.integral_limit = 10  # 积分抗饱和上限

    def update(self, error, dt):
        if dt == 0:  # 防止除以0
            dt = 0.001
        derivative = (error - self.last_error) / dt
        self.integral += error * dt
        # 积分抗饱和
        self.integral = max(min(self.integral, self.integral_limit), -self.integral_limit)
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.last_error = error
        return output

class Move():
    def __init__(self, time_target, file_path, motor_a, motor_b):
        with open(file_path, 'r') as f:
            self.file = f.readlines()
        self.time_target = time_target
        self.motor_a = motor_a
        self.motor_b = motor_b
        self.pid = PID(Kp=0.5, Ki=0.01, Kd=0.02)  # 先从保守参数开始调
        self.xp = []
        self.yp = []

    def run(self):
        self.start_time = time.time()
        last_update_time = self.start_time
        total_steps = len(self.file)
        base_sleep = self.time_target / total_steps  # 每个步骤的基准耗时

        for i in range(total_steps):
            current_time = time.time()
            line = self.file[i].strip().lower()
            
            # 计算当前进度的误差
            target_elapsed = (self.time_target / total_steps) * (i + 1)
            actual_elapsed = current_time - self.start_time
            error = target_elapsed - actual_elapsed

            # 计算DT
            dt = current_time - last_update_time
            last_update_time = current_time

            # PID调整
            adjustment = self.pid.update(error, dt)
            # 计算实际sleep时间，加边界限制
            actual_sleep = base_sleep + adjustment
            actual_sleep = max(0.1, actual_sleep)
            time.sleep(actual_sleep)

            # 记录数据
            self.xp.append(time.time() - self.start_time)
            self.yp.append(error)
            print(f'步骤 {i+1} | 误差: {error:.2f} | 调整量: {adjustment:.2f} | 实际sleep: {actual_sleep:.2f}')

        # 任务完成后的最终统计
        final_elapsed = time.time() - self.start_time
        print(f'\n任务完成！总耗时: {final_elapsed:.2f}秒，目标耗时: {self.time_target}秒，偏差: {final_elapsed - self.time_target:.2f}秒')

# 初始化并运行
m = Move(60, 'sequence.txt', 'C', 'D')
m.run()

# 绘图
xpoints = np.array(m.xp)
ypoints = np.array(m.yp)
plt.plot(xpoints, ypoints)
plt.xlabel('已用时间(秒)')
plt.ylabel('误差(秒)')
plt.title('PID时间调度误差曲线')
plt.show()

三、后续调试建议 #

1. PID参数要慢慢调：先把Kp设小（比如0.5），Ki从0.01开始，Kd从0.02开始，观察误差曲线，如果波动大就调小Kp，如果误差一直消不掉就慢慢加Ki。
2. 替换模拟逻辑到真实电机：等用sleep模拟的逻辑跑通了，把time.sleep(actual_sleep)换成真实的电机控制逻辑——PID调整的应该是电机的速度（比如把adjustment映射到电机速度的百分比），而不是sleep时间。
3. 统计真实动作时间：如果电机动作的真实耗时是不确定的，要在每个动作完成后记录实际用时，把这个真实用时纳入PID的误差计算，而不是全靠sleep模拟。

备注：内容来源于stack exchange，提问作者quietCoder