如果你正在为2026年全国大学生电子设计竞赛（电赛）做准备，或者对控制理论、嵌入式系统感兴趣，那么“杆球平衡”这个经典的控制问题，很可能就是你绕不开的“拦路虎”。它看似简单——让一个小球在倾斜的杆上保持平衡，但背后却融合了传感器融合、实时控制、运动补偿、嵌入式编程等硬核技术。

很多人以为，杆球平衡的核心就是写一个PID控制器。但真正在比赛中，尤其是在“运动补偿”这个附加条件下，你会发现事情远没有那么简单。当整个系统（包括杆和球）都在一个移动的平台上（比如小车、旋转台）时，外界的扰动会直接破坏你精心调好的平衡。这时，传统的固定参数PID往往会失效，系统要么剧烈振荡，要么直接失控。

这篇文章要解决的，正是这个进阶难题：如何在存在外部运动干扰的情况下，实现杆球系统的鲁棒平衡控制？我们将不局限于理论推导，而是从实战出发，拆解“运动补偿”的真正含义，并提供一个从硬件选型、软件框架到核心算法实现的完整方案。无论你是电赛新手，还是希望深化控制工程理解的开发者，读完本文，你将能：

1. 理解杆球平衡系统引入“运动补偿”后的核心挑战与解决思路。
2. 搭建一套包含惯性测量单元（IMU）和视觉/位置反馈的硬件原型。
3. 实现一个结合了状态观测器与自适应前馈补偿的控制算法框架。
4. 动手完成从建模、仿真到实际调参的全流程，并掌握关键调试技巧。

我们直接进入正题。

1. 杆球平衡+运动补偿：到底难在哪里？

首先，我们要明确“杆球平衡”和“运动补偿”这两个任务叠加后，带来的新问题。

一个基础的杆球平衡系统，通常假设底座（电机驱动的杆）是固定在世界坐标系中的。控制器只需要根据小球在杆上的位置（x）和速度（\dot{x}），以及杆的倾斜角度（θ）和角速度（\dot{θ}），计算出合适的电机扭矩，使小球稳定在杆的中心。

其状态空间方程可以简化为围绕平衡点的线性化模型，核心是调节四个状态量：[x, \dot{x}, θ, \dot{θ}]。

然而，“运动补偿”彻底改变了游戏规则。题目中的“运动补偿”通常意味着：整个杆球系统的底座本身也在运动。例如：

• 场景A（平移）：杆球系统安装在一个沿直线运动的小车上，小车有加速度。
• 场景B（旋转）：杆球系统安装在一个转台上，转台产生离心力和科氏力。
• 场景C（复合）：底座存在无法预测的、或已知规律的振动。

这时，小球感受到的“重力”方向在惯性坐标系中发生了变化。对于小球而言，它不再仅仅受到重力沿杆方向的分量，还叠加了一个由底座运动引起的“虚拟”力（惯性力）。控制器如果仍然使用原始的、基于固定重力方向的模型，其控制命令必然是错误的。

真正的难点在于“感知”与“解耦”：

1. 感知难点：你需要一个额外的传感器（通常是IMU）来实时测量底座本身的加速度和角速度。但IMU数据包含噪声，且测量的是“比力”（特定力），是惯性力与引力的矢量和，需要巧妙处理才能分离出我们需要的运动干扰。
2. 建模难点：系统的动力学方程变得复杂，引入了与底座运动加速度相关的耦合项。你需要重新推导状态方程，或者设计一个能在线估计并补偿这些干扰项的控制器。
3. 控制难点：干扰可能是时变的、非线性的。简单的PID反馈可能跟不上干扰的变化速度，导致稳态误差或失稳。需要引入前馈控制或自适应控制来主动抵消干扰。

所以，解决“杆球平衡运动补偿”问题的核心思路是：在经典反馈控制回路的基础上，增加一个基于IMU测量的前馈补偿通道，实时抵消底座运动对小球动力学的影响。

2. 核心概念与系统架构

在深入代码之前，我们先厘清几个关键概念和整个系统的架构。

2.1 关键概念解析

• 状态空间模型：我们将系统描述为一组状态变量（小球位置/速度，杆角度/角速度）的微分方程。控制器目标是通过输出（电机电压/占空比）将这些状态驱动到零（平衡点）。
• 线性二次型调节器（LQR）：这是一种最优状态反馈控制器。对于线性化后的杆球模型，LQR能计算出最优的状态反馈增益矩阵K，使控制量u = -Kx在最小化二次型代价函数（权衡状态误差和控制能量）的意义上最优。它是比PID更系统、性能往往更好的选择。
• 惯性测量单元（IMU）：通常包含三轴加速度计和三轴陀螺仪。加速度计测量的是“比力”，单位是m/s²。在静止时，它测量的是重力加速度。当底座运动时，其输出是重力加速度与底座运动加速度的矢量和。
• 运动补偿（前馈控制）：反馈控制是根据“误差”来反应。前馈控制则是根据“已知的干扰”提前行动。在这里，我们通过IMU实时测量到底座的线加速度和角速度，将其代入动力学模型，计算出为抵消此干扰所需的额外控制量，直接叠加到反馈控制输出上。
• 状态观测器（卡尔曼滤波器）：我们可能无法直接测量所有状态（如小球速度）。此外，传感器数据有噪声。卡尔曼滤波器可以利用系统模型和带噪声的测量值，最优地估计出系统的真实状态，是提升系统鲁棒性的关键。

2.2 系统整体架构

一个完整的、带运动补偿的杆球平衡系统，其软件控制架构可以设计如下：

[ 外部干扰 ] --> [ 底座运动 ] --> [ 杆球物理系统 ] ^ | | | v v [ IMU ] [ 传感器 ] [ 执行器 ] | (编码器) (电机) v | | [ 数据融合与状态估计 ] <--[ 数据采集 ] | | | v | [ 前馈补偿器 ] ---> (+) <---[ 反馈控制器(LQR/PID) ] | v [ 控制输出 u ]

工作流程：

1. 感知层：编码器（或视觉）测量小球位置x，电位计或编码器测量杆角度θ。IMU测量底座的加速度a_base和角速度ω_base。
2. 估计层：将小球位置、杆角度以及IMU的原始数据送入一个扩展卡尔曼滤波器（EKF）或互补滤波器。滤波器输出对全部系统状态（x, \dot{x}, θ, \dot{θ}）以及底座干扰力的最优估计。
3. 控制层：
   • 反馈回路：LQR控制器根据估计出的状态x_hat，计算基础控制量u_fb = -K * x_hat。
   • 前馈回路：前馈补偿器根据估计出的底座加速度a_base_hat和系统模型，计算补偿控制量u_ff。
4. 执行层：最终控制量u = u_fb + u_ff经过限幅和转换（如PWM占空比），驱动电机。

3. 硬件准备与选型建议

对于电赛或自行实验，一套典型的硬件清单如下：

• 主控单元：STM32F4系列或ESP32。STM32F4（如F407）浮点运算能力强，外设丰富，适合复杂控制算法。ESP32集成Wi-Fi/蓝牙，便于调试和数据传输。
• 执行机构：直流减速电机（带编码器）或步进电机。需要足够的扭矩和响应速度。推荐使用带编码器的直流电机+电机驱动板（如DRV8833、TB6612）。
• 杆与球：轻质刚性杆（如碳纤维管），小球可用钢珠或乒乓球。减小转动惯量有利于控制。
• 小球位置检测：
  • 方案一（推荐）：线性CCD摄像头（如TSL1401）。精度高，非接触。
  • 方案二：超声波传感器（如HC-SR04）。成本低，但易受干扰。
  • 方案三：电阻式位移传感器。简单直接，但有摩擦。
• 杆角度检测：高精度电位计或绝对值编码器。MPU6050等IMU自带的陀螺仪积分得到角度会漂移，不适合单独作为角度反馈。
• 运动感知（核心）：MPU6050（六轴IMU）或MPU9250（九轴）。这是实现运动补偿的关键传感器，必须安装在与杆底座固连的PCB上。
• 电源：稳定的双电源（如12V给电机，5V/3.3V给控制板和传感器）。电机瞬间电流大，务必做好电源去耦，防止单片机复位。

连线示意图（以STM32F4为例）：

• MPU6050：SDA->PB9,SCL->PB8,VCC->3.3V,GND->GND
• 电机驱动IN1/IN2->定时器PWM输出引脚(如PA8,PA9)
• 电机编码器A/B相->定时器编码器模式引脚(如PA0,PA1)
• 线性CCD SI/CLK/AO->任意GPIO和ADC引脚
• 电位计输出->ADC引脚

4. 系统建模与控制器设计

我们从一个简化的平面模型开始。假设底座只有沿杆方向的线加速度a_x（由IMU的X轴加速度计测量，需扣除重力分量）。

4.1 动力学方程推导（含底座加速度）

设：

• m: 小球质量
• r: 杆长度（小球到转轴距离）
• θ: 杆与竖直向下方向的夹角（顺时针为正）
• x: 小球在杆上的位置（转轴为原点，向右为正）
• g: 重力加速度
• a_x: 底座在水平方向（沿杆方向）的加速度（向右为正）
• τ: 电机施加在杆上的扭矩

小球在非惯性系（随杆转动的坐标系）中的运动方程，考虑底座加速度a_x后，可以推导出（推导过程略，关注结果）：

\ddot{x} = (g sinθ + a_x cosθ) + r \dot{θ}^2 - (2 \dot{x} \dot{θ}) / r?(这是一个更复杂的表达式)

实际上，更常见且实用的方法是在平衡点 (θ≈0, x≈0) 附近进行线性化。当θ很小时，sinθ ≈ θ,cosθ ≈ 1。线性化后的状态方程形式为：

\dot{x} = A x + B u + D d

其中：

• 状态向量x = [x, \dot{x}, θ, \dot{θ}]^T
• 控制输入u是电机扭矩（或等效电压）
• 干扰输入d就是底座的加速度a_x
• A,B是系统矩阵和控制矩阵，与经典杆球模型相同。
• D是干扰输入矩阵，它描述了a_x如何影响各个状态的微分。

通过分析或系统辨识，可以确定D矩阵。对于水平底座加速度干扰，它主要影响小球位置的加速度方程。一个典型的近似是：\ddot{x} 项中包含了 -a_x（即底座向右加速，小球相对向左“滑”）。

4.2 LQR反馈控制器设计

即使有干扰，我们仍先为标称系统（d=0）设计LQR控制器。

1. 建立线性化状态方程：根据你的具体硬件参数（m, r等），计算或辨识出A,B矩阵。
2. 定义代价函数：J = ∫ (x^T Q x + u^T R u) dt。Q是状态权重矩阵（对角阵），R是控制输入权重。
   • 增大Q中对角线上对应x和θ的权重，意味着控制器更迫切地希望减小位置和角度误差，但可能导致控制量过大。
   • 增大R意味着更“珍惜”控制能量，响应会变慢。
3. 求解Riccati方程：使用MATLAB、Python（control库）或在线工具求解。得到最优状态反馈增益矩阵K。

# Python示例：使用control库计算LQR增益 import numpy as np import control as ct # 假设已得到线性化后的A, B矩阵 # A = np.array([[...]]) # B = np.array([[...]]).reshape(-1,1) # 定义权重矩阵 Q = np.diag([10.0, 0.1, 100.0, 0.1]) # 重视位置x和角度θ R = np.array([[0.1]]) # 计算LQR增益K K, S, E = ct.lqr(A, B, Q, R) print("LQR 增益矩阵 K:") print(K)

4.3 前馈补偿器设计

前馈控制的目标是选择u_ff，使得干扰d对状态x的影响被完全或部分抵消。

对于线性系统，如果干扰是可测的，理想的前馈控制律可以通过求解方程得到。一个常用且直观的方法是：计算抵消干扰所需的稳态控制量。

假设干扰d（底座加速度）是常数或慢变的。在稳态时，我们希望状态导数\dot{x} = 0。代入状态方程：0 = A x + B u + D d同时，反馈控制u_fb = -K x致力于将x推向0。在稳态平衡点，我们期望x=0。那么上式在x=0时简化为：0 = B u_ff + D d因此，前馈控制量可以设计为：u_ff = - (B^T B)^{-1} B^T D d（最小二乘解，若B不是方阵） 或者，如果系统是单输入且B可逆（在概念上），则u_ff = -B^{-1} D d。

在实际中，B矩阵通常不是方阵（状态多，控制输入少），我们采用一种更工程化的方法：通过实验或仿真，确定一个前馈增益K_ff，使得u_ff = K_ff * a_x能有效补偿干扰。K_ff可以通过系统辨识或试错法整定。

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

我们以STM32 HAL库为例，展示核心模块的代码框架。

5.1 传感器数据读取与滤波

MPU6050数据读取（I2C）：

// mpu6050.c #include "mpu6050.h" #include "i2c.h" #include "math.h" #define MPU6050_ADDR 0xD0 void MPU6050_ReadAccel(MPU6050_Data *data) { uint8_t buf[6]; HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, ACCEL_XOUT_H, I2C_MEMADD_SIZE_8BIT, buf, 6, 100); // 合并数据，并转换到 m/s^2，考虑量程（例如±2g） >// filter.c float ComplementaryFilter(float angle_acc, float angle_gyro, float dt, float alpha) { static float angle = 0; // angle_acc: 由加速度计atan2(Accel_Y, Accel_Z)计算出的角度（低频可靠，高频噪声大） // angle_gyro: 对陀螺仪角速度积分得到角度（高频可靠，低频会漂移） angle = alpha * (angle + angle_gyro * dt) + (1 - alpha) * angle_acc; return angle; } // 注意：此处的角度是杆相对于重力方向的倾斜角。对于底座姿态估计，需要类似的融合。

5.2 扩展卡尔曼滤波器（EKF）状态估计

这是一个简化的EKF预测与更新步骤示意。完整实现需要根据你的具体状态方程和测量方程来编写。

// ekf.c typedef struct { float x[4]; // 状态估计: [pos, vel, angle, angular_vel] float P[4][4]; // 估计误差协方差矩阵 float Q[4][4]; // 过程噪声协方差 float R[2][2]; // 测量噪声协方差 (假设测量位置和角度) } EKF_State; void EKF_Predict(EKF_State *s, float u, float dt) { // 1. 基于状态方程 f(x, u) 预测状态 // x_pred = f(x, u) * dt + x // 这里f(x,u)是离散化后的状态转移函数 float x_pred[4]; // ... 根据你的离散化模型计算 x_pred ... // 2. 计算状态转移矩阵 F 的雅可比（在x处求导） float F[4][4]; // ... 计算 F ... // 3. 预测协方差: P_pred = F * P * F^T + Q float FP[4][4], P_pred[4][4]; Matrix_Multiply(F, s->P, FP, 4, 4, 4); Matrix_Multiply_Transpose(FP, F, P_pred, 4, 4, 4); Matrix_Add(P_pred, s->Q, P_pred, 4, 4); // 更新状态和协方差为预测值 for(int i=0; i<4; i++) s->x[i] = x_pred[i]; Matrix_Copy(P_pred, s->P, 4, 4); } void EKF_Update(EKF_State *s, float z_pos, float z_angle) { // 测量向量 z = [z_pos, z_angle]^T // 测量矩阵 H: 假设我们能直接测量位置和角度 float H[2][4] = {{1,0,0,0}, {0,0,1,0}}; // 计算卡尔曼增益 K = P * H^T * (H * P * H^T + R)^{-1} float H_T[4][2], PH_T[4][2], HPH_T[2][2], HPH_T_R[2][2]; float K[4][2]; // ... 矩阵运算 ... // 计算测量残差 y = z - H * x float y[2]; y[0] = z_pos - s->x[0]; y[1] = z_angle - s->x[2]; // 状态更新: x = x + K * y s->x[0] += K[0][0]*y[0] + K[0][1]*y[1]; // ... 更新其他状态 ... // 协方差更新: P = (I - K*H) * P float I[4][4] = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}; float KH[4][4], I_KH[4][4], P_new[4][4]; // ... 矩阵运算 ... Matrix_Copy(P_new, s->P, 4, 4); }

5.3 核心控制循环

在主定时器中断（例如1kHz）中执行：

// main.c (中断服务程序示例) void TIM2_IRQHandler(void) { if(__HAL_TIM_GET_FLAG(&htim2, TIM_FLAG_UPDATE) != RESET) { __HAL_TIM_CLEAR_FLAG(&htim2, TIM_FLAG_UPDATE); // 1. 读取传感器原始数据 MPU6050_ReadAccel(&imu_data); float ball_position = Read_Linear_CCD(); // 读取小球位置 float rod_angle = Read_Potentiometer(); // 读取杆角度 // 2. IMU数据处理：获取底座加速度 (需转换到杆坐标系) // 假设IMU的X轴与杆方向平行。需要补偿重力分量。 // 简单情况：若杆接近竖直，底座水平加速度 ≈ imu_data.Accel_X // 更精确：需要结合杆的倾斜角进行坐标旋转。 float base_accel_x = imu_data.Accel_X; // 简化处理 // 3. 状态估计 (EKF) EKF_Predict(&ekf_state, last_control_output, CONTROL_PERIOD); EKF_Update(&ekf_state, ball_position, rod_angle); float x_est = ekf_state.x[0]; // 估计位置 float v_est = ekf_state.x[1]; // 估计速度 float theta_est = ekf_state.x[2]; // 估计角度 float omega_est = ekf_state.x[3]; // 估计角速度 // 4. 反馈控制量计算 (LQR) float u_fb = 0; u_fb -= K_lqr[0] * x_est; u_fb -= K_lqr[1] * v_est; u_fb -= K_lqr[2] * theta_est; u_fb -= K_lqr[3] * omega_est; // 5. 前馈补偿量计算 float u_ff = K_ff * base_accel_x; // K_ff 需通过实验整定 // 6. 控制量合成与限幅 float u_total = u_fb + u_ff; // 限幅到电机驱动器允许的范围 if(u_total > MAX_PWM) u_total = MAX_PWM; if(u_total < -MAX_PWM) u_total = -MAX_PWM; // 7. 输出到电机驱动器 Set_Motor_PWM(u_total); last_control_output = u_total; } }

6. 调参与系统调试实战

理论完美，但不上电调试一切都是空谈。以下是关键的调试步骤和技巧。

6.1 分步调试法

第一步：让系统动起来（开环测试）

1. 验证所有传感器数据能正确读取，并在串口或OLED上显示。
2. 编写一个简单的测试程序，手动给定一个PWM值，观察电机转向和转速是否正确。确保电机驱动逻辑无误（正转、反转、刹车）。
3. 手动移动小球和杆，观察传感器读数变化是否符合预期（方向、量程）。

第二步：实现基础平衡（无运动补偿）

1. 先调角度环：将小球固定在杆中心附近，只控制杆的角度直立。使用一个简单的PD控制器：u = Kp_angle * (θ_desired - θ) + Kd_angle * (- \dot{θ})。调整Kp_angle和Kd_angle直到杆能自主保持竖直。注意安全，防止杆高速旋转伤人。
2. 再加入位置环：放开小球，尝试平衡。此时控制器需要同时调节角度和位置。建议使用LQR，直接使用之前计算好的增益。如果振荡，按比例减小所有增益。如果响应太慢，按比例增大增益。重点观察：小球能否被控制在中心附近？系统对轻微扰动（轻吹一口气）的恢复能力如何？

第三步：引入运动补偿

1. 保持系统在静止底座上稳定平衡。
2. 用手缓慢地、有规律地前后移动整个系统的底座。
3. 观察小球的行为。如果没有前馈补偿（K_ff=0），小球会随着底座移动而偏离中心。
4. 逐渐增加K_ff的值，同时用手移动底座。目标是：当底座匀速或匀加速运动时，小球依然能相对底座保持在中心位置。
5. 调试技巧：
   • 先给一个很小的K_ff，观察效果。
   • 记录底座加速度（IMU X轴输出）和控制量u的数据。在静止和移动时，分析u与a_x的关系，可以粗略估计K_ff的理论值。
   • 如果加入前馈后系统反而失稳，可能是K_ff符号反了，或者IMU加速度的方向定义与模型不符。

6.2 关键参数整定经验

• LQR权重 (Q,R)：这是调参的核心。一个经典的起点是让Q的对角元与状态量最大允许偏差的平方成反比。例如，如果允许小球最大偏离0.1m，角度最大偏离0.1 rad，则Q中对应元素可设为1/(0.1^2)=100。速度和角速度的权重可以设小一个数量级。R从1开始调。
• 前馈增益K_ff：它与小球质量m、杆长r以及电机特性有关。可以通过“单位加速度补偿所需扭矩”来估算。更有效的方法是系统辨识：固定小球，给底座一个已知加速度a，测量使杆保持不动所需的电机扭矩τ，则K_ff ≈ τ / a。
• 滤波器参数：
  • 互补滤波器系数alpha：通常在0.95~0.99之间。值越大，越信任陀螺仪（响应快，但漂移）；值越小，越信任加速度计（稳定，但噪声大）。需要根据实际传感器噪声水平调整。
  • EKF噪声协方差 (Q,R)：Q表示你对模型的信任程度，模型越不准，Q应设得越大。R表示你对传感器的信任程度，传感器噪声越大，R应设得越大。通常从Q=diag(0.001, 0.001, 0.001, 0.001)，R=diag(0.01, 0.01)开始试。

7. 常见问题与排查清单

在调试过程中，你几乎一定会遇到下面这些问题。

8. 进阶优化与最佳实践

当基本功能实现后，可以考虑以下优化来提升性能、鲁棒性和工程化水平。

1. 模型辨识与参数自适应：

   • 你的理论模型参数（m,r, 摩擦系数）可能与实际有出入。可以通过实验进行系统辨识。例如，给系统一个阶跃输入，记录响应曲线，用最小二乘法等拟合出模型参数。
   • 更进一步，可以设计自适应控制器，在线调整K_ff甚至LQR的K矩阵，以应对参数变化（如电池电压下降导致电机增益变化）。

2. 多传感器融合升级：

   • 使用MPU9250（含磁力计）进行更准确的姿态解算（AHRS），以应对底座在三维空间中的复杂运动。
   • 对于小球位置检测，线性CCD是主流，但可以考虑加入第二个CCD或摄像头进行二维定位，以检测小球是否脱离轨道。

3. 抗饱和与积分抗饱和：

   • 电机的PWM输出有物理限幅。当控制量饱和时，积分项（如果用了PID）会继续累积，导致系统脱离饱和后产生大幅超调。需要在代码中加入积分抗饱和逻辑。

4. 上位机调试工具：

   • 使用串口或Wi-Fi将关键数据（状态、控制量、传感器原始值）实时发送到电脑。
   • 利用Python的Matplotlib或专业的串口绘图工具（如SerialPlot）实时绘图。这是调参的“眼睛”，效率远超盲目修改代码。

5. 代码架构优化：

   • 将控制器、滤波器、传感器驱动模块化。
   • 使用RTOS（如FreeRTOS）来管理不同优先级的任务（控制循环、状态估计、通信、日志）。
   • 参数（如PID增益、LQR权重、滤波器系数）做成可在线调整的，避免每次修改都要重新编译下载。

实现一个带运动补偿的杆球平衡系统，是一次对自动控制原理、嵌入式系统、传感器技术和软件工程的综合演练。它清晰地展示了从理论建模、仿真设计到硬件实现、调试排错的完整闭环。关键在于理解“运动补偿”的本质是前馈控制，而成功的前馈依赖于准确的干扰测量（IMU）和合理的补偿模型。

不要试图一步到位。遵循“开环测试 -> 反馈稳定 -> 引入前馈 -> 精细调参”的步骤，耐心地观察现象、分析数据、迭代修改。当你看到小球在晃动的平台上依然稳稳地停留在杆中央时，那种对“控制”二字真切的理解和成就感，是任何书本都无法给予的。

建议你将本文的代码框架和调试方法作为一个起点，结合你的具体硬件进行适配和深化。在2026年电赛的赛场上，或是你自己的创新项目中，这套融合了状态估计与主动抗扰的思路，或许就是你脱颖而出的关键。收藏本文，动手搭建，祝你调试顺利。