【花雕学编程】Arduino BLDC 之倒立摆动态前馈与平衡控制

Arduino BLDC之倒立摆动态前馈与平衡控制，是指利用Arduino微控制器驱动无刷直流电机（BLDC）作为执行机构，构建一个旋转式或直线式倒立摆系统，并通过融合动态前馈控制（Dynamic Feedforward Control）与反馈式平衡控制（如PID、LQR等），实现对不稳定平衡点（即摆杆竖直向上）的高鲁棒性稳定控制。该系统是经典控制理论与现代嵌入式机电一体化技术结合的典型范例。

以下从主要特点、典型应用场景以及实施注意事项三个方面进行专业解析：

一、主要特点

1. 系统本质为非线性、欠驱动、不稳定系统
   倒立摆具有天然不稳定性：平衡点为开环不稳定（类似倒立的铅笔），需持续主动控制；
   系统为欠驱动（Underactuated）：仅通过小车/转台一个自由度间接控制摆杆角度；
   动力学方程高度非线性（含 sinθ,cosθ 项），通常在平衡点附近线性化后设计控制器。
2. 动态前馈控制：补偿已知非线性扰动
   基于倒立摆动力学模型（如拉格朗日方程），在摆杆加速运动或外部指令变化时，前馈提前输出补偿力矩，显著减轻反馈控制器负担，提升响应速度。
3. 反馈平衡控制：抑制建模误差与未知扰动
   常用线性二次型调节器（LQR）或极点配置PID，能有效应对参数偏差（如质量变化）、摩擦扰动、气流干扰等未建模动态；
   与纯反馈相比，前馈+反馈复合结构允许更高控制带宽而不引发振荡。
4. BLDC电机提供高动态性能支撑
   相较于有刷电机，BLDC具备：
   更高功率密度与扭矩响应速度；
   低转矩脉动（尤其采用FOC控制时）；
   长寿命、低噪声，适合长时间实验运行。
   配合编码器（电机端+摆杆端）可实现双闭环（位置+摆角）精确控制。
5. 实时状态估计依赖多传感器融合
   摆杆角度通常由高精度IMU（如MPU6050、BNO055）或光学编码器测量；
   小车位置/速度由电机编码器或直线电位器/光栅尺获取；
   采用互补滤波或卡尔曼滤波融合陀螺仪与加速度计数据，抑制漂移与高频噪声。
   二、典型应用场景
6. 控制理论教学与实验平台
   高校自动控制原理、现代控制理论、非线性控制等课程的核心实验装置；
   直观展示状态空间法、LQR、鲁棒控制、自适应控制等算法效果。
7. 机器人平衡技术原型验证
   自平衡机器人（如Segway）、双足机器人踝关节控制的简化模型；
   验证前馈-反馈架构在真实物理系统中的可行性。
8. 航天与惯性稳定系统模拟
   卫星姿态控制、火箭垂直起降（VTVL）的地面缩比验证；
   虽为二维简化，但控制思想具迁移价值。
9. 创客与竞赛项目
   全国大学生智能车竞赛、RoboMaster、IEEE CDC挑战赛等常设倒立摆任务；
   展示嵌入式系统集成能力与控制算法实现水平。
   三、需要注意的事项
10. BLDC驱动必须支持高精度转矩/位置闭环
    普通航模ESC仅支持速度开环，无法用于倒立摆；
    必须采用FOC（磁场定向控制）方案，通过电流环精确控制电磁转矩；
    推荐使用开源驱动库（如 SimpleFOC）配合支持硬件PWM和ADC的Arduino平台（如Due、Teensy 4.0、ESP32）。
11. 动力学建模精度直接影响前馈效果
    前馈依赖准确的系统参数：摆杆质心位置、转动惯量、小车质量、摩擦系数等；
    建议通过实验辨识（如阶跃响应、频率响应）获取参数，而非仅依赖CAD估算；
    可引入在线参数自适应机制应对负载变化。
12. 传感器延迟与噪声是稳定性的关键瓶颈
    IMU数据延迟 > 5ms 可能导致系统失稳；
    必须优化滤波算法（如低延迟卡尔曼滤波）并提高采样率（建议 ≥ 200 Hz）；
    编码器信号需防抖处理，避免误触发中断。
13. Arduino平台资源限制与实时性保障
    Uno/Nano等8位MCU难以胜任FOC + LQR + 多传感器融合；
    推荐平台：
    Teensy 4.0/4.1（600MHz Cortex-M7）：支持硬件QEI、高速浮点运算；
    Arduino Due（84MHz Cortex-M3）：兼容SimpleFOC，适合单轴系统；
    ESP32：双核可分工（一核控电机，一核处理通信/日志）。
    控制周期应 ≤ 5 ms（即 ≥ 200 Hz），以保证相位裕度。
14. 机械结构刚性与传动间隙
    转轴轴承松动、皮带打滑、齿轮背隙会引入非线性扰动；
    建议采用直驱（电机轴直接连接摆杆底座）或高刚性减速箱；
    所有连接件需紧固，避免共振频率接近控制带宽。
15. 安全保护机制不可或缺
    设置软件限位（如小车行程 ±20 cm）；
    检测摆角超限（如 |θ| > 30°）时立即停机；
    加装物理挡板防止摆杆撞击设备。

1、基础PD平衡控制+加速度前馈

#define ENCODER_A 2
#define ENCODER_B 3
#define MOTOR_PWM 9
#define IMU_PIN 4

volatile long encoderCount = 0;
float currentAngle = 0;
float targetAngle = 0;
float balanceVelocity = 0;

// PD控制器参数
#define Kp 80.0    // 比例系数
#define Kd 5.0     // 微分系数
#define Ki 0.1     // 积分系数（防漂移）

void encoderISR() {
  bool a = digitalRead(ENCODER_A);
  bool b = digitalRead(ENCODER_B);
  encoderCount += (a == b) ? 1 : -1;
}

float calculateDynamicFeedforward(float velocity, float acceleration) {
  // 基于动力学模型的前馈补偿
  return 0.8 * velocity + 0.3 * acceleration;  // 经验参数
}

void setup() {
  pinMode(MOTOR_PWM, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(ENCODER_A), encoderISR, CHANGE);
  Wire.begin();
  // 初始化IMU...
}

void loop() {
  static unsigned long lastTime = 0;
  unsigned long now = micros();
  float dt = (now - lastTime) / 1e6;
  lastTime = now;
  
  // 1. 读取传感器数据
  currentAngle = readIMUAngle();  // 从IMU获取角度
  float motorVelocity = (encoderCount / dt) * (60.0 / 12.0); // RPM转换
  
  // 2. 计算动态前馈
  float acceleration = (motorVelocity - balanceVelocity) / dt;
  float feedforward = calculateDynamicFeedforward(motorVelocity, acceleration);
  balanceVelocity = motorVelocity;
  
  // 3. PD平衡控制
  float error = targetAngle - currentAngle;
  static float integral = 0;
  integral += error * dt;
  
  float pdOutput = Kp * error + Kd * (error - lastError)/dt + Ki * integral;
  lastError = error;
  
  // 4. 组合控制输出
  int pwmValue = constrain(pdOutput + feedforward, -255, 255);
  analogWrite(MOTOR_PWM, abs(pwmValue));
  
  delay(10);
}

要点解读：

双闭环结构：外环角度控制 + 内环速度前馈
动态前馈：基于电机速度和加速度的开环补偿，抵消惯性影响
抗漂移设计：加入积分项防止长时间运行时的角度漂移
实时性优化：使用micros()高精度计时，确保dt准确

2、卡尔曼滤波状态估计+LQR控制

```cpp
#include <KalmanFilter.h>

// 系统状态变量
struct State {
  float angle;       // 摆杆角度
  float angularVel;  // 角速度
  float cartPos;     // 小车位置
  float cartVel;     // 小车速度
};

KalmanFilter kf(4, 2, 0); // 4状态2观测
State currentState;
State desiredState = {0, 0, 0, 0};

// LQR权重矩阵
float Q[4] = {10, 1, 0.1, 0.01};  // 状态权重
float R[1] = {0.1};               // 控制权重

void updateSystemModel(float dt) {
  // 线性化后的系统矩阵（倒立摆模型）
  float A[4][4] = {
    {1, dt, 0, 0},
    {0, 1, 0, 0},
    {0, 0, 1, dt},
    {0, 0, 0, 1}
  };
  kf.setStateTransitionMatrix(A);
}

float computeLQRGain() {
  // 简化版LQR增益计算（实际应离线计算）
  return 15.0 + 0.8*currentState.angularVel + 0.3*currentState.cartVel;
}

void loop() {
  float dt = 0.01; // 固定10ms周期
  
  // 1. 传感器数据融合
  float imuAngle = readIMU();
  float encoderPos = readEncoder();
  
  // 2. 卡尔曼滤波状态估计
  float measurement[2] = {imuAngle, encoderPos};
  currentState = kf.updateEstimate(measurement);
  
  // 3. LQR控制计算
  float errorAngle = desiredState.angle - currentState.angle;
  float errorAngularVel = desiredState.angularVel - currentState.angularVel;
  
  float lqrGain = computeLQRGain();
  float controlSignal = lqmGain * (errorAngle + errorAngularVel);
  
  // 4. 执行控制
  applyMotorControl(controlSignal);
  
  delay(10);
}

    
要点解读：

状态估计：卡尔曼滤波融合多传感器数据，抑制噪声干扰
LQR优化：线性二次调节器提供最优控制律，平衡响应速度与稳定性
自适应增益：根据当前状态动态调整控制参数
模型依赖：需要准确的系统动力学模型建立状态转移矩阵

3、能量整形控制+非线性前馈

```cpp
// 基于无源性的能量控制方法
float energyShapingControl(float currentEnergy, float desiredEnergy) {
  // 计算能量误差
  float energyError = desiredEnergy - currentEnergy;
  
  // 非线性前馈补偿
  float nonlinearFF = 0.5 * tanh(energyError * 2.0);
  
  return nonlinearFF;
}

float slidingModeControl(float error) {
  // 滑模面设计
  float s = error + 3.0 * previousError;
  
  // 符号函数控制（实际使用饱和函数替代）
  return 15.0 * sign(s);
}

void mainControlLoop() {
  float dt = 0.005;
  
  // 1. 物理量计算
  float potentialEnergy = mgl * cos(currentAngle);  // 势能
  float kineticEnergy = 0.5 * I * sq(angularVel);   // 动能
  float totalEnergy = potentialEnergy + kineticEnergy;
  
  // 2. 能量整形控制
  float esCtrl = energyShapingControl(totalEnergy, targetEnergy);
  
  // 3. 滑模平衡控制
  float smcCtrl = slidingModeControl(currentAngle);
  
  // 4. 复合控制输出
  float finalOutput = esCtrl * 0.7 + smcCtrl * 0.3; // 加权融合
  
  // 5. 安全保护
  if(abs(currentAngle) > PI/2) {
    emergencyStop();
  } else {
    setMotorVoltage(finalOutput);
  }
}

要点解读：

物理本质控制：直接基于能量守恒原理设计控制器
非线性处理：使用tanh函数平滑过渡，避免控制量突变
鲁棒性设计：滑模控制保证在大扰动下的稳定性
安全机制：超出安全角度范围立即启动紧急制动

4、经典PID平衡控制（带前馈补偿）

#include <Encoder.h>
#include <PID_v1.h>

// 硬件定义
#define MOTOR_PWM 9
#define MOTOR_DIR 8
#define ENCODER_A 2
#define ENCODER_B 3
#define ACCEL_PIN A0  // 加速度计模拟输入（简化模型）

// 系统参数
const float pendulumLength = 0.3; // 摆杆长度(m)
const float targetAngle = 0.0;    // 目标角度(rad)
double Kp = 50.0, Ki = 0.1, Kd = 20.0; // PID参数

// 状态变量
double currentAngle = 0.0;
double currentSpeed = 0.0;
double motorOutput = 0;

// 对象定义
Encoder enc(ENCODER_A, ENCODER_B);
PID pendulumPID(&currentAngle, &motorOutput, &targetAngle, Kp, Ki, Kd, DIRECT);

void setup() {
  pinMode(MOTOR_PWM, OUTPUT);
  pinMode(MOTOR_DIR, OUTPUT);
  Serial.begin(115200);
  
  // PID初始化
  pendulumPID.SetMode(AUTOMATIC);
  pendulumPID.SetOutputLimits(-255, 255);
  pendulumPID.SetSampleTime(10); // 10ms控制周期
}

void loop() {
  static unsigned long lastTime = 0;
  unsigned long now = millis();
  
  // 1. 传感器读取与状态估计
  int accelValue = analogRead(ACCEL_PIN);
  currentAngle = (accelValue - 512) * 0.01; // 模拟加速度计数据转换
  
  // 编码器测速（M法）
  static long lastCount = 0;
  long currentCount = enc.read();
  currentSpeed = (currentCount - lastCount) / (now - lastTime) * 1000.0 / 2000.0; // 2000线编码器
  lastCount = currentCount;
  lastTime = now;

  // 2. 前馈补偿（基于模型的速度前馈）
  double feedforward = 0.8 * currentSpeed; // 简化前馈项

  // 3. PID计算与输出
  pendulumPID.Compute();
  double controlOutput = motorOutput + feedforward;

  // 4. 电机驱动（带死区处理）
  if (abs(controlOutput) > 10) { // 死区阈值
    digitalWrite(MOTOR_DIR, controlOutput > 0 ? HIGH : LOW);
    analogWrite(MOTOR_PWM, constrain(abs(controlOutput), 50, 255));
  } else {
    analogWrite(MOTOR_PWM, 0);
  }

  // 5. 数据监控
  Serial.print("Angle: "); Serial.print(currentAngle);
  Serial.print(" Speed: "); Serial.print(currentSpeed);
  Serial.print(" Output: "); Serial.println(controlOutput);
  delay(10);
}

要点解读：

前馈补偿机制：通过摆杆速度估计（currentSpeed）提前调整控制量，减少相位滞后。
传感器融合：加速度计测量角度，编码器测量速度，形成互补观测。
死区处理：避免电机在低电压下的振荡，提升静态稳定性。
适用场景：低成本教学实验平台，如Arduino倒立摆套件。

5、LQR最优控制（带状态观测器）

#include <BasicLinearAlgebra.h>
#include <Encoder.h>

// 系统矩阵（简化模型）
const float A[2][2] = {{0, 1}, {9.8/0.3, 0}}; // 0.3m摆杆
const float B[2] = {0, 1.0};
const float K[2] = {-70.0, -15.0}; // LQR反馈增益（通过MATLAB计算）

// 硬件定义
#define MOTOR_PWM 9
#define MOTOR_DIR 8
#define ENCODER_A 2
#define ENCODER_B 3

// 状态变量
float x[2] = {0, 0}; // [角度, 角速度]
float x_hat[2] = {0, 0}; // 观测状态
float L[2] = {0.5, 0.1}; // 观测器增益

Encoder enc(ENCODER_A, ENCODER_B);

void setup() {
  pinMode(MOTOR_PWM, OUTPUT);
  pinMode(MOTOR_DIR, OUTPUT);
  Serial.begin(115200);
}

void loop() {
  static unsigned long lastTime = 0;
  unsigned long now = millis();
  float dt = (now - lastTime) / 1000.0;
  lastTime = now;

  // 1. 传感器读取
  int accelValue = analogRead(A0);
  float angle = (accelValue - 512) * 0.01; // 角度测量
  
  // 编码器测速（差分法）
  static long lastCount = 0;
  long count = enc.read();
  float speed = (count - lastCount) / dt / 2000.0; // 2000线编码器
  lastCount = count;

  // 2. 状态观测器（龙伯格观测器）
  float y = angle; // 测量输出
  float x_hat_dot[2] = {
    A[0][0]*x_hat[0] + A[0][1]*x_hat[1] + B[0]*motorOutput + L[0]*(y - x_hat[0]),
    A[1][0]*x_hat[0] + A[1][1]*x_hat[1] + B[1]*motorOutput + L[1]*(y - x_hat[0])
  };
  x_hat[0] += x_hat_dot[0] * dt;
  x_hat[1] += x_hat_dot[1] * dt;

  // 3. LQR控制律
  float u = -(K[0]*x_hat[0] + K[1]*x_hat[1]);

  // 4. 电机驱动
  if (abs(u) > 5) { // 最小控制量阈值
    digitalWrite(MOTOR_DIR, u > 0 ? HIGH : LOW);
    analogWrite(MOTOR_PWM, constrain(abs(u), 30, 255));
  } else {
    analogWrite(MOTOR_PWM, 0);
  }

  // 5. 数据记录
  Serial.print(x_hat[0]); Serial.print(",");
  Serial.println(u);
  delay(10);
}

要点解读：

LQR控制设计：通过MATLAB计算最优反馈增益（K矩阵），平衡控制能量与响应速度。
状态观测器：利用龙伯格观测器估计不可测状态（如角速度），降低传感器成本。
离散化实现：欧拉法进行矩阵运算离散化，适应Arduino计算能力。
适用场景：需要更高控制性能的机器人关节或平衡车。

6、模糊自适应平衡控制（抗干扰增强）

#include <Fuzzy.h>
#include <Encoder.h>

// 模糊控制器定义
Fuzzy *fuzzy = new Fuzzy();
FuzzySet *small = new FuzzySet(-30, -15, -15, 0);
FuzzySet *zero = new FuzzySet(-5, 0, 0, 5);
FuzzySet *big = new FuzzySet(0, 15, 15, 30);

// 硬件定义
#define MOTOR_PWM 9
#define MOTOR_DIR 8
#define ENCODER_A 2
#define ENCODER_B 3

// 系统状态
float angleError = 0;
float angleRate = 0;
float lastAngle = 0;

Encoder enc(ENCODER_A, ENCODER_B);

void setup() {
  // 模糊规则初始化（简化版）
  FuzzyInput *error = new FuzzyInput(1);
  error->addFuzzySet(small);
  error->addFuzzySet(zero);
  error->addFuzzySet(big);
  fuzzy->addFuzzyInput(error);

  FuzzyOutput *output = new FuzzyOutput(1);
  output->addFuzzySet(new FuzzySet(-100, -50, -50, 0));
  output->addFuzzySet(new FuzzySet(-10, 0, 0, 10));
  output->addFuzzySet(new FuzzySet(0, 50, 50, 100));
  fuzzy->addFuzzyOutput(output);

  // 模糊规则（示例）
  FuzzyRule *rule1 = new FuzzyRule(1, new FuzzyRuleAntecedent(small), new FuzzyRuleConsequent(output, 2));
  fuzzy->addFuzzyRule(rule1);

  pinMode(MOTOR_PWM, OUTPUT);
  pinMode(MOTOR_DIR, OUTPUT);
  Serial.begin(115200);
}

void loop() {
  static unsigned long lastTime = 0;
  unsigned long now = millis();
  float dt = (now - lastTime) / 1000.0;
  lastTime = now;

  // 1. 传感器读取
  int accelValue = analogRead(A0);
  float angle = (accelValue - 512) * 0.01;
  angleError = angle; // 目标角度为0

  // 2. 角速度计算（抗干扰滤波）
  static float alpha = 0.2; // 低通滤波系数
  angleRate = alpha * (angle - lastAngle)/dt + (1-alpha)*angleRate;
  lastAngle = angle;

  // 3. 模糊推理
  fuzzy->setInput(1, angleError);
  fuzzy->setInput(2, angleRate);
  fuzzy->fuzzify();
  float u = fuzzy->defuzzify(1);

  // 4. 电机驱动（带干扰抑制）
  static float lastOutput = 0;
  float output = 0.7*u + 0.3*lastOutput; // 一阶滤波
  lastOutput = output;

  if (abs(output) > 10) {
    digitalWrite(MOTOR_DIR, output > 0 ? HIGH : LOW);
    analogWrite(MOTOR_PWM, constrain(abs(output), 40, 255));
  } else {
    analogWrite(MOTOR_PWM, 0);
  }

  // 5. 数据监控
  Serial.print("Angle: "); Serial.print(angle);
  Serial.print(" Rate: "); Serial.print(angleRate);
  Serial.print(" Output: "); Serial.println(output);
  delay(10);
}

要点解读：

模糊逻辑控制：通过自然语言规则（如"如果角度偏差大，则强力修正"）处理非线性问题。
抗干扰设计：
角速度低通滤波抑制高频噪声
控制量一阶滤波防止突变
自适应特性：无需精确模型，适合参数摄动或外部干扰场景。
适用场景：复杂工况下的工业平衡系统或服务机器人。

注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。