简介本资源是一阶倒立摆系统PID与LQR双策略控制的MATLAB完整实现方案面向计算机、电子信息工程、自动化及数学等专业的本科生适用于现代控制理论课程设计、期末大作业及毕业设计等实践环节。压缩包共83个文件926KB涵盖34个.mat数据文件含预置模型参数与仿真结果、12个.xml配置文件用于Simulink模型参数管理、10幅.jpg原理图与响应曲线图、8个.slxc与4个.slx Simulink仿真模型含可视化动画界面、5个.avi动态演示视频以及4个.m主控脚本和详细注释的README.md文档。已有62人学习下载。用户可直接运行附赠案例数据无需额外配置所有控制算法均采用参数化编程比例系数、权重矩阵等关键参数集中定义、清晰标注便于对比分析不同控制器对系统稳定性、响应速度与抗扰性的影响代码逻辑分层明确从建模、线性化、控制器设计到闭环仿真与性能评估形成完整闭环显著降低控制算法理解与复现门槛。 说句真心话倒立摆这个项目控制领域的人早晚都会碰一次。不管是课程设计、毕业设计还是自己折腾机器人我几乎没见过哪个搞控制的人能绕开它。所以整理这个“一阶倒立摆的PID控制和LQR控制.zip”的时候我其实把当年踩过的坑、调参的血泪、仿真到实物的跨越过程都塞进去了。这项目看上去只是两种控制算法的对比但真正做完你会发现它几乎覆盖了控制工程从建模、仿真、控制器设计到嵌入式落地的完整流程。如果你正准备做倒立摆或者刚拿到一个类似的控制项目不知道从哪里下手这个项目足够你少走不少弯路。里面用到的PID和LQR一个是工程界应用最广的反馈控制算法一个是现代控制理论中最经典的线性最优控制方法两个正好代表了经典控制与现代控制的典型思路。下面我会从物理建模到裸机实现把整个项目的核心细节拆开讲清楚。1. 项目的来龙去脉一阶倒立摆到底是什么1.1 控制界的“Hello World”与项目适用人群先说名字。所谓“一阶倒立摆”指的是一个小车在导轨上水平运动小车上方铰接一根摆杆系统目标就是让这根摆杆保持竖直向上。之所以叫“一阶”是因为它只有一个摆杆和一个铰链不是二级甚至三级串联倒立摆。别小看这个看似简单的结构它天然具备非线性、开环不稳定和欠驱动三个特性恰好是控制理论中最典型也最棘手的问题组合。打个比方这就像你手里托着一根竖直的长杆想让它一直立着你的手必须根据杆的倾斜不断移动。杆向左倒你手就得跟着向左接住它杆倒得越快你手移动也得越快。倒立摆的控制器做的事本质上就是这个“手”的动作——根据摆杆倾斜状态计算小车应该以多大加速度去追摆杆。PID和LQR就是两种计算“这个动作量”的不同策略。这个zip包适合三类人第一类自动化和控制工程相关专业的学生用来理解课本上的状态空间、反馈控制、最优控制到底怎么跟物理系统挂钩第二类做嵌入式或机器人开发的工程师尤其是想搞“裸机PID”来控制电机、机械结构的可以直接参考里面的工程实现第三类纯粹对控制算法好奇想通过仿真理解PID和LQR区别的爱好者。你完全不需要一开始就搞懂所有数学推导跟着项目里的仿真代码和调参流程走一遍也能获得非常直观的感受。1.2 物理模型与数学推导从运动方程到状态空间要设计控制器第一步永远是建模。整个倒立摆系统可以拆成两部分小车在导轨上的平移运动以及摆杆绕铰链的转动。这里有一个很重要的细节摆杆不是固定在小车上的它因为重力作用会自然下垂而我们要做的是通过控制小车的水平加速度让摆杆“停在”竖直向上这个不稳定平衡点。我采用的推导方式是牛顿-欧拉法。设小车质量为 M摆杆质量为 m摆杆质心到铰链的距离为 l摆杆绕质心的转动惯量为 I小车受到的水平控制力为 F。在平衡点附近摆角 θ 很小做线性化近似sinθ ≈ θcosθ ≈ 1忽略高阶小量。经过整理系统的线性化运动方程为投影到水平方向和摆杆转动方向[ (M m)\ddot{x} ml\ddot{\theta} F ][ ml\ddot{x} (I ml^2)\ddot{\theta} mgl\theta ]这里的符号约定是θ 从竖直向上的位置测量逆时针为正。从运动方程能看到一个关键信息——水平方向的力和摆角之间是耦合的这决定了系统不能用简单的单输入单输出思维去处理。把方程写成矩阵形式然后定义状态向量[ \mathbf{x} [x, \dot{x}, \theta, \dot{\theta}]^T ]经过整理可以得到标准的连续线性状态空间表达式[ \dot{\mathbf{x}} A\mathbf{x} Bu ]其中 u 就是控制力 F。A 矩阵和 B 矩阵的具体形式与 M、m、l、I、g 有关。这里我从一开始就声明实际应用时如果要更精确摩擦系数、导轨阻尼、电机死区这些都可以加进 B 矩阵或 A 矩阵但对于控制算法研究先忽略摩擦完全够用。1.3 开环系统特性为什么必须用反馈有了状态方程第一件事就是看开环系统的极点在复平面的什么位置。我们取一组典型物理参数小车质量 M 1.0 kg摆杆质量 m 0.1 kg摆杆质心到铰链距离 l 0.3 m重力加速度 g 9.8 m/s²细摆杆绕质心的转动惯量 I (1/12)·m·(2l)²。把这些参数代入状态方程后计算开环系统的特征值。结果一点都不意外四个特征值里面有一对实部为零的极点对应小车位置的积分特性还有一对实部一正一负的极点正实部那个就对应摆杆的“倾倒”趋势。也就是说如果不加任何控制摆杆稍微偏离竖直位置就会加速倒下这是物理上不可能稳定的系统。这就是必须引入反馈控制的原因。PID也好LQR也好本质上都是通过状态或输出反馈把这些开环极点移动到左半平面让系统在平衡点附近稳定。2. 技术方案选型为什么同时用PID和LQR2.1 两种控制方案的底层逻辑对比这个项目把PID和LQR放在一起做对比我觉得特别有价值。两种方法代表了两种完全不同的控制思维。PID的核心思路是误差驱动测量摆杆角度跟目标角度也就是零做差根据误差大小、误差累积、误差变化率这三个维度来决定控制量。它的优势是对模型依赖低你甚至不需要知道系统的精确参数也能调出能用的控制器。但它的短板也很明显处理多变量耦合系统时结构选型非常考验经验单环PID往往不够用需要串级甚至多环。LQR的核心思路则是“状态反馈 最优控制”先建立一个包含所有状态变量和控制代价的二次型性能指标然后通过求解Riccati方程得到一组最优反馈增益。它的优势是系统化只要模型可靠、权重矩阵量纲设计得合理一次性就能得到一组完整的状态反馈增益而且能保证一定的稳定裕度。缺点是输出结果是一个反馈矩阵不像PID那样可以直观地一个参数接一个参数地调遇到模型误差较大时理论最优性可能打折。我认为这个项目选这两种算法真正的意义在于让人体会“用经典方法解决问题”和“用现代方法解决问题”之间的思维方式差异。PID是在做“事后修正”LQR是在做“事前规划”。如果你能同时掌握这两种思维再去学MPC、滑模控制这类高级算法时理解门槛会大幅降低。2.2 建模取舍从非线性到线性化的坑在仿真项目里我做了两次模型处理。第一次是直接用完整非线性运动方程来搭被控对象模型这样能真实反映物理系统的行为。第二次是线性化到平衡点上得到A、B矩阵用线性模型来设计控制器。这里有一个很隐蔽的坑如果直接用非线性模型仿真但用基于线性模型设计的LQR当摆角初始偏差比较大比如超过15度时线性化误差会导致控制器性能明显下降甚至直接失稳。因为这个原因项目里我加了一个“摆角范围检测”逻辑一旦摆角过大先执行“甩起”控制或人工扶正等摆角进入可线性化的范围再切换到平衡控制。这个细节在教科书里基本不会讲但实际系统必须处理。2.3 仿真场景设计思路zip包里的仿真部分我设置的场景是摆杆初始偏离竖直方向5度速度为0小车初始位置为0目标是把摆杆稳定在竖直状态同时小车尽量回到原点。选择5度初始偏角是有讲究的它足够大能看出控制器的动态响应差异又不会大到突破线性化近似范围。在这个场景下PID和LQR的仿真结果会有明显差别正好用来对比两者的反应速度、超调量和控制力峰值。如果你想要更严苛的测试可以把初始角改成10度甚至15度两种算法的差距会更明显。3. PID控制设计与代码实现3.1 控制结构选择单环还是双环做倒立摆的PID控制最容易犯的错误就是试图用一个单环PID搞定一切。我之前第一次做的时候天真地只用角度误差来算控制力结果小车疯狂移动位置发散摆杆也稳不住。原因很简单单个角度环只能让摆杆变直但无法控制小车的位置小车会一直朝一个方向加速运动直到撞上限位。实际项目里我更推荐使用串级PID结构。外环是小车位置环输出的是目标摆角内环是摆角环输出的是控制力。这样小车位置偏移了外环会给一个轻微的角度偏移指令让摆杆略微倾斜再利用倾斜产生的水平分力把小车拉回来。这个思路本质上就是人在单车上保持平衡的原理——身体倾斜控制方向方向控制位置。内环使用PD控制就够了因为目标是让角度误差归零系统本身没有稳态摩擦累积积分项反而容易引入饱和问题。外环位置环则可以考虑加一点积分项用来消除静摩擦力导致的小车位置稳态误差。串级PID的结构如下位置环输入是小车位置目标反馈是实际位置输出是目标摆角限幅。角度环输入是目标摆角一般为0或由位置环给定反馈是实际角度输出是PWM/力信号。3.2 参数整定流程与经验值整定串级PID时我的经验是先内环后外环先比例后微分。第一步把外环断开固定目标摆角为零只调内环的Kp。从小到大逐步增加直到摆杆能在小扰动下回到竖直状态但会出现缓慢的等幅振荡说明Kp已经接近临界值再降回大约60%左右。第二步加入Kd看到阻尼增强、振荡明显收敛。Kd太大时高频噪声会被放大表现为电机发出尖锐噪声、摆杆高频抖动这时候需要果断回调。第三步接入外环位置环。位置环的Kp不需要太大太小的话小车回中点很慢太大会引起系统振荡甚至发散。位置环的Kd也同样重要它相当于对小车的速度做阻尼避免小车在零点附近来回振荡。以我项目中用的物理参数为例实测下来一个比较稳的起始参数范围是角度环 Kp80~120Kd10~20角度单位是弧度位置环 Kp0.5~2Kd0.5~2位置单位是米需要特别强调PID参数没有“万能值”它跟你机械结构的质量、电机响应速度、传感器采样频率都有关系。上面这个范围只能作为起点不要照搬。3.3 仿真代码与结果解读在仿真阶段我用Python写了一个完整的离散时间模拟器。核心代码如下import numpy as np class InvertedPendulum: def __init__(self, M1.0, m0.1, l0.3, g9.8): self.M M self.m m self.l l self.g g self.I (1.0 / 12.0) * m * (2 * l) ** 2 self.dt 0.005 self.reset() def reset(self): self.x 0.0 self.dx 0.0 self.theta np.radians(5.0) self.dtheta 0.0 def step(self, F): # 非线性动力学数值积分用欧拉法或RK4 sin_t np.sin(self.theta) cos_t np.cos(self.theta) A_mat np.array([ [self.M self.m, self.m * self.l * cos_t], [self.m * self.l * cos_t, self.I self.m * self.l**2] ]) b_vec np.array([ F - self.m * self.l * self.dtheta**2 * sin_t, self.m * self.g * self.l * sin_t ]) acc np.linalg.solve(A_mat, b_vec) ddx, ddtheta acc[0], acc[1] self.x self.dx * self.dt self.dx ddx * self.dt self.theta self.dtheta * self.dt self.dtheta ddtheta * self.dt # 串级PID控制器 class CascadePID: def __init__(self): # 角度环PD self.kp_angle 100.0 self.kd_angle 15.0 # 位置环PD self.kp_pos 1.0 self.kd_pos 0.8 # 角度目标限幅 self.angle_limit np.radians(20.0) self.prev_angle_error 0.0 self.prev_pos_error 0.0 def update(self, pos, angle, target_pos0.0): pos_error target_pos - pos # 位置环输出目标角度 target_angle self.kp_pos * pos_error self.kd_pos * (pos_error - self.prev_pos_error) / dt target_angle np.clip(target_angle, -self.angle_limit, self.angle_limit) self.prev_pos_error pos_error # 角度环PD输出控制力 angle_error target_angle - angle force self.kp_angle * angle_error self.kd_angle * (angle_error - self.prev_angle_error) / dt self.prev_angle_error angle_error return force dt 0.005 pendulum InvertedPendulum() controller CascadePID()仿真结果中串级PID能把摆角从5度拉回到零但整个过程会出现几次衰减振荡角度曲线呈明显的阻尼振荡形态。需要留意的是位置环介入后小车会先向一个方向运动一段距离再缓慢回到原点。这个“先让位置跑出去再拉回来”的行为是串级PID的正常表现如果外环参数调得过强会出现位置和角度互相打架的极限环振荡这是调参时需要特别注意的。4. LQR控制最优控制的设计与实现4.1 LQR原理与设计目标LQR的全称是Linear Quadratic Regulator线性二次型调节器。它的基本思路是给定一个线性系统[ \dot{\mathbf{x}} A\mathbf{x} Bu ]找一个状态反馈控制 law[ u -K\mathbf{x} ]使得下面这个性能指标最小[ J \int_{0}^{\infty} (\mathbf{x}^T Q \mathbf{x} u^T R u) dt ]这里面Q矩阵的每个对角元表达了我们对某个状态偏差的“厌恶程度”R表达了对控制力大小的“厌恶程度”。如果把系统比作一辆车那你开车时既要尽快回到车道中央状态偏差小又不想猛打方向盘控制量小Q和R就是这两者之间的权重裁判。求解LQR的关键在于解Riccati方程。好消息是现实里我们根本不需要手动解这个矩阵方程。Matlab有lqr函数Python的scipy.signal也有lqr函数直接把A、B、Q、R扔进去就能返回最优反馈增益K。4.2 Q和R矩阵的选择方法与计算K值这是LQR设计里最需要经验的一步。Q、R矩阵的选择原则是Q中对应角度状态的权重和R之间的比值决定了系统的响应速度和执行器消耗之间的平衡。权重比越大系统越“激进”反应越快但控制力峰值也越大甚至可能超出电机能力。权重比太小系统会变得迟钝稳定时间拉长。我用的是归一化方法先预估每个状态变量的最大允许值然后按平方的倒数设置Q对角元。比如摆角允许偏差0.1弧度小车位置允许偏差0.1米速度允许偏差0.5米/秒角速度允许偏差1.0弧度/秒那么Q11 1 / (0.1)^2 100Q22 1 / (0.5)^2 4Q33 1 / (0.1)^2 100Q44 1 / (1.0)^2 1然后给R赋一个初始值比如1后续根据仿真结果调整。这个方法的优点是不需要凭感觉调各个状态之间的权重可以直接从物理意义推算出来。在Python中的求解过程如下from scipy.signal import lqr # A, B 由线性化模型得到 Q np.diag([100, 4, 100, 1]) R np.array([[1.0]]) K, S, E lqr(A, B, Q, R) print(LQR Feedback Gain K:, K)以我的参数为例计算出来的K大约是这样的形式K [-10.0, -11.5, -65.0, -10.3]这意味着控制力[ u -K\mathbf{x} 10.0x 11.5\dot{x} 65.0\theta 10.3\dot{\theta} ]注意θ的单位是弧度这里角度项的权重比位置项大得多因为5度的初始偏差约0.087弧度乘以65得到的控制量跟0.1米的位置偏差乘以10得到的控制量是同一量级说明权重分配是合理的。4.3 LQR仿真结果与PID对比在同样的5度初始偏差下LQR控制效果和PID有明显区别。LQR的角度曲线回落更快且几乎没有超调因为状态反馈相当于把四个状态变量位置、速度、角度、角速度同时纳入控制计算系统从第一时刻就知道所有状态的情况而不是像串级PID那样要一层层传递误差信息。实际控制力方面LQR在启动瞬间的控制力峰值明显高于PID这是因为它会“预判”系统未来的运动趋势一开始就施加较大的反向力矩。这一点要特别注意如果你是在真实硬件上做LQR参数选太激进的话电机电流瞬间可能过冲甚至触发驱动器的过流保护。LQR也并非万能。它的鲁棒性不如PID想象中那么高一旦真实系统与模型误差过大比如摆杆长度不准、电机响应延迟明显LQR的“最优性”会大幅下降。这也是为什么很多工业系统明知道LQR理论更漂亮最终还是用PID或PID的改进版。5. 裸机PID控制从仿真到真实硬件的最后一公里5.1 裸机PID控制的意义很多人在仿真里把PID调得飞起一到真机就傻眼。这个zip包里特意加入了裸机PID的工程代码就是为了解决“仿真能跑、实物趴窝”的问题。所谓“裸机PID”指的是不依赖操作系统和现成控制库在单片机上直接编写PID控制逻辑。这在实际项目中几乎是必备技能因为很多电机控制场景比如无人机、平衡车、机械臂关节并不会给你一个现成的控制框架你必须自己在中断服务函数里完成采样、计算PID、输出PWM的全过程。裸机PID与仿真PID最大的差异在于仿真里我们处理的是理想化的连续信号而裸机上你得面对传感器噪声、PWM分辨率、控制周期抖动、电源电压波动这些现实问题。解决好这些问题才算是真正“会做控制”。5.2 传感器、执行器与接口细节这个项目用的是常见的倒立摆硬件方案增量式旋转编码器测量摆杆角度导轨上还有一个编码器或电位器测量小车位置电机通过同步带驱动小车滑动。主控芯片我用的是STM32F103系列底层驱动封装好之后主循环只需要调用PID函数。传感器数据是裸机PID最容易翻车的地方。增量式编码器输出的是脉冲数需要通过M法或T法换算成角度和角速度。这里有一个关键陷阱直接对角度求差分得到的角速度噪声非常大因为编码器低通采样时量化误差会被差分放大。实测下来我必须先对角度做一阶低通滤波截止频率大约50Hz再计算角速度才能得到可用的微分项。如果你想逻辑更清晰可以直接用MPU6050这类IMU的角度输出但要注意零漂问题。5.3 电机控制中的PID落地要点PID的输出最终要换算成PWM占空比。这中间需要处理几个细节第一是电机死区。很多直流电机在PWM占空比小于某个阈值时不会转动表现为PID输出小时小车不动一加大又猛地一窜。解决办法是在PID输出做死区补偿如果输出绝对值小于死区阈值直接置零如果大于阈值把死区量叠加到输出上让实际控制量与命令值线性化。第二是输出限幅。PID计算出的控制力可能远超电机实际能力必须在最终输出前做限幅否则PID积分项会一直累积造成严重的积分饱和。更严格的做法是抗积分饱和anti-windup当输出达到限幅值时停止继续积分或积分值回退。第三是PWM方向处理。我用的电机驱动是H桥方向由两个IO来控制。PID输出为正时一个方向输出占空比输出为负时另一个方向输出占空比。看起来简单但实际调试时很容易在输出过零附近出现方向频繁切换导致电机嗡嗡响。我的解决办法是加一个滞回区间比如输出绝对值小于0.5%时强制输出零避免抖动。裸机PID的核心代码我放在了项目里结构上可以这样组织typedef struct { float kp; float ki; float kd; float integral; float prev_error; float output_limit; float integral_limit; } Pid; float Pid_Update(Pid *pid, float setpoint, float feedback, float dt) { float error setpoint - feedback; pid-integral error * dt; // 抗积分饱和 if (pid-integral pid-integral_limit) pid-integral pid-integral_limit; if (pid-integral -pid-integral_limit) pid-integral -pid-integral_limit; float derivative (error - pid-prev_error) / dt; pid-prev_error error; float output pid-kp * error pid-ki * pid-integral pid-kd * derivative; // 输出限幅 if (output pid-output_limit) output pid-output_limit; if (output -pid-output_limit) output -pid-output_limit; return output; }在STM32的主循环或定时器中断里调用这个函数的频率需要保持固定。我用的是1kHz定时器中断也就是控制周期1ms。千万不要在主循环里直接调用因为主循环的执行时间会受其他任务影响控制周期一旦抖动微分项的计算就会严重失真。5.4 从仿真参数到裸机参数的转换仿真的PID参数拿到真机上第一反应往往是“小车乱窜”因为仿真里力矩单位是牛顿而真机上你输出的是PWM占空比两者之间差了一个电机和机械传动的增益系数。我的做法是在真机上先做一次开环标定给电机一个固定占空比记录小车的加速度换算出一个近似的“占空比-加速度”比例系数。然后把这个系数作为前馈补偿抵消掉电机和机械传动的非线性。然后在这个基础上把仿真PID参数除以这个系数作为真机调参的起点。这样虽然不能保证一次成功但至少比从零开始盲调要快得多。另外由于真机存在各种摩擦和延迟实际能稳定的参数范围通常比仿真要窄。我实测下来真机上能把Kp调到50到80Kd调到5到10就已经很勉强再大就开始抖跟仿真里轻松上百的数值差距明显。6. 常见问题与排查技巧实录6.1 故障速查表我把调试过程中遇到的问题整理成了一个速查表方便你对着排查。现象可能原因排查方法解决办法摆杆一直朝一个方向倒下角度反馈符号接反手动旋转摆杆观察角度值是否随之增大/减小交换编码器A/B相或软件取反摆杆高频抖动微分项增益过大或传感器噪声大把Kd降为0看是否停止抖动加强低通滤波降低Kd小车猛烈来回撞限位角度环和位置环耦合过强先断开外环单独调内环降低位置环Kp增大角度环Kd摆杆能立住但小车一直漂移位置环积分不足或摩擦力不对称观察小车漂移方向增加外环积分项或做摩擦补偿有规律的嗡嗡声PWM频率落在人耳听觉范围内确认PWM频率把PWM频率提高到16kHz以上PID输出正常但电机不动电机死区开环测试最小启动占空比做死区补偿上电后摆杆快速倒下并伴有电流尖峰控制器参数过于激进检查输出限幅是否生效降低K或限制最大输出6.2 我踩过的坑与独家避坑技巧第一个坑是在角度单位上。仿真时我一直用弧度但真机编码器换算角度时我一开始用了度数degree结果PID参数按照弧度量级计算差了几十倍真机疯狂振荡。这个错误低级但极其隐蔽我只提醒一句在使用增量式编码器时角度单位必须全程统一最好在底层驱动里就固定用弧度不要在应用层反复换算。第二个坑是微分项的噪声放大。在仿真里微分项天经地义在真机上直接差分计算出的角速度信号噪声大到根本不能直接用。我试过把微分增益调到很小效果依然很差。最终解决方案不是换更贵的传感器而是在计算微分前对角度做移动平均滤波同时对滤波后的信号再做一次一阶低通实测噪声降低了一个数量级以上。如果你追求更极致的效果可以上卡尔曼滤波或互补滤波但对一阶倒立摆来说简单低通滤波足够。第三个坑是控制周期不稳定。我开始把PID放在主循环while(1)里执行结果因为LCD刷新、串口打印等任务阻塞控制周期从1ms到5ms随机波动系统时好时坏特别折磨人。后来把所有控制逻辑全部搬到定时器中断里任务调度简单粗暴但极其有效。如果你也遇到这种“仿真没事、真机随缘”的问题优先检查你的控制周期是否稳定。还有一个很重要的技巧是给控制器加“软启动”。开机瞬间不管控制器算出的PWM是多少我都让它在100ms内从零缓慢加到目标值避免上电瞬间电机猛冲造成机械冲击。这个处理虽然不在控制理论范围内但对硬件寿命和保护自己的调试心情来说非常值得。结语做一阶倒立摆这个项目我的体会是PID和LQR并不存在谁取代谁的关系它们在本质上是两种互补的控制思想。PID像是一位经验丰富的老师傅不需要懂模型也能把活干好但面对多变量耦合时会力不从心LQR则像一位理论扎实的工程师先建模再优化系统化程度高但极度依赖模型的准确性。如果你现在正准备做类似的平衡控制项目我强烈建议你在看完我这个项目后自己也手推一遍动力学方程不要把仿真代码复制粘贴了事。只有亲手把方程推出来再看着PID和LQR的结果差异你才会真正理解控制系统中‘模型’和‘反馈’这两个词的分量。再有一个小建议调参的时候一定要一次只改一个参数改完记录曲线这样才能建立‘参数到响应’的直觉比乱试一百次有效率得多。本文还有配套的精品资源点击获取 一阶倒立摆的PID与LQR控制：从建模到裸机实现
简介本资源是一阶倒立摆系统PID与LQR双策略控制的MATLAB完整实现方案面向计算机、电子信息工程、自动化及数学等专业的本科生适用于现代控制理论课程设计、期末大作业及毕业设计等实践环节。压缩包共83个文件926KB涵盖34个.mat数据文件含预置模型参数与仿真结果、12个.xml配置文件用于Simulink模型参数管理、10幅.jpg原理图与响应曲线图、8个.slxc与4个.slx Simulink仿真模型含可视化动画界面、5个.avi动态演示视频以及4个.m主控脚本和详细注释的README.md文档。已有62人学习下载。用户可直接运行附赠案例数据无需额外配置所有控制算法均采用参数化编程比例系数、权重矩阵等关键参数集中定义、清晰标注便于对比分析不同控制器对系统稳定性、响应速度与抗扰性的影响代码逻辑分层明确从建模、线性化、控制器设计到闭环仿真与性能评估形成完整闭环显著降低控制算法理解与复现门槛。 说句真心话倒立摆这个项目控制领域的人早晚都会碰一次。不管是课程设计、毕业设计还是自己折腾机器人我几乎没见过哪个搞控制的人能绕开它。所以整理这个“一阶倒立摆的PID控制和LQR控制.zip”的时候我其实把当年踩过的坑、调参的血泪、仿真到实物的跨越过程都塞进去了。这项目看上去只是两种控制算法的对比但真正做完你会发现它几乎覆盖了控制工程从建模、仿真、控制器设计到嵌入式落地的完整流程。如果你正准备做倒立摆或者刚拿到一个类似的控制项目不知道从哪里下手这个项目足够你少走不少弯路。里面用到的PID和LQR一个是工程界应用最广的反馈控制算法一个是现代控制理论中最经典的线性最优控制方法两个正好代表了经典控制与现代控制的典型思路。下面我会从物理建模到裸机实现把整个项目的核心细节拆开讲清楚。1. 项目的来龙去脉一阶倒立摆到底是什么1.1 控制界的“Hello World”与项目适用人群先说名字。所谓“一阶倒立摆”指的是一个小车在导轨上水平运动小车上方铰接一根摆杆系统目标就是让这根摆杆保持竖直向上。之所以叫“一阶”是因为它只有一个摆杆和一个铰链不是二级甚至三级串联倒立摆。别小看这个看似简单的结构它天然具备非线性、开环不稳定和欠驱动三个特性恰好是控制理论中最典型也最棘手的问题组合。打个比方这就像你手里托着一根竖直的长杆想让它一直立着你的手必须根据杆的倾斜不断移动。杆向左倒你手就得跟着向左接住它杆倒得越快你手移动也得越快。倒立摆的控制器做的事本质上就是这个“手”的动作——根据摆杆倾斜状态计算小车应该以多大加速度去追摆杆。PID和LQR就是两种计算“这个动作量”的不同策略。这个zip包适合三类人第一类自动化和控制工程相关专业的学生用来理解课本上的状态空间、反馈控制、最优控制到底怎么跟物理系统挂钩第二类做嵌入式或机器人开发的工程师尤其是想搞“裸机PID”来控制电机、机械结构的可以直接参考里面的工程实现第三类纯粹对控制算法好奇想通过仿真理解PID和LQR区别的爱好者。你完全不需要一开始就搞懂所有数学推导跟着项目里的仿真代码和调参流程走一遍也能获得非常直观的感受。1.2 物理模型与数学推导从运动方程到状态空间要设计控制器第一步永远是建模。整个倒立摆系统可以拆成两部分小车在导轨上的平移运动以及摆杆绕铰链的转动。这里有一个很重要的细节摆杆不是固定在小车上的它因为重力作用会自然下垂而我们要做的是通过控制小车的水平加速度让摆杆“停在”竖直向上这个不稳定平衡点。我采用的推导方式是牛顿-欧拉法。设小车质量为 M摆杆质量为 m摆杆质心到铰链的距离为 l摆杆绕质心的转动惯量为 I小车受到的水平控制力为 F。在平衡点附近摆角 θ 很小做线性化近似sinθ ≈ θcosθ ≈ 1忽略高阶小量。经过整理系统的线性化运动方程为投影到水平方向和摆杆转动方向[ (M m)\ddot{x} ml\ddot{\theta} F ][ ml\ddot{x} (I ml^2)\ddot{\theta} mgl\theta ]这里的符号约定是θ 从竖直向上的位置测量逆时针为正。从运动方程能看到一个关键信息——水平方向的力和摆角之间是耦合的这决定了系统不能用简单的单输入单输出思维去处理。把方程写成矩阵形式然后定义状态向量[ \mathbf{x} [x, \dot{x}, \theta, \dot{\theta}]^T ]经过整理可以得到标准的连续线性状态空间表达式[ \dot{\mathbf{x}} A\mathbf{x} Bu ]其中 u 就是控制力 F。A 矩阵和 B 矩阵的具体形式与 M、m、l、I、g 有关。这里我从一开始就声明实际应用时如果要更精确摩擦系数、导轨阻尼、电机死区这些都可以加进 B 矩阵或 A 矩阵但对于控制算法研究先忽略摩擦完全够用。1.3 开环系统特性为什么必须用反馈有了状态方程第一件事就是看开环系统的极点在复平面的什么位置。我们取一组典型物理参数小车质量 M 1.0 kg摆杆质量 m 0.1 kg摆杆质心到铰链距离 l 0.3 m重力加速度 g 9.8 m/s²细摆杆绕质心的转动惯量 I (1/12)·m·(2l)²。把这些参数代入状态方程后计算开环系统的特征值。结果一点都不意外四个特征值里面有一对实部为零的极点对应小车位置的积分特性还有一对实部一正一负的极点正实部那个就对应摆杆的“倾倒”趋势。也就是说如果不加任何控制摆杆稍微偏离竖直位置就会加速倒下这是物理上不可能稳定的系统。这就是必须引入反馈控制的原因。PID也好LQR也好本质上都是通过状态或输出反馈把这些开环极点移动到左半平面让系统在平衡点附近稳定。2. 技术方案选型为什么同时用PID和LQR2.1 两种控制方案的底层逻辑对比这个项目把PID和LQR放在一起做对比我觉得特别有价值。两种方法代表了两种完全不同的控制思维。PID的核心思路是误差驱动测量摆杆角度跟目标角度也就是零做差根据误差大小、误差累积、误差变化率这三个维度来决定控制量。它的优势是对模型依赖低你甚至不需要知道系统的精确参数也能调出能用的控制器。但它的短板也很明显处理多变量耦合系统时结构选型非常考验经验单环PID往往不够用需要串级甚至多环。LQR的核心思路则是“状态反馈 最优控制”先建立一个包含所有状态变量和控制代价的二次型性能指标然后通过求解Riccati方程得到一组最优反馈增益。它的优势是系统化只要模型可靠、权重矩阵量纲设计得合理一次性就能得到一组完整的状态反馈增益而且能保证一定的稳定裕度。缺点是输出结果是一个反馈矩阵不像PID那样可以直观地一个参数接一个参数地调遇到模型误差较大时理论最优性可能打折。我认为这个项目选这两种算法真正的意义在于让人体会“用经典方法解决问题”和“用现代方法解决问题”之间的思维方式差异。PID是在做“事后修正”LQR是在做“事前规划”。如果你能同时掌握这两种思维再去学MPC、滑模控制这类高级算法时理解门槛会大幅降低。2.2 建模取舍从非线性到线性化的坑在仿真项目里我做了两次模型处理。第一次是直接用完整非线性运动方程来搭被控对象模型这样能真实反映物理系统的行为。第二次是线性化到平衡点上得到A、B矩阵用线性模型来设计控制器。这里有一个很隐蔽的坑如果直接用非线性模型仿真但用基于线性模型设计的LQR当摆角初始偏差比较大比如超过15度时线性化误差会导致控制器性能明显下降甚至直接失稳。因为这个原因项目里我加了一个“摆角范围检测”逻辑一旦摆角过大先执行“甩起”控制或人工扶正等摆角进入可线性化的范围再切换到平衡控制。这个细节在教科书里基本不会讲但实际系统必须处理。2.3 仿真场景设计思路zip包里的仿真部分我设置的场景是摆杆初始偏离竖直方向5度速度为0小车初始位置为0目标是把摆杆稳定在竖直状态同时小车尽量回到原点。选择5度初始偏角是有讲究的它足够大能看出控制器的动态响应差异又不会大到突破线性化近似范围。在这个场景下PID和LQR的仿真结果会有明显差别正好用来对比两者的反应速度、超调量和控制力峰值。如果你想要更严苛的测试可以把初始角改成10度甚至15度两种算法的差距会更明显。3. PID控制设计与代码实现3.1 控制结构选择单环还是双环做倒立摆的PID控制最容易犯的错误就是试图用一个单环PID搞定一切。我之前第一次做的时候天真地只用角度误差来算控制力结果小车疯狂移动位置发散摆杆也稳不住。原因很简单单个角度环只能让摆杆变直但无法控制小车的位置小车会一直朝一个方向加速运动直到撞上限位。实际项目里我更推荐使用串级PID结构。外环是小车位置环输出的是目标摆角内环是摆角环输出的是控制力。这样小车位置偏移了外环会给一个轻微的角度偏移指令让摆杆略微倾斜再利用倾斜产生的水平分力把小车拉回来。这个思路本质上就是人在单车上保持平衡的原理——身体倾斜控制方向方向控制位置。内环使用PD控制就够了因为目标是让角度误差归零系统本身没有稳态摩擦累积积分项反而容易引入饱和问题。外环位置环则可以考虑加一点积分项用来消除静摩擦力导致的小车位置稳态误差。串级PID的结构如下位置环输入是小车位置目标反馈是实际位置输出是目标摆角限幅。角度环输入是目标摆角一般为0或由位置环给定反馈是实际角度输出是PWM/力信号。3.2 参数整定流程与经验值整定串级PID时我的经验是先内环后外环先比例后微分。第一步把外环断开固定目标摆角为零只调内环的Kp。从小到大逐步增加直到摆杆能在小扰动下回到竖直状态但会出现缓慢的等幅振荡说明Kp已经接近临界值再降回大约60%左右。第二步加入Kd看到阻尼增强、振荡明显收敛。Kd太大时高频噪声会被放大表现为电机发出尖锐噪声、摆杆高频抖动这时候需要果断回调。第三步接入外环位置环。位置环的Kp不需要太大太小的话小车回中点很慢太大会引起系统振荡甚至发散。位置环的Kd也同样重要它相当于对小车的速度做阻尼避免小车在零点附近来回振荡。以我项目中用的物理参数为例实测下来一个比较稳的起始参数范围是角度环 Kp80~120Kd10~20角度单位是弧度位置环 Kp0.5~2Kd0.5~2位置单位是米需要特别强调PID参数没有“万能值”它跟你机械结构的质量、电机响应速度、传感器采样频率都有关系。上面这个范围只能作为起点不要照搬。3.3 仿真代码与结果解读在仿真阶段我用Python写了一个完整的离散时间模拟器。核心代码如下import numpy as np class InvertedPendulum: def __init__(self, M1.0, m0.1, l0.3, g9.8): self.M M self.m m self.l l self.g g self.I (1.0 / 12.0) * m * (2 * l) ** 2 self.dt 0.005 self.reset() def reset(self): self.x 0.0 self.dx 0.0 self.theta np.radians(5.0) self.dtheta 0.0 def step(self, F): # 非线性动力学数值积分用欧拉法或RK4 sin_t np.sin(self.theta) cos_t np.cos(self.theta) A_mat np.array([ [self.M self.m, self.m * self.l * cos_t], [self.m * self.l * cos_t, self.I self.m * self.l**2] ]) b_vec np.array([ F - self.m * self.l * self.dtheta**2 * sin_t, self.m * self.g * self.l * sin_t ]) acc np.linalg.solve(A_mat, b_vec) ddx, ddtheta acc[0], acc[1] self.x self.dx * self.dt self.dx ddx * self.dt self.theta self.dtheta * self.dt self.dtheta ddtheta * self.dt # 串级PID控制器 class CascadePID: def __init__(self): # 角度环PD self.kp_angle 100.0 self.kd_angle 15.0 # 位置环PD self.kp_pos 1.0 self.kd_pos 0.8 # 角度目标限幅 self.angle_limit np.radians(20.0) self.prev_angle_error 0.0 self.prev_pos_error 0.0 def update(self, pos, angle, target_pos0.0): pos_error target_pos - pos # 位置环输出目标角度 target_angle self.kp_pos * pos_error self.kd_pos * (pos_error - self.prev_pos_error) / dt target_angle np.clip(target_angle, -self.angle_limit, self.angle_limit) self.prev_pos_error pos_error # 角度环PD输出控制力 angle_error target_angle - angle force self.kp_angle * angle_error self.kd_angle * (angle_error - self.prev_angle_error) / dt self.prev_angle_error angle_error return force dt 0.005 pendulum InvertedPendulum() controller CascadePID()仿真结果中串级PID能把摆角从5度拉回到零但整个过程会出现几次衰减振荡角度曲线呈明显的阻尼振荡形态。需要留意的是位置环介入后小车会先向一个方向运动一段距离再缓慢回到原点。这个“先让位置跑出去再拉回来”的行为是串级PID的正常表现如果外环参数调得过强会出现位置和角度互相打架的极限环振荡这是调参时需要特别注意的。4. LQR控制最优控制的设计与实现4.1 LQR原理与设计目标LQR的全称是Linear Quadratic Regulator线性二次型调节器。它的基本思路是给定一个线性系统[ \dot{\mathbf{x}} A\mathbf{x} Bu ]找一个状态反馈控制 law[ u -K\mathbf{x} ]使得下面这个性能指标最小[ J \int_{0}^{\infty} (\mathbf{x}^T Q \mathbf{x} u^T R u) dt ]这里面Q矩阵的每个对角元表达了我们对某个状态偏差的“厌恶程度”R表达了对控制力大小的“厌恶程度”。如果把系统比作一辆车那你开车时既要尽快回到车道中央状态偏差小又不想猛打方向盘控制量小Q和R就是这两者之间的权重裁判。求解LQR的关键在于解Riccati方程。好消息是现实里我们根本不需要手动解这个矩阵方程。Matlab有lqr函数Python的scipy.signal也有lqr函数直接把A、B、Q、R扔进去就能返回最优反馈增益K。4.2 Q和R矩阵的选择方法与计算K值这是LQR设计里最需要经验的一步。Q、R矩阵的选择原则是Q中对应角度状态的权重和R之间的比值决定了系统的响应速度和执行器消耗之间的平衡。权重比越大系统越“激进”反应越快但控制力峰值也越大甚至可能超出电机能力。权重比太小系统会变得迟钝稳定时间拉长。我用的是归一化方法先预估每个状态变量的最大允许值然后按平方的倒数设置Q对角元。比如摆角允许偏差0.1弧度小车位置允许偏差0.1米速度允许偏差0.5米/秒角速度允许偏差1.0弧度/秒那么Q11 1 / (0.1)^2 100Q22 1 / (0.5)^2 4Q33 1 / (0.1)^2 100Q44 1 / (1.0)^2 1然后给R赋一个初始值比如1后续根据仿真结果调整。这个方法的优点是不需要凭感觉调各个状态之间的权重可以直接从物理意义推算出来。在Python中的求解过程如下from scipy.signal import lqr # A, B 由线性化模型得到 Q np.diag([100, 4, 100, 1]) R np.array([[1.0]]) K, S, E lqr(A, B, Q, R) print(LQR Feedback Gain K:, K)以我的参数为例计算出来的K大约是这样的形式K [-10.0, -11.5, -65.0, -10.3]这意味着控制力[ u -K\mathbf{x} 10.0x 11.5\dot{x} 65.0\theta 10.3\dot{\theta} ]注意θ的单位是弧度这里角度项的权重比位置项大得多因为5度的初始偏差约0.087弧度乘以65得到的控制量跟0.1米的位置偏差乘以10得到的控制量是同一量级说明权重分配是合理的。4.3 LQR仿真结果与PID对比在同样的5度初始偏差下LQR控制效果和PID有明显区别。LQR的角度曲线回落更快且几乎没有超调因为状态反馈相当于把四个状态变量位置、速度、角度、角速度同时纳入控制计算系统从第一时刻就知道所有状态的情况而不是像串级PID那样要一层层传递误差信息。实际控制力方面LQR在启动瞬间的控制力峰值明显高于PID这是因为它会“预判”系统未来的运动趋势一开始就施加较大的反向力矩。这一点要特别注意如果你是在真实硬件上做LQR参数选太激进的话电机电流瞬间可能过冲甚至触发驱动器的过流保护。LQR也并非万能。它的鲁棒性不如PID想象中那么高一旦真实系统与模型误差过大比如摆杆长度不准、电机响应延迟明显LQR的“最优性”会大幅下降。这也是为什么很多工业系统明知道LQR理论更漂亮最终还是用PID或PID的改进版。5. 裸机PID控制从仿真到真实硬件的最后一公里5.1 裸机PID控制的意义很多人在仿真里把PID调得飞起一到真机就傻眼。这个zip包里特意加入了裸机PID的工程代码就是为了解决“仿真能跑、实物趴窝”的问题。所谓“裸机PID”指的是不依赖操作系统和现成控制库在单片机上直接编写PID控制逻辑。这在实际项目中几乎是必备技能因为很多电机控制场景比如无人机、平衡车、机械臂关节并不会给你一个现成的控制框架你必须自己在中断服务函数里完成采样、计算PID、输出PWM的全过程。裸机PID与仿真PID最大的差异在于仿真里我们处理的是理想化的连续信号而裸机上你得面对传感器噪声、PWM分辨率、控制周期抖动、电源电压波动这些现实问题。解决好这些问题才算是真正“会做控制”。5.2 传感器、执行器与接口细节这个项目用的是常见的倒立摆硬件方案增量式旋转编码器测量摆杆角度导轨上还有一个编码器或电位器测量小车位置电机通过同步带驱动小车滑动。主控芯片我用的是STM32F103系列底层驱动封装好之后主循环只需要调用PID函数。传感器数据是裸机PID最容易翻车的地方。增量式编码器输出的是脉冲数需要通过M法或T法换算成角度和角速度。这里有一个关键陷阱直接对角度求差分得到的角速度噪声非常大因为编码器低通采样时量化误差会被差分放大。实测下来我必须先对角度做一阶低通滤波截止频率大约50Hz再计算角速度才能得到可用的微分项。如果你想逻辑更清晰可以直接用MPU6050这类IMU的角度输出但要注意零漂问题。5.3 电机控制中的PID落地要点PID的输出最终要换算成PWM占空比。这中间需要处理几个细节第一是电机死区。很多直流电机在PWM占空比小于某个阈值时不会转动表现为PID输出小时小车不动一加大又猛地一窜。解决办法是在PID输出做死区补偿如果输出绝对值小于死区阈值直接置零如果大于阈值把死区量叠加到输出上让实际控制量与命令值线性化。第二是输出限幅。PID计算出的控制力可能远超电机实际能力必须在最终输出前做限幅否则PID积分项会一直累积造成严重的积分饱和。更严格的做法是抗积分饱和anti-windup当输出达到限幅值时停止继续积分或积分值回退。第三是PWM方向处理。我用的电机驱动是H桥方向由两个IO来控制。PID输出为正时一个方向输出占空比输出为负时另一个方向输出占空比。看起来简单但实际调试时很容易在输出过零附近出现方向频繁切换导致电机嗡嗡响。我的解决办法是加一个滞回区间比如输出绝对值小于0.5%时强制输出零避免抖动。裸机PID的核心代码我放在了项目里结构上可以这样组织typedef struct { float kp; float ki; float kd; float integral; float prev_error; float output_limit; float integral_limit; } Pid; float Pid_Update(Pid *pid, float setpoint, float feedback, float dt) { float error setpoint - feedback; pid-integral error * dt; // 抗积分饱和 if (pid-integral pid-integral_limit) pid-integral pid-integral_limit; if (pid-integral -pid-integral_limit) pid-integral -pid-integral_limit; float derivative (error - pid-prev_error) / dt; pid-prev_error error; float output pid-kp * error pid-ki * pid-integral pid-kd * derivative; // 输出限幅 if (output pid-output_limit) output pid-output_limit; if (output -pid-output_limit) output -pid-output_limit; return output; }在STM32的主循环或定时器中断里调用这个函数的频率需要保持固定。我用的是1kHz定时器中断也就是控制周期1ms。千万不要在主循环里直接调用因为主循环的执行时间会受其他任务影响控制周期一旦抖动微分项的计算就会严重失真。5.4 从仿真参数到裸机参数的转换仿真的PID参数拿到真机上第一反应往往是“小车乱窜”因为仿真里力矩单位是牛顿而真机上你输出的是PWM占空比两者之间差了一个电机和机械传动的增益系数。我的做法是在真机上先做一次开环标定给电机一个固定占空比记录小车的加速度换算出一个近似的“占空比-加速度”比例系数。然后把这个系数作为前馈补偿抵消掉电机和机械传动的非线性。然后在这个基础上把仿真PID参数除以这个系数作为真机调参的起点。这样虽然不能保证一次成功但至少比从零开始盲调要快得多。另外由于真机存在各种摩擦和延迟实际能稳定的参数范围通常比仿真要窄。我实测下来真机上能把Kp调到50到80Kd调到5到10就已经很勉强再大就开始抖跟仿真里轻松上百的数值差距明显。6. 常见问题与排查技巧实录6.1 故障速查表我把调试过程中遇到的问题整理成了一个速查表方便你对着排查。现象可能原因排查方法解决办法摆杆一直朝一个方向倒下角度反馈符号接反手动旋转摆杆观察角度值是否随之增大/减小交换编码器A/B相或软件取反摆杆高频抖动微分项增益过大或传感器噪声大把Kd降为0看是否停止抖动加强低通滤波降低Kd小车猛烈来回撞限位角度环和位置环耦合过强先断开外环单独调内环降低位置环Kp增大角度环Kd摆杆能立住但小车一直漂移位置环积分不足或摩擦力不对称观察小车漂移方向增加外环积分项或做摩擦补偿有规律的嗡嗡声PWM频率落在人耳听觉范围内确认PWM频率把PWM频率提高到16kHz以上PID输出正常但电机不动电机死区开环测试最小启动占空比做死区补偿上电后摆杆快速倒下并伴有电流尖峰控制器参数过于激进检查输出限幅是否生效降低K或限制最大输出6.2 我踩过的坑与独家避坑技巧第一个坑是在角度单位上。仿真时我一直用弧度但真机编码器换算角度时我一开始用了度数degree结果PID参数按照弧度量级计算差了几十倍真机疯狂振荡。这个错误低级但极其隐蔽我只提醒一句在使用增量式编码器时角度单位必须全程统一最好在底层驱动里就固定用弧度不要在应用层反复换算。第二个坑是微分项的噪声放大。在仿真里微分项天经地义在真机上直接差分计算出的角速度信号噪声大到根本不能直接用。我试过把微分增益调到很小效果依然很差。最终解决方案不是换更贵的传感器而是在计算微分前对角度做移动平均滤波同时对滤波后的信号再做一次一阶低通实测噪声降低了一个数量级以上。如果你追求更极致的效果可以上卡尔曼滤波或互补滤波但对一阶倒立摆来说简单低通滤波足够。第三个坑是控制周期不稳定。我开始把PID放在主循环while(1)里执行结果因为LCD刷新、串口打印等任务阻塞控制周期从1ms到5ms随机波动系统时好时坏特别折磨人。后来把所有控制逻辑全部搬到定时器中断里任务调度简单粗暴但极其有效。如果你也遇到这种“仿真没事、真机随缘”的问题优先检查你的控制周期是否稳定。还有一个很重要的技巧是给控制器加“软启动”。开机瞬间不管控制器算出的PWM是多少我都让它在100ms内从零缓慢加到目标值避免上电瞬间电机猛冲造成机械冲击。这个处理虽然不在控制理论范围内但对硬件寿命和保护自己的调试心情来说非常值得。结语做一阶倒立摆这个项目我的体会是PID和LQR并不存在谁取代谁的关系它们在本质上是两种互补的控制思想。PID像是一位经验丰富的老师傅不需要懂模型也能把活干好但面对多变量耦合时会力不从心LQR则像一位理论扎实的工程师先建模再优化系统化程度高但极度依赖模型的准确性。如果你现在正准备做类似的平衡控制项目我强烈建议你在看完我这个项目后自己也手推一遍动力学方程不要把仿真代码复制粘贴了事。只有亲手把方程推出来再看着PID和LQR的结果差异你才会真正理解控制系统中‘模型’和‘反馈’这两个词的分量。再有一个小建议调参的时候一定要一次只改一个参数改完记录曲线这样才能建立‘参数到响应’的直觉比乱试一百次有效率得多。本文还有配套的精品资源点击获取