这是一个面向非结构化、高动态地面环境的先进移动机器人控制方案，旨在解决传统轮式机器人在复杂地形（如沙地、泥泞、草地、斜坡）上因车轮滑移导致控制失效的核心问题。其核心思想是从开环/速度闭环控制升级为基于力/力矩观测的滑移感知与自适应控制，从而确保机器人的动力、导航精度和稳定性。

一、 主要特点与技术原理
滑移检测与估计
滑移的定义：分为纵向滑移（车轮线速度与车身实际速度不一致）和侧向滑移（车轮平面方向与实际运动方向存在夹角）。
检测方法：传统方法依赖多传感器融合。
惯性测量单元：提供车体三轴加速度和角速度。这是运动参考基准。
轮速编码器：提供每个驱动轮的转速。
观测器算法：通过对比IMU推算的速度/位姿与轮式里程计积分的结果，可以计算出滑移率。例如，在短时间Δt内，IMU双积分得到位移S_imu，编码器计算得到位移S_enc，则纵向滑移率 s = (S_enc - S_imu) / S_enc。s > 0为打滑，s < 0为拖滑。
负载观测与自适应
问题：机器人负载变化、爬坡时，电机负载扭矩剧烈变化。传统PID速度环的固定参数无法适应，可能导致响应迟钝或振荡。
解决方案：引入扰动观测器或自适应控制器。
扰动观测器：将负载变化、地面阻力等所有未建模动态视为“总扰动”，通过电机电流、转速等状态实时估计出扰动值，并在控制指令中前馈补偿，使被控对象（电机+负载）表现得像一个理想的无扰动系统。
模型参考自适应控制：在线调整控制器参数（如PID增益），使得实际系统的输出始终跟随一个理想参考模型的输出，从而适应负载和地面特性的变化。
基于滑移的扭矩分配与容错控制
扭矩再分配：当检测到某个车轮发生严重滑移（如悬空）时，系统可动态降低该轮的动力指令，避免其空转消耗能量；同时增加附着力良好车轮的扭矩，以维持总推进力。
容错导航：将滑移信息（如滑移率、估计的地面附着系数）反馈给上层的定位与导航模块。导航算法可据此动态降低对轮式里程计的信任权重，更多地依赖IMU、视觉或激光数据，或重新规划一条附着条件更好的路径。

二、 系统架构（基于Arduino+BLDC）

[传感器层]
├── IMU (MPU6050/BNO085): 提供车身本体加速度、角速度。
├── 高分辨率轮编码器: 提供各轮精确转速。
└── 电流传感器 (如ACS712/INA219): 测量电机相电流，用于估算输出扭矩。

[控制层 - 运行于高性能Arduino兼容板]
├── 状态估计器: 融合IMU与编码器数据，使用卡尔曼滤波等算法，解算车体速度、位置及**各轮滑移率**。
├── 扰动观测器: 基于电机模型(电流-扭矩关系)和实测转速，实时估算作用于各轮的**负载扰动扭矩**。
├── 自适应控制器: 以目标速度为输入，结合滑移率与扰动观测值，动态计算各电机所需的**抗滑移扭矩指令**。
└── 底层驱动器: 将扭矩指令通过FOC或PWM方式发送给各BLDC的ESC。

[决策层]
└── 导航调整: 接收滑移状态，调整路径跟踪的侵略性或切换定位源。

三、 核心应用场景
野外/全地形移动机器人：用于农业、矿业、野外救援。机器人在松软土质、碎石坡、冰雪路面行进时，必须实时感知滑移并调整扭矩，防止陷车。
高速竞技机器人：如RoboMaster、大学生方程式无人车。在高速过弯、急加速/制动时，轮胎处于非线性附着区域。自适应控制可最大限度地利用地面附着力，实现更快的圈速和更稳定的操控。
重载AGV/叉车：在工厂光滑地面（环氧地坪）搬运重物时，启动/制动易打滑。负载自适应控制可确保平稳、精确的“寸动”，保护货物和人员安全。
星球车/极限环境探测车：在火星、月球等低附着系数、未知地形环境下，这是必备技术。通过滑移检测判断地形力学参数，并自动调整控制策略以保障安全。

四、 注意事项与挑战
IMU噪声与漂移
核心矛盾：滑移检测极度依赖IMU数据的短期精度，但低成本MEMS-IMU噪声大，且存在零偏漂移。直接用其积分求速度/位移，数秒内误差就会累积到不可接受的程度。
解决方案：必须采用传感器融合算法。最经典的是轮速-IMU融合的卡尔曼滤波器。轮速提供高频相对位移但会累积滑移误差，IMU提供绝对加速度但会漂移，滤波器可最优地结合两者，输出更准确的车体速度估计，并同步解算出滑移率。
电机-负载模型的准确性
扰动观测器的性能，高度依赖于电机扭矩常数、减速比、轮径、车体转动惯量等模型参数的准确性。参数不准，观测的扰动也不准。
应对：必须进行严谨的系统辨识实验，在已知负载下标定电机电流与输出扭矩/推力的关系。同时，模型参数（如转动惯量）应能根据已知的负载质量进行在线更新。
控制器的复杂性、实时性与算力
卡尔曼滤波、扰动观测器、自适应律都涉及矩阵运算和积分，计算负担远大于简单PID。
硬件升级：8位Arduino Uno基本无法胜任。必须使用32位、带FPU的MCU，如STM32F4、ESP32、Teensy 4.0。控制环路频率可适当降低（如100Hz），但滤波和观测器更新频率需保持稳定。
执行器（ESC/BLDC）的性能要求
该方案要求电机能快速、精确地响应扭矩指令。传统的廉价款航模ESC（使用方波驱动、单向控制）无法满足要求。
必须使用：支持FOC控制和双向扭矩控制的智能电调。这类电调通常通过CAN、UART或DShot协议接收扭矩/电流指令，而非简单的转速指令，并能高带宽地闭环控制相电流，从而精确输出所需扭矩。
系统集成与安全边界
扭矩限制：自适应控制器在努力补偿滑移时，可能会计算出极大的扭矩指令。必须在软件中设置基于电机和驱动器最大电流的硬限制，防止烧毁。
失效处理：当IMU或编码器失效时，系统必须有优雅降级策略，如切换回保守的固定参数PID模式，并报警。

总结：该方案标志着轮式机器人控制从“理想化模型控制”走向“基于物理交互的感知控制”。其实质是通过状态估计和扰动观测，让控制系统“看见”车轮与地面的相互作用力，并智能地调整输出。这不仅需要更复杂的算法，更依赖于高信噪比的传感器、精确的标定、高性能的计算平台以及扭矩可控的执行器。这是一项系统工程，其成功实施能将机器人的移动能力从“铺装路面”真正扩展到充满不确定性的现实世界。

1、差速驱动机器人滑移补偿控制（电流+编码器双反馈）

#include <PID_v1.h>
#define ESC_PIN_LEFT 9
#define ESC_PIN_RIGHT 10
#define ENCODER_LEFT 2
#define ENCODER_RIGHT 3
#define CURRENT_PIN_LEFT A0
#define CURRENT_PIN_RIGHT A1

volatile int countLeft = 0, countRight = 0;
double setpointLeft = 100, setpointRight = 100; // 目标转速(RPM)
double inputLeft = 0, inputRight = 0;
double outputLeft = 0, outputRight = 0;
double currentLeft = 0, currentRight = 0;
bool slipDetectedLeft = false, slipDetectedRight = false;

// PID控制器（带自适应参数）
double KpLeft = 0.8, KiLeft = 0.2, KdLeft = 0.05;
double KpRight = 0.8, KiRight = 0.2, KdRight = 0.05;
PID pidLeft(&inputLeft, &outputLeft, &setpointLeft, KpLeft, KiLeft, KdLeft, DIRECT);
PID pidRight(&inputRight, &outputRight, &setpointRight, KpRight, KiRight, KdRight, DIRECT);

void setup() {
  Serial.begin(115200);
  pinMode(ESC_PIN_LEFT, OUTPUT);
  pinMode(ESC_PIN_RIGHT, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(ENCODER_LEFT), countPulseLeft, RISING);
  attachInterrupt(digitalPinToInterrupt(ENCODER_RIGHT), countPulseRight, RISING);
  pidLeft.SetMode(AUTOMATIC);
  pidRight.SetMode(AUTOMATIC);
  pidLeft.SetOutputLimits(-500, 500);
  pidRight.SetOutputLimits(-500, 500);
  // ESC安全启动
  analogWrite(ESC_PIN_LEFT, 0);
  analogWrite(ESC_PIN_RIGHT, 0);
  delay(3000);
}

void loop() {
  static unsigned long lastTime = 0;
  if (millis() - lastTime >= 100) { // 100ms采样周期
    lastTime = millis();
    
    // 1. 计算实际转速（编码器）
    inputLeft = (countLeft * 60.0) / (20 * 0.1); // 假设20脉冲/转
    inputRight = (countRight * 60.0) / (20 * 0.1);
    countLeft = 0;
    countRight = 0;

    // 2. 读取电流（负载检测）
    currentLeft = analogRead(CURRENT_PIN_LEFT) * 0.185; // 假设ACS712 5A版（185mV/A）
    currentRight = analogRead(CURRENT_PIN_RIGHT) * 0.185;

    // 3. 滑移检测（转速骤降+电流突增）
    static double lastSpeedLeft = 0, lastSpeedRight = 0;
    if (inputLeft < lastSpeedLeft * 0.7 && currentLeft > 3.0) slipDetectedLeft = true;
    if (inputRight < lastSpeedRight * 0.7 && currentRight > 3.0) slipDetectedRight = true;
    lastSpeedLeft = inputLeft;
    lastSpeedRight = inputRight;

    // 4. 自适应PID调整（检测到滑移时增强P项）
    if (slipDetectedLeft) {
      KpLeft = 1.2; // 临时增强比例增益
      setpointLeft = inputLeft * 1.1; // 轻微增加目标以恢复牵引
    } else {
      KpLeft = 0.8; // 恢复默认参数
    }
    if (slipDetectedRight) {
      KpRight = 1.2;
      setpointRight = inputRight * 1.1;
    } else {
      KpRight = 0.8;
    }

    // 更新PID参数（需重新初始化PID对象或动态修改）
    // 简化处理：实际项目中建议使用PID库的动态参数修改功能
    pidLeft.SetTunings(KpLeft, KiLeft, KdLeft);
    pidRight.SetTunings(KpRight, KiRight, KdRight);

    // 5. 计算PID输出
    pidLeft.Compute();
    pidRight.Compute();

    // 输出到ESC（带滑移恢复逻辑）
    int pwmLeft = 1500 + outputLeft;
    int pwmRight = 1500 + outputRight;
    if (slipDetectedLeft && outputLeft > 0) pwmLeft += 50; // 滑移时短暂增加动力
    if (slipDetectedRight && outputRight > 0) pwmRight += 50;
    pwmLeft = constrain(pwmLeft, 1000, 2000);
    pwmRight = constrain(pwmRight, 1000, 2000);

    // 通过Servo库输出（需取消注释并安装库）
    // escLeft.writeMicroseconds(pwmLeft);
    // escRight.writeMicroseconds(pwmRight);

    // 调试输出
    Serial.print("L: "); Serial.print(inputLeft); Serial.print(" RPM, ");
    Serial.print(currentLeft); Serial.print(" A, Slip: "); Serial.print(slipDetectedLeft);
    Serial.print(" | R: "); Serial.print(inputRight); Serial.print(" RPM, ");
    Serial.print(currentRight); Serial.print(" A, Slip: "); Serial.println(slipDetectedRight);

    // 重置滑移标志（300ms后自动恢复）
    if (slipDetectedLeft || slipDetectedRight) {
      static unsigned long slipTime = millis();
      if (millis() - slipTime > 300) {
        slipDetectedLeft = slipDetectedRight = false;
      }
    }
  }
}

void countPulseLeft() { countLeft++; }
void countPulseRight() { countRight++; }

技术要点：
双模检测：结合编码器转速骤降和电流突增判断滑移，提高准确性。
动态PID调整：滑移时临时增强P项以快速恢复牵引力。
恢复机制：滑移标志300ms后自动重置，避免持续过补偿。

2、机械臂关节负载自适应控制（电流前馈+滑移保护）

#include <PID_v1.h>
#define ESC_PIN 6
#define CURRENT_PIN A0
#define POT_PIN A1 // 目标位置电位器

double setpoint = 0; // 目标角度（通过电位器映射）
double input = 0;    // 实际角度（需编码器或陀螺仪反馈）
double output = 0;
double current = 0;
bool slipDetected = false;

// PID控制器（带电流前馈补偿）
double Kp = 1.0, Ki = 0.1, Kd = 0.02;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(115200);
  pinMode(ESC_PIN, OUTPUT);
  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255, 255);
  // ESC安全启动
  analogWrite(ESC_PIN, 0);
  delay(3000);
}

void loop() {
  // 1. 读取目标位置（电位器）
  int potValue = analogRead(POT_PIN);
  setpoint = map(potValue, 0, 1023, 0, 180); // 映射到0-180度

  // 2. 模拟实际角度（实际需用编码器替换）
  static float simulatedAngle = 0;
  // 假设简单模型：角度 = 积分(速度)，速度受输出和负载影响
  float loadFactor = constrain(current * 0.5, 0.8, 1.2); // 电流越大负载越重
  simulatedAngle += (output / loadFactor) * 0.1; // 输出除以负载因子
  simulatedAngle = constrain(simulatedAngle, 0, 180);
  input = simulatedAngle;

  // 3. 读取电流
  current = analogRead(CURRENT_PIN) * 0.185;

  // 4. 滑移检测（电流持续过高但角度未达目标）
  static unsigned long stuckTime = 0;
  static double lastCurrent = 0;
  if (abs(setpoint - input) > 5 && current > 2.0 && current > lastCurrent) {
    if (millis() - stuckTime > 500) slipDetected = true; // 持续500ms未到达目标
  } else {
    stuckTime = millis();
  }
  lastCurrent = current;

  // 5. 自适应控制
  if (slipDetected) {
    Kp = 1.5; // 增强比例增益
    output *= 0.8; // 临时降低输出防止损坏
  } else {
    Kp = 1.0;
    // 电流前馈补偿：根据负载动态调整目标
    setpoint += (current - 1.0) * 0.5; // 假设1A为无负载基准
  }
  pid.SetTunings(Kp, Ki, Kd);

  // 计算PID输出
  pid.Compute();

  // 输出到ESC（带滑移保护）
  int pwm = output;
  if (slipDetected) pwm = 0; // 滑移时停止电机
  analogWrite(ESC_PIN, constrain(pwm + 128, 0, 255)); // 假设中立点为128

  Serial.print("Target: "); Serial.print(setpoint);
  Serial.print(" deg, Actual: "); Serial.print(input);
  Serial.print(" deg, Current: "); Serial.print(current);
  Serial.print(" A, Slip: "); Serial.println(slipDetected);

  delay(50);
}

技术要点：
电流前馈补偿：根据负载电流动态调整目标位置，抵消重力或摩擦影响。
滑移保护：检测到卡滞时立即停止电机，防止机械损坏。
负载建模：通过电流估算负载因子，简化实际系统的复杂性。

3、AGV小车坡道负载自适应控制（加速度反馈+滑移恢复）

#include <PID_v1.h>
#include <NewPing.h>
#define ESC_PIN 5
#define ACCEL_PIN A2 // 加速度传感器（如MPU6050）
#define CURRENT_PIN A3
#define TRIG_PIN 7
#define ECHO_PIN 8
NewPing sonar(TRIG_PIN, ECHO_PIN, 400);

double setpoint = 50; // 目标速度(cm/s)
double input = 0;     // 实际速度（通过编码器或模型估算）
double output = 0;
double current = 0;
double acceleration = 0;
bool slopeDetected = false;
bool slipDetected = false;

// PID控制器（带坡道补偿）
double Kp = 0.6, Ki = 0.3, Kd = 0.01;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(115200);
  pinMode(ESC_PIN, OUTPUT);
  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-100, 100);
  // ESC安全启动
  analogWrite(ESC_PIN, 0);
  delay(3000);
}

void loop() {
  static unsigned long lastTime = 0;
  if (millis() - lastTime >= 100) {
    lastTime = millis();

    // 1. 障碍物检测（动态调整目标速度）
    unsigned int distance = sonar.ping_cm();
    if (distance < 30 && distance > 0) {
      setpoint = map(distance, 0, 30, 0, 20); // 近距减速
    } else {
      setpoint = 50;
    }

    // 2. 模拟实际速度（实际需用编码器）
    static float simulatedSpeed = 0;
    float loadFactor = 1.0;
    if (slopeDetected) loadFactor = 1.5; // 坡道增加负载
    if (slipDetected) loadFactor = 0.5; // 滑移时降低负载估计
    simulatedSpeed += (output / loadFactor) * 1.0;
    simulatedSpeed = constrain(simulatedSpeed, 0, 100);
    input = simulatedSpeed;

    // 3. 读取加速度和电流
    acceleration = (analogRead(ACCEL_PIN) - 512) / 100.0; // 简化处理，实际需校准
    current = analogRead(CURRENT_PIN) * 0.185;

    // 4. 坡道检测（持续负加速度）
    static bool lastSlopeState = false;
    if (acceleration < -0.2 && current > 2.0) {
      slopeDetected = true;
    } else {
      slopeDetected = false;
    }

    // 5. 滑移检测（速度骤降+电流突增）
    static double lastSpeed = 0;
    if (input < lastSpeed * 0.7 && current > 3.0) slipDetected = true;
    lastSpeed = input;

    // 6. 自适应PID调整
    if (slopeDetected) {
      Kp = 0.8; // 坡道时增强P项
      setpoint = min(setpoint, 30); // 限制坡道速度
    } else if (slipDetected) {
      Kp = 1.0; // 滑移时更强响应
      output *= 0.7; // 临时降低输出
    } else {
      Kp = 0.6; // 默认参数
    }
    pid.SetTunings(Kp, Ki, Kd);

    // 计算PID输出
    pid.Compute();

    // 输出到ESC（带坡道/滑移补偿）
    int pwm = output;
    if (slopeDetected) pwm *= 1.2; // 坡道增加动力
    if (slipDetected) pwm = 0; // 滑移时停止
    analogWrite(ESC_PIN, map(pwm, -100, 100, 0, 255));

    Serial.print("Speed: "); Serial.print(input);
    Serial.print(" cm/s, Accel: "); Serial.print(acceleration);
    Serial.print(" g, Current: "); Serial.print(current);
    Serial.print(" A, Slope: "); Serial.print(slopeDetected);
    Serial.print(", Slip: "); Serial.println(slipDetected);
  }
}

技术要点：
多传感器融合：结合加速度计、电流和速度判断坡道与滑移。
分级响应：坡道时增加动力，滑移时立即停止，避免失控。
负载估计：通过加速度和电流动态调整负载因子，简化模型。

要点解读
滑移检测策略：
转速-电流联合检测：编码器转速骤降+电流突增是滑移的可靠标志。
时间阈值：避免因短暂负载变化误触发（如案例1中的300ms重置）。
自适应PID实现：
动态参数调整：滑移时增强P项快速恢复，坡道时调整目标速度。
前馈补偿：根据电流或加速度预调整输出，抵消可预测负载（如案例2的电流前馈）。
安全机制设计：
输出限制：PID输出和PWM范围需严格约束（如案例1的constrain）。
故障恢复：滑移后自动重置标志，避免持续保护导致停滞。
传感器融合与滤波：
加速度计校准：需去除重力分量（如案例3的acceleration计算）。
电流滤波：使用移动平均或低通滤波消除噪声（示例中简化处理）。
实时性保障：
固定采样周期：PID计算和传感器读取需在固定时间间隔内完成（如案例1的100ms）。
中断优先级：编码器计数使用硬件中断，避免主循环延迟。

4、基于 IMU 加速度突变检测的防滑控制
功能描述：这是最直观的滑移检测方式。利用 MPU6050 检测机器人的纵向加速度。当电机加速但机器人实际加速度（由 IMU 测得）远小于理论值，或者出现异常的高频震动时，判定为打滑，立即降低电机扭矩。

#include <Wire.h>
#include <Adafruit_MPU6050.h> // 需安装 Adafruit MPU6050 库
#include <SimpleFOC.h>        // 假设使用 SimpleFOC 驱动 BLDC

// --- 硬件定义 ---
Adafruit_MPU6050 mpu;
BLDCMotor motor(7); // 示例电机
// ... 驱动器和编码器定义 ...

// --- 滑移检测参数 ---
float max_accel_threshold = 3.0; // 最大物理加速度阈值 (m/s^2)
float slip_detected_threshold = 0.5; // 加速度抖动阈值
unsigned long last_slip_time = 0;

void setup() {
  Serial.begin(115200);
  
  // 初始化 IMU
  if (!mpu.begin()) {
    Serial.println("MPU6050 初始化失败!");
    while (1);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_4_G); // 设置量程
  
  // 初始化电机...
  motor.init();
  motor.initFOC();
}

void loop() {
  // 1. 获取 IMU 数据
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  float ax = a.acceleration.x; // 假设 X 轴为前进方向

  // 2. 滑移判断逻辑
  // 如果加速度瞬间极大（轮胎空转甩动）或 剧烈波动
  bool isSlipping = (abs(ax) > max_accel_threshold);

  // 3. 自适应控制策略
  float target_velocity = 2.0; // 目标速度 rad/s
  
  if (isSlipping) {
    // 检测到打滑：强制降低目标速度，模拟牵引力控制 (TCS)
    target_velocity = 0.5; 
    Serial.println("⚠️ 检测到打滑！降低扭矩...");
    last_slip_time = millis();
  }
  
  // 4. 执行电机控制
  motor.move(target_velocity);
  
  Serial.print("Accel X: "); Serial.print(ax);
  Serial.print(" | Status: "); Serial.println(isSlipping ? "SLIP" : "GRIP");
  
  delay(10);
}

5、基于电流与坡度融合的负载自适应 PID
功能描述：此案例结合了电流传感器（检测负载）和 IMU 倾角（检测坡度）。当机器人上坡或负重增加时，电流会上升。

#include <PID_v1.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>

// --- 硬件定义 ---
Adafruit_MPU6050 mpu;
const int currentPin = A0; // 电流传感器引脚

// --- PID 变量 ---
double Setpoint = 1.0; // 目标速度 m/s
double Input, Output;
double Kp = 1.0, Ki = 0.5, Kd = 0.1; // 基础参数
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

// --- 负载状态 ---
float load_factor = 1.0; // 负载系数，1.0 为平路空载

void setup() {
  Serial.begin(115200);
  mpu.begin();
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(0, 255);
}

void loop() {
  // 1. 采集传感器数据
  // 读取电流 (假设 0-5V 对应 0-10A)
  float current_val = analogRead(currentPin) * (5.0 / 1023.0) / 0.185; // ACS712 示例
  
  // 读取坡度 (俯仰角)
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  float pitch_angle = atan2(a.acceleration.y, a.acceleration.z) * 180.0 / PI;

  // 2. 计算负载因子
  // 坡度补偿：角度越大，需要的扭矩越大
  float slope_comp = abs(pitch_angle) / 30.0; // 归一化 0-1
  // 电流补偿：电流越大，负载越重
  float current_comp = constrain(current_val / 5.0, 0, 1); 

  load_factor = 1.0 + slope_comp + current_comp;

  // 3. 自适应调整 PID 参数
  // 负载越重，增大 P 和 I 以增强响应和消除静差的能力
  float adaptive_Kp = Kp * load_factor;
  float adaptive_Ki = Ki * load_factor;
  
  myPID.SetTunings(adaptive_Kp, adaptive_Ki, Kd);

  // 4. 模拟速度读取与 PID 计算
  Input = 0.8; // 模拟实际速度
  myPID.Compute();

  // 5. 输出
  // analogWrite(MOTOR_PIN, Output);

  Serial.print("Load Factor: "); Serial.print(load_factor);
  Serial.print(" | New Kp: "); Serial.println(adaptive_Kp);
  
  delay(50);
}

6、基于速度标准差统计的打滑识别
功能描述：这是一种纯软件算法，不需要额外传感器。通过统计学方法，计算短时间内编码器速度读数的标准差。如果速度读数忽快忽慢（标准差大），说明轮胎与地面接触不稳定（打滑或跳变）。

#include <math.h>

// --- 统计参数 ---
const int WINDOW_SIZE = 10;
float speed_history[WINDOW_SIZE];
int speed_index = 0;

// --- 模拟数据 ---
float read_encoder_speed() {
  // 模拟正常行驶
  return 100.0 + random(-2, 2); 
  // 模拟打滑时：return 100.0 + random(-50, 50);
}

// --- 计算标准差函数 ---
float calculate_std_dev() {
  float sum = 0, sum_sq = 0;
  for (int i = 0; i < WINDOW_SIZE; i++) {
    sum += speed_history[i];
    sum_sq += speed_history[i] * speed_history[i];
  }
  float mean = sum / WINDOW_SIZE;
  return sqrt(sum_sq / WINDOW_SIZE - mean * mean);
}

void loop() {
  // 1. 获取当前速度
  float current_speed = read_encoder_speed();
  
  // 2. 存入环形缓冲区
  speed_history[speed_index] = current_speed;
  speed_index = (speed_index + 1) % WINDOW_SIZE;

  // 3. 计算波动率 (标准差)
  float std_dev = calculate_std_dev();

  // 4. 自适应策略
  // 如果速度波动剧烈 (标准差 > 5.0)，判定为打滑
  if (std_dev > 5.0) {
    // 策略：降低 PID 增益，防止控制器对噪声过度反应
    // 或者：暂停加速，等待抓地力恢复
    Serial.println("⚠️ 速度波动大，疑似打滑，平滑控制...");
    // set_pid_gain(LOW_GAIN);
  } else {
    // 正常行驶
    // set_pid_gain(NORMAL_GAIN);
  }

  Serial.print("StdDev: "); Serial.println(std_dev);
  delay(20);
}

要点解读
多传感器融合的必要性
单一传感器往往存在盲区。例如，仅靠电流检测无法区分是“重载上坡”还是“轮胎卡死”；仅靠 IMU 无法检测低速下的微小打滑。案例5展示了如何结合电流（负载）和倾角（坡度），通过加权计算得出一个综合的“负载因子”，从而实现更精准的自适应。
滑移检测的本质是“不一致性”
案例4和案例6的核心逻辑都是寻找“预期”与“实际”的不一致。
IMU 方案：对比“电机指令加速度”与“IMU 实测加速度”。
统计方案：对比“当前速度”与“历史平均速度”的偏差。
当这种不一致性超过阈值，即判定为物理模型失效（打滑）。
自适应 PID 的双向调节
很多人误以为自适应只是“增大”参数。因为此时增大扭矩只会让轮子转得更快，加剧打滑（正反馈恶性循环）。
统计学方法的延迟问题
案例6使用的标准差算法需要采集一组数据（如 10 个点）才能计算。这意味着系统存在时间窗口延迟。在高速运动的机器人中，这种延迟可能导致反应滞后。优化方案是使用卡尔曼滤波或减小窗口大小，但这会增加计算负担。
硬件在环的实时性挑战
在 Arduino 上同时运行 PID 循环、IMU I2C 通信和复杂的浮点运算（如 sqrt, atan2）是非常消耗资源的。如果主循环 loop() 耗时过长，会导致控制频率下降，系统变得不稳定。建议将滑移检测逻辑放入定时器中断，或者使用性能更强的 MCU（如 ESP32）。

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