这套系统本质上是一个倒立摆式的实时姿态闭环：IMU 提供角速度和加速度信息，互补滤波输出稳定的俯仰角，直立环再把角度误差转换成 BLDC 的恢复力矩。 如果用 BLDC 做自平衡，建议把 FOC 当作底层执行环，直立环只输出期望力矩或期望转速，避免在姿态主循环里直接处理换相细节。

主要特点
互补滤波解决“快”和“稳”的矛盾
陀螺仪动态响应好，但积分会漂移；加速度计能提供重力方向参考，但受振动和运动加速度影响很大。互补滤波把陀螺仪的高频动态和加速度计的低频稳定信息融合起来，计算量小，适合 Arduino 这类资源受限平台。
直立环通常以 PD 为主，I 项要谨慎
P 项根据倾角误差产生恢复力矩，D 项根据角速度提供阻尼，抑制“点头”和高频振荡。I 项可以补偿静摩擦、结构偏置或地面坡度，但容易引发积分饱和和慢发散，所以很多直立环先用 PD，确认稳定后再少量加入 I。
BLDC FOC 比开环驱动更适合直立控制
自平衡需要低速、频繁换向、快速扭矩响应。BLDC 配合 FOC 可以通过 q 轴电流直接控制力矩，低速更平滑，死区和齿槽效应更小，比普通开环 ESC 更适合姿态伺服。
控制频率和定时精度很关键
直立环通常需要 200 Hz 左右的稳定周期，dt 不能只靠 delay 估计，最好用定时器或 micros 测量真实间隔。姿态解算、PID 输出、FOC 速度环/电流环应分层执行。
嵌入式实现适合分层架构
底层做 FOC 电流环和速度环，中间层做直立环 PD，上层做互补滤波和状态机。这样调试时可以分别看角度是否稳定、直立环是否振荡、FOC 扭矩是否跟得上。

应用场景
两轮自平衡机器人 / 倒立摆平台
最典型场景，用于验证姿态解算、直立环和 FOC 协同。
教育科研与嵌入式控制实验
适合学习 IMU 标定、互补滤波、PID 整定、FOC 力矩控制。
单轮或球形平衡机器人
对姿态解算延迟和执行器带宽要求更高，直立环需要更保守的 D 项和更稳定的角度源。
四足或双足机器人的躯干平衡
可用类似思路维持俯仰/滚转稳定，但通常需要更多自由度和更复杂的状态机。
BLDC 云台稳定平台
互补滤波输出姿态误差，BLDC 反向补偿扰动，原理上与直立环相似，但负载和动力学不同。

需要注意的事项
IMU 必须先做零偏校准
上电静止时采集陀螺仪零偏，否则角度会缓慢漂移。加速度计安装角度也要尽量对齐车体俯仰轴。
互补滤波系数不能盲目照抄
常见形式是 angle = α * (angle + gyro * dt) + (1 - α) * accel_angle。α 越大越信任陀螺仪，动态响应好但漂移校正慢；α 过小会让角度受振动影响。
dt 要用实际采样间隔
直立环周期波动会直接造成角度估计误差。建议在定时器中断或固定周期任务中更新，并对异常 dt 做限幅。
直立环优先调 P、再调 D、最后加 I
P 不足站不起来，P 过大容易高频抖动；D 能抑制振荡，但过大会放大噪声；I 只在有稳定静态偏置时少量加入，并必须做抗饱和。
BLDC 不能只用普通开环 ESC
普通航模 ESC 通常只接受油门指令，缺乏稳定的力矩/速度闭环。自平衡更推荐使用 SimpleFOC 或类似 FOC 方案，并配合编码器或磁编码器。
电源和 EMC 要隔离
BLDC 换相和启停会产生较大电流冲击，容易干扰 IMU 和主控。建议电机驱动与控制板分开供电，IMU 电源加 LC 滤波，地线布局尽量短。
机械重心和轮径会影响参数
重心越高、轮径越大、质量分布越不对称，直立环参数差异越明显。换电池、加负载后通常需要重新整定。
必须加入摔倒保护和软启动
倾角超过阈值时应立即切断力矩输出，避免电机持续硬拉。启动时也应逐步建立控制量，防止突然给扭矩导致翻车。

1、MPU6050 + 互补滤波 + 位置式 PID 直立环（最简核心框架）
适用场景：两轮自平衡机器人的最小可运行版本。只做“原地站立”，不涉及行走。核心是互补滤波公式 angle = α * (angle + gyro * dt) + (1-α) * accel_angle 和 PID 输出直接映射为电机电压/力矩 。

#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(9, 10, 11, 8), drvR(3, 5, 6, 7);
Encoder encL(2, 3, 2048), encR(18, 19, 2048);

// 互补滤波
float alpha = 0.98;
float dt = 0.01;
float filteredAngle = 0;

// PID 直立环
float Kp = 40.0, Ki = 0.0, Kd = 0.5;  // 平衡环通常 Ki=0
float targetAngle = 0.0;
float integral = 0, lastError = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);

  encL.init(); encR.init();
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  drvL.voltage_power_supply = 24; drvR.voltage_power_supply = 24;
  drvL.init(); drvR.init();
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.controller = MotionControlType::torque;
  motorR.controller = MotionControlType::torque;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();

  // 1. 读取 IMU
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // 2. 互补滤波
  float accelAngle = atan2(ay, az) * RAD_TO_DEG;
  float gyroRate = gx / 131.0;  // ±250°/s 量程
  filteredAngle = alpha * (filteredAngle + gyroRate * dt) + (1 - alpha) * accelAngle;

  // 3. PID 直立环
  float error = targetAngle - filteredAngle;
  integral += error * dt;
  integral = constrain(integral, -5, 5);  // 积分限幅
  float derivative = (error - lastError) / dt;
  lastError = error;

  float output = Kp * error + Ki * integral + Kd * derivative;
  output = constrain(output, -6.0, 6.0);  // 力矩限幅

  // 4. 双轮同向输出（差速转向暂不实现）
  motorL.move(output);
  motorR.move(output);

  delay(10);
}

解读：该案例直接复现了搜索结果中的基础平衡框架。alpha = 0.98 表示系统更信任陀螺仪的短期动态响应，同时用加速度计的低频分量缓慢修正漂移 。Ki 设为 0 是平衡环的常见做法——积分项在快速倒立摆控制中容易累积导致过冲 。constrain(output, -6, 6) 是安全底线，防止 PID 输出失控时电机暴走。

2、带安全监控与故障计数的工程化直立环
场景：案例一在理想条件下可工作，但实际调试中 IMU 可能受电机振动干扰产生异常读数，或角度超限时机器人已经倾倒。本案例在直立环外层增加角度超限检测、角速度限幅、故障计数与紧急停止，避免调试时反复摔坏机械结构 。

#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(9, 10, 11, 8), drvR(3, 5, 6, 7);
Encoder encL(2, 3, 2048), encR(18, 19, 2048);

float alpha = 0.98, dt = 0.01;
float filteredAngle = 0;

float Kp = 40.0, Kd = 0.5;
float targetAngle = 0.0;
float lastError = 0;

// 安全参数
const float MAX_ANGLE = 30.0;      // 超过 30° 判定倾倒
const float MAX_ANGULAR_VEL = 300.0;
int faultCount = 0;
const int MAX_FAULTS = 10;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();

  encL.init(); encR.init();
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  drvL.voltage_power_supply = 24; drvR.voltage_power_supply = 24;
  drvL.init(); drvR.init();
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.controller = MotionControlType::torque;
  motorR.controller = MotionControlType::torque;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();

  Serial.println("等待 3 秒，手动扶正机器人...");
  delay(3000);
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();

  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float accelAngle = atan2(ay, az) * RAD_TO_DEG;
  float gyroRate = gx / 131.0;
  filteredAngle = alpha * (filteredAngle + gyroRate * dt) + (1 - alpha) * accelAngle;

  // ===== 安全检查 =====
  if (fabs(filteredAngle) > MAX_ANGLE || fabs(gyroRate) > MAX_ANGULAR_VEL) {
    faultCount++;
    if (faultCount >= MAX_FAULTS) {
      motorL.move(0);
      motorR.move(0);
      motorL.disable();
      motorR.disable();
      Serial.println("紧急停止：角度/角速度持续超限");
      while(1);  // 锁死，等待复位
    }
  } else {
    if (faultCount > 0) faultCount--;  // 缓慢恢复
  }

  // ===== 直立环 =====
  float error = targetAngle - filteredAngle;
  float derivative = (error - lastError) / dt;
  lastError = error;

  float output = Kp * error + Kd * derivative;
  output = constrain(output, -6.0, 6.0);

  motorL.move(output);
  motorR.move(output);

  delay(10);
}

解读：该案例借鉴了搜索结果中的安全监控思路 。faultCount 的渐进计数设计避免了单次 IMU 噪声尖峰导致误停机——需要连续多次异常才触发紧急停止。motor.disable() 在倾倒后彻底卸力，防止电机堵转烧毁。while(1) 锁死是调试阶段的保守选择，量产版本应替换为复位按钮或自动恢复逻辑。

3、速度环 + 直立环串级控制（让机器人能“走”）
场景：案例一和案例二只能让机器人原地站立。一旦受到推力，机器人会向反方向加速移动以恢复平衡，无法停在原地。要让机器人保持位置或按指令行走，需要引入速度环作为外环：速度环的输出不是直接给电机，而是作为直立环的目标倾角——机器人通过主动倾斜来产生加速度，从而消除速度误差 。

#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(9, 10, 11, 8), drvR(3, 5, 6, 7);
Encoder encL(2, 3, 2048), encR(18, 19, 2048);

float alpha = 0.98, dt = 0.01;
float filteredAngle = 0;

// 直立环 PD
float Kp_balance = 35.0, Kd_balance = 0.8;
float maxTilt = 15.0;  // 最大目标倾角限制

// 速度环 PI
float Kp_speed = 0.5, Ki_speed = 0.02;
float targetSpeed = 0.0;  // 目标速度 0 = 原地保持
float integral_speed = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();

  encL.init(); encR.init();
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  drvL.voltage_power_supply = 24; drvR.voltage_power_supply = 24;
  drvL.init(); drvR.init();
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.controller = MotionControlType::torque;
  motorR.controller = MotionControlType::torque;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();

  delay(3000);
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();

  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float accelAngle = atan2(ay, az) * RAD_TO_DEG;
  float gyroRate = gx / 131.0;
  filteredAngle = alpha * (filteredAngle + gyroRate * dt) + (1 - alpha) * accelAngle;

  // 当前速度（左右轮平均）
  float currentSpeed = (motorL.shaftVelocity() + motorR.shaftVelocity()) / 2.0;

  // ===== 外环：速度环 → 输出目标倾角 =====
  float speedError = targetSpeed - currentSpeed;
  integral_speed += speedError * dt;
  integral_speed = constrain(integral_speed, -3, 3);

  float targetTilt = Kp_speed * speedError + Ki_speed * integral_speed;
  targetTilt = constrain(targetTilt, -maxTilt, maxTilt);

  // ===== 内环：直立环 → 输出力矩 =====
  float angleError = targetTilt - filteredAngle;
  // 微分项直接用陀螺仪角速度，比误差微分更干净
  float output = Kp_balance * angleError - Kd_balance * gyroRate;
  output = constrain(output, -8.0, 8.0);

  motorL.move(output);
  motorR.move(output);

  delay(10);
}

解读：该案例实现了搜索结果中描述的双环串级结构 。关键设计是 targetTilt 作为速度环的输出：如果机器人被向前推，编码器检测到正速度（向前移动），速度环输出一个负的 targetTilt（向后倾斜），直立环随之驱动轮子向后加速，把机器人拉回原位。Kd_balance * gyroRate 直接使用陀螺仪原始角速度作为微分项，避免了“误差微分”带来的噪声放大问题 。maxTilt = 15° 限制了速度环的激进程度，防止速度误差过大时机器人猛倾翻车。

要点解读

1. 互补滤波的 alpha 是“信任分配旋钮”，不是调参玄学
   公式 angle = α * (angle + gyro * dt) + (1-α) * accel_angle 的本质是：α 越大，越信任陀螺仪的短期动态（响应快，但积分会漂移）；α 越小，越信任加速度计的长期绝对方向（不漂移，但振动噪声大）。搜索结果中给出的经验值是 0.95~0.98 。自平衡机器人电机振动较大，加速度计噪声明显，因此 α 取 0.98 偏向陀螺仪是合理的起点。如果发现角度缓慢漂移，可微调至 0.96；如果发现对快速倾斜响应迟钝，可调至 0.99。

2. 直立环的 Ki 通常设为 0，这不是偷懒而是控制律的要求
   自平衡机器人是一个开环不稳定系统（倒立摆），直立环的任务是“快速拉回”，而不是“消除稳态误差”。积分项在平衡环中会累积历史误差，当机器人从倾斜状态恢复时，积分项仍然在“推”，导致反向过冲 。如果确实需要消除位置偏移，应该由外层速度环（案例三）来实现，而不是在直立环里加积分。搜索结果的多个示例中 Ki 均为 0 或极小值 。

3. 微分项直接使用陀螺仪角速度，优于对误差做数值微分
   案例三中 output = Kp * angleError - Kd * gyroRate 的设计来自搜索结果中的实践经验：derivative = (error - lastError) / dt 在 dt 较小时会放大 IMU 的量化噪声，导致输出抖动。而陀螺仪本身输出的就是角速度，物理含义与“误差的变化率”一致（当 targetTilt 恒定时），且噪声特性更可控 。这是一个用传感器原始数据替代数值微分的典型工程优化。

4. 速度环的输出必须被限幅为“目标倾角”，这是串级控制的安全边界
   案例三中 targetTilt = constrain(targetTilt, -15, 15) 是不可省略的。速度环看到“速度误差很大”时会倾向于输出一个很大的倾角指令，但如果倾角指令超过物理极限（比如 30°），直立环会驱动电机全力加速，机器人直接前扑或后仰翻车。限幅值应该根据机器人的重心高度和轮距计算：重心越高、轮距越窄，安全倾角越小。搜索结果中 SimpleFOC 官方平衡器项目的串级控制同样将速度环输出约束在合理范围内 。

5. 采样周期必须用 millis() 硬约束，delay(10) 在完整代码中不够可靠
   搜索结果反复强调：禁止用 delay() 作为控制周期的唯一来源 。案例中的 delay(10) 在只有 PID 计算时大致可靠，但如果加入串口打印、传感器校准或安全检测，实际循环周期会漂移。dt 如果随之变化，互补滤波的积分项和 PID 的微分项都会失真。正确做法是：用 millis() 记录上次循环时间，计算实际 dt，并设置 if (dt > 0.05) dt = 0.01; 之类的保护，防止偶发卡顿导致滤波和 PID 输出异常。对于 Arduino UNO，控制频率建议 ≤100Hz；ESP32 可放宽至 200-500Hz。

4、互补滤波姿态解算
核心是四件事：上电陀螺零偏标定、加速度计低通、陀螺低通、互补融合。符号约定：pitch 正值 = 车头下压（前倾），对应陀螺 Y 轴正转。

/* ============================================================
   姿态解算层：MPU6050 + 互补滤波
   输出：pitch（rad，前倾为正）、pitchRate（rad/s，前倾为正）、roll
   ============================================================ */
#include <Wire.h>
#include <MPU6050.h>

MPU6050 imu;

/* ---------- 滤波参数：必须与采样频率绑定 ---------- */
const float DT_IMU   = 0.002f;    // 500 Hz 解算周期
const float TAU_CF   = 0.30f;     // 互补滤波时间常数 s
const float TAU_ACC  = 0.012f;    // 加速度计低通（抑制电机振动）
const float TAU_GYRO = 0.004f;    // 陀螺低通
const float A_ACC    = DT_IMU / (TAU_ACC  + DT_IMU);
const float A_GYRO   = DT_IMU / (TAU_GYRO + DT_IMU);
const float ALPHA    = TAU_CF / (TAU_CF + DT_IMU);   // ≈0.993

/* ---------- 状态量 ---------- */
float pitch = 0.0f, roll = 0.0f;
float accLp[3] = {0, 0, 0};
float gyroLp[3] = {0, 0, 0};
float gyroBias[3] = {0, 0, 0};    // rad/s

/* ---------- 上电静置标定：车体水平、绝对静止 ---------- */
void imuCalibrate(uint16_t n) {
  float s[3] = {0, 0, 0};
  for (uint16_t i = 0; i < n; i++) {
    int16_t a[3], g[3];
    imu.getMotion6(&a[0], &a[1], &a[2], &g[0], &g[1], &g[2]);
    s[0] += g[0]; s[1] += g[1]; s[2] += g[2];
    delay(2);
  }
  for (int i = 0; i < 3; i++)
    gyroBias[i] = (s[i] / n) / 131.0f * DEG_TO_RAD;      // ±250°/s 量程

  /* 初始倾角直接取加速度计，避免上电瞬间从 0 开始收敛 */
  int16_t a[3], g[3];
  imu.getMotion6(&a[0], &a[1], &a[2], &g[0], &g[1], &g[2]);
  accLp[0] = a[0] / 16384.0f * 9.80665f;
  accLp[1] = a[1] / 16384.0f * 9.80665f;
  accLp[2] = a[2] / 16384.0f * 9.80665f;
  pitch = atan2f(accLp[0], sqrtf(accLp[1]*accLp[1] + accLp[2]*accLp[2]));
  roll  = atan2f(accLp[1], accLp[2]);
  gyroLp[1] = (g[1] / 131.0f) * DEG_TO_RAD - gyroBias[1];
}

/* ---------- 单次解算，由 500 Hz 定时任务调用 ---------- */
void imuUpdate(float &pitchOut, float &pitchRateOut, float &rollOut) {
  int16_t a[3], g[3];
  imu.getMotion6(&a[0], &a[1], &a[2], &g[0], &g[1], &g[2]);

  float ax = a[0] / 16384.0f * 9.80665f;
  float ay = a[1] / 16384.0f * 9.80665f;
  float az = a[2] / 16384.0f * 9.80665f;

  /* 一阶低通：振动若直接进 D 项，等价于给直立环注入正反馈 */
  accLp[0] += A_ACC * (ax - accLp[0]);
  accLp[1] += A_ACC * (ay - accLp[1]);
  accLp[2] += A_ACC * (az - accLp[2]);

  /* 陀螺：零偏扣除 + 低通 */
  float gx = (g[0] / 131.0f) * DEG_TO_RAD - gyroBias[0];
  float gy = (g[1] / 131.0f) * DEG_TO_RAD - gyroBias[1];
  float gz = (g[2] / 131.0f) * DEG_TO_RAD - gyroBias[2];
  gyroLp[0] += A_GYRO * (gx - gyroLp[0]);
  gyroLp[1] += A_GYRO * (gy - gyroLp[1]);
  gyroLp[2] += A_GYRO * (gz - gyroLp[2]);

  /* 加速度计倾角：对振动敏感，但无长期漂移 */
  float pitchAcc = atan2f(accLp[0], sqrtf(accLp[1]*accLp[1] + accLp[2]*accLp[2]));
  float rollAcc  = atan2f(accLp[1], accLp[2]);

  /* 互补融合：陀螺积分走高通，加速度计走低通 */
  pitch = ALPHA * (pitch + gyroLp[1] * DT_IMU) + (1.0f - ALPHA) * pitchAcc;
  roll  = ALPHA * (roll  + gyroLp[0] * DT_IMU) + (1.0f - ALPHA) * rollAcc;

  pitchOut     = pitch;
  pitchRateOut = gyroLp[1];
  rollOut      = roll;
}

void setup() {
  Wire.begin();
  Wire.setClock(400000);          // 400 kHz，缩短读取阻塞时间
  imu.initialize();
  imu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  imu.setDLPFMode(MPU6050_DLPF_BW_42);   // 硬件级 42 Hz 低通，先挡掉一部分振动
  delay(100);                     // 等传感器上电稳定再标定
  imuCalibrate(1000);
}

5、直立环 PD → 力矩指令
直立环的被控量是倾角，输出必须是力矩（等效电流）。两轮对称分配，无转向时左右各承担一半。

/* ============================================================
   直立环：pitch PD → 单轮力矩 → SimpleFOC 力矩模式
   ============================================================ */
#include <SimpleFOC.h>

BLDCMotor      M_L(7), M_R(7);
BLDCDriver3PWM D_L(32,33,25), D_R(26,27,14);
Encoder        E_L(34,35,2048), E_R(36,39,2048);

const float R_W    = 0.0325f;   // 轮半径 m
const float TRACK  = 0.16f;     // 轮距 m
const float GEAR   = 1.0f;      // 减速比，直驱为 1

/* ---------- 直立环参数（必须实车标定） ---------- */
const float KP_ANG = 4.20f;     // N·m/rad
const float KD_ANG = 0.34f;     // N·m·s/rad
const float ANG_DEAD = 0.008f;  // rad，抑制静止时的微抖
const float T_MAX    = 0.55f;   // N·m，单轮力矩上限

/* ---------- 力矩常数标定 ---------- */
const float KT = 0.085f;        // N·m/A，电流模式；瞬时力矩 = KT × iq
/* 若用电压模式：改标 KT_EFF = N·m/V，且需按母线电压归一化 */

static float TcmdFilt = 0.0f;

/* ---------- 直立环：返回整车所需力矩 ---------- */
float balanceTorque(float pitch, float pitchRate, float pitchRef) {
  float e = pitch - pitchRef;
  if (fabsf(e) < ANG_DEAD) e = 0.0f;   // 死区只作用于 P，不作用于 D

  float T = KP_ANG * e + KD_ANG * pitchRate;

  /* 一阶低通：把力矩指令的高频分量压掉，避免激励机械谐振 */
  TcmdFilt += 0.55f * (T - TcmdFilt);
  return constrain(TcmdFilt, -T_MAX * 2.0f, T_MAX * 2.0f);
}

/* ---------- 力矩 → 双轮电流指令 ---------- */
void applyTorque(float T_total, float turnCmd) {
  float half = T_total * 0.5f;
  float dT   = turnCmd * 0.5f;            // 差动力矩，用于转向

  float tL = constrain(half - dT, -T_MAX, T_MAX);
  float tR = constrain(half + dT, -T_MAX, T_MAX);

  M_L.target = tL / KT;                    // foc_current 模式下 target 单位是 A
  M_R.target = tR / KT;
}

void motorInit() {
  M_L.linkDriver(&D_L); M_R.linkDriver(&D_R);
  M_L.linkSensor(&E_L); M_R.linkSensor(&E_R);
  D_L.voltage_power_supply = D_R.voltage_power_supply = 12.0f;
  D_L.init(); D_R.init();
  M_L.init(); M_R.init();
  M_L.initFOC(); M_R.initFOC();

  /* 关键：必须用力矩模式，绝不能用速度模式 */
  M_L.torque_controller = TorqueControlType::foc_current;
  M_R.torque_controller = TorqueControlType::foc_current;

  /* 电流限幅作为最后一道防线，防止摔倒时堵转烧线圈 */
  M_L.current_limit = 4.0f;
  M_R.current_limit = 4.0f;
}

/* ---------- 定周期主控 1 kHz ---------- */
void controlISR() {
  static uint32_t tPrev = 0;
  uint32_t now = micros();
  float dt = (now - tPrev) * 1e-6f;
  if (dt <= 0 || dt > 0.02f) dt = 1e-3f;
  tPrev = now;

  float pitch, pitchRate, roll;
  imuUpdate(pitch, pitchRate, roll);

  /* 跌倒保护：超出可恢复范围立即卸载 */
  if (fabsf(pitch) > 0.70f) {              // ≈40°
    M_L.target = 0.0f; M_R.target = 0.0f;
    TcmdFilt = 0.0f;
    M_L.loopFOC(); M_L.move();
    M_R.loopFOC(); M_R.move();
    return;
  }

  float T = balanceTorque(pitch, pitchRate, 0.0f);
  applyTorque(T, 0.0f);

  M_L.loopFOC(); M_L.move();
  M_R.loopFOC(); M_R.move();
}

6、速度外环串级 + 起摆与状态机
只有角度环的车会原地缓慢漂移，必须加速度外环；同时直立车需要一个显式的启动与故障状态机，否则上电瞬间就是事故。

/* ============================================================
   串级结构：速度外环 → 倾角参考 → 直立内环 → 力矩
   状态机：CAL → KICK → BALANCE → FALL / FAULT
   ============================================================ */
enum BalState { ST_CAL, ST_KICK, ST_BALANCE, ST_FALL, ST_FAULT };
static BalState st = ST_CAL;

/* ---------- 速度外环参数 ---------- */
const float KV = 0.28f;      // rad/(m/s)，速度误差→倾角参考
const float KI_V = 0.045f;   // 积分项，消稳态漂移
const float I_V_MAX = 0.10f; // 积分限幅（rad），防翻车后饱和
static float iV = 0.0f;

/* ---------- 起摆参数 ---------- */
const float KICK_T = 0.20f;  // N·m
const uint32_t KICK_MS = 120;

/* ---------- 车体速度：编码器差分 ---------- */
float bodyVelocity() {
  float vL = E_L.getVelocity() * R_W / GEAR;
  float vR = E_R.getVelocity() * R_W / GEAR;
  return 0.5f * (vL + vR);
}

/* ---------- 定周期主控 1 kHz ---------- */
void controlISR() {
  static uint32_t tPrev = 0;
  uint32_t now = micros();
  float dt = (now - tPrev) * 1e-6f;
  if (dt <= 0 || dt > 0.02f) dt = 1e-3f;
  tPrev = now;

  float pitch, pitchRate, roll;
  imuUpdate(pitch, pitchRate, roll);
  float v = bodyVelocity();

  /* 目标速度：来自遥控/上位机；内部缓变，避免突变引起点头 */
  static float vRef = 0.0f;
  static float vTarget = 0.0f;
  vRef += 0.8f * (vTarget - vRef) * dt;     // ~1.25 s 时间常数

  switch (st) {

  case ST_CAL:
    /* 上电自检：倾角在水平附近且车速为零才允许起摆 */
    if (fabsf(pitch) < 0.15f && fabsf(v) < 0.05f && millis() > 800) st = ST_KICK;
    M_L.target = 0.0f; M_R.target = 0.0f;
    break;

  case ST_KICK: {
    /* 起摆：车体静止时无倾角误差，PD 输出为零，必须主动给一脚 */
    static uint32_t tKick = 0;
    if (tKick == 0) tKick = millis();
    M_L.target = KICK_T / KT;
    M_R.target = KICK_T / KT;
    if (millis() - tKick > KICK_MS) { tKick = 0; st = ST_BALANCE; }
    break;
  }

  case ST_BALANCE: {
    /* 跌倒判定：先于限幅执行，避免饱和指令把车"顶"出去 */
    if (fabsf(pitch) > 0.70f || fabsf(roll) > 0.50f) { st = ST_FALL; break; }

    /* 外环：速度误差 → 倾角参考（带积分，限幅） */
    float ev = v - vRef;
    iV += KI_V * ev * dt;
    iV = constrain(iV, -I_V_MAX, I_V_MAX);
    float pitchRef = -(KV * ev + iV);       // 超速→参考角后仰（负）
    pitchRef = constrain(pitchRef, -0.22f, 0.22f);

    /* 内环：直立 PD */
    float T = balanceTorque(pitch, pitchRate, pitchRef);

    /* 转向：差动力矩，独立于平衡通道 */
    static float turnCmd = 0.0f;
    applyTorque(T, turnCmd);
    break;
  }

  case ST_FALL:
    /* 卸载 + 清零所有积分，防止扶起瞬间冲出去 */
    M_L.target = 0.0f; M_R.target = 0.0f;
    TcmdFilt = 0.0f; iV = 0.0f;
    if (fabsf(pitch) < 0.12f && fabsf(v) < 0.03f) st = ST_CAL;
    else if (millis() % 2000 < 2) st = ST_FAULT;   // 超时未扶起则报故障
    break;

  case ST_FAULT:
  default:
    M_L.target = 0.0f; M_R.target = 0.0f;
    /* 此处应停机、亮灯、上报；需人工复位后才回到 ST_CAL */
    break;
  }

  M_L.loopFOC(); M_L.move();
  M_R.loopFOC(); M_R.move();
}

要点解读
互补滤波的时间常数是相位与噪声的分配开关，不是随手填的系数。 τ 越大，越信任陀螺、越抑制加速度计的振动噪声，但代价是倾角估计对真实姿态变化的滞后越大；τ 越小则相反。直立环的 D 项吃的是角速度、P 项吃的是角度，两者对滞后的容忍度不同——P 项滞后会直接吃掉相位裕度，表现为车体低频晃动甚至失稳。τ 取 0.2~0.5 s 是常见区间，且必须注意 α 与采样周期强绑定：α = τ/(τ+dt)，把解算频率从 500 Hz 改到 1 kHz 而不重算 α，滤波特性会悄悄改变，这是排查"换个主频就晃"类问题的第一现场。

陀螺零偏的温漂是静止漂移的唯一主因，且它不会自己消失。 上电静置标定只解决了常温下的一致性。电机运行后线圈发热、板载器件温升都会让零偏差值漂移，几度每小时的误差经过积分就是持续的倾角估计偏差，表现为车在原地缓慢向前或向后"爬"。工程上有两个对策：一是标定后加一段很慢的零偏在线修正——只在"直立且在平衡点附近、轮速长期接近零"的窗口内，用加速度计倾角与滤波器输出之差做极慢速积分；二是把 IMU 远离电机与功率器件布置，或选带板载温补的 IMU（如 ICM 系列），从源头减小漂移量。

直立环必须工作在力矩/电流模式，速度模式是错的。 自平衡的控制机理是"把轮子当作反作用飞轮，通过加减速把底盘移到质心正下方"。速度模式下电机试图维持轮速，底盘无法及时响应质心的移动，等效于把平衡点变成一个随负载漂移的游动目标，并且速度环积分会与角度环互相激励，产生典型的低频振荡。改用 foc_current 模式后，PD 输出直接就是轮上力矩，物理意义清晰：T = Kp·θ + Kd·θ̇，前倾给正力矩把底盘送出去，角速度前倾加速时追加力矩。另外要意识到力矩饱和的边界：当 Kp 大到在最大倾角下指令持续触顶，车不会更稳，而是失去调节余量、以更快的速度摔倒，所以 current_limit 应当设在"能恢复最大允许倾角"的所需电流之上一点，而不是越大越好。

串级结构中，外环带宽必须显著低于内环，且积分要严格限幅。 角度内环通常工作在 5~10 Hz，速度外环应在 0.5~1 Hz，相差将近一个数量级。外环过快时，速度误差会被直接翻译成倾角参考的快速摆动，等价于给直立环叠加一个外部扰动，现象是"车在原地不停点头但速度并没有变好"。速度积分项是消除稳态漂移所必需的，但它的限幅（案例中 I_V_MAX = 0.10 rad）必须给得克制：摔倒瞬间积分会持续累积，若不限幅，扶起来的第一秒会得到一个接近最大倾角参考，车会直接冲出去——这是自平衡调试中最常见的一次性摔机原因。

符号约定、采样抖动与机械安装是三个必须先解决的工程前提。 倾角正方向、陀螺积分正方向、电机力矩正方向、编码器计数正方向，这四个符号只要有一个反相，现象都是"上电即全速冲向某个方向"，而这类问题无法通过调增益解决。首次调试建议按固定顺序执行：轮子悬空验证编码器方向与力矩方向、用限流电源把电流上限压到 1 A、给足倾角限幅（案例中的 0.70 rad）后短暂上电，确认倾斜方向与轮子转动方向一致再放平调试。此外直立环对采样抖动极其敏感——D 项直接放大时间抖动，所以必须用定时器或 micros() 做绝对周期累加，而不是 delay()；IMU 必须刚性固定在车体上并做减振，用泡棉软安装会引入十几毫秒的机械滞后，效果等于给 D 项注入正反馈，越调 D 越抖。

请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。