简介基于双闭环PID控制的一阶倒立摆系统课程设计文档面向自动控制原理学习者与从事PID控制算法研究的工程技术人员。内容系统梳理了一阶倒立摆的数学建模过程涵盖对象模型与线性化模型的处理方法并结合SIMULINK仿真平台给出完整的建模、封装及验证流程同时围绕内环电机速度控制与外环摆杆角度伺服控制两部分详细说明双闭环PID控制器的设计思路与参数调整步骤。包内为1个PDF文件即完整设计说明书压缩包大小约1.75MB。文档覆盖从理论推导、仿真实验到鲁棒性检验的完整闭环设计流程并附有性能指标求解程序与附录技术细节。目前已有916人学习浏览读者可借此掌握倒立摆系统的PID控制设计方法也可将其作为自动化课程设计的参考模板快速迁移到类似非线性、自不稳定系统的控制方案中。1. 双闭环PID控制的一阶倒立摆为什么值得自己动手做一遍一阶倒立摆通常指小车-摆杆系统几乎是控制理论里“最便宜的不稳定系统”。它天生只有一个稳定平衡点——摆杆竖直朝下你想要的竖直朝上状态本质是持续施加控制力才能维持的不稳定平衡。任何一点点扰动比如地面震动、电机齿槽转矩波动、编码器量化噪声都会在几百毫秒内把摆杆甩下去。正因为这样倒立摆成了验证控制算法实时性和鲁棒性的标准试金石。双闭环PID控制是目前工程上最容易落地、也最常被低估的方案。常见的认知误区是“PID太基础不如直接上LQR或滑模”。但实际调过就会明白单环PID面对倒立摆这种强耦合、欠驱动、非线性系统很难同时兼顾快速性和稳定性而双闭环把“角度稳定”和“速度/位置跟踪”拆成内外两个环每个环各管一段参数物理意义清晰调试起来非常直观。这篇文从建模、仿真到参数整定把一阶倒立摆的双闭环PID方案完整走一遍既给能直接跑的代码也讲清楚每个参数为什么这么设、调参时初学者最容易在哪一步把系统调崩。无论你是准备课程设计还是在研究更复杂控制器前想找一个可靠的基线方案这套流程都值得完整跟一遍。2. 一阶倒立摆建模先立起状态方程才知道双闭环要控制什么2.1 动力学模型推导从受力分析到状态空间一阶倒立摆的标准构成是一辆小车加一根自由摆杆。小车受电机驱动力F在水平轨道上运动摆杆通过转轴安装在小车上可以在竖直平面内自由旋转。建模的目标是搞清楚“给小车施加多少力摆杆角加速度会怎么变”这是设计任何控制器的前提。常见做法是用拉格朗日方程推导。设小车质量M摆杆质量m摆杆长度l摆杆质心到转轴距离为l/2小车位移为x向右为正摆杆与竖直向上方向的夹角为θ顺时针为正小车与轨道之间的摩擦系数为b摆杆转轴处的阻尼为c电机驱动力为F。先写出系统动能和势能。小车动能是T_car 1/2 * M * x_dot²摆杆质心位置需要通过车位置和摆角表示。摆杆质心的水平坐标是x (l/2)*sin(θ)竖直坐标是(l/2)*cos(θ)所以摆杆质心速度的平方是水平分量平方加竖直分量平方。经过化简摆杆动能包含平动和转动两部分T_pend 1/2 * m * (x_dot² l * x_dot * cos(θ) * θ_dot (l²/4) * θ_dot²) 1/2 * J * θ_dot²其中转动惯量J (1/12) * m * l²绕质心。系统总动能是两者之和总势能是摆杆质心的重力势能V m * g * (l/2) * cos(θ)把拉格朗日量L T - V代入拉格朗日方程d/dt(∂L/∂x_dot) - ∂L/∂x F - b * x_dot d/dt(∂L/∂θ_dot) - ∂L/∂θ -c * θ_dot经过整理这是推导中最容易出错的部分建议用符号计算工具验证一次得到两个非线性微分方程(M m) * x_ddot m * l/2 * cos(θ) * θ_ddot - m * l/2 * sin(θ) * θ_dot² F - b * x_dot m * l/2 * cos(θ) * x_ddot (J m * l²/4) * θ_ddot - m * g * l/2 * sin(θ) -c * θ_dot写成矩阵形式更清晰| Mm m*l/2*cos(θ) | | x_ddot | | m*l/2*sin(θ)*θ_dot² - b*x_dot | | m*l/2*cos(θ) Jm*l²/4 | | θ_ddot | | m*g*l/2*sin(θ) - c*θ_dot |2.2 线性化与状态空间表达双闭环设计的理论地基倒立摆的任务是让摆杆稳定在竖直向上即θ0附近。在这个平衡点附近做小角度近似令sin(θ)≈θcos(θ)≈1忽略高阶项上述非线性方程可以简化为线性方程(M m) * x_ddot (m*l/2) * θ_ddot F - b * x_dot (m*l/2) * x_ddot (J m*l²/4) * θ_ddot - (m*g*l/2) * θ -c * θ_ddot从这个方程组可以看到一个关键特征小车的直线加速度和摆杆的角加速度耦合在一起而且系统矩阵中m*g*l/2这一项是正反馈项——摆角偏离竖直向上时重力会进一步加大偏离这就是系统不稳定的根源。选取状态变量x [x, x_dot, θ, θ_dot]ᵀ输入u F输出取y [x, θ]ᵀ可以写出标准状态空间形式x_dot A * x B * u y C * x D * u其中A矩阵包含了一个正项来自重力这决定了开环系统有一个极点位于右半平面系统是不稳定的。B矩阵描述控制力如何同时影响小车加速度和摆杆角加速度。拿到状态空间模型后可以验证系统的可控性。很多教材直接给出“倒立摆系统可控”的结论但实际做一次ctrb计算会让你对系统有更直观的理解如果模型参数不准确比如忽略了电机电枢电感导致的执行器动态可控性结论可能改变。实际操作中我不会直接依赖可控性矩阵的秩而是把它作为一个必要的检查步骤秩不满的话说明模型化简过度需要回到动力学方程重新检查。2.3 为什么双闭环而不是单闭环或状态反馈有了状态空间模型理论上可以选择LQR或者极点配置来设计全状态反馈控制器。但工程上有三个现实问题第一小车位置x需要测量摆杆角度θ需要测量而角速度θ_dot通常没有直接的传感器要么加装陀螺仪要么通过编码器差分近似。微分在离散系统中会放大高频噪声这在控制周期短时尤其致命。第二LQR的权重矩阵Q和R的物理意义不直观。一套Q、R参数调好后如果改了机械结构比如换了更长的摆杆需要重新调两个矩阵调试成本高。第三也是最重要的一点工程上通常希望摆杆能稳定在某个目标位置甚至跟踪一个运动轨迹。这时候需要同时控制位置和角度单环PID很难兼顾“摆角稳定”和“位置跟踪”两个目标——你紧着摆角调位置响应会变慢紧着位置调摆角容易振荡。双闭环的解法是把任务拆开内环角度环以最快的速度稳住摆角这是“稳住”的核心外环位置环根据小车当前位置偏差产生一个角度给定值让小车“跟着走”。注意这里有个巧妙的设计位置环的输出不是直接的力而是一个角度——等于在告诉角度环“你把摆杆往这个方向偏一点车就往反方向走”。这个θ_ref经过角度环解算成控制力F。两层各自闭环各自调参互不纠缠。以下对比表可以直观看出差异维度单环PID直接角度到力双闭环PID位置环角度环控制目标只能稳住摆角同时稳住摆角跟踪位置抗扰能力摆杆受扰后恢复慢内环快速抑制扰动外环修正位置参数物理意义一组参数兼管两个目标耦合严重角度环管快位置环管准互不干扰调试难度参数敏感容易极限环振荡先调内环再调外环分步收敛对模型依赖低低仅需粗略模型辅助整定需要说明的是双闭环PID不等于“比LQR差”。在倒立摆这种执行器带有明显延迟和饱和的系统中串级结构天然有抗饱和优势内环先饱和外环的偏差累积会被内环的饱和限制住不容易出现大幅超调。这也是工业过程控制中串级控制长期占据主流的原因。实际项目中我见过很多用LQR调了几个月最后退回双闭环PID的案例先把双闭环PID调明白再考虑更高级的控制器是比较靠谱的路线。3. 用Python在本地搭一阶倒立摆双闭环仿真先看到系统在动再谈调参3.1 仿真框架设计离散化方法是第一道坑写仿真之前需要想清楚一个问题控制周期和物理仿真的时间尺度怎么分离。真实系统是连续的但控制器是离散的每隔一个固定时间间隔采样一次、计算一次输出。仿真里需要同时处理这两个时间尺度。我一般这样设计使用固定步长的数值积分模拟真实物理系统步长dt_physics取0.1毫秒控制器每隔dt_control 5毫秒执行一次也就是控制频率200Hz。为什么控制频率取200Hz而不是更高这由执行器常见直流电机驱动器的实际带宽决定高了没有额外收益反而会放大编码器和电流传感器的噪声低了内环相位裕度不够容易振荡。Python里做数值积分有很多选择SciPy的solve_ivp或odeint都可以。但对于倒立摆仿真我更推荐自己写一个简单的四阶Runge-KuttaRK4积分器。原因是倒立摆系统本身不复杂自己写RK4只有十几行代码能精确控制积分步长还能顺便验证一下自己对模型的理解是否正确用通用求解器反而容易在事件检测上踩坑。import numpy as np def pendulum_dynamics(state, F, M, m, l, b, c, g9.81): 一阶倒立摆连续动力学方程非线性模型 默认单位kg, m, s, N 参数说明 state [x, x_dot, theta, theta_dot] - x: 小车位置向右为正单位 m - theta: 摆杆与竖直向上方向的夹角顺时针为正单位 rad - F: 作用在小车上的水平外力向右为正单位 N 返回状态导数 [x_dot, x_ddot, theta_dot, theta_ddot] x, x_dot, theta, theta_dot state J (1/12) * m * l**2 # 质量矩阵2x2来源于拉格朗日方程整理后的矩阵形式 M_matrix np.array([ [M m, m * l/2 * np.cos(theta)], [m * l/2 * np.cos(theta), J m * l**2/4] ]) # 右侧向量包含外部力、摩擦力、重力、科氏力项 rhs np.array([ F - b * x_dot m * l/2 * np.sin(theta) * theta_dot**2, m * g * l/2 * np.sin(theta) - c * theta_dot ]) # 解2x2线性方程组得到加速度 acc np.linalg.solve(M_matrix, rhs) x_ddot, theta_ddot acc[0], acc[1] return np.array([x_dot, x_ddot, theta_dot, theta_ddot]) def rk4_step(func, state, F, dt, *args): 四阶Runge-Kutta积分器 dt: 物理仿真步长单位 s k1 func(state, F, *args) k2 func(state 0.5 * dt * k1, F, *args) k3 func(state 0.5 * dt * k2, F, *args) k4 func(state dt * k3, F, *args) return state (dt / 6.0) * (k1 2 * k2 2 * k3 k4)这段代码的关键在于pendulum_dynamics里的质量矩阵求解。很多人第一次写倒立摆仿真时会把两个二阶方程强行消元成一个显式的θ_ddot表达式然后在推导过程中丢掉某个交叉项直接保留矩阵形式并用np.linalg.solve求解既不容易出错阅读代码的人也更容易对照前面的推导过程验证。3.2 双闭环控制器的离散实现内环快、外环准控制器的核心是串级结构。先定义两个独立的PID类参数可以分别设置class PID: def __init__(self, kp, ki, kd, output_limitsNone, setpoint0.0): 离散PID控制器 参数说明 - kp: 比例增益内环单位通常是 N/rad外环单位通常是 rad/m - ki: 积分增益注意这是对误差的累积增益不是时间积分 - kd: 微分增益 - output_limits: (min, max) 输出限幅内环输出是力要限幅;外环输出是角度也要限幅 self.kp kp self.ki ki self.kd kd self.output_limits output_limits self.setpoint setpoint self._integral 0.0 self._last_error 0.0 def compute(self, measurement, dt): 计算PID输出 dt: 控制周期单位 s error self.setpoint - measurement self._integral error * dt derivative (error - self._last_error) / dt output self.kp * error self.ki * self._integral self.kd * derivative self._last_error error # 输出限幅倒立摆中必须加限幅否则计算出的力远超电机能力 if self.output_limits is not None: output np.clip(output, self.output_limits[0], self.output_limits[1]) # 简单的积分抗饱和如果输出被限幅停止积分累积 if output self.output_limits[0] or output self.output_limits[1]: self._integral - error * dt return output def set_setpoint(self, sp): self.setpoint sp这里有一个细节值得单独说明积分项的累积方式。self._integral error * dt使用的是误差对时间的积分而不是直接累加误差。这保证了dt改变时积分项的行为是连续的如果你直接累加误差、不乘dt同样的PID参数在不同控制周期下效果会完全不一样。我见过不少仿真实例没有乘dt参数看起来“能跑”一换控制周期就发散根源就在这。双环的连接方式写在主仿真循环里# 系统参数一组典型的实物参数 M 1.0 # 小车质量 kg m 0.2 # 摆杆质量 kg l 0.5 # 摆杆长度 m b 0.1 # 小车与轨道摩擦系数 c 0.01 # 摆杆转轴阻尼 # 控制器参数初始值后面调 angle_pid PID(kp50.0, ki10.0, kd5.0, output_limits(-30.0, 30.0), setpoint0.0) position_pid PID(kp1.2, ki0.0, kd0.8, output_limits(-0.3, 0.3), setpoint0.0) # 仿真参数 dt_physics 0.0001 # 物理积分步长 0.1ms dt_control 0.005 # 控制周期 5ms pos_target 0.0 # 小车目标位置 if __name__ __main__: state np.array([0.05, 0.0, 0.1, 0.0]) # 初始状态位置偏移5cm摆角偏移0.1rad sim_time 10.0 n_physics int(dt_control / dt_physics) time 0.0 while time sim_time: # 每个控制周期内用固定步长RK4跑多个物理步 for _ in range(n_physics): # 外环输出角度给定 theta_ref position_pid.compute(state[0], dt_control) angle_pid.set_setpoint(theta_ref) # 内环输出控制力 F angle_pid.compute(state[2], dt_control) F np.clip(F, -30.0, 30.0) # 执行器限幅 # 积分一步 state rk4_step(pendulum_dynamics, state, F, dt_physics, M, m, l, b, c) time dt_physics看一下这个循环的逻辑顺序有个很容易搞错的点外环的采样时刻应该是和控制周期同步的但物理积分步长更小所以外环在每个控制周期内只计算一次而不是在每个物理步里都计算。上面代码里position_pid.compute和angle_pid.compute在每个物理步都被调用了但传入的dt_control是5ms而不是物理步长——这会导致多个物理步内累计多次PID计算行为与真实控制器不一致。正确的写法是让每个控制周期只执行一次PID计算然后剩余的物理步继续使用这个控制力while time sim_time: # 仅在每个控制周期的起始时刻计算一次 theta_ref position_pid.compute(state[0], dt_control) angle_pid.set_setpoint(theta_ref) F angle_pid.compute(state[2], dt_control) F np.clip(F, -30.0, 30.0) for _ in range(n_physics): state rk4_step(pendulum_dynamics, state, F, dt_physics, M, m, l, b, c) time dt_physics F 0.0 # 本周期结束先清零等下周期重新计算第二种写法对应真实的定时器中断控制逻辑控制中断到来时采集位置和角度计算输出写入DAC或PWM寄存器然后这5ms内输出值保持不变。仿真只有反映了这种零阶保持特性调出的参数才能平滑地迁移到实物上。3.3 仿真结果怎么判断有效性四条曲线一个指标跑完一次仿真不要只看最终“没倒”要看四条曲线的行为和它们之间的时序关系。这四条曲线是小车位置x、摆杆角度θ、外环输出θ_ref、内环输出F。外加一个关键指标——稳态时θ_ref不为零的持续时间。我一般在仿真里做个简单的统计函数输出这些指标判断标准如下摆杆角度θ的超调量应小于0.05rad约3度调整时间进入±0.02rad误差带小于1秒。注意摆角不能无振荡回零否则说明系统阻尼偏高抗扰能力不足。小车位置x应该单调趋向目标位置超调小于15%稳态误差收敛到±1cm以内。位置环的响应速度明显慢于角度环这是正常现象如果位置环比角度环还快说明内外环带宽没有拉开系统会变得非常敏感。θ_ref外环输出应当是小振幅的平滑信号幅度小于0.1rad频率集中在0.5-2Hz。如果θ_ref出现高频抖动大概率是位置环的微分项被噪声放大需要压低kd。控制力F应当远低于限幅值且不出现周期性饱和。如果F频繁顶到30N上说明系统的控制器增益不足或者限幅太紧对执行器有额外要求。判断仿真有效后下一步才是完整的参数整定流程。4. 一阶倒立摆双闭环PID参数整定先内后外分步逼近别想一步到位4.1 内环角度环整定从临界比例度法起步角度环是整个系统的基石它的目标是“让摆角快速且无超调地回到零”。角度环整定不正确外环再怎么调都没用。常见做法是从纯比例控制开始做临界比例度实验把Ki和Kd设为0逐步增大Kp直到系统产生等幅振荡。记录此时的临界增益Ku和振荡周期Tu。注意这个实验是在闭环状态下做的——也就是说把摆角传感器接上、控制器输出接上执行器但给一个很小的初始摆角扰动观察系统是否衰减或发散。以下是一个典型的整定顺序先给一组初始值Kp1Ki0Kd0。运行仿真如果初始摆角0.05rad后系统很快发散说明增益太低。以1.5倍增长Kp每次仿真3秒观察摆角行为。当发现摆角不再发散而是持续振荡幅度不变记下当前Kp为Ku振荡周期为Tu。用齐格勒-尼古拉斯法给出PID初值Kp0.6KuKi0.5TuKd0.125Tu。这里Ki和Kd的单位要和你代码里的实现方式对齐——如果代码里Ki是误差积分增益乘了dt那么KI应该等于Kp乘以某个时间常数的倒数。然后在这组初值基础上做手工微调。微调的方向有规律可循Kp决定系统的响应速度和稳态精度。Kp偏小时摆角响应慢稳态时存在一个小的恒定角度误差因为需要用摆角偏差来维持小车加速或抵抗摩擦力Kp偏大时系统趋向振荡。判断标志是给一个初始扰动后摆角以2-3次的衰减振荡回零这是接近最优的状态如果只有一次过冲然后缓慢回零说明Kp相对Ki偏小。Ki用来消除稳态误差。在倒立摆中它主要补偿电机死区和摩擦的不对称性。但要注意Ki过大会造成低速持续振荡通常表现为摆角在小幅范围内周期抖动频率在0.5Hz左右。很多初学者在角度环加上Ki后系统开始振荡第一反应是减小Kp结果越减越糟。正确的做法是保持Kp不变先把Ki降下来观察振荡是否消失。Kd提供阻尼用于抑制超调。但角度环的Kd有天然上限如果Kd过大高频分量被严重放大表现为摆角信号上叠加明显的锯齿噪声。离散控制系统的经验法则是Kd的等效微分时间不应超过控制周期的3-5倍超过后微分项的意义就不大了。角度环调好后做两个补充测试一是推一下摆杆让它偏离0.3rad观察恢复时间二是持续一段时间不控制确认摆杆掉落后系统能重新立起来这在仿真里需要增加整定逻辑才能测到但在实物上很容易表现为电机堵转。如果这两个测试都通过内环暂告一段落。4.2 外环位置环整定带宽分离是核心原则外环整定的前提是内环已经能可靠地稳定摆角。外环的输入是小车位置偏差输出是角度给定。注意外环的输出通常要经过限幅默认最大给0.3rad约17度限制过大会让内环饱和过小会让小车移动速度过慢。外环比内环更容易被忽略的问题是内外环带宽分离。内环闭环带宽应该至少是外环的3-5倍。判断方法可以从阶跃响应估计内环的上升时间约0.2秒外环的上升时间应设置在约1秒以上。如果外环太快比如上升时间0.5秒以下内外环会产生共振系统的摆角会呈现出等幅振荡甚至发散。位置环的整定也是先Kp后Kd再Ki的顺序。初始设置Kp0.5输出限制在±0.2rad以内Ki0Kd0。给一个小车位置偏差比如0.3m观察位置响应。位置环的Kp物理意义是“小车每偏离目标1米给角度环下发多少弧度的角度给定”这个值不应太大通常0.5-2.0之间。设置过大的后果是小车距离目标还很远时角度给定就达到上限此时内环一直输出最大力让小车加速速度会不断增长到达目标时超调巨大。位置环的Kd作用是抑制小车速度过快效果是给角度给定的变化率限幅。Kd过大会导致一个问题小车稍有速度变化角度给定就剧烈摆动反而干扰了内环稳定的摆角。一个简便的验证方法是仿真中把位置环Kd调到0观察位置超调是否可接受如果可接受就不加Kd不可接受再加。位置环的Ki通常是三者中最小的甚至可以为0。因为位置环的稳态误差主要来自轨道摩擦而摩擦力在倒立摆中可以被内环的动态抵消掉一部分。如果你希望小车精确停在目标位置可以加一点点Ki。但如果小车在到达目标后出现位置方向的周期蠕动大约0.5-1Hz的慢振荡多半是Ki过大。4.3 一组典型参数与仿真现象对照表为了给完全没有调参经验的人一个参考以下给出两组经过仿真验证的参数分别对应“快速响应”和“平稳跟踪”两种目标取向参数快速响应调节时间短平稳跟踪超调小内环Kp8045内环Ki208内环Kd86内环输出限幅±40 N±25 N外环Kp1.80.8外环Ki0.20.0外环Kd0.50.3外环输出限幅±0.35 rad±0.25 rad控制周期2 ms5 ms注意一点快速响应组的控制周期设成2ms是考虑到系统带宽提高后需要更快的采样来保证稳定性。不要在同一组机械结构上只改PID参数而不改控制周期那样对比不客观。用快速响应组跑10秒仿真摆角和位置的响应曲线会呈现这样的特征初始摆角0.1rad时前0.5秒内摆角收敛到±0.01rad同时小车位置开始平移小车从0.1m位置移动到目标位置的时间在3-4秒之间超调约8%稳态时摆角维持在0.005rad附近因为需要轻微倾斜来产生抵抗摩擦的推力。这个稳态摆角存在是正常现象不是误差。4.4 调参失败的常见症状与对策参数调到系统发散时不要急着全部清零重调先判断是哪个环出了问题。典型症状有两类边界情况。第一类是内环发散。表现是初始摆角很小0.2秒内摆角迅速增大直到超出数值范围控制力F始终贴近限幅值。处理办法是把外环的Kp、Ki、Kd全部置零角度环给定固定为0只调内环。等内环稳定后再逐步恢复外环参数。第二类是内外环共振。表现是位置缓慢做成衰减振荡的同时摆角也在同步振荡相位关系大约差180度——小车往左时摆杆偏右。这时先检查位置环Kp是否过大其次检查内环Kd是否太小。如果内环阻尼足够共振幅度会明显减弱。有一个容易忽略的干扰源物理仿真步长与控制器频率的关系。如果物理步长设成了1ms而控制周期是5ms控制周期内积分5次还算合理但有人直接把物理步长设成与控制周期相同RK4在一个大的固定步长下精度不够会导致仿真结果偏“硬”实际系统反而没有仿真中那么振荡。反过来物理步长太小会让仿真时间变长建议保持在控制周期的1/10到1/50之间。参数整定的核心心法每次只改一个参数改动幅度不超过当前值的30%仿真后记录行为再决定下一步方向。用Excel记录调参历史比靠脑子记可靠得多特别是当你把参数从“能跑”调到“好”的时候中间每一步的失败数据都对后续判断有价值。5. 仿真到实物迁移的三个必做处理积分饱和、微分滤波和角度限幅仿真调通后把控制代码搬到真实系统时总会发现“仿真里好好的实物上一通电就倒”。问题几乎总是出在三个容易被忽略的工程细节上。第一个是积分饱和。仿真里的限幅很简单但实物执行器在输出饱和时积分项还在继续累积等误差反向时积分项需要很长时间才能退饱和导致大超调甚至发散。处理方式是上一章代码里写过的简单抗积分饱和输出达到限幅时暂停积分累积。但仅靠这一条还不够建议进一步做积分限幅给积分项本身设定一个上限防止积分项在饱和期内积累到异常大的值。内环积分项限幅一般设为输出限幅的10%-20%外环积分项限幅设为输出限幅的5%-10%。第二个是微分项的噪声放大。实物中的角度信号来自编码器或IMU编码器差分求角速度会把量化噪声放大IMU的角速度信号则有温漂。常见做法是不直接使用原始微分项而是使用带一阶低通滤波的微分。以下给出带滤波的PID变体代码这个写法在STM32或LinuxCNC等系统上都能直接套用class PIDWithFilter: def __init__(self, kp, ki, kd, filter_alpha, output_limitsNone): 带一阶低通滤波的离散PID 参数说明 - filter_alpha: 低通滤波系数0.1-0.5之间越小滤波越强但相位滞后越大 微分通道执行y[n] alpha * raw_derivative (1 - alpha) * y[n-1] self.kp kp self.ki ki self.kd kd self.alpha filter_alpha self.output_limits output_limits self._integral 0.0 self._last_measurement None self._filtered_derivative 0.0 def compute(self, measurement, dt): if self._last_measurement is None: self._last_measurement measurement derivative 0.0 else: derivative (measurement - self._last_measurement) / dt self._last_measurement measurement # 一阶低通滤波系数越小噪声衰减越好但相位滞后越明显 self._filtered_derivative (self.alpha * derivative (1 - self.alpha) * self._filtered_derivative) error self.setpoint - measurement self._integral error * dt output (self.kp * error self.ki * self._integral self.kd * self._filtered_derivative) # 抗积分饱和处理 if self.output_limits is not None: output np.clip(output, self.output_limits[0], self.output_limits[1]) # 如果输出饱和回退本次积分累积 if output self.output_limits[0] or output self.output_limits[1]: self._integral - error * dt return output这里的filter_alpha取值很有讲究。太小比如0.05会让微分通道几乎失效系统阻尼不足太大比如0.9滤波效果不明显。实际经验是从0.2开始调观察摆角的噪声幅值和超调量的变化。如果摆角曲线的毛刺明显减小但超调没有增大可以继续减小如果超调开始明显增加说明滤波带来的相位滞后已经损害了稳定域。第三个是角度限幅和跳变的处理。编码器角度在跨越±180度时会出现跳变在竖直向上这个工作点附近摆杆的测量范围恰好跨越了这个跳变点。如果不对原始角度做展开处理PID会看到一个从179度瞬间跳到-179度的巨大误差输出剧烈冲击系统直接倒下。常见做法是把角度差映射到[-π, π]区间def wrap_angle_diff(theta_measured, theta_ref): 计算角度误差并映射到 [-pi, pi]避免编码器过零跳变导致的误差突变 diff theta_measured - theta_ref while diff np.pi: diff - 2 * np.pi while diff -np.pi: diff 2 * np.pi return diff这段函数很短但几乎是倒立摆实物调试中救命级别的代码。缺失它的代价往往是一通电就听到电机猛抽一下然后摆杆砸向限位。最后一个值得花的功夫是做一个简单的上位机监控页面实时画出摆角和位置波形。无论是用Python的matplotlib动画还是写一个简单的WebSocket推流到浏览器目的都是让你能直观地看到系统在控制器参数变化时的行为变化。调参不能盲调曲线是你唯一的客观依据。从原本的说明书式推导到现在经历了建模、仿真、整定到实物迁移四个阶段应该能够独立赶走一台一阶倒立摆了。本文还有配套的精品资源点击获取