反作用轮倒立摆实战：从LQR仿真到Arduino PID控制的完整实现

1. 项目概述：一个用反作用轮“驯服”倒立摆的实战记录

倒立摆，这个在控制理论教材里反复出现的经典不稳定系统，一直是检验控制器设计能力的“试金石”。传统的解决方案，比如让小车在轨道上跑来跑去保持摆杆平衡，虽然直观，但对执行机构的响应速度和移动范围要求极高，不太适合在桌面尺度上实现。这次，我想分享一个更“优雅”的方案：反作用轮倒立摆。它的核心思想不是移动底座，而是通过一个高速旋转的飞轮产生的反作用力矩，来对抗摆杆的倾倒趋势。这听起来有点像宇航员在太空转身——不推任何东西，只靠转动手臂。我们的目标，就是用一块Arduino板子、一个电机和一个IMU传感器，把这个理论变成现实。

整个项目走了一条标准的控制系统开发路径：从纸上谈兵的数学建模与MATLAB仿真，到充满妥协与意外的硬件搭建，最后是软件层面的滤波、控制与调试。你会看到LQR（线性二次型调节器）这种“高大上”的最优控制理论，如何与接地气的PID调参、卡尔曼滤波的手动整定相结合，最终让一个由乐高和3D打印件组成的简陋装置，稳稳地立在那里。无论你是对嵌入式系统感兴趣的学生，还是想亲手实践现代控制算法的工程师，这个项目都能提供一个从理论到落地的完整视角。接下来，我就拆开揉碎了，讲讲我们是怎么一步步把它做出来的。

2. 系统核心原理与方案选型

在动手焊第一根线之前，必须想清楚物理原理和控制策略。为什么选择反作用轮？LQR和PID到底该用哪个？这部分就是整个项目的“战略”层规划。

2.1 为什么是反作用轮？力学原理深析

传统倒立摆（小车-摆杆系统）通过控制底盘的平移加速度来产生使摆杆回正的力。这要求底盘电机有极快的加速能力和足够的行程空间。而反作用轮方案将执行机构“浓缩”到了摆杆本身上。其物理基础是角动量守恒定律：对于一个孤立系统，当合外力矩为零时，其总角动量保持不变。

具体到我们的系统：电机驱动反作用轮加速旋转，根据牛顿第三定律，电机会受到一个大小相等、方向相反的反作用力矩。这个力矩直接作用在摆杆-电机支架构成的刚体上，从而推动摆杆朝反方向转动。简单来说，想让摆杆向左转，就命令反作用轮向右加速旋转；当摆杆达到目标角度时，让反作用轮停止加速（保持匀速或减速），力矩消失，摆杆便停在那个位置。

注意：反作用轮产生的力矩大小与飞轮的转动惯量和角加速度成正比（τ = J * α）。因此，要产生足够的控制力矩，要么增加飞轮质量（转动惯量J），要么提高电机加速能力（角加速度α）。在桌面项目中，电机扭矩通常有限，因此设计一个质量分布靠外的大直径飞轮来增大J，是性价比更高的选择。

这种方案的优势非常明显：

1. 无需移动底座：整个系统可以固定在一个支点上，硬件结构大大简化。
2. 响应速度快：电机扭矩直接转化为摆杆的角加速度，没有中间传动机构的延迟。
3. 理论简洁：系统自由度相对单一，更易于建模和分析。

当然，缺点也存在：反作用轮存在“饱和”问题。当飞轮转速达到电机所能提供的最大值时，它就无法再通过加速来产生新的控制力矩了，此时系统失控。我们的软件中设计的“起跳”逻辑，正是为了应对这种情况。

2.2 控制策略对决：LQR vs. PID，以及为何结合使用

面对倒立摆这个不稳定系统，我们有两个主要的控制器候选：经典的PID和现代控制理论中的LQR。

• PID控制：基于误差的比例、积分、微分项进行计算。对于倒立摆，通常使用摆杆角度误差的PD（比例-微分）控制。它的优点是直观、易于实现、计算量小。但缺点是对非线性系统（如大角度摆动）效果不佳，且参数（Kp， Kd）需要手动调试，缺乏系统性设计方法，性能可能不是最优。
• LQR控制：全状态反馈的最优控制器。它需要一个系统的状态空间模型，通过求解一个代数Riccati方程，得到一个最优的状态反馈矩阵K。使用这个K，控制器命令u = -Kx，其中x是状态向量（如角度、角速度）。LQR的优点是它是从全局性能指标（权衡状态误差和控制能量消耗）出发，通过数学优化得出的控制器，理论上是“最优”的，并且天然地处理多变量系统（同时考虑角度和角速度）。

在我们的项目中，我们采取了“仿真用LQR，实机用PID”的混合策略。为什么？

1. 设计阶段：在MATLAB中，我们基于线性化模型设计LQR控制器。这个过程让我们深刻理解了系统的能控性、需要多大的反馈增益，并通过阶跃响应、鲁棒性分析验证了理论上的可行性。LQR为我们提供了性能上限和设计指导。
2. 实现阶段：转移到Arduino实机时，我们面临模型失配（乐高结构有间隙、线缆有阻力、电机非线性）、传感器噪声、计算资源有限等问题。此时，完全依赖精确模型的LQR控制器性能会下降。而PID控制器结构简单，参数物理意义明确（P项对应“刚度”，D项对应“阻尼”），非常适合在现场通过“观察-调整”的方式快速调试。此外，我们的状态观测（角度和角速度）本身也来自传感器融合，并非完美。

因此，LQR在前期提供了科学的设计依据，而PID在后期提供了灵活的工程实现手段。两者结合，确保了项目从理论到实践的成功过渡。

3. 从理论到仿真：系统建模与控制器设计

这一部分是项目的基石，所有后续的硬件和软件工作都建立在正确的数学模型之上。

3.1 建立系统动力学模型

我们首先对系统进行受力分析。将系统简化为两个主要部分：摆杆（含电机、飞轮支架）和反作用轮。定义以下参数：

• θ: 摆杆偏离竖直向上位置的角度（顺时针为正）。
• φ: 反作用轮相对于摆杆的转角。
• J_p: 摆杆（不含飞轮）绕支点的转动惯量。
• J_w: 反作用轮绕其自身转轴的转动惯量。
• m_p, m_w: 摆杆和飞轮的质量。
• l_p: 摆杆质心到支点的距离。
• τ: 电机施加在反作用轮上的扭矩（同时也是反作用在摆杆上的扭矩）。

应用刚体转动定律，可以分别列出摆杆和反作用轮的动力学方程：

1. 对于摆杆：受到重力矩和电机反作用力矩。J_p * θ'' = m_p * g * l_p * sinθ - τ（注意：这是简化模型，实际τ的方向与φ''有关）
2. 对于反作用轮：只受电机扭矩驱动。J_w * (θ'' + φ'') = τ

通过联立方程，可以消去内部扭矩τ，得到描述系统运动的状态方程。这是一个非线性方程，因为含有sinθ项。

3.2 线性化与状态空间表达

倒立摆的控制目标是在竖直向上位置（θ=0）附近保持平衡。在这个平衡点附近，我们可以进行线性化：sinθ ≈ θ。将非线性方程在θ=0， θ'=0， φ'=0处进行线性化处理，得到线性微分方程。

选择状态变量为：x = [θ, θ', φ']^T。即摆杆角度、摆杆角速度、飞轮转速。控制输入u为电机电压（与扭矩τ成正比）。经过推导，可以得到标准的状态空间方程：x' = A * x + B * uy = C * x + D * u

其中，输出y通常就是我们能测量到的量，比如摆杆角度θ。通过计算系统的能控性矩阵，可以验证这个系统是完全能控的，意味着理论上我们可以通过控制电机电压，将系统状态驱动到任意位置（包括平衡点）。

3.3 基于MATLAB的LQR控制器设计与仿真

在MATLAB中，我们首先根据实际的物理参数（通过测量或估算得到）计算出A和B矩阵。接下来就是LQR设计：

1. 选择权重矩阵Q和R：这是LQR设计的艺术所在。Q是对状态误差的惩罚矩阵，R是对控制输入的惩罚矩阵。我们希望摆杆角度和角速度尽快归零，所以给Q矩阵中对应θ和θ'的元素赋予较大的值。同时，我们不希望电机命令过大（耗电、可能饱和），所以给R一个正数。通常从简单的对角矩阵开始，例如Q = diag([q1, q2, q3])， R = 1。通过调整q1， q2， q3的相对大小，可以调节控制器对角度、角速度、飞轮转速的“重视程度”。
2. 求解Riccati方程与反馈矩阵K：在MATLAB中，这步非常简单，一行命令搞定：K = lqr(A, B, Q, R)。这个K就是最优状态反馈增益矩阵。
3. 构建闭环系统并仿真：闭环系统动态为x' = (A - B*K) * x。使用step()或initial()命令仿真闭环系统的阶跃响应或初始条件响应。观察摆杆角度是否能快速、平稳地收敛到0，且没有超调或持续振荡。同时，检查控制输入u的幅度是否在电机实际能力范围内。

我们通过仿真反复调整Q和R，直到得到一个响应快、超调小、控制量合理的控制器。这个仿真结果为我们后续的实机调试树立了一个“理想目标”。

4. 硬件系统搭建与核心器件选型

理论仿真很完美，但硬件是实现所有想法的基础。这里的每一个选择都充满了工程上的权衡。

4.1 机械结构设计与实现要点

结构决定了系统的物理特性（转动惯量、摩擦、共振频率），至关重要。

1. 主体框架：我们选择了乐高积木。它的优势是快速原型、易于修改、连接牢固。支点使用了乐高轴和孔梁，确保摆杆只能在一个平面内转动。关键点：支点的摩擦力要尽可能小。我们使用了乐高的光滑轴套，并确保没有横向挤压。
2. 反作用轮：这是核心执行机构。为了获得大的转动惯量J_w，我们使用3D打印了一个质量圈（rim-weighted wheel）。设计时，在保证结构强度的前提下，将质量尽可能分布在边缘。材料选用PLA，如果追求更高性能，可以填充配重（如嵌入螺母）。
3. 电机选型与安装：我们最初尝试了ELEGOO套件中的520电机（小型直流减速电机），扭矩不足，只能维持平衡，无法实现从下垂位置的“起跳”。后来升级为370直流电机（更大尺寸），扭矩显著提升。电机通过扎带和乐高件直接固定在摆杆顶部。一个重大教训：电机振动会直接传递到紧挨着的MPU6050传感器，严重污染加速度计数据。在最终版本中，我们虽未物理隔离，但通过软件滤波（卡尔曼）极力抑制了这部分影响。
4. 配重设计：我们在摆杆底部（电机对侧）增加了可调节的配重块。这有两个目的：一是降低整个摆杆的质心，减小重力矩（m_p * g * l_p），使得电机需要的平衡力矩变小；二是调整摆杆自身的转动惯量J_p。这是一个非常重要的调试手段。
5. 线缆管理：这是最容易被忽略但影响巨大的细节！连接电机和传感器的电线如果太硬或太紧，会产生额外的扭转刚度，相当于给摆杆增加了一个不可预测的弹簧，严重干扰动力学模型。我们最终选用了极细的硅胶导线，并留出足够的松弛度，确保其产生的力矩可忽略不计。

4.2 电路设计与电源管理

电路是系统的神经和血管，稳定可靠是唯一要求。

• 主控：Arduino Nano Every。选择它是因为尺寸小巧，引脚够用，且价格便宜。其16MHz的主频对于运行PID循环和卡尔曼滤波绰绰有余。
• 传感器：MPU6050。这是一款集成了3轴加速度计和3轴陀螺仪的IMU芯片，通过I2C通信。它提供了计算角度所需的最原始数据：加速度计感知重力方向，陀螺仪感知旋转角速度。
• 电机驱动：BTS7960大电流H桥模块。为什么不用L298N？因为BTS7960的驱动能力更强（峰值电流可达40A以上），内阻更小，发热更少，并且自带隔离和保护电路。它只需要两个PWM引脚（分别控制两个半桥）即可实现电机的正反转和调速，控制逻辑简单。
• 电源方案：
  • 逻辑部分：Arduino和MPU6050、BTS7960的逻辑电平端，由电脑USB口供电（5V）。这是最稳定的来源。
  • 电机部分：由一块2S锂聚合物电池（标称7.4V）独立供电。非常重要！必须将电机电源与逻辑电源分开。电机启停会造成巨大的电压波动和噪声，如果共用电源，会直接导致Arduino复位或传感器读数异常。BTS7960模块有独立的电源输入端子（VM）和逻辑电平端子（VCC），正好实现隔离。
• 布线要点：
  1. 电机驱动线（连接BTS7960输出到电机）要粗而短，以减少损耗和电磁干扰。
  2. I2C线（SDA， SCL）上最好加上拉电阻（通常MPU6050模块已集成）。
  3. 所有电源走线尽量粗，并在靠近芯片的位置（如Arduino的5V和GND引脚之间）并联一个100nF的陶瓷电容进行去耦。

5. 软件算法实现：从数据采集到控制输出

硬件是躯体，软件是灵魂。这一部分将传感器数据转化为稳定的控制命令。

5.1 传感器数据融合：卡尔曼滤波的实战调参

MPU6050给出的原始数据不能直接使用：

• 加速度计：通过测量重力加速度在轴上的分量，可以计算出姿态角。angle_acc = atan2(accY, accZ) * 180/PI。它在静态或缓变时很准，但对振动（如电机振动）极其敏感，高频噪声大。
• 陀螺仪：测量角速度，通过积分可以得到角度变化。angle_gyro += gyroX * dt。它短期精度高、响应快，但存在漂移（积分误差会随时间累积）。

我们需要融合两者优点，得到准确、实时、平滑的角度估计。这就是卡尔曼滤波的用武之地。我们将系统状态设为[角度， 角速度]，建立简单的状态转移模型，将加速度计读数作为观测量。

然而，理论很丰满，现实很骨感。我们遇到了两个问题：

1. 模型不准：我们的物理模型是理想化的，实际的乐高结构存在非线性摩擦、间隙，电机振动更是难以建模的干扰。
2. 采样周期不定：Arduino的loop()函数执行时间并非严格恒定，取决于代码执行路径，导致积分步长dt有微小变化。

因此，我们放弃了基于精确模型计算卡尔曼增益矩阵的方法，转而采用手动调参的互补滤波思想，但披着卡尔曼滤波的外衣（即我们只使用了其预测-更新框架，但Q、R矩阵作为调参旋钮）。我们编写了一个调试程序，将加速度计角度（蓝色）、陀螺仪积分角度（红色）和滤波后角度（绿色）通过串口发送到电脑绘图。然后手动摆动摆杆，同时让电机空转模拟振动，实时调整过程噪声协方差矩阵Q和测量噪声协方差矩阵R的参数，直到绿色曲线既快速跟随真实角度变化（响应性），又能有效滤除电机振动引起的蓝色曲线的毛刺（平滑性）。

实操心得：最终，我们采用的是一种强依赖于陀螺仪，仅用加速度计进行缓慢校正的策略。等效的互补滤波公式类似于：angle = 0.98 * (angle + gyro * dt) + 0.02 * angle_acc。这个0.98和0.02就是通过上述“手调”过程确定的。关键在于，在振动大的时候，要更相信陀螺仪（给加速度计的权重更低）。

5.2 控制律实现与PID整定策略

在得到可靠的摆杆角度θ和通过陀螺仪直接获得的角速度θ_dot后，就可以进行控制了。我们实现了PD控制器：motor_command = Kp * (θ_target - θ) + Kd * (0 - θ_dot)其中θ_target是目标角度（竖直向上为0）。注意角速度误差项是(0 - θ_dot)，因为我们希望角速度为零（静止平衡）。

PID整定过程（“玄学”的艺术）：

1. 先调Kp（比例）：将Kd设为0。逐渐增大Kp，直到摆杆在平衡点附近开始出现等幅振荡。这个Kp值记为Ku。
2. 再调Kd（微分）：引入Kd，它提供阻尼。逐渐增大Kd，观察振荡是否被抑制。目标是消除振荡，让摆杆能快速且无超调地回到平衡点。Kd太大会导致系统响应迟钝，太小则阻尼不足。
3. 微调：在平衡点附近手动轻推摆杆，观察其恢复能力。调整Kp和Kd，在“响应速度”和“稳定性”之间取得平衡。我们的目标是：受到小扰动后，能在1-2个摆动周期内稳定下来，且不出现大幅振荡。

5.3 抗饱和与“起跳”逻辑设计

这是让项目从“能平衡”升级到“能起摆”的关键。

• 问题：当摆杆从自然下垂位置（θ=180度）启动时，误差极大。如果直接套用PD公式，计算出的motor_command会要求电机全力加速。但电机扭矩和飞轮转速有限，很快飞轮就会达到最大转速（饱和）。此时，电机无法再产生新的控制力矩，摆杆无法被拉起。
• 解决方案：我们设计了一个状态机逻辑。
  1. 正常平衡模式：当摆杆角度在平衡点附近（例如±30度内）时，运行标准的PD控制。
  2. 起跳模式：当检测到摆杆处于下垂位置（角度绝对值很大）且电机命令持续输出最大值但摆杆角速度变化很小时，判定为飞轮饱和，系统“卡住”。
  3. 执行“起跳”：程序强制让电机反向驱动，使飞轮快速减速至零甚至反向旋转。这个过程会在短时间内给摆杆施加一个巨大的反向力矩冲击（角冲量），足以将摆杆向上“踢”起。
  4. 切换回平衡：一旦摆杆角度进入平衡模式的范围，立即切换回PD控制，接管并稳定系统。

这个逻辑使得系统具备了“自救”能力，即使被手动扳倒，也能自己挣扎着重新站起来，大大增强了鲁棒性。

6. 系统集成调试与问题排查实录

这是将理论、硬件、软件拼凑在一起并让它工作的过程，也是最耗时、最考验耐心的阶段。

6.1 调试流程与工具

一个科学的调试流程能事半功倍：

1. 分模块测试：
   • 传感器：单独编写程序，通过串口监视器输出MPU6050的原始数据和计算出的角度，用手转动模块，检查数据是否合理、响应是否灵敏。
   • 电机驱动：单独测试BTS7960，用程序让电机正转、反转、调速，确认接线正确，PWM信号有效。
   • 卡尔曼滤波：如前所述，使用绘图工具可视化滤波效果，确保角度估计准确平滑。
2. 开环测试：将控制律注释掉，用固定占空比的PWM驱动电机。观察摆杆在恒定力矩下的运动是否符合预期（例如，电机正转，摆杆应缓慢向反方向加速）。这验证了从命令到力矩的基本链路是通的。
3. 闭环调试：先用手扶住摆杆在平衡点附近，然后给一个很小的Kp值，松开手。观察系统是缓慢倒下还是发散的振荡。逐步增加Kp、引入Kd，遵循前述整定方法。
4. 串口数据记录：在控制循环中，将关键变量（如角度、角速度、电机命令、飞轮转速估计值）通过串口输出。可以保存为文件，后期用MATLAB或Python绘图分析，这对于诊断复杂问题（如极限环振荡、饱和现象）至关重要。

6.2 常见问题与解决方案速查表

下表列出了我们在开发过程中遇到的主要问题及解决方法：

6.3 性能优化与最终效果

经过多轮迭代调试，我们得到了两个版本的平衡效果：

• V1（温和版）：使用520电机，PD参数调校得比较保守。该系统在手动扶正至平衡点附近后，可以非常平滑、稳定地保持平衡，抗轻微干扰能力强。但因为扭矩有限，无法实现自主起跳。
• V2（激进版）：升级为370电机，采用更具攻击性的PD参数，并完整实现了抗饱和起跳逻辑。这个版本可以从自然下垂位置自行摆动并起跳至平衡状态，展示了完整的自恢复能力。代价是平衡时的稳态抖动稍大，需要更长时间达到完全平静。

这两个版本体现了控制系统设计中的经典权衡：鲁棒性与性能。V1更鲁棒（对模型误差不敏感），V2性能更强（控制作用更猛）。选择哪个，取决于你的具体需求。

7. 项目总结与延伸思考

回顾整个项目，它是一次非常典型的“理论指导实践，实践修正理论”的工程演练。LQR给了我们一个科学的设计起点和性能参考，但最终让这个乐高机器立起来的，是那些教科书上不会写的细节：硅胶导线的选用、卡尔曼滤波矩阵的手动微调、对抗电机振动的软硬件策略、以及那个让系统“绝地求生”的起跳逻辑。

这个项目的意义远不止于让一个摆杆立起来。它涉及的状态空间建模、最优控制、传感器融合、嵌入式实时编程、硬件抗干扰等知识，是机器人、无人机、自动驾驶等领域的通用基础。你可以在此基础上进行无数扩展：比如，将摆杆放在一个移动小车上，实现“旋转倒立摆”或“两轮平衡车”；或者引入视觉传感器，让摆杆去追踪一个移动的目标；甚至尝试用更复杂的控制算法，如滑模控制或模型预测控制，来应对更大的干扰和不确定性。

最后，给想要复现或改进这个项目的朋友几点建议：一是务必重视机械结构的刚性和低摩擦，这是好模型的物理基础；二是调试时务必循序渐进，从开环测试到简单的P控制，再到完整的PD，每一步都确认无误；三是善用数据可视化工具，眼睛看不到的细节，数据图表会告诉你。控制工程就是这样，一半是科学，一半是艺术，而所有的艺术，都藏在那些耐心调试和解决问题的细节之中。