基于 Arduino 平台构建的 BLDC 自平衡机器人（Dual-Wheel + IMU），是控制理论、电子工程与机械设计完美结合的经典范例。该系统本质上是一个倒立摆（Inverted Pendulum），通过 IMU（惯性测量单元） 实时感知姿态，并利用 BLDC（无刷直流电机） 的快速扭矩响应来维持动态平衡。

1、主要特点
本质不稳定系统的动态稳定
自平衡机器人的核心物理模型是一个不稳定的倒立摆。其质心位于支撑点（轮轴）之上，静止时无法保持平衡，任何微小的倾角都会在重力作用下被放大。
动态平衡原理：系统通过控制算法，让轮子向倾倒的方向加速运动，利用惯性力抵消重力矩，从而“追”回重心。这要求控制器必须进行持续、主动的干预，而非简单的静态维持。
零转弯半径：由于采用两轮差速驱动，该底盘具备零转弯半径的特性，机动性极强，适合在狭窄空间内作业。
高精度姿态感知与传感器融合
IMU 是系统的“内耳”，提供了维持平衡所需的核心反馈数据。
多传感器融合：IMU 通常集成了三轴加速度计和三轴陀螺仪。加速度计提供静态重力方向（长期稳定但动态噪声大），陀螺仪提供动态角速度（响应快但存在积分漂移）。
姿态解算算法：通过 互补滤波（Complementary Filter） 或 卡尔曼滤波（Kalman Filter） 融合两者数据，计算出精确、低延迟的实时俯仰角（Pitch Angle）。这是整个闭环控制的基石，数据的纯净度直接决定了平衡的稳定性。
高性能扭矩控制与快速响应
BLDC 电机 相较于有刷电机，具备更高的功率密度、更长的寿命和更低的发热，是实现长时间稳定运行的理想选择。
力矩控制模式：自平衡的本质是力矩平衡。系统通常工作在电流环/力矩模式，通过控制电机的相电流来直接控制输出扭矩。这比速度环控制具有更快的响应速度，能更迅速地产生恢复力矩。
FOC（磁场定向控制）：为了获得平滑的低速扭矩输出，减少换相脉动对平衡的干扰，高端方案通常采用 FOC 算法驱动 BLDC 电机，确保在微小倾角下也能输出线性的恢复力。

2、应用场景
个人移动交通工具
这是最直接的商业化应用。如两轮平衡车（Segway 类）、电动扭扭车，利用自平衡技术实现人体姿态跟随，提供便捷的短途出行解决方案。
服务与巡检机器人底盘
在商场、展厅、酒店等场景中，自平衡底盘作为移动平台，具备占地面积小、转向灵活、视野开阔（重心高）的优势，常用于搭载显示屏、摄像头或服务机械臂。
教育与控制理论教学平台
该系统是学习 PID 控制、状态空间分析、LQR（线性二次型调节器） 等高级控制算法的“完美教具”。学生可以通过亲手调试参数，直观理解反馈控制、系统稳定性、阻尼比等抽象概念。
特种作业与救援机器人
在地形复杂或空间受限的救援现场（如地震废墟、火灾现场），自平衡机器人可以穿越狭窄通道，搭载传感器进行环境探测或物资投递。

3、注意事项
硬件选型与机械结构
重心设计：机械结构的重心高度直接影响系统的稳定性裕度。重心越高，系统越不稳定，对传感器精度和控制频率的要求也越高。建议将电池等重物尽量放置在底部，降低重心。
IMU 安装位置：IMU 应安装在车体的几何中心且尽量靠近轮轴平面，以减少杠杆臂效应带来的测量误差。安装必须牢固，避免振动引入噪声。
电机扭矩匹配：BLDC 电机必须具备足够的低速扭矩。若扭矩不足，当车体倾角稍大时，电机将无法产生足够的恢复力矩，导致失衡倒地。
控制算法与参数整定
滤波器调参：互补滤波的系数需要仔细调整。若过于信任加速度计，系统会受振动干扰；若过于信任陀螺仪，角度会随时间漂移。建议通过串口绘图，观察滤波前后的波形进行优化。
PID 参数整定：平衡控制通常采用 PD 控制器（比例-微分）。
积分项（I）的使用：通常平衡环不建议加入积分项，因为积分累积容易导致系统在受到持续扰动后产生过大的超调甚至失控。
电源与系统安全
电源隔离：BLDC 电机启停时电流冲击大，容易导致电源电压跌落，使 Arduino 复位。必须使用独立的稳压模块（如 LM2596 或开关电源）为逻辑电路供电，并在电源端并联大容量电解电容（如 2200μF）。
软件保护：必须设置倾角软限位。当检测到倾角超过一定阈值（如 ±30°）时，判定为已失去平衡，应立即切断电机输出（BRAKE），防止电机疯狂旋转造成危险或损坏设备。
物理限位与调试：在初调阶段，建议使用细绳悬挂车体或安装辅助轮，防止因参数错误导致车体猛烈撞击地面，损坏机械结构或电机。

1、基础互补滤波+PID控制（MPU6050）

#include <Wire.h>
#include <Servo.h> // 用于生成BLDC驱动PWM

// IMU变量
float accAngle, gyroAngle, angle, prevTime;
float gyroRate, dt;
const float alpha = 0.98; // 互补滤波系数

// PID参数
float Kp = 40, Ki = 0.5, Kd = 0.8;
float error, prevError, integral, derivative;
float targetAngle = 0; // 目标平衡角度（垂直时为0°）

// 电机控制
Servo motorLeft, motorRight;
const int motorPinL = 9, motorPinR = 10;
int motorOutput;

void setup() {
  Wire.begin();
  initMPU6050(); // 初始化IMU（代码略，需配置寄存器）
  motorLeft.attach(motorPinL);
  motorRight.attach(motorPinR);
  prevTime = millis();
}

void loop() {
  // 1. 读取IMU数据并计算角度
  readIMU();
  dt = (millis() - prevTime) / 1000.0;
  prevTime = millis();
  
  // 互补滤波融合加速度计和陀螺仪数据
  accAngle = atan2(accY, accZ) * RAD_TO_DEG;
  gyroRate = gyroX / 131.0; // 转换为°/s
  gyroAngle += gyroRate * dt;
  angle = alpha * (angle + gyroAngle) + (1 - alpha) * accAngle;

  // 2. PID计算
  error = angle - targetAngle;
  integral += error * dt;
  derivative = (error - prevError) / dt;
  motorOutput = Kp * error + Ki * integral + Kd * derivative;
  prevError = error;

  // 3. 电机驱动（双向控制需处理PWM中点）
  int pwmLeft = constrain(1500 + motorOutput, 1000, 2000);
  int pwmRight = constrain(1500 - motorOutput, 1000, 2000); // 反向控制
  motorLeft.writeMicroseconds(pwmLeft);
  motorRight.writeMicroseconds(pwmRight);

  delay(10); // 控制循环频率
}

2、卡尔曼滤波+动态PID调参（带调试串口）

#include <Wire.h>
#include <BasicMPU6050.h> // 第三方库简化IMU读取

BasicMPU6050 mpu;
float kalmanAngle;
float Kp = 35, Ki = 0.3, Kd = 0.7;
float Kp_min = 20, Kp_max = 60; // 动态调参范围

void setup() {
  Serial.begin(115200);
  mpu.setup();
  mpu.setBias(-0.5, 1.2, 0); // 校准偏移
  initMotors(); // 电机初始化（代码略）
}

void loop() {
  mpu.update();
  kalmanAngle = kalmanFilter(mpu.angX(), mpu.gyroX());

  // 动态PID调整（根据倾斜角度变化率）
  float tiltRate = abs(mpu.gyroX()) / 100.0;
  Kp = constrain(Kp_min + tiltRate * 50, Kp_min, Kp_max);

  // PID计算
  float error = kalmanAngle - 0;
  static float integral, derivative, prevError;
  integral += error * 0.01; // 假设循环周期10ms
  derivative = (error - prevError) / 0.01;
  prevError = error;

  int output = Kp * error + Ki * integral + Kd * derivative;
  driveMotors(output); // 电机驱动函数（代码略）

  // 串口调试
  Serial.print("Angle:"); Serial.print(kalmanAngle);
  Serial.print(" Kp:"); Serial.println(Kp);
}

// 简化卡尔曼滤波实现
float kalmanFilter(float newAngle, float newRate) {
  static float q = 0.01, r = 10; // 过程噪声和测量噪声
  static float p = 1, x = 0;     // 估计误差协方差和角度估计值
  
  p = p + q;
  float k = p / (p + r);
  x = x + k * (newAngle - x);
  p = (1 - k) * p;
  return x;
}

3、蓝牙遥控+摔倒保护（HM-10模块）

#include <SoftwareSerial.h>
#include <Wire.h>

SoftwareSerial bluetooth(2, 3); // RX, TX
float targetAngle = 0;
bool isUpright = true;
unsigned long fallDetectTime;

void setup() {
  bluetooth.begin(9600);
  initIMU(); // IMU初始化（代码略）
  initMotors(); // 电机初始化（代码略）
}

void loop() {
  // 1. 蓝牙遥控指令解析
  if (bluetooth.available()) {
    char cmd = bluetooth.read();
    if (cmd == 'F') targetAngle = -2; // 轻微前倾加速
    else if (cmd == 'B') targetAngle = 2; // 后倾减速
    else targetAngle = 0; // 停止时恢复垂直
  }

  // 2. 读取IMU并计算角度
  float currentAngle = getIMUAngle(); // 需实现互补滤波/卡尔曼滤波

  // 3. 摔倒检测（角度持续超过45°）
  if (abs(currentAngle) > 45) {
    if (isUpright) fallDetectTime = millis();
    if (millis() - fallDetectTime > 500) {
      emergencyStop();
      isUpright = false;
    }
  } else {
    isUpright = true;
  }

  // 4. 正常平衡控制
  if (isUpright) {
    float error = currentAngle - targetAngle;
    int output = pidControl(error); // PID函数（代码略）
    driveMotors(output);
  }
}

void emergencyStop() {
  setMotorSpeed(0, 0); // 停止电机
  bluetooth.println("FALL DETECTED!");
}

要点解读
传感器融合关键性
仅用加速度计噪声大，仅用陀螺仪会漂移，互补滤波（案例1）或卡尔曼滤波（案例2）是必需的。
实际项目中建议使用DMP（MPU6050内置）或专用传感器（如BNO055）直接输出融合角度。
电机控制双向性
BLDC需通过ESC控制正反转（如案例1中1500±output实现双向驱动）。
需测试电机极性，确保正输出对应前进方向。
安全机制设计
案例3的摔倒检测通过持续大角度触发，结合时间阈值避免误判。
实际可增加：
电压监测（低电保护）
通信超时停止（蓝牙断开时刹停）
动态调参与调试
案例2展示根据倾斜速率调整Kp，适应不同负载。
调试建议：
先断开电机，通过串口观察角度数据
使用PID_AutoTune库自动整定参数
实时性优化
控制循环频率建议50-200Hz（案例1的10ms延迟需根据实际调整）。
避免使用delay()，改用millis()计时或中断驱动。

4、基础自平衡PID控制器
场景：入门级自平衡小车，使用MPU6050和双BLDC电机。
核心逻辑：IMU角度读取 + PID控制 + 互补滤波。

#include <SimpleFOC.h>
#include <MPU6050_tockn.h>
#include <Wire.h>
#include <PID_v1.h>

// IMU传感器
MPU6050 mpu6050(Wire);
#define IMU_SDA 21
#define IMU_SCL 22

// 双BLDC电机
BLDCMotor motorLeft(7);
BLDCMotor motorRight(7);
Encoder encLeft(2, 3, 2048);
Encoder encRight(18, 19, 2048);
void doA_left() { encLeft.handleA(); }
void doB_left() { encLeft.handleB(); }
void doA_right() { encRight.handleA(); }
void doB_right() { encRight.handleB(); }

// 机器人物理参数
#define WHEEL_RADIUS 0.035     // 轮子半径3.5cm
#define WHEEL_DISTANCE 0.20    // 轮距20cm
#define ROBOT_HEIGHT 0.15      // 机器人高度15cm
#define ROBOT_MASS 2.0         // 质量2kg
#define GRAVITY 9.81           // 重力加速度

// 平衡控制参数
struct BalanceControl {
  float targetAngle = 0.0;     // 目标角度（垂直）
  float currentAngle = 0.0;    // 当前角度
  float angleError = 0.0;      // 角度误差
  float angleVelocity = 0.0;   // 角速度
  
  // PID参数
  float kp_angle = 25.0;       // 角度P
  float ki_angle = 0.5;        // 角度I
  float kd_angle = 0.2;        // 角度D
  
  float kp_velocity = 0.1;     // 速度P
  float ki_velocity = 0.01;    // 速度I
  float kd_velocity = 0.0;     // 速度D
  
  // 滤波器
  float alpha = 0.98;          // 互补滤波系数
  float gyroLPF = 0.1;         // 陀螺仪低通
  
  // 限制
  float maxAngleError = 0.3;   // 最大角度误差30°
  float maxControl = 12.0;     // 最大控制量
  float deadZone = 0.02;       // 死区2°
};
BalanceControl balance;

// 互补滤波状态
float compFilterAngle = 0.0;
float compFilterVelocity = 0.0;
float lastGyroZ = 0.0;

// 移动控制
struct Movement {
  float targetVelocity = 0.0;  // 目标速度
  float currentVelocity = 0.0; // 当前速度
  float position = 0.0;        // 位置
  float velocityError = 0.0;   // 速度误差
  
  // 移动PID
  float kp_move = 1.0;
  float ki_move = 0.1;
  float kd_move = 0.05;
  
  float moveIntegral = 0.0;
  float lastMoveError = 0.0;
};
Movement movement;

// 安全状态
enum SafetyState {
  SAFE_STARTUP,     // 启动
  SAFE_BALANCING,   // 平衡中
  SAFE_FAULT,       // 故障
  SAFE_FALLEN       // 跌倒
};
SafetyState safetyState = SAFE_STARTUP;

void setup() {
  Serial.begin(115200);
  Serial.println("===== 双轮自平衡机器人控制系统 =====");
  Serial.println("BLDC + MPU6050 + PID平衡控制");
  
  // 1. 初始化I2C
  Wire.begin(IMU_SDA, IMU_SCL);
  delay(100);
  
  // 2. 初始化IMU
  initIMU();
  
  // 3. 初始化BLDC电机
  initBalanceMotors();
  
  // 4. 校准IMU
  calibrateIMU();
  
  // 5. 启动自检
  startupSelfTest();
  
  Serial.println("系统就绪，等待平衡启动");
  Serial.println("按空格键开始平衡");
}

void loop() {
  static unsigned long lastControlTime = 0;
  unsigned long now = micros();
  float dt = (now - lastControlTime) / 1000000.0;
  
  if (dt >= 0.005) {  // 200Hz控制频率
    // 1. 读取IMU数据
    readIMUData(dt);
    
    // 2. 平衡控制
    balanceControl(dt);
    
    // 3. 移动控制
    movementControl(dt);
    
    // 4. 电机控制
    motorControl(dt);
    
    // 5. 安全检查
    safetyCheck();
    
    // 6. 执行FOC
    motorLeft.loopFOC();
    motorRight.loopFOC();
    
    lastControlTime = now;
  }
  
  // 状态显示
  static unsigned long lastDisplay = 0;
  if (millis() - lastDisplay >= 50) {  // 20Hz显示
    displayBalanceStatus();
    lastDisplay = millis();
  }
  
  // 处理控制命令
  if (Serial.available()) {
    handleControlCommand();
  }
}

void initIMU() {
  Serial.println("初始化MPU6050...");
  
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
  
  // 配置IMU
  mpu6050.setGyroOffsets(-1.32, 0.79, 0.06);
  mpu6050.setAccOffsets(0.07, 0.03, 0.01);
  
  Serial.println("IMU初始化完成");
}

void calibrateIMU() {
  Serial.println("IMU校准中...");
  Serial.println("保持机器人静止10秒");
  
  float accXSum = 0, accYSum = 0, accZSum = 0;
  float gyroXSum = 0, gyroYSum = 0, gyroZSum = 0;
  int samples = 1000;
  
  for (int i = 0; i < samples; i++) {
    mpu6050.update();
    
    accXSum += mpu6050.getAccX();
    accYSum += mpu6050.getAccY();
    accZSum += mpu6050.getAccZ();
    gyroZSum += mpu6050.getGyroZ();
    
    delay(10);
    
    if (i % 100 == 0) {
      Serial.print(".");
    }
  }
  
  // 计算零点偏移
  balance.targetAngle = atan2(accYSum / samples, accZSum / samples);
  lastGyroZ = gyroZSum / samples;
  
  Serial.println();
  Serial.print("校准完成，零位角度: ");
  Serial.print(balance.targetAngle * 180 / PI);
  Serial.println("°");
}

void readIMUData(float dt) {
  // 读取并处理IMU数据
  
  mpu6050.update();
  
  // 读取原始数据
  float accX = mpu6050.getAccX();
  float accY = mpu6050.getAccY();
  float accZ = mpu6050.getAccZ();
  float gyroZ = mpu6050.getGyroZ() * PI / 180.0;  // 转换为rad/s
  
  // 低通滤波陀螺仪
  gyroZ = lastGyroZ * (1 - balance.gyroLPF) + gyroZ * balance.gyroLPF;
  lastGyroZ = gyroZ;
  
  // 计算加速度计角度
  float accAngle = atan2(accY, accZ);
  
  // 互补滤波
  compFilterAngle = balance.alpha * (compFilterAngle + gyroZ * dt) + 
                    (1 - balance.alpha) * accAngle;
  
  // 计算角速度
  compFilterVelocity = gyroZ;
  
  // 更新状态
  balance.currentAngle = compFilterAngle;
  balance.angleVelocity = compFilterVelocity;
  balance.angleError = balance.targetAngle - balance.currentAngle;
  
  // 限制角度误差
  if (abs(balance.angleError) > balance.maxAngleError) {
    balance.angleError = (balance.angleError > 0) ? balance.maxAngleError : -balance.maxAngleError;
  }
}

void balanceControl(float dt) {
  // 平衡控制
  
  static float angleIntegral = 0;
  static float lastAngleError = 0;
  
  // 死区处理
  if (abs(balance.angleError) < balance.deadZone) {
    balance.angleError = 0;
  }
  
  // 角度PID
  angleIntegral += balance.angleError * dt;
  angleIntegral = constrain(angleIntegral, -1.0, 1.0);
  
  float angleDerivative = (balance.angleError - lastAngleError) / dt;
  lastAngleError = balance.angleError;
  
  // 计算角度控制量
  float angleControl = balance.kp_angle * balance.angleError + 
                      balance.ki_angle * angleIntegral + 
                      balance.kd_angle * angleDerivative;
  
  // 速度反馈
  float velocityControl = balance.kp_velocity * balance.angleVelocity;
  
  // 总平衡控制量
  float balanceControl = angleControl + velocityControl;
  
  // 限制控制量
  balanceControl = constrain(balanceControl, -balance.maxControl, balance.maxControl);
  
  // 应用到左右轮
  motorLeft.target = balanceControl;
  motorRight.target = -balanceControl;  // 反向，因为轮子方向相反
}

void movementControl(float dt) {
  // 移动控制
  
  if (safetyState != SAFE_BALANCING) return;
  
  // 读取目标速度
  movement.targetVelocity = readTargetVelocity();
  
  // 计算实际速度
  float leftSpeed = motorLeft.shaft_velocity * WHEEL_RADIUS;
  float rightSpeed = motorRight.shaft_velocity * WHEEL_RADIUS;
  movement.currentVelocity = (leftSpeed + rightSpeed) / 2.0;
  
  // 计算速度误差
  movement.velocityError = movement.targetVelocity - movement.currentVelocity;
  
  // 速度PID
  movement.moveIntegral += movement.velocityError * dt;
  movement.moveIntegral = constrain(movement.moveIntegral, -0.5, 0.5);
  
  float moveDerivative = (movement.velocityError - movement.lastMoveError) / dt;
  movement.lastMoveError = movement.velocityError;
  
  // 计算移动控制量
  float moveControl = movement.kp_move * movement.velocityError + 
                     movement.ki_move * movement.moveIntegral + 
                     movement.kd_move * moveDerivative;
  
  // 转换为角度偏移
  balance.targetAngle += moveControl * 0.1;  // 缩放
  
  // 限制目标角度
  balance.targetAngle = constrain(balance.targetAngle, 
                                 -balance.maxAngleError, 
                                 balance.maxAngleError);
}

void motorControl(float dt) {
  // 电机控制
  
  // 计算轮子速度
  float leftSpeed = motorLeft.shaft_velocity;
  float rightSpeed = motorRight.shaft_velocity;
  
  // 同步控制
  float speedDiff = leftSpeed + rightSpeed;  // 注意：轮子方向相反
  float syncCorrection = speedDiff * 0.1;
  
  // 应用控制
  motorLeft.move(motorLeft.target + syncCorrection);
  motorRight.move(motorRight.target - syncCorrection);
}

void safetyCheck() {
  // 安全检查
  
  // 检查角度
  if (abs(balance.currentAngle) > 0.5) {  // 超过30°
    safetyState = SAFE_FALLEN;
    Serial.println("机器人跌倒!");
    emergencyStop();
    return;
  }
  
  // 检查角速度
  if (abs(balance.angleVelocity) > 3.0) {  // 超过3rad/s
    safetyState = SAFE_FAULT;
    Serial.println("角速度过大!");
    emergencyStop();
    return;
  }
  
  // 检查电机电流
  static unsigned long lastCurrentCheck = 0;
  if (millis() - lastCurrentCheck > 100) {
    DQCurrent_s leftCurrent = motorLeft.current;
    DQCurrent_s rightCurrent = motorRight.current;
    
    float leftCurrentMag = sqrt(leftCurrent.d*leftCurrent.d + leftCurrent.q*leftCurrent.q);
    float rightCurrentMag = sqrt(rightCurrent.d*rightCurrent.d + rightCurrent.q*rightCurrent.q);
    
    if (leftCurrentMag > 5.0 || rightCurrentMag > 5.0) {
      safetyState = SAFE_FAULT;
      Serial.println("电机过流!");
      emergencyStop();
    }
    
    lastCurrentCheck = millis();
  }
  
  // 检查IMU数据有效性
  static int imuErrorCount = 0;
  mpu6050.update();
  float accMag = sqrt(mpu6050.getAccX()*mpu6050.getAccX() + 
                     mpu6050.getAccY()*mpu6050.getAccY() + 
                     mpu6050.getAccZ()*mpu6050.getAccZ());
  
  if (accMag < 0.5 || accMag > 1.5) {  // 加速度幅值应该在1g附近
    imuErrorCount++;
    if (imuErrorCount > 10) {
      safetyState = SAFE_FAULT;
      Serial.println("IMU数据异常!");
      emergencyStop();
    }
  } else {
    imuErrorCount = 0;
  }
}

void emergencyStop() {
  // 紧急停止
  
  Serial.println("紧急停止激活!");
  
  // 停止电机
  motorLeft.disable();
  motorRight.disable();
  
  // 发送停止信号
  motorLeft.move(0);
  motorRight.move(0);
  
  // 等待重启
  delay(1000);
  
  // 重新启用
  motorLeft.enable();
  motorRight.enable();
  
  // 重置状态
  safetyState = SAFE_STARTUP;
  balance.targetAngle = 0;
}

5、卡尔曼滤波平衡控制
场景：高性能自平衡，需要更精确的姿态估计。
核心逻辑：卡尔曼滤波 + LQR控制 + 状态观测器。

#include <SimpleFOC.h>
#include <BasicLinearAlgebra.h>  // 线性代数库
#include <math.h>

// 使用BLA库进行矩阵运算
using namespace BLA;

// 系统状态: [角度, 角速度, 位置, 速度]
#define STATE_DIM 4
#define MEAS_DIM 2
#define CONTROL_DIM 1

// 卡尔曼滤波器
class KalmanFilter {
private:
  // 状态矩阵
  Matrix<STATE_DIM, STATE_DIM> F;  // 状态转移
  Matrix<STATE_DIM, CONTROL_DIM> B;  // 控制输入
  Matrix<MEAS_DIM, STATE_DIM> H;     // 观测矩阵
  Matrix<STATE_DIM, STATE_DIM> P;    // 误差协方差
  Matrix<STATE_DIM, STATE_DIM> Q;    // 过程噪声
  Matrix<MEAS_DIM, MEAS_DIM> R;      // 测量噪声
  
  // 状态
  Matrix<STATE_DIM, 1> x;  // 状态向量
  
  // 机器人参数
  float m = 2.0;      // 质量 kg
  float l = 0.15;     // 质心高度 m
  float g = 9.81;     // 重力加速度
  float b = 0.1;      // 阻尼系数
  
public:
  KalmanFilter() {
    // 初始化矩阵
    initMatrices();
  }
  
  void initMatrices() {
    float dt = 0.005;  // 5ms
    
    // 状态转移矩阵 (线性倒立摆模型)
    F = {1, dt, 0, 0,
         (m*g*l*dt)/1.0, 1, 0, 0,
         0, 0, 1, dt,
         0, 0, 0, 1};
    
    // 控制输入矩阵
    B = {0, dt/m, 0, dt};
    
    // 观测矩阵 (只能观测角度和位置)
    H = {1, 0, 0, 0,
         0, 0, 1, 0};
    
    // 初始化误差协方差
    P.Identity();
    P *= 0.1;
    
    // 过程噪声
    Q.Identity();
    Q *= 0.01;
    
    // 测量噪声
    R.Identity();
    R *= 0.1;
    
    // 初始状态
    x = {0, 0, 0, 0};
  }
  
  Matrix<STATE_DIM, 1> update(float angleMeas, float positionMeas, float control, float dt) {
    // 卡尔曼滤波更新
    
    // 预测步骤
    predict(control, dt);
    
    // 更新步骤
    updateMeasurement(angleMeas, positionMeas);
    
    return x;
  }
  
private:
  void predict(float control, float dt) {
    // 预测步骤
    
    // 更新状态转移矩阵
    updateF(dt);
    updateB(dt);
    
    // 状态预测: x = F*x + B*u
    x = F * x + B * control;
    
    // 协方差预测: P = F*P*F' + Q
    P = F * P * ~F + Q;
  }
  
  void updateMeasurement(float angleMeas, float positionMeas) {
    // 测量更新
    
    Matrix<MEAS_DIM, 1> z = {angleMeas, positionMeas};
    
    // 计算卡尔曼增益: K = P*H'*(H*P*H' + R)^-1
    Matrix<MEAS_DIM, MEAS_DIM> S = H * P * ~H + R;
    Matrix<STATE_DIM, MEAS_DIM> K = P * ~H * Inverse(S);
    
    // 状态更新: x = x + K*(z - H*x)
    Matrix<MEAS_DIM, 1> y = z - H * x;
    x = x + K * y;
    
    // 协方差更新: P = (I - K*H)*P
    Matrix<STATE_DIM, STATE_DIM> I;
    I.Identity();
    P = (I - K * H) * P;
  }
  
  void updateF(float dt) {
    // 更新状态转移矩阵
    
    // 倒立摆线性化模型
    float a11 = 1;
    float a12 = dt;
    float a21 = (m * g * l) * dt;
    float a22 = 1 - (b / (m * l * l)) * dt;
    
    F = {a11, a12, 0, 0,
         a21, a22, 0, 0,
         0, 0, 1, dt,
         0, 0, 0, 1};
  }
  
  void updateB(float dt) {
    // 更新控制输入矩阵
    B = {0, dt/(m*l*l), 0, dt/m};
  }
};

KalmanFilter kalman;

// LQR控制器
class LQRController {
private:
  // 权重矩阵
  Matrix<STATE_DIM, STATE_DIM> Q;
  Matrix<CONTROL_DIM, CONTROL_DIM> R;
  
  // 反馈增益
  Matrix<CONTROL_DIM, STATE_DIM> K;
  
  // 求解Riccati方程
  Matrix<STATE_DIM, STATE_DIM> P;
  
public:
  LQRController() {
    // 初始化权重
    Q.Identity();
    Q *= 10.0;  // 状态权重
    
    R.Identity();
    R *= 1.0;   // 控制权重
    
    // 计算LQR增益
    calculateGain();
  }
  
  float calculate(Matrix<STATE_DIM, 1> x) {
    // 计算LQR控制: u = -K*x
    Matrix<CONTROL_DIM, 1> u = -K * x;
    return u(0);
  }
  
  void updateWeights(float angleWeight, float velocityWeight, 
                    float positionWeight, float speedWeight, 
                    float controlWeight) {
    // 更新权重
    Q = {angleWeight, 0, 0, 0,
         0, velocityWeight, 0, 0,
         0, 0, positionWeight, 0,
         0, 0, 0, speedWeight};
    
    R = {controlWeight};
    
    // 重新计算增益
    calculateGain();
  }
  
private:
  void calculateGain() {
    // 使用迭代法求解Riccati方程
    
    float dt = 0.005;
    int maxIter = 100;
    float tolerance = 1e-6;
    
    // 离散化系统
    Matrix<STATE_DIM, STATE_DIM> A = {1, dt, 0, 0,
                                     (2.0 * 9.81 * 0.15)*dt, 1, 0, 0,
                                     0, 0, 1, dt,
                                     0, 0, 0, 1};
    Matrix<STATE_DIM, CONTROL_DIM> B = {0, dt/(2.0 * 0.15 * 0.15), 0, dt/2.0};
    
    // 初始化P
    P = Q;
    
    // 迭代求解
    for (int i = 0; i < maxIter; i++) {
      Matrix<STATE_DIM, STATE_DIM> P_prev = P;
      
      // 计算增益: K = (R + B'*P*B)^-1 * B'*P*A
      Matrix<CONTROL_DIM, CONTROL_DIM> temp = R + ~B * P * B;
      K = Inverse(temp) * ~B * P * A;
      
      // 更新P: P = A'*P*A - A'*P*B*K + Q
      P = ~A * P * A - ~A * P * B * K + Q;
      
      // 检查收敛
      float diff = 0;
      for (int r = 0; r < STATE_DIM; r++) {
        for (int c = 0; c < STATE_DIM; c++) {
          diff += abs(P(r,c) - P_prev(r,c));
        }
      }
      
      if (diff < tolerance) break;
    }
  }
};

LQRController lqr;

// 扩展状态观测器
class ExtendedStateObserver {
private:
  // 状态: [角度, 角速度, 扰动]
  Matrix<3, 1> x;
  Matrix<3, 3> A;
  Matrix<3, 1> B;
  Matrix<1, 3> C;
  Matrix<3, 3> L;  // 观测器增益
  
  float dt = 0.005;
  
public:
  ExtendedStateObserver() {
    // 初始化观测器
    initObserver();
  }
  
  Matrix<3, 1> estimate(float angleMeas, float control) {
    // 估计状态
    
    // 预测: x = A*x + B*u
    x = A * x + B * control;
    
    // 更新: x = x + L*(y - C*x)
    float y = angleMeas;
    float error = y - (C * x)(0);
    x = x + L * error;
    
    return x;
  }
  
  float getDisturbance() {
    return x(2);
  }
  
private:
  void initObserver() {
    // 初始化观测器矩阵
    
    A = {1, dt, 0,
         (2.0 * 9.81 * 0.15)*dt, 1, dt,
         0, 0, 1};
    
    B = {0, dt/(2.0 * 0.15 * 0.15), 0};
    
    C = {1, 0, 0};
    
    // 观测器增益 (极点配置)
    float pole1 = 0.9;
    float pole2 = 0.8;
    float pole3 = 0.7;
    
    // 简化增益计算
    L = {3*(1-pole1)/dt,
         3*(1-pole2)/(dt*dt),
         (1-pole3)/(dt*dt*dt)};
    
    x = {0, 0, 0};
  }
};

ExtendedStateObserver eso;

// 自适应LQR控制
void advancedBalanceControl(float dt) {
  // 高级平衡控制
  
  // 1. 卡尔曼滤波估计状态
  float angleMeas = balance.currentAngle;
  float positionMeas = movement.position;
  float control = 0;  // 上一时刻控制量
  
  Matrix<STATE_DIM, 1> state = kalman.update(angleMeas, positionMeas, control, dt);
  
  // 2. 扩展状态观测器估计扰动
  Matrix<3, 1> esoState = eso.estimate(angleMeas, control);
  float disturbance = eso.getDisturbance();
  
  // 3. LQR控制
  float lqrControl = lqr.calculate(state);
  
  // 4. 扰动补偿
  float disturbanceCompensation = -disturbance * 0.5;
  
  // 5. 自适应调整
  adaptiveControlTuning(state, dt);
  
  // 6. 计算最终控制量
  float totalControl = lqrControl + disturbanceCompensation;
  
  // 7. 应用控制
  applyControl(totalControl, dt);
}

void adaptiveControlTuning(Matrix<STATE_DIM, 1> state, float dt) {
  // 自适应控制参数调整
  
  static float lastAngleError = 0;
  static float errorIntegral = 0;
  
  float angle = state(0);
  float angleVel = state(1);
  float position = state(2);
  float velocity = state(3);
  
  // 计算性能指标
  float angleError = abs(angle);
  float velocityError = abs(velocity);
  
  errorIntegral += angleError * dt;
  
  // 自适应调整LQR权重
  if (angleError > 0.1) {  // 角度误差大
    lqr.updateWeights(50.0, 5.0, 1.0, 0.1, 1.0);  // 增加角度权重
  } else if (velocityError > 0.5) {  // 速度误差大
    lqr.updateWeights(10.0, 1.0, 5.0, 10.0, 1.0);  // 增加速度权重
  } else {
    lqr.updateWeights(20.0, 2.0, 2.0, 1.0, 1.0);  // 默认权重
  }
  
  // 自适应调整卡尔曼噪声
  static Matrix<MEAS_DIM, MEAS_DIM> R_adapt;
  R_adapt = {angleError*0.1 + 0.01, 0,
             0, velocityError*0.1 + 0.01};
  
  lastAngleError = angleError;
}

6、神经网络自适应平衡控制
场景：智能自适应平衡，应对复杂环境。
核心逻辑：神经网络控制器 + 强化学习 + 在线适应。

#include <SimpleFOC.h>
#include <TensorFlowLite_ESP32.h>
#include <eloquent_tinyml.h>

// 神经网络平衡控制器
class NeuralBalanceController {
private:
  // 神经网络结构
  #define NN_INPUTS 6      // 状态维度
  #define NN_HIDDEN 8      // 隐藏层
  #define NN_OUTPUTS 2     // 输出: 左轮控制, 右轮控制
  
  // 神经网络权重
  float W1[NN_INPUTS][NN_HIDDEN];
  float b1[NN_HIDDEN];
  float W2[NN_HIDDEN][NN_OUTPUTS];
  float b2[NN_OUTPUTS];
  
  // 自适应参数
  float learningRate = 0.001;
  float explorationRate = 0.1;
  float discountFactor = 0.99;
  
  // 经验回放
  struct Experience {
    float state[NN_INPUTS];
    float action[NN_OUTPUTS];
    float reward;
    float nextState[NN_INPUTS];
    bool done;
  };
  Experience replayBuffer[100];
  int bufferIndex = 0;
  
  // 目标网络
  float W1_target[NN_INPUTS][NN_HIDDEN];
  float b1_target[NN_HIDDEN];
  float W2_target[NN_HIDDEN][NN_OUTPUTS];
  float b2_target[NN_OUTPUTS];
  
public:
  NeuralBalanceController() {
    initNetwork();
  }
  
  void getControl(float* state, float* control, float dt) {
    // 获取神经网络控制
    
    // 探索策略
    if (random(100) / 100.0 < explorationRate) {
      // 探索: 随机控制
      control[0] = random(-1000, 1000) / 1000.0;
      control[1] = random(-1000, 1000) / 1000.0;
    } else {
      // 利用: 神经网络控制
      forwardPropagation(state, control);
    }
    
    // 限制控制量
    control[0] = constrain(control[0], -1.0, 1.0);
    control[1] = constrain(control[1], -1.0, 1.0);
  }
  
  void learn(float* state, float* action, float reward, 
             float* nextState, bool done, float dt) {
    // 强化学习
    
    // 存储经验
    storeExperience(state, action, reward, nextState, done);
    
    // 训练
    if (bufferIndex >= 10) {
      trainFromExperience(dt);
    }
    
    // 衰减探索率
    explorationRate *= 0.9995;
    explorationRate = max(explorationRate, 0.01);
  }
  
private:
  void initNetwork() {
    // 初始化神经网络权重
    
    float scale = sqrt(2.0 / NN_INPUTS);
    
    // 初始化第一层
    for (int i = 0; i < NN_INPUTS; i++) {
      for (int j = 0; j < NN_HIDDEN; j++) {
        W1[i][j] = randomGaussian() * scale;
        W1_target[i][j] = W1[i][j];
      }
    }
    
    for (int j = 0; j < NN_HIDDEN; j++) {
      b1[j] = 0.1;
      b1_target[j] = b1[j];
    }
    
    // 初始化第二层
    scale = sqrt(2.0 / NN_HIDDEN);
    for (int i = 0; i < NN_HIDDEN; i++) {
      for (int j = 0; j < NN_OUTPUTS; j++) {
        W2[i][j] = randomGaussian() * scale;
        W2_target[i][j] = W2[i][j];
      }
    }
    
    for (int j = 0; j < NN_OUTPUTS; j++) {
      b2[j] = 0.1;
      b2_target[j] = b2[j];
    }
  }
  
  float randomGaussian() {
    float u1 = random(1000) / 1000.0;
    float u2 = random(1000) / 1000.0;
    return sqrt(-2.0 * log(u1)) * cos(2.0 * PI * u2);
  }
  
  void forwardPropagation(float* input, float* output) {
    // 前向传播
    
    // 隐藏层
    float hidden[NN_HIDDEN];
    for (int j = 0; j < NN_HIDDEN; j++) {
      hidden[j] = 0;
      for (int i = 0; i < NN_INPUTS; i++) {
        hidden[j] += input[i] * W1[i][j];
      }
      hidden[j] += b1[j];
      hidden[j] = relu(hidden[j]);
    }
    
    // 输出层
    for (int j = 0; j < NN_OUTPUTS; j++) {
      output[j] = 0;
      for (int i = 0; i < NN_HIDDEN; i++) {
        output[j] += hidden[i] * W2[i][j];
      }
      output[j] += b2[j];
      output[j] = tanh(output[j]);  // 输出范围[-1, 1]
    }
  }
  
  void storeExperience(float* state, float* action, float reward, 
                       float* nextState, bool done) {
    if (bufferIndex < 100) {
      memcpy(replayBuffer[bufferIndex].state, state, NN_INPUTS * sizeof(float));
      memcpy(replayBuffer[bufferIndex].action, action, NN_OUTPUTS * sizeof(float));
      replayBuffer[bufferIndex].reward = reward;
      memcpy(replayBuffer[bufferIndex].nextState, nextState, NN_INPUTS * sizeof(float));
      replayBuffer[bufferIndex].done = done;
      bufferIndex++;
    }
  }
  
  void trainFromExperience(float dt) {
    // 从经验中学习
    
    int batchSize = 10;
    float totalLoss = 0;
    
    for (int b = 0; b < batchSize; b++) {
      int idx = random(bufferIndex);
      Experience exp = replayBuffer[idx];
      
      // 计算目标Q值
      float targetQ = exp.reward;
      if (!exp.done) {
        float nextQ[NN_OUTPUTS];
        forwardPropagationTarget(exp.nextState, nextQ);
        targetQ += discountFactor * max(nextQ[0], nextQ[1]);
      }
      
      // 计算当前Q值
      float currentQ[NN_OUTPUTS];
      forwardPropagation(exp.state, currentQ);
      float currentActionQ = 0;
      for (int i = 0; i < NN_OUTPUTS; i++) {
        currentActionQ += currentQ[i] * exp.action[i];
      }
      
      // 计算TD误差
      float tdError = targetQ - currentActionQ;
      
      // 反向传播
      backpropagate(exp.state, exp.action, tdError, dt);
      
      totalLoss += tdError * tdError;
    }
    
    // 软更新目标网络
    softUpdateTargetNetwork();
  }
  
  void forwardPropagationTarget(float* input, float* output) {
    // 目标网络前向传播
    
    float hidden[NN_HIDDEN];
    for (int j = 0; j < NN_HIDDEN; j++) {
      hidden[j] = 0;
      for (int i = 0; i < NN_INPUTS; i++) {
        hidden[j] += input[i] * W1_target[i][j];
      }
      hidden[j] += b1_target[j];
      hidden[j] = relu(hidden[j]);
    }
    
    for (int j = 0; j < NN_OUTPUTS; j++) {
      output[j] = 0;
      for (int i = 0; i < NN_HIDDEN; i++) {
        output[j] += hidden[i] * W2_target[i][j];
      }
      output[j] += b2_target[j];
      output[j] = tanh(output[j]);
    }
  }
  
  void backpropagate(float* state, float* action, float tdError, float dt) {
    // 反向传播
    
    // 前向传播计算激活
    float hidden[NN_HIDDEN];
    float hiddenActivation[NN_HIDDEN];
    
    for (int j = 0; j < NN_HIDDEN; j++) {
      hidden[j] = 0;
      for (int i = 0; i < NN_INPUTS; i++) {
        hidden[j] += state[i] * W1[i][j];
      }
      hidden[j] += b1[j];
      hiddenActivation[j] = relu(hidden[j]);
    }
    
    // 输出层梯度
    float outputGrad[NN_OUTPUTS];
    for (int j = 0; j < NN_OUTPUTS; j++) {
      outputGrad[j] = tdError * action[j] * tanhDeriv(hiddenActivation, j);
    }
    
    // 隐藏层梯度
    float hiddenGrad[NN_HIDDEN];
    for (int i = 0; i < NN_HIDDEN; i++) {
      hiddenGrad[i] = 0;
      for (int j = 0; j < NN_OUTPUTS; j++) {
        hiddenGrad[i] += outputGrad[j] * W2[i][j];
      }
      hiddenGrad[i] *= reluDeriv(hidden[i]);
    }
    
    // 更新第二层权重
    for (int i = 0; i < NN_HIDDEN; i++) {
      for (int j = 0; j < NN_OUTPUTS; j++) {
        W2[i][j] -= learningRate * outputGrad[j] * hiddenActivation[i];
      }
    }
    
    // 更新第二层偏置
    for (int j = 0; j < NN_OUTPUTS; j++) {
      b2[j] -= learningRate * outputGrad[j];
    }
    
    // 更新第一层权重
    for (int i = 0; i < NN_INPUTS; i++) {
      for (int j = 0; j < NN_HIDDEN; j++) {
        W1[i][j] -= learningRate * hiddenGrad[j] * state[i];
      }
    }
    
    // 更新第一层偏置
    for (int j = 0; j < NN_HIDDEN; j++) {
      b1[j] -= learningRate * hiddenGrad[j];
    }
  }
  
  void softUpdateTargetNetwork() {
    // 软更新目标网络
    
    float tau = 0.001;
    
    for (int i = 0; i < NN_INPUTS; i++) {
      for (int j = 0; j < NN_HIDDEN; j++) {
        W1_target[i][j] = tau * W1[i][j] + (1 - tau) * W1_target[i][j];
      }
    }
    
    for (int j = 0; j < NN_HIDDEN; j++) {
      b1_target[j] = tau * b1[j] + (1 - tau) * b1_target[j];
    }
    
    for (int i = 0; i < NN_HIDDEN; i++) {
      for (int j = 0; j < NN_OUTPUTS; j++) {
        W2_target[i][j] = tau * W2[i][j] + (1 - tau) * W2_target[i][j];
      }
    }
    
    for (int j = 0; j < NN_OUTPUTS; j++) {
      b2_target[j] = tau * b2[j] + (1 - tau) * b2_target[j];
    }
  }
  
  float relu(float x) { return max(0.0f, x); }
  float reluDeriv(float x) { return (x > 0) ? 1.0f : 0.0f; }
  float tanhDeriv(float* hidden, int j) { 
    float x = 0;
    for (int i = 0; i < NN_HIDDEN; i++) {
      x += hidden[i] * W2[i][j];
    }
    x += b2[j];
    return 1.0 - tanh(x) * tanh(x);
  }
};

NeuralBalanceController neuralController;

// 自适应奖励函数
class AdaptiveReward {
private:
  float angleWeight = 1.0;
  float velocityWeight = 0.1;
  float positionWeight = 0.01;
  float controlWeight = 0.001;
  float energyWeight = 0.0001;
  
  // 自适应调整
  float learningRate = 0.01;
  float performanceHistory[100] = {0};
  int historyIndex = 0;
  
public:
  float calculate(float angle, float angleVel, float position, 
                  float velocity, float control, float dt) {
    // 计算奖励
    
    float reward = 0;
    
    // 角度惩罚
    reward -= angleWeight * angle * angle;
    
    // 角速度惩罚
    reward -= velocityWeight * angleVel * angleVel;
    
    // 位置惩罚
    reward -= positionWeight * position * position;
    
    // 速度惩罚
    reward -= controlWeight * velocity * velocity;
    
    // 控制量惩罚
    reward -= energyWeight * control * control;
    
    // 平衡奖励
    if (abs(angle) < 0.05 && abs(angleVel) < 0.1) {
      reward += 1.0;
    }
    
    // 记录性能
    recordPerformance(abs(angle));
    
    // 自适应调整权重
    adaptWeights(dt);
    
    return reward;
  }
  
  void adaptWeights(float dt) {
    // 自适应调整权重
    
    // 计算平均性能
    float avgPerformance = 0;
    for (int i = 0; i < 100; i++) {
      avgPerformance += performanceHistory[i];
    }
    avgPerformance /= 100.0;
    
    // 基于性能调整权重
    if (avgPerformance < 0.05) {
      // 性能好，减小控制惩罚
      controlWeight *= 0.99;
      energyWeight *= 0.99;
    } else if (avgPerformance > 0.1) {
      // 性能差，增加角度惩罚
      angleWeight *= 1.01;
      velocityWeight *= 1.01;
    }
    
    // 限制范围
    angleWeight = constrain(angleWeight, 0.5, 2.0);
    velocityWeight = constrain(velocityWeight, 0.05, 0.2);
    controlWeight = constrain(controlWeight, 0.0005, 0.002);
  }
  
  void recordPerformance(float angleError) {
    performanceHistory[historyIndex] = angleError;
    historyIndex = (historyIndex + 1) % 100;
  }
};

AdaptiveReward adaptiveReward;

// 智能平衡控制
void intelligentBalanceControl(float dt) {
  // 智能平衡控制
  
  // 1. 构建状态向量
  float state[6];
  buildStateVector(state);
  
  // 2. 保存上一状态
  static float lastState[6];
  static float lastControl[2] = {0, 0};
  
  // 3. 神经网络控制
  float control[2];
  neuralController.getControl(state, control, dt);
  
  // 4. 应用控制
  applyNeuralControl(control, dt);
  
  // 5. 获取新状态
  float newState[6];
  buildStateVector(newState);
  
  // 6. 计算奖励
  float angle = state[0];
  float angleVel = state[1];
  float position = state[2];
  float velocity = state[3];
  float controlEffort = (abs(control[0]) + abs(control[1])) / 2.0;
  
  float reward = adaptiveReward.calculate(angle, angleVel, position, 
                                         velocity, controlEffort, dt);
  
  // 7. 学习
  neuralController.learn(state, control, reward, newState, false, dt);
  
  // 8. 保存状态
  memcpy(lastState, state, 6 * sizeof(float));
  memcpy(lastControl, control, 2 * sizeof(float));
}

void buildStateVector(float* state) {
  // 构建状态向量
  
  state[0] = balance.currentAngle;          // 角度
  state[1] = balance.angleVelocity;         // 角速度
  state[2] = movement.position;             // 位置
  state[3] = movement.currentVelocity;      // 速度
  state[4] = motorLeft.shaft_velocity;      // 左轮速度
  state[5] = motorRight.shaft_velocity;     // 右轮速度
  
  // 归一化
  state[0] = normalize(state[0], -0.5, 0.5);
  state[1] = normalize(state[1], -5.0, 5.0);
  state[2] = normalize(state[2], -1.0, 1.0);
  state[3] = normalize(state[3], -2.0, 2.0);
  state[4] = normalize(state[4], -20.0, 20.0);
  state[5] = normalize(state[5], -20.0, 20.0);
}

float normalize(float x, float min, float max) {
  return (x - min) / (max - min);
}

void applyNeuralControl(float* control, float dt) {
  // 应用神经网络控制
  
  // 缩放控制量
  float leftControl = control[0] * 12.0;  // 映射到±12V
  float rightControl = control[1] * 12.0;
  
  // 限制变化率
  static float lastLeftControl = 0, lastRightControl = 0;
  float maxChange = 100.0 * dt;
  
  leftControl = limitRate(leftControl, lastLeftControl, maxChange);
  rightControl = limitRate(rightControl, lastRightControl, maxChange);
  
  // 应用控制
  motorLeft.move(leftControl);
  motorRight.move(rightControl);
  
  lastLeftControl = leftControl;
  lastRightControl = rightControl;
}

float limitRate(float current, float last, float maxChange) {
  float change = current - last;
  if (abs(change) > maxChange) {
    return last + (change > 0 ? maxChange : -maxChange);
  }
  return current;
}

要点解读
IMU数据融合的三种算法对比
互补滤波：案例4的alpha=0.98融合陀螺仪积分（短期准）和加速度计（长期准）。计算量最小，适合MCU。
卡尔曼滤波：案例5的线性卡尔曼，最优估计但需模型。状态矩阵F基于倒立摆模型，Q、R需调试。
Madgwick/Mahony：未展示但常用，四元数表示，适合3D姿态。比卡尔曼简单，比互补滤波准。
关键点：必须处理陀螺仪零漂。案例4 calibrateIMU()采集1000点平均。MPU6050零漂约1°/s，10秒积累10°误差。
平衡控制的层级结构
内环平衡：快速响应，200Hz。案例4的角度PIDkp=25产生恢复力矩。
外环移动：慢速，50-100Hz。通过调整targetAngle实现移动。targetAngle += moveControl0.1。
同步控制：防止两轮不同步。案例4 syncCorrection = speedDiff0.1。
抗饱和：积分项必须限幅，案例4 angleIntegral = constrain(angleIntegral, -1.0, 1.0)。
前馈：角速度反馈velocityControl提高阻尼，防止振荡。
LQR与PID的对比选择
PID优势：简单，易调，案例4适用。但多变量耦合处理差。
LQR优势：案例5的多变量最优控制。需状态空间模型[θ, θ̇, x, v]，解Riccati方程求增益K。
权重选择：Q对角元素对应状态权重。角度误差最重，案例二Q(0,0)=50。
实时性：LQR需矩阵运算，4×4矩阵在MCU上可行。案例二迭代100次求解，可离线计算。
自适应LQR：基于性能在线调整Q、R。角度误差大时增加角度权重。
神经网络控制的嵌入式实现挑战
网络规模：案例6 6-8-2结构，6×8+8×2=64权重，可实时运行。
激活函数：隐藏层ReLU计算快，输出层tanh输出[-1,1]。
在线学习：TD误差targetQ - currentQ，经验回放100条。但MCU上训练极慢，通常预训练。
探索策略：ε-greedy，ε=0.1。必须探索但需保证安全。
奖励设计：案例6奖励 = -角度² - 0.1×角速度² - 0.01×控制量²。这是强化学习核心。
安全机制的必备要素
跌倒检测：角度>30°或角速度>3rad/s触发保护。案例一safetyCheck()。
电流保护：电机电流>5A停止。需快速响应防止烧MOS。
IMU失效检测：加速度幅值应在1g附近。案例4检查0.5<|acc|<1.5。
软件看门狗：控制循环需定期喂狗，超时急停。
软启动：启动时缓慢增加控制量，防止跳跃。
紧急停止策略：
立即置零PWM
减速停止
记录状态
等待手动恢复

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