“Arduino BLDC 之巡检机器人复杂崎岖地形 + 四驱自适应步态 + 倾角校正”可以理解为：四路 BLDC 独立驱动底盘在 IMU 实时姿态反馈下，根据俯仰角、横滚角、轮端打滑和触地状态动态调整步高、步长、支撑占空比和扭矩分配；当机身倾斜时，通过倾角校正改变重心补偿和各轮力矩，使巡检机器人在碎石、斜坡、沟坎和松软地面上仍能保持稳定通行。
一、主要特点

1. 四驱独立驱动形成差速与扭矩矢量能力
   四个轮端各自闭环控制，可分别输出不同速度或扭矩。遇到单侧悬空、打滑或负载不均时，系统能把驱动力转移到附着更好的轮端，类似电子限滑效果。
2. 自适应步态不只是“换步序”，而是参数化调制
   在崎岖地形中，步高、步长、支撑占空比、抬轮时机和重心位置都需要随坡度与障碍动态调整。上坡时通常需增加步高、减小步长、强化后腿驱动；下坡时则需降低步高、增加前腿缓冲、重心适度后置。
3. 倾角校正由 IMU 融合提供闭环反馈
   IMU 通过互补滤波、Madgwick 或卡尔曼滤波融合加速度计与陀螺仪数据，输出稳定的俯仰角和横滚角。 当检测到坡度变化或车身冲击时，控制系统可调整期望姿态、重心补偿和轮端扭矩，避免前倾俯冲或后仰抬升。
4. BLDC FOC 提供柔顺力矩输出
   越障瞬间负载突变明显，方波驱动容易出现抖动和失步。FOC 通过 Id/Iq 解耦，使扭矩输出更连续，适合低速攀爬、斜坡起步、柔顺触地和冲击吸收。
5. 打滑检测与力矩重分配提升通过性
   当某轮编码器速度异常升高、电流下降、车身加速度与期望不符时，可判定打滑。系统应降低该轮扭矩权重，将驱动力转移到健康轮，必要时进入跛行模式。
6. 分层控制更适合嵌入式平台
   感知层负责 IMU、编码器、电流和触觉反馈；决策层负责地形识别与越障策略；分配层负责整车力矩分解；执行层负责 BLDC FOC 电流环/速度环。ATmega328P 更适合承担单轮 FOC 执行和简单分配，复杂视觉、在线优化或 MPC 建议放在 ESP32-S3、STM32F4/F7、Teensy 4.1 或上位机。
   二、应用场景
7. 野外/园区巡检机器人
   在草地、碎石、泥地、电缆沟盖板和斜坡环境中移动，四驱自适应可避免单轮悬空导致整机停滞。
8. 变电站/光伏/管廊巡检
   地面可能存在台阶、排水沟、金属盖板和坡度变化，倾角校正能维持传感器朝向稳定，提升数据采集质量。
9. 废墟搜救与灾后探测
   瓦砾、倾斜板、缝隙和松软地面会导致轮端频繁失去附着，力矩重分配和自适应步态能提高穿越成功率。
10. 农业温室与山地巡检
    垄沟、湿滑泥土和小坡度常见，机器人需要根据坡度调整步长和驱动分配，防止打滑或侧倾。
11. 轮腿复合越障平台
    若底盘带有主动悬挂或轮腿机构，可在台阶、门槛、沟渠处调整机身姿态和轮端扭矩，FOC 使触地与离地过程更柔顺。
12. 教育与科研验证平台
    适合验证 IMU 姿态融合、四驱力矩分配、打滑检测、越障状态机、FOC 速度/扭矩闭环和自适应步态参数整定。
    三、需要注意的事项
13. 算力边界要提前规划
    FOC 电流环、速度环、IMU 融合、步态状态机和力矩分配都需要稳定周期。8 位 Arduino 难以同时承担四路 FOC、复杂滤波和在线优化，建议采用分层架构。
14. 倾角校正不能替代机械稳定裕度
    IMU 校正只能补偿姿态误差，不能解决重心过高、轮距过窄或悬挂行程不足带来的翻车风险。机械结构必须预留足够的静态稳定裕度。
15. 步态参数必须随坡度连续调节
    固定步高和步长在坡面上容易导致磕碰或打滑。应根据俯仰角动态调整重心偏移、步长、步高和支撑占空比，并保持切换平滑。
16. 力矩分配必须有约束
    电机扭矩上限、电池电压、轮胎摩擦圆和车身姿态稳定裕度都应作为约束。否则可能出现单轮过载、电压跌落、车身俯仰过大甚至翻车。
17. 打滑判断需多源融合
    仅看编码器速度会误判，仅看电流也会受地形负载干扰。更稳妥的是融合轮速残差、IMU 加速度、车身横摆率和电流响应，并加入滑动窗口滤波。
18. 越障时优先保证支撑稳定
    跨越台阶或沟坎时，应先确认支撑轮可靠触地，再转移重心。抬轮过高会增加冲击，过低则容易磕碰；应根据障碍高度加安全余量动态调整。
19. 分配指令要平滑，避免扭矩跳变
    力矩指令突变会引起车身冲击、电流尖峰和机械松动。应对分配输出做限幅、斜率限制或低通滤波，并在模态切换时采用渐变过渡。
20. IMU 必须刚性安装并校准
    IMU 应固定在车身重心附近，避免松动和振动耦合。上电前需完成陀螺仪零偏校准、加速度计水平校准和安装角补偿。
21. 电源与 EMC 不能忽视
    四路 BLDC 同时加减速会产生较大电流冲击和电磁噪声，可能干扰 IMU、编码器和 MCU。控制电源应独立稳压，动力线与控制线分开布线，并预留足够电容和过流保护。
22. 必须保留失效保护
    当编码器失联、电流异常、IMU 发散、电池欠压或通信中断时，应立即降低扭矩、停机或进入保守等待。算法层面的容错不能替代硬件级保护。

整体链路是：IMU 融合输出俯仰/横滚角 → 识别坡度与地形状态 → 调整步高、步长、支撑占空比和重心补偿 → 检测打滑与悬空 → 动态重分配四轮扭矩 → BLDC FOC 柔顺执行 → 倾角校正维持机身稳定 → 障碍通过后恢复常规步态。做原型时，建议按“单轮 FOC 速度环 → 四驱独立驱动 → IMU 倾角读取与滤波 → 平地自适应步态 → 斜坡重心补偿 → 打滑检测与力矩重分配 → 越障状态机与失效保护”逐步推进。

1、IMU倾角反馈 + 四驱力矩动态分配（基于姿态的重心补偿）
适用场景：四驱巡检车在斜坡或单侧隆起地形上行驶时，车身俯仰/横滚导致各轮法向载荷不均。根据IMU姿态角动态调整前后、左右轮组的力矩分配，上坡时后轮增扭、横滚时低侧轮增扭，抑制打滑与失稳。核心逻辑参考了搜索结果中四驱力矩动态分配与IMU姿态补偿的融合思路。

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

// 四驱 BLDC（左前/右前/左后/右后）
BLDCMotor motors[4] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};
BLDCDriver3PWM drivers[4] = {
  BLDCDriver3PWM(9, 10, 11, 8),    // 左前
  BLDCDriver3PWM(3, 5, 6, 4),      // 右前
  BLDCDriver3PWM(44, 45, 46, 43),  // 左后
  BLDCDriver3PWM(22, 23, 24, 21)   // 右后
};
Encoder encoders[4] = {
  Encoder(18, 19, 2048), Encoder(20, 21, 2048),
  Encoder(30, 31, 2048), Encoder(32, 33, 2048)
};

MPU6050 imu;
float pitch = 0, roll = 0;

// 基础力矩与补偿参数
const float BASE_TORQUE = 0.8;
const float PITCH_GAIN = 0.025;   // 俯仰补偿增益
const float ROLL_GAIN  = 0.020;   // 横滚补偿增益
const float TORQUE_LIMIT = 2.0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  imu.initialize();
  imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2G);
  imu.setFullScaleGyroRange(MPU6050_GYRO_FS_250DPS);

  for (int i = 0; i < 4; i++) {
    encoders[i].init();
    motors[i].linkSensor(&encoders[i]);
    drivers[i].voltage_power_supply = 24;
    drivers[i].init();
    motors[i].linkDriver(&drivers[i]);
    // 力矩模式：精准控制每轮出力
    motors[i].controller = MotionControlType::torque;
    motors[i].torque_controller = TorqueControlType::estimated_current;
    motors[i].phase_resistance = 2.5;
    motors[i].KV_rating = 120;
    motors[i].init();
    motors[i].initFOC();
  }
}

void readIMU() {
  int16_t ax, ay, az, gx, gy, gz;
  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float accPitch = atan2(ay, az) * 180.0 / PI;
  float accRoll  = atan2(-ax, az) * 180.0 / PI;
  // 互补滤波（陀螺仪积分 + 加速度计校正）
  static float pitchEst = 0, rollEst = 0;
  pitchEst = 0.98 * (pitchEst + (gx / 131.0) * 0.01) + 0.02 * accPitch;
  rollEst  = 0.98 * (rollEst  + (gy / 131.0) * 0.01) + 0.02 * accRoll;
  pitch = pitchEst;
  roll  = rollEst;
}

void loop() {
  for (int i = 0; i < 4; i++) motors[i].loopFOC();

  readIMU();

  // ===== 力矩动态分配 =====
  // 俯仰补偿：上坡（pitch>0）后轮增扭，前轮减扭
  float pitchComp = pitch * PITCH_GAIN;
  // 横滚补偿：右倾（roll>0）左侧轮增扭，右侧轮减扭
  float rollComp  = roll * ROLL_GAIN;

  // 索引：0=左前 1=右前 2=左后 3=右后
  float torque[4];
  torque[0] = BASE_TORQUE - pitchComp + rollComp;   // 左前
  torque[1] = BASE_TORQUE - pitchComp - rollComp;   // 右前
  torque[2] = BASE_TORQUE + pitchComp + rollComp;   // 左后
  torque[3] = BASE_TORQUE + pitchComp - rollComp;   // 右后

  for (int i = 0; i < 4; i++) {
    torque[i] = constrain(torque[i], 0, TORQUE_LIMIT);
    motors[i].move(torque[i]);
  }

  // 安全保护：倾角超限卸力
  if (fabs(pitch) > 25 || fabs(roll) > 25) {
    for (int i = 0; i < 4; i++) motors[i].move(0);
    Serial.println("倾角超限，安全卸力");
  }

  delay(10);
}

解读：该案例将IMU解算的俯仰/横滚角映射为四轮力矩的加减偏置。pitchComp 使上坡时后轮增扭以补偿重心后移，rollComp 使横滚时低侧轮增扭以抑制侧翻趋势。BLDC的FOC力矩模式（estimated_current）确保每个轮的力矩指令被相对精确地执行，这是“动态分配”区别于“速度分配”的前提。倾角超限卸力是四驱越野场景不可省略的安全层。

2、地形分类 + 四驱参数库切换（自适应步态的查表实现）
场景：四驱巡检车在硬化路面、碎石地、泥地、浅沟等不同地形上，需要切换轮速、力矩上限、转向灵敏度等参数。搜索结果中四足机器人的“步态库查表”逻辑可直接迁移为四驱的“地形参数库”。本案例用超声波+IMU做地形分类，查表切换四驱参数。

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

BLDCMotor motors[4] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};
BLDCDriver3PWM drivers[4] = {
  BLDCDriver3PWM(9, 10, 11, 8), BLDCDriver3PWM(3, 5, 6, 4),
  BLDCDriver3PWM(44, 45, 46, 43), BLDCDriver3PWM(22, 23, 24, 21)
};
Encoder encoders[4] = {
  Encoder(18, 19, 2048), Encoder(20, 21, 2048),
  Encoder(30, 31, 2048), Encoder(32, 33, 2048)
};

MPU6050 imu;
#define TRIG 40
#define ECHO 41

// ===== 四驱参数库：轮速、力矩上限、转向增益 =====
struct DriveProfile {
  float wheelSpeed;      // rad/s
  float torqueLimit;     // Nm
  float steerGain;       // 转向灵敏度
};

// 索引：0=硬化平地, 1=碎石, 2=泥地/松软, 3=浅沟/凸起
DriveProfile profileLib[4] = {
  {8.0, 1.2, 1.0},   // 平地：高速、低扭限
  {4.5, 2.0, 0.8},   // 碎石：中速、高扭限、降转向
  {2.5, 2.5, 0.6},   // 泥地：低速、最高扭限、低转向
  {1.5, 2.5, 0.5}    // 浅沟：极低速、高扭限
};

int currentProfile = 0;
DriveProfile active = profileLib[0];

float readSonar() {
  digitalWrite(TRIG, LOW); delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  long dur = pulseIn(ECHO, HIGH, 20000);
  return (dur == 0) ? 999 : dur * 0.0343 / 2.0;
}

void setup() {
  Serial.begin(115200);
  Wire.begin();
  imu.initialize();
  imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2G);
  pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);

  for (int i = 0; i < 4; i++) {
    encoders[i].init();
    motors[i].linkSensor(&encoders[i]);
    drivers[i].voltage_power_supply = 24;
    drivers[i].init();
    motors[i].linkDriver(&drivers[i]);
    motors[i].controller = MotionControlType::velocity;
    motors[i].init();
    motors[i].initFOC();
  }
}

void loop() {
  for (int i = 0; i < 4; i++) motors[i].loopFOC();

  // ===== 地形分类 =====
  int16_t ax, ay, az;
  imu.getAcceleration(&ax, &ay, &az);
  float pitch = atan2(ay, az) * 180 / PI;
  float frontDist = readSonar();

  int detected = 0;
  if (frontDist > 0 && frontDist < 25) {
    detected = 3;  // 近距障碍/浅沟
  } else if (fabs(pitch) > 10.0) {
    detected = 2;  // 斜坡/松软
  } else if (frontDist < 60) {
    detected = 1;  // 碎石/粗糙
  } else {
    detected = 0;
  }

  // 迟滞：仅在分类变化时切换（简化，实际应加阈值带）
  if (detected != currentProfile) {
    currentProfile = detected;
    active = profileLib[currentProfile];
    Serial.print("地形切换 → "); Serial.println(currentProfile);
  }

  // ===== 四驱执行 =====
  float vL = active.wheelSpeed;
  float vR = active.wheelSpeed;
  // 转向修正（简化：保持直行）
  for (int i = 0; i < 4; i++) {
    float target = (i % 2 == 0) ? vL : vR;
    motors[i].move(target / 0.05);  // 轮半径 0.05m
  }

  delay(20);
}

解读：该案例将四足机器人的步态库查表结构迁移到四驱底盘。DriveProfile 存储轮速、力矩上限、转向增益，detected 根据IMU俯仰角和超声波距离分类后查表赋值。四驱的“自适应步态”在此表现为参数集的切换：碎石地降低轮速以提高接地稳定性，泥地提高力矩上限以克服松软地面阻力。BLDC速度环确保切换后的轮速指令被平滑跟踪。

3、倾角校正状态机 + 四驱差速补偿（斜坡直行保持）
场景：四驱巡检车在斜坡上直行时，重力分量使车辆向坡下偏航。单纯依靠驾驶员修正不够及时，需要在控制层用IMU横滚角/俯仰角计算差速补偿量，使左右轮组产生对抗偏航的力矩差。本案例借鉴搜索结果中“坡面摩擦自适应与力矩重分配”的思路，将倾角校正作为独立状态机运行。

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

BLDCMotor motorL_F(7), motorR_F(7), motorL_R(7), motorR_R(7);
BLDCDriver3PWM drvLF(9, 10, 11, 8), drvRF(3, 5, 6, 4);
BLDCDriver3PWM drvLR(44, 45, 46, 43), drvRR(22, 23, 24, 21);
Encoder encLF(18, 19, 2048), encRF(20, 21, 2048);
Encoder encLR(30, 31, 2048), encRR(32, 33, 2048);

MPU6050 imu;
float roll = 0, pitch = 0;

// 倾角校正参数
const float YAW_CORRECTION_GAIN = 0.8;  // 偏航补偿增益
const float SPEED_BASE = 6.0;           // 基础轮速 rad/s
const float ROLL_DEADZONE = 2.0;        // 横滚死区（度）

// 状态
enum TiltState { NORMAL, CORRECTING, SAFE_STOP };
TiltState tiltState = NORMAL;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  imu.initialize();
  imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2G);
  imu.setFullScaleGyroRange(MPU6050_GYRO_FS_250DPS);

  encLF.init(); encRF.init(); encLR.init(); encRR.init();
  motorL_F.linkSensor(&encLF); motorR_F.linkSensor(&encRF);
  motorL_R.linkSensor(&encLR); motorR_R.linkSensor(&encRR);
  drvLF.voltage_power_supply = 24; drvRF.voltage_power_supply = 24;
  drvLR.voltage_power_supply = 24; drvRR.voltage_power_supply = 24;
  drvLF.init(); drvRF.init(); drvLR.init(); drvRR.init();
  motorL_F.linkDriver(&drvLF); motorR_F.linkDriver(&drvRF);
  motorL_R.linkDriver(&drvLR); motorR_R.linkDriver(&drvRR);
  motorL_F.controller = motorR_F.controller = MotionControlType::velocity;
  motorL_R.controller = motorR_R.controller = MotionControlType::velocity;
  motorL_F.init(); motorL_F.initFOC();
  motorR_F.init(); motorR_F.initFOC();
  motorL_R.init(); motorL_R.initFOC();
  motorR_R.init(); motorR_R.initFOC();
}

void readIMU() {
  int16_t ax, ay, az, gx, gy, gz;
  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float accPitch = atan2(ay, az) * 180.0 / PI;
  float accRoll  = atan2(-ax, az) * 180.0 / PI;
  static float pEst = 0, rEst = 0;
  pEst = 0.98 * (pEst + (gx / 131.0) * 0.01) + 0.02 * accPitch;
  rEst = 0.98 * (rEst + (gy / 131.0) * 0.01) + 0.02 * accRoll;
  pitch = pEst;
  roll = rEst;
}

void loop() {
  motorL_F.loopFOC(); motorR_F.loopFOC();
  motorL_R.loopFOC(); motorR_R.loopFOC();

  readIMU();

  // ===== 状态判定 =====
  if (fabs(roll) > 30 || fabs(pitch) > 30) {
    tiltState = SAFE_STOP;
  } else if (fabs(roll) > ROLL_DEADZONE) {
    tiltState = CORRECTING;
  } else {
    tiltState = NORMAL;
  }

  // ===== 倾角校正执行 =====
  float speedL = SPEED_BASE;
  float speedR = SPEED_BASE;

  if (tiltState == CORRECTING) {
    // 横滚向右（roll>0）→ 左侧轮加速，右侧轮减速，产生对抗偏航的力矩差
    float correction = roll * YAW_CORRECTION_GAIN;
    correction = constrain(correction, -2.0, 2.0);
    speedL += correction;
    speedR -= correction;
  } else if (tiltState == SAFE_STOP) {
    speedL = 0; speedR = 0;
    Serial.println("倾角超限，安全停车");
  }

  // 四驱差速执行
  motorL_F.move(speedL / 0.05);
  motorL_R.move(speedL / 0.05);
  motorR_F.move(speedR / 0.05);
  motorR_R.move(speedR / 0.05);

  delay(10);
}

解读：该案例将倾角校正从“力矩偏置”升级为速度差补偿。当横滚角超过死区时，correction = roll * GAIN 使左侧轮加速、右侧轮减速，产生一个对抗坡面偏航趋势的差速力矩。ROLL_DEADZONE = 2° 避免在近似水平路面上因IMU噪声频繁修正。SAFE_STOP 状态在倾角超过30°时直接卸力，这是四驱巡检车在陡坡/侧翻风险下的最后防线。与案例一的力矩偏置相比，速度差补偿的响应更直接，但需要速度环的精确跟踪才能生效。

要点解读

1. 四驱底盘的“自适应步态”本质是参数库切换，而非四足的相位步态
   搜索结果中四足机器人的步态库存储的是步长、步高、相位差等周期参数。四驱轮式底盘不存在“步态周期”，其自适应体现在地形参数集的切换：不同地形对应不同的轮速上限、力矩限幅、转向灵敏度。案例二的 DriveProfile 结构反映了这一差异。将四足步态库直接照搬到四驱是架构错位。

2. 倾角校正必须区分“力矩偏置”与“速度差补偿”两种执行路径
   案例一用IMU倾角直接偏置四轮力矩（torque[i] = BASE + pitchComp ± rollComp），适合FOC力矩模式，响应快但依赖力矩估算精度。案例三用倾角计算左右轮速差（speedL += correction），适合速度模式，执行更直接但需要速度环跟踪。搜索结果的IMU姿态补偿代码采用了速度差路径（motorSpeeds[i] = speedComp + yawCorrection ± rollCorrection），而四驱力矩分配框架则倾向力矩偏置。两者可以叠加使用：力矩偏置做底层补偿，速度差做上层修正。

3. 四驱力矩分配的前提是FOC力矩/电流环，速度环无法实现“分配”
   搜索结果中四驱力矩动态分配明确要求 MotionControlType::torque 模式。如果底层是速度环，上层下发的“力矩分配”指令会变成“速度目标”，打滑轮会因为积分作用而持续增扭，反而加剧打滑。案例一使用 TorqueControlType::estimated_current 使 move(torque) 接收的是力矩估算值而非电压，分配逻辑才有物理意义。

4. 倾角校正需要死区与安全阈值的双层保护
   案例三的 ROLL_DEADZONE = 2° 防止IMU噪声导致平地频繁修正，SAFE_STOP（30°）在极端倾角下强制卸力。搜索结果的姿态稳定代码同样设置了 TILT_DEADZONE 和 MAX_TILT_ANGLE 两级阈值。没有死区的倾角校正在平地上会持续微调，浪费能量并加速轮胎磨损；没有安全阈值的校正逻辑在翻车风险下无法保护机械结构。

5. Arduino算力下，四驱FOC + IMU姿态解算 + 倾角校正必须分频执行
   案例中的 delay(10) 对应100Hz的IMU读取与校正计算。四路BLDC的 loopFOC() 在Arduino Uno上已接近极限，IMU的互补滤波和三角函数运算会进一步挤占时间。合理的架构是：FOC环以最大频率运行，IMU读取与倾角校正降频到50-100Hz。搜索结果明确指出ATmega328P仅适合单腿关节控制与简单FSM，四驱巡检的多传感器融合与校正计算更适合ESP32-S3或Teensy 4.x。若必须用Uno，应将倾角校正简化为查表（预计算roll→correction的映射表），避免实时三角函数。

4、四驱轮速自适应 + 横滚角补偿（最小闭环）
适用硬件：ESP32 + 4×SimpleFOC BLDC（四轮独立驱动）+ 编码器 + MPU6050。

核心思路：坡道上四驱车的核心问题是“重力沿横滚方向的分量会把车往下拽”，表现为走不直、单侧轮打滑。做法分两步——运动学补偿（左右轮速差抵消偏航漂移）与动力学补偿（低侧轮加扭矩，高侧轮减扭矩）。两者必须分开，因为一个改的是“走多快”，一个改的是“使多大劲”。

/*
 * 案例4：四驱 + 横滚/俯仰补偿 + 姿态低通
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

BLDCMotor mFL(7), mFR(7), mRL(7), mRR(7);
BLDCDriver3PWM dFL( 9,10,11, 8), dFR(13,14,15,12),
               dRL(22,23,24,25), dRR(26,27,28,29);
Encoder eFL(2,3,2048), eFR(4,5,2048), eRL(16,17,2048), eRR(18,19,2048);

BLDCMotor*   M[4] = {&mFL, &mFR, &mRL, &mRR};
Encoder*     E[4] = {&eFL, &eFR, &eRL, &eRR};
BLDCDriver3PWM* D[4] = {&dFL, &dFR, &dRL, &dRR};

void o0(){E[0]->handleA();} void o1(){E[1]->handleA();}
void o2(){E[2]->handleA();} void o3(){E[3]->handleA();}

MPU6050 imu;

const float R_W = 0.085f, TRACK = 0.42f, WB = 0.52f;

// ---- 姿态（互补滤波）----
float roll = 0, pitch = 0;
const float ALPHA = 0.98f;          // 陀螺权重（高频靠陀螺，低频靠加速度计）

// ---- 补偿增益 ----
const float K_ROLL_SPD  = 0.55f;    // 横滚 -> 轮速差 (m/s per rad)
const float K_ROLL_TRQ  = 0.85f;    // 横滚 -> 扭矩偏置 (Nm per rad)
const float K_PITCH_TRQ = 0.60f;    // 俯仰 -> 前后扭矩偏置

// ---- 安全阈值 ----
const float ROLL_WARN = 25.0f * DEG_TO_RAD;
const float ROLL_STOP = 35.0f * DEG_TO_RAD;

float vCmd = 0.45f;

// ---- 各轮目标 ----
float vTgt[4];
float tauBias[4];

void readAttitude(float dt) {
  imu.update();
  // 加速度计角度
  float aRoll  = atan2f(imu.getAccelerationY(), imu.getAccelerationZ());
  float aPitch = atan2f(-imu.getAccelerationX(),
                        sqrtf(imu.getAccelerationY()*imu.getAccelerationY() +
                              imu.getAccelerationZ()*imu.getAccelerationZ()));
  // 陀螺积分
  float gRoll  = roll  + (imu.getRotationX() / 131.0f) * DEG_TO_RAD * dt;
  float gPitch = pitch + (imu.getRotationY() / 131.0f) * DEG_TO_RAD * dt;

  roll  = ALPHA * gRoll  + (1.0f - ALPHA) * aRoll;
  pitch = ALPHA * gPitch + (1.0f - ALPHA) * aPitch;
}

void setup() {
  for (int i = 0; i < 4; i++) {
    E[i]->init();
    M[i]->linkSensor(E[i]);
    M[i]->controller = MotionControlType::velocity;
    M[i]->PID_velocity.P = 0.35f;
    M[i]->PID_velocity.I = 4.0f;
    M[i]->PID_velocity.output_ramp = 400.0f;
    M[i]->LPF_velocity.Tf = 0.01f;
    M[i]->voltage_limit = 24.0f;
    M[i]->current_limit = 6.0f;
    M[i]->init();
    D[i]->voltage_power_supply = 24.0f;
    D[i]->init();
    M[i]->linkDriver(D[i]);
    M[i]->initFOC();
  }
  E[0]->enableInterrupts(o0); E[1]->enableInterrupts(o1);
  E[2]->enableInterrupts(o2); E[3]->enableInterrupts(o3);

  Wire.begin(); imu.initialize();
  Serial.begin(115200);
}

void loop() {
  static unsigned long tPrev = millis();
  unsigned long now = millis();
  float dt = (now - tPrev)/1000.0f; tPrev = now;
  if (dt <= 0 || dt > 0.2f) dt = 0.02f;

  readAttitude(dt);

  // ---- 1. 横滚 -> 轮速差（抵消下坡侧的偏航漂移）----
  // 右侧抬起（roll>0）时机体向右倾，需左侧加速、右侧减速以维持航向
  float dv = K_ROLL_SPD * roll;
  float vBase = vCmd;

  vTgt[0] = vBase - dv;      // 左前
  vTgt[1] = vBase + dv;      // 右前
  vTgt[2] = vBase - dv;      // 左后
  vTgt[3] = vBase + dv;      // 右后

  // ---- 2. 横滚 -> 扭矩偏置（低侧加力，抑制侧滑）----
  float tRoll = K_ROLL_TRQ * roll;
  tauBias[0] = -tRoll;  tauBias[1] = +tRoll;
  tauBias[2] = -tRoll;  tauBias[3] = +tRoll;

  // ---- 3. 俯仰 -> 前后扭矩偏置（爬坡后轮加载）----
  float tPitch = K_PITCH_TRQ * sinf(pitch);
  tauBias[0] += -tPitch * 0.5f;  tauBias[1] += -tPitch * 0.5f;   // 前轮卸载
  tauBias[2] += +tPitch * 0.8f;  tauBias[3] += +tPitch * 0.8f;   // 后轮加载

  // ---- 4. 姿态闭环：小角度用速度环，大角度切力矩补偿 ----
  bool torqueMode = (fabsf(roll) > 8.0f * DEG_TO_RAD);

  for (int i = 0; i < 4; i++) {
    M[i]->loopFOC();
    if (torqueMode) {
      // 力矩模式：速度环输出 + 姿态偏置
      float tau = M[i]->PID_velocity(M[i]->target - M[i]->shaft_velocity)
                + tauBias[i];
      M[i]->move(constrain(tau, -1.5f, 1.5f));
    } else {
      M[i]->move(vTgt[i] / R_W);
    }
  }

  // ---- 5. 翻覆预警 ----
  if (fabsf(roll) > ROLL_STOP) {
    for (int i = 0; i < 4; i++) M[i]->move(0);
    Serial.println("[SAFE] rollover risk");
  } else if (fabsf(roll) > ROLL_WARN) {
    vCmd = min(vCmd, 0.15f);
  }

  Serial.print("roll="); Serial.print(roll*RAD_TO_DEG, 1);
  Serial.print(" pitch="); Serial.print(pitch*RAD_TO_DEG, 1);
  Serial.print(" tm="); Serial.println(torqueMode);
  delay(20);
}

关键点：torqueMode 的切换阈值（8°）是关键。小角度时用速度环就够，速度差本身就能把车拉回来；但横滚超过 8° 后，仅靠速度差已经压不住侧滑，必须直接给低侧轮加扭矩。这两层如果混在一起调，会互相打架。

案例5：FreeRTOS 三层 + 地形判级 + 载荷转移扭矩分配
适用硬件：ESP32 双核 + 4×BLDC + 编码器 + IMU + 4 路轮速/电流。

核心思路：四驱在崎岖地形的真正难点不是“补偿角度”，而是“按附着极限分配扭矩”。爬坡时后轮加载、前轮卸载，如果四轮平分扭矩，后轮早就超附着打滑、前轮却还有余量。这一版用载荷转移公式算出各轮法向力，再按 τ_i ∝ N_i 分配，并且整体受 μ·N·r 约束。

/*
 * 案例5：载荷转移 + 附着极限分配 + 三层分离
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor mFL(7), mFR(7), mRL(7), mRR(7);
// 驱动/编码器初始化同案例一（略）
BLDCMotor* M[4] = {&mFL, &mFR, &mRL, &mRR};
MPU6050 imu;
SemaphoreHandle_t mux;

const float R_W = 0.085f, TRACK = 0.42f, WB = 0.52f;
const float MASS = 22.0f, G = 9.81f;
const float H_CG = 0.20f;             // 重心高度
const float MU = 0.65f;               // 附着系数（在线估计）

// ---- 共享 ----
volatile float g_roll=0, g_pitch=0, g_ax=0, g_ay=0;
volatile float g_tau[4] = {0,0,0,0};
volatile float g_vRef = 0;
volatile bool  g_stop = false;

// ---- 地形等级 ----
uint8_t grade = 0;
const float V_TBL[4] = {0.80f, 0.55f, 0.35f, 0.18f};

// ---- 互补滤波姿态（同案例一，略去展开）----
float roll = 0, pitch = 0;
void updateAttitude(float dt) { /* 同案例一 readAttitude */ }

// ---- 载荷转移：法向力分配 ----
void computeLoads(float ax, float ay, float &nFL, float &nFR,
                  float &nRL, float &nRR) {
  float total = MASS * G;
  // 纵向转移（加速时后轴加载）
  float dLong = MASS * ax * H_CG / WB;
  // 横向转移（向右侧加速时左侧加载）
  float dLat  = MASS * ay * H_CG / TRACK;

  float nF = total * 0.45f - dLong;      // 前轴
  float nR = total * 0.55f + dLong;      // 后轴
  nF = max(nF, 0.0f); nR = max(nR, 0.0f);

  nFL = max(nF*0.5f - dLat*0.5f, 0.0f);
  nFR = max(nF*0.5f + dLat*0.5f, 0.0f);
  nRL = max(nR*0.5f - dLat*0.5f, 0.0f);
  nRR = max(nR*0.5f + dLat*0.5f, 0.0f);
}

// ---- 任务1：感知 50Hz ----
void taskSense(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    imu.update();
    float dt = 0.02f;
    updateAttitude(dt);

    xSemaphoreTake(mux, portMAX_DELAY);
    g_roll  = roll;
    g_pitch = pitch;
    g_ax    = imu.getAccelerationX() / 16384.0f * G;
    g_ay    = imu.getAccelerationY() / 16384.0f * G;
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(20));
  }
}

// ---- 任务2：地形判级 + 分配 50Hz ----
void taskAlloc(void*) {
  TickType_t last = xTaskGetTickCount();
  float vI = 0;

  for (;;) {
    float rl, pt, ax, ay;
    xSemaphoreTake(mux, portMAX_DELAY);
    rl = g_roll; pt = g_pitch; ax = g_ax; ay = g_ay;
    xSemaphoreGive(mux);

    // ---- 地形判级：俯仰 + 振动 ----
    float score = 0;
    if (fabsf(pt) > 0.21f) score += 1.5f;
    else if (fabsf(pt) > 0.10f) score += 0.8f;
    if (fabsf(rl) > 0.17f) score += 1.2f;
    else if (fabsf(rl) > 0.08f) score += 0.6f;
    float jolt = sqrtf(ax*ax + ay*ay);
    if (jolt > 3.0f) score += 1.5f;
    else if (jolt > 1.2f) score += 0.7f;

    uint8_t g = (score >= 3.0f) ? 3 : (score >= 1.8f) ? 2
              : (score >= 0.8f) ? 1 : 0;

    // 滞回：升级立即、降级需 2s
    static unsigned long dnT = 0;
    if (g > grade) { grade = g; dnT = 0; }
    else if (g < grade) {
      if (dnT == 0) dnT = millis();
      else if (millis() - dnT > 2000) { grade = g; dnT = 0; }
    } else dnT = 0;

    // ---- 载荷与附着 ----
    float nFL, nFR, nRL, nRR;
    computeLoads(ax, ay, nFL, nFR, nRL, nRR);
    float N[4] = {nFL, nFR, nRL, nRR};

    // ---- 总扭矩需求 ----
    float vRef = g_vRef;
    float eV = card(vRef) - vRef;      // 简化：用速度环近似
    float tauTotal = constrain(3.2f*eV, -12.0f, 12.0f);

    // ---- 按 N_i 分配 + 附着上限 ----
    float sumN = 0;
    for (int i = 0; i < 4; i++) sumN += N[i];
    if (sumN < 1e-3f) sumN = 1e-3f;

    float cap[4], capSum = 0;
    for (int i = 0; i < 4; i++) {
      cap[i] = min(MU * N[i] * R_W, 4.0f);
      capSum += cap[i];
    }
    float scale = (fabsf(tauTotal) > capSum && capSum > 1e-3f)
                ? capSum / fabsf(tauTotal) : 1.0f;

    for (int i = 0; i < 4; i++) {
      float share = N[i] / sumN;
      float t = tauTotal * scale * share;
      g_tau[i] = constrain(t, -cap[i], cap[i]);
    }

    g_stop = (fabsf(rl) > 35.0f*DEG_TO_RAD);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(20));
  }
}

// ---- 任务3：执行 1kHz ----
void taskExec(void*) {
  TickType_t last = xTaskGetTickCount();
  float prev[4] = {0,0,0,0};
  for (;;) {
    bool stop;
    float tgt[4];
    xSemaphoreTake(mux, portMAX_DELAY);
    stop = g_stop;
    for (int i = 0; i < 4; i++) tgt[i] = g_tau[i];
    xSemaphoreGive(mux);

    for (int i = 0; i < 4; i++) {
      M[i]->loopFOC();
      if (stop) { prev[i] = 0; M[i]->move(0); continue; }
      prev[i] += constrain(tgt[i] - prev[i], -0.15f, 0.04f);  // 升慢降快
      M[i]->move(prev[i]);
    }
    vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
  }
}

void setup() {
  // 电机初始化同案例一（力矩模式）
  Wire.begin(); imu.initialize();
  mux = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(taskSense, "sens", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskAlloc, "alloc",4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskExec,  "exec", 4096, NULL, 3, NULL, 1);
  Serial.begin(115200);
}
void loop() { vTaskDelay(portMAX_DELAY); }

关键点：cap[i] = min(MU*N[i]*R_W, 4.0f) 这一行把“附着力”和“电机能力”取了较小值。四驱车的越障上限永远是附着力，不是扭矩——再多给电流只会空转，反而加速轮胎磨损。

案例6：崎岖越障状态机 + 单轮悬空识别 + 扭矩转移 + 翻覆保护
适用硬件：ESP32 + 4×BLDC + 编码器 + IMU + 前向测距。

核心思路：崎岖地形的真实场景是“四个轮子不完全接地”——可能一个轮子悬空、另一个打滑、车身斜着过坎。这时四驱的优势才真正体现：把悬空轮的扭矩卸掉，转给接地轮。同时俯仰/横滚一旦接近翻覆极限，立即停车而不是硬顶。

/*
 * 案例6：悬空识别 + 扭矩转移 + 越障状态机 + 翻覆保护
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor mFL(7), mFR(7), mRL(7), mRR(7);
// 初始化同案例一（略）
BLDCMotor* M[4] = {&mFL, &mFR, &mRL, &mRR};
MPU6050 imu;
SemaphoreHandle_t mux;

const float R_W = 0.085f, TRACK = 0.42f, WB = 0.52f;
const float I_IDLE = 0.30f;      // 空载电流

// ---- 轮态 ----
enum WState { W_GROUND, W_FLOAT, W_SLIP };
volatile WState wst[4] = {W_GROUND, W_GROUND, W_GROUND, W_GROUND};

// ---- 越障状态 ----
enum OState { O_CRUISE, O_APPROACH, O_CLIMB, O_TILT, O_EXIT, O_BACKOFF, O_FAULT };
OState ost = O_CRUISE;
unsigned long ostT0 = 0;
int retry = 0;

// ---- 共享 ----
volatile float g_roll=0, g_pitch=0;
volatile float g_tau[4] = {0,0,0,0};
volatile bool  g_stop = false;
float roll = 0, pitch = 0;

float readFront() { /* 超声，略 */ return 9.0f; }

void updateAttitude(float dt) { /* 互补滤波，同案例一 */ }

// ---- 轮态识别：高速低流 = 悬空；高速有流 = 打滑 ----
void classifyWheels(float vRef) {
  for (int i = 0; i < 4; i++) {
    float vW = M[i]->shaft_velocity * R_W;
    float iA = fabsf(M[i]->current);

    if (vRef < 0.05f) { wst[i] = W_GROUND; continue; }

    bool hi = (vW > vRef * 1.5f);
    bool lo = (iA < I_IDLE * 0.5f);
    if (hi && lo)       wst[i] = W_FLOAT;
    else if (hi)        wst[i] = W_SLIP;
    else                wst[i] = W_GROUND;
  }
}

void setup() {
  // 电机初始化同案例一
  Wire.begin(); imu.initialize();
  mux = xSemaphoreCreateMutex();
  Serial.begin(115200);
}

void loop() {
  static unsigned long tPrev = millis();
  unsigned long now = millis();
  float dt = (now - tPrev)/1000.0f; tPrev = now;
  if (dt <= 0 || dt > 0.2f) dt = 0.02f;

  imu.update();
  updateAttitude(dt);

  // ---- 参考速度：只用接地轮 ----
  float vSum = 0; int cnt = 0;
  for (int i = 0; i < 4; i++) {
    if (wst[i] == W_GROUND) { vSum += M[i]->shaft_velocity*R_W; cnt++; }
  }
  float vRef = (cnt > 0) ? vSum/cnt : 0.0f;
  classifyWheels(vRef);

  float tIn = (millis() - ostT0)/1000.0f;
  float tauTotal = 0, vTgt = 0;

  // ---- 状态机 ----
  switch (ost) {
    case O_CRUISE:
      vTgt = 0.35f;
      if (fabsf(pitch) > 0.20f || fabsf(roll) > 0.15f) { ost = O_TILT; ostT0 = millis(); }
      break;
    case O_APPROACH:
      vTgt = 0.12f;
      if (tIn > 0.8f) { ost = O_CLIMB; ostT0 = millis(); }
      break;
    case O_CLIMB:
      vTgt = 0.10f;
      tauTotal = 6.0f;                          // 加大总扭矩
      if (fabsf(pitch) < 0.10f && tIn > 1.0f) { ost = O_EXIT; ostT0 = millis(); }
      else if (tIn > 4.0f) { ost = O_BACKOFF; ostT0 = millis(); retry++; }
      break;
    case O_TILT:
      vTgt = 0.08f;
      if (fabsf(roll) < 0.06f && fabsf(pitch) < 0.10f) { ost = O_CRUISE; ostT0 = millis(); }
      else if (tIn > 3.0f) { ost = O_BACKOFF; ostT0 = millis(); retry++; }
      break;
    case O_EXIT:
      vTgt = 0.20f;
      if (tIn > 1.5f) { ost = O_CRUISE; ostT0 = millis(); retry = 0; }
      break;
    case O_BACKOFF:
      vTgt = -0.15f;
      if (tIn > 1.5f) { ost = (retry >= 3) ? O_FAULT : O_CRUISE; ostT0 = millis(); }
      break;
    case O_FAULT:
      vTgt = 0; tauTotal = 0;
      break;
  }

  // ---- 扭矩分配（含悬空/打滑降权）----
  float wgt[4] = {1,1,1,1};
  for (int i = 0; i < 4; i++) {
    if (wst[i] == W_FLOAT) wgt[i] = 0.03f;
    else if (wst[i] == W_SLIP) wgt[i] = 0.35f;
  }

  // 基础速度环扭矩
  float eV = vTgt - vRef;
  tauTotal += constrain(3.5f * eV, -8.0f, 8.0f);

  float wsum = 0;
  for (int i = 0; i < 4; i++) wsum += wgt[i];
  if (wsum < 1e-3f) wsum = 1e-3f;

  for (int i = 0; i < 4; i++) {
    float share = wgt[i]/wsum;
    float t = tauTotal * share;
    // 倾角偏置：低侧加力
    t += 0.6f * roll * (i == 0 || i == 2 ? -1.0f : 1.0f);
    g_tau[i] = constrain(t, -4.0f, 4.0f);
  }

  // ---- 翻覆保护：最高优先级 ----
  g_stop = (fabsf(roll) > 35.0f*DEG_TO_RAD) || (fabsf(pitch) > 40.0f*DEG_TO_RAD);
  if (g_stop) {
    for (int i = 0; i < 4; i++) g_tau[i] = 0;
    ost = O_FAULT;
  }

  // ---- 执行 ----
  for (int i = 0; i < 4; i++) {
    M[i]->loopFOC();
    if (g_stop) { M[i]->move(0); continue; }
    // 悬空轮：速度环给 0 = 主动制动，防止飞转后突然接地打齿
    if (wst[i] == W_FLOAT) M[i]->move(0);
    else M[i]->move(g_tau[i]);
  }

  Serial.print("ost="); Serial.print(ost);
  Serial.print(" roll="); Serial.print(roll*RAD_TO_DEG, 1);
  Serial.print(" pitch="); Serial.print(pitch*RAD_TO_DEG, 1);
  Serial.print(" w=");
  for (int i = 0; i < 4; i++) Serial.print((int)wst[i]);
  Serial.println();
  delay(20);
}

关键点：悬空轮用 move(0) 主动制动，而不是 move(0.0) 之外的“断电滑行”。悬空时轮子在飞转，落地瞬间线速度差极大，会打齿、撕裂胎面；主动制动让轮子停住，落地时相对速度接近零。

要点解读
一、四驱的价值在“按附着分配”，而不是“四个轮子都给力”
四轮同时驱动听起来是“力气大一倍”，但在崎岖地形上真正的收益是扭矩可以在四轮之间转移。爬坡时后轮加载、前轮卸载，如果四轮平分，后轮早就超过 μ·N·r 打滑，前轮却还有余量——结果是“总扭矩够但车不走”。案例5按法向力分配、案例6按轮态降权，本质都是同一件事：把扭矩给到还抓得住地的那个轮子。

二、倾角校正必须分两层：运动学补偿改“走多快”，动力学补偿改“使多大劲”
这是最容易混淆的地方。横滚 10° 时，靠左右轮速差就能维持航向（运动学层，改的是速度指令）；但横滚到 20° 以上，仅靠速度差已经压不住侧滑，必须直接给低侧轮加扭矩（动力学层，改的是力矩）。两层混在一起调，会出现“速度环拼命加速、扭矩却把车往反方向推”的对抗。案例一用 torqueMode 阈值（8°）明确切开，就是这个道理。

三、姿态闭环的带宽必须远低于速度环，否则会和悬挂打架
车体有悬挂、有轮胎弹性，本身就是一个低通系统（固有频率通常在 2~ 5 Hz）。如果姿态闭环带宽做到 20 Hz，会直接激励悬挂共振，表现为车身越调越晃。经验值是姿态环 1 ~2 Hz、速度环 100~500 Hz、电流环 1~5 kHz，三者差一个数量级以上。案例5把姿态处理放在 50 Hz 的任务里、执行放 1 kHz，就是带宽分层的落地。

四、崎岖地形上“测准姿态”比“控好姿态”更难也更重要
MPU6050 装在振动的车体上，加速度计读数会被冲击污染（过坎瞬间可能出现几十 g 的尖峰），单纯用它算角度会剧烈跳变。所以必须做互补滤波：高频信陀螺、低频信加速度计（案例4 ALPHA=0.98）。更关键的是安装位置——IMU 尽量靠近重心、加减震垫，否则测到的是“车架振动”而不是“车身姿态”，后面的补偿全是错的。

五、翻覆保护是最后一道防线，必须独立于所有控制逻辑
四驱车在斜坡上有一个物理极限：重心投影超出支撑多边形就翻，任何控制器都救不回来。所以必须有独立的、不经过任何控制环的保护通道：|roll| > 35° 或 |pitch| > 40° 直接切零力矩（案例4、案例6都是先判再执行，判断语句在任何分配逻辑之前）。而且要区分“警告”和“停止”两档——25° 降速、35° 停车，给驾驶员/上位机留出反应时间，而不是一刀切。

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