3D空间机器人三轴速度PID控制器仿真计算错误排查咨询

第一版代码数值发散最终变为NaN的原因 #

• PID公式错误：标准离散PID的比例项（P）直接等于当前误差，不需要乘以delta_time，你误将P项和积分项（I）的计算逻辑混淆，导致比例增益随运行周期动态变化，参数完全失配。
• 缺少输出限幅：你定义了__min__、__max__阈值但没有实际使用，误差存在时积分项持续累加，输出数值不断攀升最终溢出变为NaN。
• 仿真逻辑不符合物理规律：直接将PID输出赋值为当前速度，相当于假设被控对象是零延迟、增益为1的理想环节，实际机器人速度受电机惯性、负载影响不可能跳变，这种赋值会导致闭环系统容易发散。
• 时间差处理存在隐患：当循环执行速度极快时，delta_time会取极小值1e-16，微分项（D）除以该值会直接产生超大数值，触发数值溢出。

第二版代码无超调的原因 #

• 仿真模型无惯性：仍然直接将PID输出赋值给当前速度，没有模拟实际系统的滞后特性，误差一旦出现就能被立刻修正，不会出现超调。
• 参数配置问题：kd仅为0.002，几乎不起阻尼作用，且ki大于kp，积分项会快速消除稳态误差，响应全程单调趋近设定值。
• 固定时间步长规避了周期波动问题，且增加了输出限幅避免了数值溢出，但仿真结果和实际物理场景的适配性很低。

• 模拟真实被控对象：如果PID输出为加速度控制量，当前速度更新逻辑改为self.current += outputs * delta_time；也可以加一阶惯性环节模拟电机响应，比如self.current = self.current * 0.95 + outputs * 0.05，模拟系统滞后，这样调试出来的参数才能直接用到实际机器人上。
• 固定控制周期：每次循环添加time.sleep(0.01)（可根据实际需求调整周期），保证delta_time稳定，避免计算速度波动影响PID效果。
• 数据可视化验证：不要循环打印输出，将每次的时间、设定值、当前速度、PID输出存入列表，测试10~30秒后停止，用Matplotlib绘制响应曲线，可直观观察超调量、调节时间、稳态误差等核心指标。
• 补充边界测试：测试阶跃响应（设定值突然跳变）、干扰响应（人为给当前速度加一个偏移量），验证PID的抗干扰能力和动态特性。
• 增加积分限幅：对i_errors做钳位处理，比如限制在[-self.__max__/self.ki, self.__max__/self.ki]范围内，避免积分饱和导致的超调、响应滞后问题。

• 采用经典Ziegler-Nichols整定法：
  1. 先将ki、kd设为0，逐渐增大kp直到系统出现等幅振荡，记录此时的临界增益Ku和临界振荡周期Tu
  2. 按公式计算初始参数：kp = 0.6*Ku，ki = 1.2*Ku / Tu，kd = 3*Ku*Tu / 40
• 分步调参逻辑：
  1. 先调P：将ki、kd置0，增大kp直到系统出现小幅振荡，再将kp降到振荡消失值的70%左右，保证响应速度的同时避免振荡。
  2. 再调I：固定kp后缓慢增大ki，直到稳态误差能在可接受时间内消除，注意不要将ki设的比kp还大，容易引发超调和积分饱和。
  3. 最后调D：从小到大增加kd，直到阶跃响应的超调量符合要求，kd不要过大，否则系统会对传感器噪声过于敏感。
• 轴独立配置参数：将kp、ki、kd都改为3元素的NumPy数组，三个轴可以独立调参适配不同的负载特性，示例初始化逻辑：self.kp = np.array(kp) if isinstance(kp, (list, np.ndarray)) else np.array([kp]*3, dtype=float)。
• 无超调需求适配：如果要求响应全程无超调，可以适当降低kp和ki，小幅提升kd，牺牲少量响应速度换取稳定性。

内容的提问来源于stack exchange，提问作者NotALolicon