基于 Arduino 平台实现 BLDC 机器人负载自适应 + 滑移检测 + 扭矩分配，是将基础电机控制升级为智能运动控制的典型范例。该系统不再仅仅是“开环”地驱动电机，而是通过传感器融合与算法闭环，使机器人具备了类似生物体的“本体感知”与“环境适应”能力。

1、主要特点

1. 负载自适应控制 (Load Adaptive Control)
   该模块使机器人能够根据外部负载的变化自动调整动力输出，维持期望的运动性能。
   核心原理：通过电流环反馈估算电机输出的转矩。当系统检测到在相同油门下转速下降（或电流异常升高），即判定负载增大。
   动态补偿机制：采用自适应 PID 算法，当检测到负载突变（如爬坡、撞击障碍物）时，自动调整 PID 参数或直接增加目标电流，以维持速度环的稳定性，防止因负载过大导致的失速或震荡。
   能效优化：在轻载时降低输出功率，减少能耗和发热，延长电池续航。
2. 滑移检测 (Slip Detection)
   该模块用于识别驱动轮与地面是否发生打滑，是保证里程计（Odometry）精度和运动稳定性的关键。
   速度一致性校验：利用编码器实时读取左右轮的实际转速。在正常直线行驶时，两轮速度应与指令值保持一定的比例关系。若某一侧轮子的实际转速远大于理论值（例如空转），而另一侧转速正常，则判定为单侧打滑。
   惯性辅助检测：融合IMU（惯性测量单元） 的角速度数据。若电机指令为直线行驶（期望角速度为 0），但 IMU 检测到车身发生了旋转（实际角速度不为 0），则说明轮子发生了侧滑。
   电流特征分析：打滑时，电机负载通常会瞬间降低（电流减小）。通过监测电流的突降特征，可以辅助判断是否发生打滑。
3. 扭矩分配 (Torque Distribution)
   该模块根据控制指令和物理限制，将总的动力需求合理地分配给各个电机。
   运动学解算：在差速或全向底盘中，根据期望的底盘线速度 ，通过运动学逆解矩阵计算出每个轮子所需的目标转速。
   防打滑扭矩限制：基于滑移检测的结果，动态限制分配给每个电机的最大输出扭矩。例如，当检测到某侧轮子即将打滑时，主动限制该侧电机的电流上限，防止因过度输出扭矩导致陷入困境。
   电子差速：在转弯时，自动分配内外轮的扭矩，确保内侧轮转速慢、外侧轮转速快，且总功率输出符合物理极限，实现平滑过弯。

2、 应用场景
全地形巡检机器人
在电力巡检、管道检测或农业喷洒场景中，机器人常面临草地、泥泞、沙地等低附着力路面。该系统能有效防止机器人在松软地面上陷车，通过检测打滑并限制扭矩，利用“蠕动”的方式脱困。
重载物流 AGV/AMR
在工厂搬运重型货物时，由于货物重心变化或惯性影响，各驱动轮的负载会动态变化。负载自适应和扭矩分配能确保多轮驱动的 AGV 在满载和空载状态下都能稳定行驶，防止因某一驱动轮过载而损坏。
特种救援机器人
在地震废墟等非结构化环境中，机器人需要跨越瓦砾堆。滑移检测和扭矩分配能帮助机器人在部分履带或轮子悬空时，切断或减小悬空轮的扭矩，将动力集中到有附着力的轮子上，提高越障能力。
高精度定位移动平台
对于需要高精度 SLAM（同步定位与地图构建）的机器人，轮子打滑是里程计误差的主要来源。通过滑移检测，系统可以在检测到打滑时暂时屏蔽里程计数据，或触发重定位机制，保证定位的准确性。

3、 注意事项
硬件性能瓶颈
算力要求：同时运行负载辨识、滑移检测、扭矩分配和 PID 控制算法，计算量巨大。严禁使用 Arduino Uno/Nano 等 8 位单片机。必须选用 ESP32、STM32H7 或 Teensy 4.0/4.1 等高性能平台，以保证控制回路的实时性（建议控制周期 ≤ 10ms）。
传感器精度：低成本编码器分辨率不足会导致速度计算波动，误判为打滑。建议使用分辨率 ≥ 1000 PPR 的磁编码器。电流传感器需具备足够的带宽以捕捉电流的瞬态变化。
算法逻辑与稳定性
滑移判定阈值：滑移检测的阈值（如速度差阈值、电流变化率阈值）需要根据实际路面进行大量标定。阈值过小会导致频繁误触发，机器人动力不足；阈值过大则无法及时检测到打滑。
积分饱和防护：在打滑期间，速度环误差会持续累积，导致 PID 积分项饱和。当打滑结束恢复正常附着力时，过大的积分值会瞬间输出极大扭矩，导致机械冲击。必须在软件中加入积分抗饱和（Integral Anti-windup）机制。
电源与驱动匹配
电流限制：扭矩分配的最终体现是电流输出。必须确保电池的放电倍率和 ESC 的持续电流规格能满足最大扭矩输出的需求，防止电池过放或 ESC 过热保护。
电源隔离：大功率电机动作会引起电源波动。务必使用独立的 DC-DC 模块为 Arduino 主控供电，防止电压跌落导致系统复位。
机械结构限制
传动间隙：机械传动机构（如减速箱、皮带）的间隙会导致“回差”，在扭矩反向分配时产生滞后和冲击。软件上需加入死区补偿算法。
轮子一致性：左右轮子的直径差异是导致“直线跑偏”的元凶，容易被误判为打滑。组装时需精确测量并校准轮径参数。

1、负载自适应控制（基于电流检测）

#include <Servo.h>
Servo esc;
const int escPin = 9;
const int currentPin = A0; // 电流传感器引脚（如ACS712）

float targetSpeed = 1500; // 目标转速（PWM值）
float Kp = 0.8, Ki = 0.2; // PID参数
int lastError = 0, integral = 0;

void setup() {
  esc.attach(escPin, 1000, 2000);
  Serial.begin(9600);
  esc.writeMicroseconds(1000); // 安全启动
  delay(3000);
}

void loop() {
  // 读取电流（假设传感器输出0-5V对应0-50A）
  int currentRaw = analogRead(currentPin);
  float current = map(currentRaw, 0, 1023, 0, 50); // 转换为实际电流值

  // 模拟负载变化：电流超过阈值时降低速度
  if (current > 15.0) { // 假设15A为负载过重阈值
    targetSpeed = constrain(targetSpeed - 5, 1000, 1500); // 动态降速
  } else {
    targetSpeed = constrain(targetSpeed + 2, 1200, 1800); // 恢复速度
  }

  // 简单PID控制（实际需更精确的编码器反馈）
  int error = targetSpeed - analogRead(A1); // 假设A1连接转速传感器
  integral += error;
  int output = targetSpeed + Kp * error + Ki * integral;
  output = constrain(output, 1000, 2000);
  esc.writeMicroseconds(output);

  Serial.print("Current: "); Serial.print(current);
  Serial.print(" | Adjusted Speed: "); Serial.println(output);
  delay(50);
}

应用场景：搬运机器人根据负载重量自动调整运动速度。
关键点：
通过电流传感器检测负载变化，动态调整电机输出。
需校准电流传感器零点与比例系数。

2、滑移检测与补偿（基于加速度计）

#include <Servo.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h> // 使用MPU6050检测滑移

Servo esc;
Adafruit_MPU6050 mpu;
const int escPin = 9;
float targetSpeed = 1600;

void setup() {
  esc.attach(escPin, 1000, 2000);
  mpu.begin();
  mpu.setAccelerometerRange(MPU6050_RANGE_4G);
  Serial.begin(9600);
  esc.writeMicroseconds(1000);
  delay(3000);
}

void loop() {
  sensors_event_t accel;
  mpu.getEvent(&accel);

  // 计算纵向加速度（假设Z轴为运动方向）
  float longitudinalAccel = accel.acceleration.z;

  // 滑移检测：实际加速度与期望值偏差过大
  static float lastSpeed = 0;
  float expectedAccel = (targetSpeed - lastSpeed) * 0.1; // 简化模型
  lastSpeed = targetSpeed;

  if (abs(longitudinalAccel - expectedAccel) > 2.0) { // 阈值需实验调整
    Serial.println("Slip Detected! Increasing Torque...");
    targetSpeed += 50; // 临时增加扭矩
  }

  esc.writeMicroseconds(constrain(targetSpeed, 1000, 2000));
  delay(50);
}

应用场景：轮式机器人爬坡或湿滑路面时防止打滑。
关键点：
通过加速度计对比实际加速度与期望值，检测滑移。
需结合编码器速度反馈提高准确性。

3、多电机扭矩分配（差速驱动机器人）

#include <Servo.h>
Servo escLeft, escRight;
const int leftPin = 9, rightPin = 10;
int targetSpeed = 1500;
float torqueRatio = 0.5; // 默认左右轮扭矩分配比例

void setup() {
  escLeft.attach(leftPin, 1000, 2000);
  escRight.attach(rightPin, 1000, 2000);
  Serial.begin(9600);
  escLeft.writeMicroseconds(1000);
  escRight.writeMicroseconds(1000);
  delay(3000);
}

void loop() {
  // 模拟转向控制：通过调整扭矩分配比例
  if (Serial.available()) {
    char cmd = Serial.read();
    if (cmd == 'L') torqueRatio = 0.3; // 左转（左轮低扭矩）
    else if (cmd == 'R') torqueRatio = 0.7; // 右转（右轮低扭矩）
    else torqueRatio = 0.5; // 直行
  }

  // 分配扭矩（实际需根据负载动态调整）
  int leftSpeed = targetSpeed * (1 - torqueRatio);
  int rightSpeed = targetSpeed * (1 + torqueRatio);

  escLeft.writeMicroseconds(constrain(leftSpeed, 1000, 2000));
  escRight.writeMicroseconds(constrain(rightSpeed, 1000, 2000));

  Serial.print("Left: "); Serial.print(leftSpeed);
  Serial.print(" | Right: "); Serial.println(rightSpeed);
  delay(50);
}

应用场景：差速驱动机器人（如AGV）实现灵活转向。
关键点：
通过调整左右电机PWM比例实现扭矩分配。
需结合编码器反馈优化分配策略（如PID控制）。
五点要点解读
传感器融合
负载自适应需电流传感器，滑移检测需加速度计/编码器，扭矩分配需多路速度反馈。
单一传感器可能误判，建议多传感器数据融合（如卡尔曼滤波）。
实时性要求
BLDC控制需高频更新（≥50Hz），避免使用delay()阻塞代码。
使用定时器中断或millis()实现非阻塞控制。
安全阈值设定
电流过载、滑移加速度等阈值需通过实验标定。
示例中硬编码阈值（如15A、2.0g）需替换为实际系统参数。
硬件扩展性
高功率应用需外接电机驱动模块（如DRV8323），Arduino仅作为控制核心。
电流传感器建议选用隔离型（如INA219）以提高抗干扰性。
算法优化方向
负载自适应可升级为模型预测控制（MPC）。
滑移检测可结合机器学习（如LSTM）识别复杂工况。
扭矩分配可引入动态优化算法（如二次规划QP）。

4、基础负载自适应与滑移检测
场景：通用移动机器人，需要基本的负载适应和防滑。
核心逻辑：电流估算负载 + 速度差检测滑移。

#include <SimpleFOC.h>
#include <MovingAverage.h>  // 滑动平均滤波

// 双电机系统
BLDCMotor motorLeft(7);
BLDCMotor motorRight(7);

// 编码器
Encoder encLeft(2, 3, 2048);
Encoder encRight(18, 19, 2048);

// 电流传感器
InlineCurrentSense currentSenseLeft(0.001, 20.0, A0, A1, A2);
InlineCurrentSense currentSenseRight(0.001, 20.0, A3, A4, A5);

// 负载估计
struct LoadEstimate {
  float leftTorque = 0.0;     // 左轮估计扭矩
  float rightTorque = 0.0;    // 右轮估计扭矩
  float leftLoad = 0.0;       // 左轮负载系数
  float rightLoad = 0.0;      // 右轮负载系数
  float totalLoad = 0.0;      // 总负载系数
  float grade = 0.0;          // 坡度估计
};
LoadEstimate load;

// 滑移检测
struct SlipDetection {
  float leftSlip = 0.0;       // 左轮滑移率
  float rightSlip = 0.0;      // 右轮滑移率
  float slipThreshold = 0.3;  // 30%滑移阈值
  float accelThreshold = 10.0; // 加速度阈值 rad/s²
  bool leftSlipping = false;
  bool rightSlipping = false;
  int slipCount[2] = {0, 0};
};
SlipDetection slip;

// 自适应PID
class AdaptivePID {
private:
  float kp, ki, kd;
  float errorIntegral = 0;
  float lastError = 0;
  float adaptiveGain = 1.0;
  float baseKp, baseKi, baseKd;
  
public:
  AdaptivePID(float p, float i, float d) : 
    kp(p), ki(i), kd(d), baseKp(p), baseKi(i), baseKd(d) {}
  
  float calculate(float error, float dt, float loadFactor) {
    // 基于负载调整增益
    adaptGain(loadFactor);
    
    // PID计算
    errorIntegral += error * dt;
    errorIntegral = constrain(errorIntegral, -10, 10);
    
    float errorDeriv = (error - lastError) / dt;
    lastError = error;
    
    float output = (kp * adaptiveGain) * error + 
                   (ki * adaptiveGain) * errorIntegral + 
                   (kd * adaptiveGain) * errorDeriv;
    
    return output;
  }
  
  void adaptGain(float loadFactor) {
    // 负载越大，增益越小
    adaptiveGain = 1.0 / (1.0 + loadFactor * 0.5);
  }
  
  void reset() {
    errorIntegral = 0;
    lastError = 0;
  }
};

AdaptivePID leftPID(2.0, 5.0, 0.1);
AdaptivePID rightPID(2.0, 5.0, 0.1);

// 移动平均滤波
MovingAverage<float> leftSpeedFilter(10);
MovingAverage<float> rightSpeedFilter(10);
MovingAverage<float> leftCurrentFilter(10);
MovingAverage<float> rightCurrentFilter(10);

void setup() {
  Serial.begin(115200);
  
  Serial.println("===== 负载自适应 + 滑移检测系统 =====");
  Serial.println("基于电流和速度的实时监测");
  
  // 1. 初始化电机
  initMotors();
  
  // 2. 初始化电流传感器
  initCurrentSensors();
  
  // 3. 校准系统
  calibrateSystem();
  
  // 4. 学习负载特性
  learnLoadCharacteristics();
  
  Serial.println("系统就绪");
  Serial.println("格式: 负载L,负载R,滑移L,滑移R,状态");
}

void loop() {
  static unsigned long lastControlTime = 0;
  unsigned long now = micros();
  float dt = (now - lastControlTime) / 1000000.0;
  
  if (dt >= 0.01) {  // 100Hz控制
    // 1. 读取传感器
    SensorData sensor = readSensors();
    
    // 2. 估计负载
    estimateLoad(sensor, dt);
    
    // 3. 检测滑移
    detectSlip(sensor, dt);
    
    // 4. 自适应控制
    adaptiveControl(sensor, dt);
    
    // 5. 执行FOC
    motorLeft.loopFOC();
    motorRight.loopFOC();
    
    // 6. 保护机制
    safetyProtection();
    
    lastControlTime = now;
  }
  
  // 状态显示
  static unsigned long lastDisplay = 0;
  if (millis() - lastDisplay >= 200) {
    displayStatus();
    lastDisplay = millis();
  }
}

SensorData readSensors() {
  SensorData data;
  
  // 读取速度
  data.leftSpeed = motorLeft.shaft_velocity;
  data.rightSpeed = motorRight.shaft_velocity;
  
  // 滤波
  data.leftSpeed = leftSpeedFilter.add(data.leftSpeed);
  data.rightSpeed = rightSpeedFilter.add(data.rightSpeed);
  
  // 读取电流
  DQCurrent_s leftCurrent = currentSenseLeft.getFOCCurrents();
  DQCurrent_s rightCurrent = currentSenseRight.getFOCCurrents();
  
  data.leftCurrent = sqrt(leftCurrent.d * leftCurrent.d + leftCurrent.q * leftCurrent.q);
  data.rightCurrent = sqrt(rightCurrent.d * rightCurrent.d + rightCurrent.q * rightCurrent.q);
  
  // 滤波
  data.leftCurrent = leftCurrentFilter.add(data.leftCurrent);
  data.rightCurrent = rightCurrentFilter.add(data.rightCurrent);
  
  return data;
}

void estimateLoad(SensorData& sensor, float dt) {
  // 基于电流和加速度估计负载
  
  static float lastLeftSpeed = 0, lastRightSpeed = 0;
  
  // 计算加速度
  float leftAccel = (sensor.leftSpeed - lastLeftSpeed) / dt;
  float rightAccel = (sensor.rightSpeed - lastRightSpeed) / dt;
  
  // 估计扭矩 (简化模型: τ = Kt * I)
  float kt = 0.1;  // 扭矩常数 Nm/A
  load.leftTorque = sensor.leftCurrent * kt;
  load.rightTorque = sensor.rightCurrent * kt;
  
  // 估计负载系数 (扭矩/加速度)
  if (abs(leftAccel) > 0.1) {
    load.leftLoad = abs(load.leftTorque / leftAccel);
  }
  if (abs(rightAccel) > 0.1) {
    load.rightLoad = abs(load.rightTorque / rightAccel);
  }
  
  // 总负载
  load.totalLoad = (load.leftLoad + load.rightLoad) / 2.0;
  
  // 估计坡度 (基于左右扭矩差)
  load.grade = (load.rightTorque - load.leftTorque) * 0.1;
  
  lastLeftSpeed = sensor.leftSpeed;
  lastRightSpeed = sensor.rightSpeed;
}

void detectSlip(SensorData& sensor, float dt) {
  // 滑移检测
  
  static float lastLeftSpeed = 0, lastRightSpeed = 0;
  static float targetLeftSpeed = 0, targetRightSpeed = 0;
  
  // 获取目标速度
  targetLeftSpeed = motorLeft.shaft_velocity_sp;
  targetRightSpeed = motorRight.shaft_velocity_sp;
  
  // 计算加速度
  float leftAccel = (sensor.leftSpeed - lastLeftSpeed) / dt;
  float rightAccel = (sensor.rightSpeed - lastRightSpeed) / dt;
  
  // 检测异常加速度
  if (abs(leftAccel) > slip.accelThreshold) {
    slip.slipCount[0]++;
  } else {
    slip.slipCount[0] = max(slip.slipCount[0] - 1, 0);
  }
  
  if (abs(rightAccel) > slip.accelThreshold) {
    slip.slipCount[1]++;
  } else {
    slip.slipCount[1] = max(slip.slipCount[1] - 1, 0);
  }
  
  // 计算滑移率
  if (abs(targetLeftSpeed) > 0.1) {
    slip.leftSlip = abs((targetLeftSpeed - sensor.leftSpeed) / targetLeftSpeed);
  } else {
    slip.leftSlip = 0;
  }
  
  if (abs(targetRightSpeed) > 0.1) {
    slip.rightSlip = abs((targetRightSpeed - sensor.rightSpeed) / targetRightSpeed);
  } else {
    slip.rightSlip = 0;
  }
  
  // 判断是否滑移
  slip.leftSlipping = (slip.leftSlip > slip.slipThreshold) || (slip.slipCount[0] > 5);
  slip.rightSlipping = (slip.rightSlip > slip.slipThreshold) || (slip.slipCount[1] > 5);
  
  // 滑移恢复
  if (!slip.leftSlipping && slip.slipCount[0] > 0) {
    slip.slipCount[0]--;
  }
  if (!slip.rightSlipping && slip.slipCount[1] > 0) {
    slip.slipCount[1]--;
  }
  
  lastLeftSpeed = sensor.leftSpeed;
  lastRightSpeed = sensor.rightSpeed;
}

void adaptiveControl(SensorData& sensor, float dt) {
  // 自适应控制
  
  // 获取目标速度
  float targetLeft = motorLeft.shaft_velocity_sp;
  float targetRight = motorRight.shaft_velocity_sp;
  
  // 计算误差
  float leftError = targetLeft - sensor.leftSpeed;
  float rightError = targetRight - sensor.rightSpeed;
  
  // 应用滑移补偿
  if (slip.leftSlipping) {
    leftError *= 0.5;  // 滑移时减小误差
  }
  if (slip.rightSlipping) {
    rightError *= 0.5;
  }
  
  // 自适应PID控制
  float leftOutput = leftPID.calculate(leftError, dt, load.leftLoad);
  float rightOutput = rightPID.calculate(rightError, dt, load.rightLoad);
  
  // 负载均衡
  leftOutput = balanceLoad(leftOutput, 0, dt);
  rightOutput = balanceLoad(rightOutput, 1, dt);
  
  // 应用输出
  motorLeft.move(leftOutput);
  motorRight.move(rightOutput);
}

float balanceLoad(float output, int wheel, float dt) {
  // 负载均衡
  
  static float imbalanceAccumulator = 0;
  
  // 计算负载差异
  float loadDiff = load.leftLoad - load.rightLoad;
  
  // 积分负载差异
  imbalanceAccumulator += loadDiff * dt;
  imbalanceAccumulator = constrain(imbalanceAccumulator, -2, 2);
  
  // 应用补偿
  float compensation = 0;
  if (wheel == 0) {  // 左轮
    compensation = -imbalanceAccumulator * 0.5;
  } else {  // 右轮
    compensation = imbalanceAccumulator * 0.5;
  }
  
  return output + compensation;
}

5、高级扭矩矢量分配
场景：高性能机器人，需要精确扭矩分配和动态调整。
核心逻辑：实时扭矩估算 + 最优分配算法。

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

// 扭矩矢量控制
struct TorqueVector {
  float longitudinal;  // 纵向扭矩
  float lateral;       // 横向扭矩
  float yaw;          // 偏航扭矩
};
TorqueVector desiredTorque = {0, 0, 0};
TorqueVector actualTorque = {0, 0, 0};
TorqueVector limitedTorque = {0, 0, 0};

// 轮胎模型
struct TireModel {
  float maxGrip = 1000.0;    // 最大抓地力
  float slipRatioOpt = 0.1;  // 最佳滑移率
  float stiffness = 100.0;   // 刚度
  
  float calculateForce(float slipRatio, float normalForce) {
    // Pacejka魔术公式简化版
    float B = stiffness;
    float C = 1.5;
    float D = maxGrip * normalForce;
    float E = 0.5;
    
    float x = slipRatio;
    float y = D * sin(C * atan(B * x - E * (B * x - atan(B * x))));
    
    return y;
  }
};
TireModel tireModel;

// 最优分配求解器
class TorqueAllocator {
private:
  // 权重矩阵
  Eigen::Matrix3f W;  // 扭矩权重
  Eigen::Matrix4f R;  // 执行器权重
  Eigen::Matrix<float, 3, 4> B;  // 分配矩阵
  
  // 约束
  float maxWheelTorque = 10.0;
  float minWheelTorque = -10.0;
  float maxTotalPower = 200.0;
  
public:
  TorqueAllocator() {
    // 初始化权重
    W = Eigen::Matrix3f::Identity();
    R = Eigen::Matrix4f::Identity();
    
    // 分配矩阵 (4轮车辆)
    B << 1, 1, 1, 1,     // 纵向
         -1, 1, -1, 1,   // 横向
         -0.5, 0.5, 0.5, -0.5;  // 偏航
  }
  
  Eigen::Vector4f allocate(TorqueVector torque, Eigen::Vector4f slipRatios) {
    // 二次规划求解最优分配
    
    // 构建目标函数: min 0.5*u'*R*u + (B*u - τ)'*W*(B*u - τ)
    Eigen::Matrix4f H = R + B.transpose() * W * B;
    Eigen::Vector4f f = -B.transpose() * W * torqueToVector(torque);
    
    // 约束: u_min ≤ u ≤ u_max
    Eigen::Vector4f lb = Eigen::Vector4f::Constant(minWheelTorque);
    Eigen::Vector4f ub = Eigen::Vector4f::Constant(maxWheelTorque);
    
    // 考虑滑移率的约束调整
    for (int i = 0; i < 4; i++) {
      if (abs(slipRatios[i]) > 0.2) {
        ub[i] *= 0.5;  // 滑移时降低扭矩限制
      }
    }
    
    // 求解QP问题
    Eigen::Vector4f wheelTorques = solveQP(H, f, lb, ub);
    
    return wheelTorques;
  }
  
  Eigen::Vector4f allocateSimple(TorqueVector torque, float* slipRatios) {
    // 简化分配算法
    
    Eigen::Vector4f torques;
    
    // 基础分配
    torques[0] = torque.longitudinal * 0.25 - torque.lateral * 0.25 - torque.yaw * 0.125;  // FL
    torques[1] = torque.longitudinal * 0.25 + torque.lateral * 0.25 + torque.yaw * 0.125;  // FR
    torques[2] = torque.longitudinal * 0.25 - torque.lateral * 0.25 + torque.yaw * 0.125;  // RL
    torques[3] = torque.longitudinal * 0.25 + torque.lateral * 0.25 - torque.yaw * 0.125;  // RR
    
    // 滑移补偿
    for (int i = 0; i < 4; i++) {
      if (slipRatios[i] > 0.1) {
        // 减小滑移轮的扭矩
        torques[i] *= (1.0 - slipRatios[i]);
      }
    }
    
    // 扭矩限制
    for (int i = 0; i < 4; i++) {
      torques[i] = constrain(torques[i], -maxWheelTorque, maxWheelTorque);
    }
    
    // 功率限制
    limitTotalPower(torques);
    
    return torques;
  }
  
private:
  Eigen::Vector3f torqueToVector(TorqueVector t) {
    return Eigen::Vector3f(t.longitudinal, t.lateral, t.yaw);
  }
  
  void limitTotalPower(Eigen::Vector4f& torques) {
    // 限制总功率
    
    float totalPower = 0;
    for (int i = 0; i < 4; i++) {
      totalPower += abs(torques[i]) * 10.0;  // 简化功率计算
    }
    
    if (totalPower > maxTotalPower) {
      float scale = maxTotalPower / totalPower;
      for (int i = 0; i < 4; i++) {
        torques[i] *= scale;
      }
    }
  }
  
  Eigen::Vector4f solveQP(Eigen::Matrix4f& H, Eigen::Vector4f& f, 
                         Eigen::Vector4f& lb, Eigen::Vector4f& ub) {
    // 简化的QP求解器
    
    // 使用梯度下降
    Eigen::Vector4f x = Eigen::Vector4f::Zero();
    float alpha = 0.1;  // 学习率
    
    for (int iter = 0; iter < 100; iter++) {
      // 计算梯度
      Eigen::Vector4f grad = H * x + f;
      
      // 梯度下降
      x = x - alpha * grad;
      
      // 投影到可行域
      for (int i = 0; i < 4; i++) {
        x[i] = max(min(x[i], ub[i]), lb[i]);
      }
    }
    
    return x;
  }
};

TorqueAllocator torqueAllocator;

// 滑移率估计
class SlipRatioEstimator {
private:
  float wheelRadius = 0.05;
  float vehicleSpeed = 0.0;
  float wheelSpeeds[4] = {0, 0, 0, 0};
  float slipRatios[4] = {0, 0, 0, 0};
  
public:
  void update(float* motorSpeeds, float vx, float vy, float omega) {
    // 更新滑移率估计
    
    // 计算轮心速度
    float wheelVx[4], wheelVy[4];
    
    // FL
    wheelVx[0] = vx - omega * 0.15;
    wheelVy[0] = vy - omega * 0.15;
    
    // FR
    wheelVx[1] = vx + omega * 0.15;
    wheelVy[1] = vy - omega * 0.15;
    
    // RL
    wheelVx[2] = vx - omega * 0.15;
    wheelVy[2] = vy + omega * 0.15;
    
    // RR
    wheelVx[3] = vx + omega * 0.15;
    wheelVy[3] = vy + omega * 0.15;
    
    // 计算轮子纵向速度
    for (int i = 0; i < 4; i++) {
      float wheelSpeed = motorSpeeds[i] * wheelRadius;
      wheelSpeeds[i] = wheelSpeed;
      
      // 计算轮子纵向速度
      float wheelLongSpeed = wheelVx[i];
      
      // 计算滑移率
      if (abs(wheelLongSpeed) > 0.1) {
        slipRatios[i] = (wheelSpeed - wheelLongSpeed) / abs(wheelLongSpeed);
      } else {
        slipRatios[i] = 0;
      }
      
      // 限制范围
      slipRatios[i] = constrain(slipRatios[i], -1.0, 1.0);
    }
  }
  
  float* getSlipRatios() { return slipRatios; }
  float* getWheelSpeeds() { return wheelSpeeds; }
};

SlipRatioEstimator slipEstimator;

// 扭矩观察器
class TorqueObserver {
private:
  float wheelInertia = 0.01;  // 轮子转动惯量
  float vehicleMass = 10.0;   // 车辆质量
  float dt = 0.01;
  
  // 状态估计
  Eigen::Vector4f estimatedTorques = Eigen::Vector4f::Zero();
  Eigen::Vector4f wheelAccels = Eigen::Vector4f::Zero();
  
  // 卡尔曼滤波
  Eigen::Matrix4f P = Eigen::Matrix4f::Identity();
  Eigen::Matrix4f Q = Eigen::Matrix4f::Identity() * 0.01;
  Eigen::Matrix4f R = Eigen::Matrix4f::Identity() * 0.1;
  
public:
  void update(float* motorCurrents, float* wheelAccelerations) {
    // 预测步骤
    Eigen::Vector4f x_pred = estimatedTorques;
    Eigen::Matrix4f P_pred = P + Q;
    
    // 测量更新
    Eigen::Vector4f z = Eigen::Vector4f::Zero();
    for (int i = 0; i < 4; i++) {
      z[i] = motorCurrents[i] * 0.1;  // 电流到扭矩转换
    }
    
    // 卡尔曼增益
    Eigen::Matrix4f K = P_pred * (P_pred + R).inverse();
    
    // 状态更新
    estimatedTorques = x_pred + K * (z - x_pred);
    P = (Eigen::Matrix4f::Identity() - K) * P_pred;
    
    // 更新轮加速度
    for (int i = 0; i < 4; i++) {
      wheelAccels[i] = wheelAccelerations[i];
    }
  }
  
  Eigen::Vector4f getEstimatedTorques() { return estimatedTorques; }
  Eigen::Vector4f getWheelAccelerations() { return wheelAccels; }
  
  float getTotalLongitudinalTorque() {
    return estimatedTorques.sum();
  }
  
  float getTotalLateralTorque() {
    return -estimatedTorques[0] + estimatedTorques[1] - estimatedTorques[2] + estimatedTorques[3];
  }
  
  float getTotalYawTorque() {
    return -estimatedTorques[0] + estimatedTorques[1] + estimatedTorques[2] - estimatedTorques[3];
  }
};

TorqueObserver torqueObserver;

void advancedTorqueControl(float dt) {
  // 高级扭矩控制
  
  // 1. 读取传感器
  float motorCurrents[4];
  float wheelSpeeds[4];
  readAllSensors(motorCurrents, wheelSpeeds);
  
  // 2. 计算轮加速度
  static float lastWheelSpeeds[4] = {0, 0, 0, 0};
  float wheelAccels[4];
  for (int i = 0; i < 4; i++) {
    wheelAccels[i] = (wheelSpeeds[i] - lastWheelSpeeds[i]) / dt;
    lastWheelSpeeds[i] = wheelSpeeds[i];
  }
  
  // 3. 更新扭矩观察器
  torqueObserver.update(motorCurrents, wheelAccels);
  
  // 4. 获取实际扭矩
  actualTorque.longitudinal = torqueObserver.getTotalLongitudinalTorque();
  actualTorque.lateral = torqueObserver.getTotalLateralTorque();
  actualTorque.yaw = torqueObserver.getTotalYawTorque();
  
  // 5. 计算滑移率
  float vx = estimateVehicleSpeed();
  float vy = 0;
  float omega = estimateYawRate();
  slipEstimator.update(wheelSpeeds, vx, vy, omega);
  float* slipRatios = slipEstimator.getSlipRatios();
  
  // 6. 扭矩分配
  Eigen::Vector4f wheelTorques = torqueAllocator.allocateSimple(desiredTorque, slipRatios);
  
  // 7. 应用轮胎模型限制
  applyTireModelLimits(wheelTorques, slipRatios);
  
  // 8. 转换为电机控制
  for (int i = 0; i < 4; i++) {
    float motorTorque = wheelTorques[i];
    float motorCurrent = motorTorque / 0.1;  // 转换为电流
    
    // 设置电机电流
    setMotorCurrent(i, motorCurrent);
  }
}

void applyTireModelLimits(Eigen::Vector4f& torques, float* slipRatios) {
  // 应用轮胎模型限制
  
  float normalForce = 25.0;  // 每个轮子法向力
  
  for (int i = 0; i < 4; i++) {
    // 计算最大可用扭矩
    float maxTorque = tireModel.calculateForce(slipRatios[i], normalForce) * 0.05;  // 轮半径0.05m
    
    // 限制扭矩
    torques[i] = constrain(torques[i], -maxTorque, maxTorque);
    
    // 如果滑移率过大，进一步限制
    if (abs(slipRatios[i]) > 0.3) {
      torques[i] *= 0.5;
    }
  }
}

6、AI驱动自适应控制
场景：智能机器人，使用机器学习实现自适应控制。
核心逻辑：神经网络 + 强化学习 + 在线适应。

#include <SimpleFOC.h>
#include <TensorFlowLite_ESP32.h>
#include <tensorflow/lite/micro/all_ops_resolver.h>
#include <tensorflow/lite/micro/micro_interpreter.h>
#include <tensorflow/lite/schema/schema_generated.h>

// TFLite模型
const unsigned char* adaptive_model = nullptr;  // 预训练模型
const int tensor_arena_size = 100 * 1024;       // 100KB
uint8_t tensor_arena[tensor_arena_size];

tflite::MicroInterpreter* interpreter = nullptr;
TfLiteTensor* input = nullptr;
TfLiteTensor* output = nullptr;

// 神经网络输入
struct NNInput {
  float wheelSpeeds[4];      // 轮速
  float wheelCurrents[4];    // 电流
  float slipRatios[4];       // 滑移率
  float vehicleSpeed;        // 车速
  float vehicleAccel;        // 加速度
  float steeringAngle;       // 转向角
  float terrainRoughness;    // 地形粗糙度
  float batteryVoltage;      // 电池电压
  float motorTemps[4];       // 电机温度
};
NNInput nnInput;

// 神经网络输出
struct NNOutput {
  float torqueAllocation[4];  // 扭矩分配
  float pidGains[6];          // PID增益 [Kp_left, Ki_left, Kd_left, Kp_right, Ki_right, Kd_right]
  float slipThreshold;        // 滑移阈值
  float adaptationFactor;     // 适应因子
};
NNOutput nnOutput;

// 强化学习代理
class RLAgent {
private:
  // 状态空间
  enum StateIndex {
    SPEED,
    ACCEL,
    SLIP,
    LOAD,
    TERRAIN,
    STATE_DIM
  };
  
  // 动作空间
  enum ActionIndex {
    TORQUE_SCALE,
    SLIP_THRESHOLD,
    KP_ADJUST,
    ACTION_DIM
  };
  
  // Q表
  float Q[32][8];  // 32个离散状态，8个离散动作
  
  // 学习参数
  float alpha = 0.1;    // 学习率
  float gamma = 0.9;    // 折扣因子
  float epsilon = 0.3;  // 探索率
  
  // 状态历史
  float lastState[STATE_DIM];
  int lastAction = 0;
  float lastReward = 0;
  
public:
  void initialize() {
    // 初始化Q表
    for (int s = 0; s < 32; s++) {
      for (int a = 0; a < 8; a++) {
        Q[s][a] = 0.1;
      }
    }
  }
  
  int selectAction(float state[STATE_DIM]) {
    // ε-greedy策略
    
    if (random(100) / 100.0 < epsilon) {
      // 探索
      return random(8);
    } else {
      // 利用
      int stateIndex = discretizeState(state);
      return getBestAction(stateIndex);
    }
  }
  
  void update(float state[STATE_DIM], int action, float reward, float nextState[STATE_DIM]) {
    // Q学习更新
    
    int s = discretizeState(state);
    int s_prime = discretizeState(nextState);
    int a_prime = getBestAction(s_prime);
    
    float td_error = reward + gamma * Q[s_prime][a_prime] - Q[s][action];
    Q[s][action] += alpha * td_error;
    
    // 保存历史
    memcpy(lastState, state, STATE_DIM * sizeof(float));
    lastAction = action;
    lastReward = reward;
  }
  
  float getActionValue(int actionIndex) {
    // 获取动作值
    switch(actionIndex) {
      case TORQUE_SCALE:
        return map(actionIndex, 0, 7, 0.5, 1.5);
      case SLIP_THRESHOLD:
        return map(actionIndex, 0, 7, 0.1, 0.5);
      case KP_ADJUST:
        return map(actionIndex, 0, 7, 0.5, 2.0);
      default:
        return 1.0;
    }
  }
  
private:
  int discretizeState(float state[STATE_DIM]) {
    // 状态离散化
    
    int index = 0;
    int bits[STATE_DIM];
    
    // 每个维度2位
    for (int i = 0; i < STATE_DIM; i++) {
      bits[i] = constrain((int)(state[i] * 3), 0, 3);
      index |= (bits[i] << (2 * i));
    }
    
    return index & 0x1F;  // 5位，32个状态
  }
  
  int getBestAction(int stateIndex) {
    // 获取最佳动作
    
    int bestAction = 0;
    float bestValue = Q[stateIndex][0];
    
    for (int a = 1; a < 8; a++) {
      if (Q[stateIndex][a] > bestValue) {
        bestValue = Q[stateIndex][a];
        bestAction = a;
      }
    }
    
    return bestAction;
  }
};

RLAgent rlAgent;

// 在线学习系统
class OnlineLearner {
private:
  // 经验回放
  struct Experience {
    NNInput state;
    NNOutput action;
    float reward;
    NNInput nextState;
    bool done;
  };
  
  Experience replayBuffer[1000];
  int bufferIndex = 0;
  bool bufferFull = false;
  
  // 学习统计
  float totalReward = 0;
  int episodes = 0;
  int steps = 0;
  float avgReward = 0;
  
public:
  void addExperience(NNInput state, NNOutput action, float reward, NNInput nextState, bool done) {
    // 添加经验
    replayBuffer[bufferIndex] = {state, action, reward, nextState, done};
    bufferIndex = (bufferIndex + 1) % 1000;
    if (bufferIndex == 0) bufferFull = true;
    
    totalReward += reward;
    steps++;
    
    if (done) {
      episodes++;
      avgReward = totalReward / episodes;
    }
  }
  
  void trainBatch(int batchSize) {
    // 训练批次
    
    if (bufferIndex < batchSize && !bufferFull) return;
    
    // 采样批次
    Experience batch[batchSize];
    for (int i = 0; i < batchSize; i++) {
      int idx = random(bufferFull ? 1000 : bufferIndex);
      batch[i] = replayBuffer[idx];
    }
    
    // 转换为模型输入
    float inputs[batchSize * 20];  // 20个输入特征
    float targets[batchSize * 11]; // 11个输出
    
    for (int i = 0; i < batchSize; i++) {
      Experience exp = batch[i];
      
      // 构建输入
      int base = i * 20;
      for (int j = 0; j < 4; j++) inputs[base + j] = exp.state.wheelSpeeds[j];
      for (int j = 0; j < 4; j++) inputs[base + 4 + j] = exp.state.wheelCurrents[j];
      for (int j = 0; j < 4; j++) inputs[base + 8 + j] = exp.state.slipRatios[j];
      inputs[base + 12] = exp.state.vehicleSpeed;
      inputs[base + 13] = exp.state.vehicleAccel;
      inputs[base + 14] = exp.state.steeringAngle;
      inputs[base + 15] = exp.state.terrainRoughness;
      inputs[base + 16] = exp.state.batteryVoltage;
      for (int j = 0; j < 4; j++) inputs[base + 17 + j] = exp.state.motorTemps[j];
      
      // 构建目标
      int tbase = i * 11;
      for (int j = 0; j < 4; j++) targets[tbase + j] = exp.action.torqueAllocation[j];
      for (int j = 0; j < 6; j++) targets[tbase + 4 + j] = exp.action.pidGains[j];
      targets[tbase + 10] = exp.action.slipThreshold;
      
      // 奖励信号
      for (int j = 0; j < 11; j++) {
        targets[tbase + j] += 0.1 * exp.reward;  // 奖励影响
      }
    }
    
    // 训练模型
    trainModel(inputs, targets, batchSize);
  }
  
private:
  void trainModel(float* inputs, float* targets, int batchSize) {
    // 模型训练
    
    // 这里简化处理，实际应使用梯度下降
    // 由于嵌入式限制，通常在线学习是简化版本
    
    // 简单的在线调整
    float learningRate = 0.001;
    
    // 调整神经网络权重
    adjustNetworkWeights(inputs, targets, batchSize, learningRate);
  }
};

OnlineLearner onlineLearner;

// 自适应控制器
class AIController {
private:
  // 神经网络推理
  tflite::MicroInterpreter* interpreter;
  
  // 自适应参数
  float adaptationRate = 0.01;
  float explorationRate = 0.1;
  float minSlipThreshold = 0.1;
  float maxSlipThreshold = 0.5;
  
  // 性能指标
  float slipPerformance = 0;
  float speedPerformance = 0;
  float efficiency = 0;
  float stability = 0;
  
public:
  void setup() {
    // 加载TFLite模型
    loadModel();
    
    // 初始化RL代理
    rlAgent.initialize();
  }
  
  NNOutput computeControl(NNInput input) {
    // 神经网络推理
    
    // 准备输入
    float nn_input[20];
    prepareNNInput(input, nn_input);
    
    // 运行推理
    runInference(nn_input);
    
    // 获取输出
    NNOutput output;
    getNNOutput(output);
    
    // 强化学习调整
    adjustWithRL(output, input);
    
    return output;
  }
  
  void learnFromExperience(NNInput state, NNOutput action, float reward, NNInput nextState) {
    // 从经验中学习
    
    // 添加经验
    onlineLearner.addExperience(state, action, reward, nextState, false);
    
    // 定期训练
    static int trainCounter = 0;
    if (trainCounter++ % 100 == 0) {
      onlineLearner.trainBatch(32);
    }
  }
  
private:
  void loadModel() {
    // 加载TFLite模型
    
    static tflite::MicroMutableOpResolver<10> resolver;
    resolver.AddFullyConnected();
    resolver.AddSoftmax();
    resolver.AddQuantize();
    resolver.AddDequantize();
    
    static tflite::MicroInterpreter static_interpreter(
      adaptive_model, resolver, tensor_arena, tensor_arena_size);
    
    interpreter = &static_interpreter;
    interpreter->AllocateTensors();
    
    input = interpreter->input(0);
    output = interpreter->output(0);
  }
  
  void prepareNNInput(NNInput& in, float* out) {
    // 准备神经网络输入
    
    int idx = 0;
    for (int i = 0; i < 4; i++) out[idx++] = normalize(in.wheelSpeeds[i], 0, 100);
    for (int i = 0; i < 4; i++) out[idx++] = normalize(in.wheelCurrents[i], 0, 20);
    for (int i = 0; i < 4; i++) out[idx++] = normalize(in.slipRatios[i], -1, 1);
    out[idx++] = normalize(in.vehicleSpeed, 0, 10);
    out[idx++] = normalize(in.vehicleAccel, -10, 10);
    out[idx++] = normalize(in.steeringAngle, -PI, PI);
    out[idx++] = normalize(in.terrainRoughness, 0, 1);
    out[idx++] = normalize(in.batteryVoltage, 10, 15);
    for (int i = 0; i < 4; i++) out[idx++] = normalize(in.motorTemps[i], 20, 100);
  }
  
  void runInference(float* input_data) {
    // 运行推理
    
    // 复制输入
    for (int i = 0; i < 20; i++) {
      input->data.f[i] = input_data[i];
    }
    
    // 推理
    interpreter->Invoke();
  }
  
  void getNNOutput(NNOutput& out) {
    // 获取神经网络输出
    
    for (int i = 0; i < 4; i++) {
      out.torqueAllocation[i] = output->data.f[i] * 2.0;  // 缩放
    }
    
    for (int i = 0; i < 6; i++) {
      out.pidGains[i] = output->data.f[4 + i];
    }
    
    out.slipThreshold = output->data.f[10] * 0.5;
    out.adaptationFactor = output->data.f[11];
  }
  
  void adjustWithRL(NNOutput& output, NNInput& input) {
    // RL调整
    
    // 获取状态
    float state[RLAgent::STATE_DIM];
    state[0] = input.vehicleSpeed;
    state[1] = input.vehicleAccel;
    state[2] = average(input.slipRatios, 4);
    state[3] = average(input.wheelCurrents, 4) * 0.1;  // 负载估计
    state[4] = input.terrainRoughness;
    
    // 选择动作
    int action = rlAgent.selectAction(state);
    float actionValue = rlAgent.getActionValue(action);
    
    // 应用动作
    switch(action % 3) {
      case 0:  // 扭矩缩放
        for (int i = 0; i < 4; i++) {
          output.torqueAllocation[i] *= actionValue;
        }
        break;
      case 1:  // 滑移阈值
        output.slipThreshold = actionValue;
        break;
      case 2:  // PID增益调整
        for (int i = 0; i < 6; i++) {
          output.pidGains[i] *= actionValue;
        }
        break;
    }
  }
  
  float normalize(float x, float min, float max) {
    return (x - min) / (max - min);
  }
  
  float average(float* arr, int n) {
    float sum = 0;
    for (int i = 0; i < n; i++) sum += arr[i];
    return sum / n;
  }
};

要点解读
负载自适应算法的三个层面
电流估算：案例4基于τ = Kt × I估算扭矩，Kt=0.1是典型值。这是最直接的方法，但需校准。
加速度反算：案例4load = τ/α，从加速度反算负载。但低速时加速度噪声大，需滤波。
神经网络估计：案例6用NN学习负载特性，可识别复杂负载模式（如爬坡、拖拽、侧风）。
关键技巧：电流读数必须滤波。案例4的MovingAverage滑动平均，窗口10个样本。
滑移检测的多指标融合
速度差检测：案例4slipRate = |(V_target - V_actual)/V_target|。阈值0.3（30%）。
加速度检测：轮子加速度突然增大可能是打滑。案例4accelThreshold=10 rad/s²。
统计检测：案例4 slipCount累加异常次数，>5次触发滑移标志。防瞬时干扰。
模型预测：案例5用轮胎模型预测最大可用扭矩，超限则为滑移。
误报处理：滑移恢复时slipCount递减，防止粘滞。
扭矩分配的最优策略
等比例分配：简单但非最优。案例5的allocateSimple()实现基础分配。
QP优化分配：案例5的二次规划求解min 0.5u’Ru + (Bu-τ)'W(Bu-τ)。这是理论最优。
滑移补偿：滑移轮减少扭矩，相邻轮补偿。案例5 torques[i] *= (1.0 - slipRatios[i])。
功率限制：案例5 limitTotalPower()防止电池过载。maxTotalPower=200W是典型值。
实时性：QP求解需<1ms。案例5简化求解用100次梯度下降迭代。
AI/ML在自适应控制中的角色
神经网络推理：案例6 TFLite模型，20输入→11输出，推理时间<5ms。
强化学习：案例6的RLAgent实现状态-动作学习，32状态×8动作Q表。
在线学习：案例6 OnlineLearner经验回放，批量训练。但嵌入式在线训练极其困难，通常预训练。
特征工程：NN输入包括速度、电流、滑移率、温度、电压、地形等20个特征。
奖励设计：RL学习的关键。应综合：奖励 = 速度跟踪 - 滑移惩罚 - 能耗惩罚。
工程实施的传感器需求
必须传感器：
编码器：分辨率≥1000线，用于速度检测
电流传感器：精度±0.1A，带宽≥5kHz
推荐传感器：
温度传感器：每电机一个，贴绕组
电压传感器：精度±0.1V
IMU：检测实际加速度，校正估计
采样频率：
电流环：20kHz
速度环：1-5kHz
滑移检测：100-500Hz
负载估计：50-100Hz
校准要求：
电流传感器零偏校准
扭矩常数Kt标定
轮胎半径精确测量
车辆质量称重

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