Arduino BLDC 之差速越野机器人“滑移感知 + 自适应扭矩控制”的核心，是把左右轮从“固定速度差速”升级为“基于滑移率反馈的独立扭矩管理”：先通过编码器、IMU 和电机电流估计轮地滑移状态，再动态调节左右轮扭矩输出，使机器人在沙地、碎石、泥泞、斜坡等低附着或附着突变地面上保持牵引力、航向稳定和姿态可控。

主要特点
. 差速驱动 + 扭矩矢量分配
差速底盘通常由左右两个 BLDC 独立驱动，运动指令为线速度 V 和角速度 ω。传统做法是把 V、ω 直接拆成左右轮速度；而扭矩矢量分配则把目标转为左右轮扭矩或 q 轴电流：
左轮扭矩 ≈ 基础扭矩 − 转向补偿扭矩
右轮扭矩 ≈ 基础扭矩 + 转向补偿扭矩
在 SimpleFOC 中，这通常对应 MotionControlType::torque 或电流环控制。扭矩模式比纯速度模式更适合越野场景，因为速度环在打滑时会持续加大输出，反而加剧空转；扭矩模式则可以直接限制轮端输出力。
. 滑移感知：从“转速差”到“滑移率估计”
滑移率描述的是轮子理论线速度与实际车体速度之间的偏差。工程上可写作：
驱动滑移率 s ≈ (v_wheel − v_body) / max(v_wheel, v_body)
其中 v_wheel 来自编码器转速，v_body 来自 IMU 加速度积分、多轮融合或低速下的运动学估计。当左右轮转速差明显增大、电机角加速度异常、电流上升但速度不上升、IMU 横摆率与转向指令不一致时，都可以作为打滑判据。
更稳健的做法不是只看单一阈值，而是结合：
左右轮转速差
轮速加速度变化率
电机电流饱和程度
IMU 横摆角速度与纵向加速度
编码器累计里程与期望轨迹偏差
这样可以减少误判，比如把原地转向、越障抬轮或短暂悬空误判为打滑。
. 自适应扭矩控制：抑制打滑并恢复牵引
一旦检测到某侧轮滑移率升高，控制器应降低该侧扭矩，同时把剩余可用扭矩分配给附着更好的一侧，类似电子限滑差速器逻辑：
打滑侧：快速降扭，避免继续刨地或空转
附着侧：适度增扭，维持前进牵引力
转向侧：根据横摆误差补偿左右扭矩差，防止跑偏
恢复阶段：缓慢增加扭矩，避免再次打滑
如果追求更精细的控制，可以引入最优滑移率跟踪。轮胎或履带与地面之间通常存在一个“最大附着点”，滑移率过小牵引不足，过大则进入空转。自适应控制的目标不是把滑移率压到零，而是让它保持在可牵引区间内。
. 分层实时控制架构
从嵌入式实现角度，建议分为三层：
上层：接收遥控、路径规划或自主导航指令，输出期望线速度和角速度
中层：估计滑移率、判断附着状态，计算左右轮目标扭矩
底层：FOC 电流环执行扭矩指令，并读取编码器、IMU、电流和温度反馈
在 Arduino 生态中，AVR 可以跑轻量差速和简单滑移抑制；若要同时做 FOC、IMU 融合、滑移率估计和扭矩分配，更推荐 ESP32、Teensy 4.x 或 STM32F4/F7。

应用场景
野外巡检与地质探测：草地、碎石、泥地、沙地等地面附着变化剧烈，滑移感知能避免机器人越加速越陷车。
农业与林业移动平台：垄沟、湿土、落叶层容易使单侧轮打滑，自适应扭矩可减少跑偏和土壤扰动。
救援与废墟侦察：瓦砾、斜坡、湿滑地面会导致差速底盘姿态突变，扭矩矢量可辅助保持航向稳定。
矿山、隧道、工地巡检：地面可能有积水、油污、碎石和坡度变化，单侧低附着时传统差速容易失控。
科研与竞赛平台：适合验证滑移率估计、扭矩分配、IMU 融合、FOC 伺服和嵌入式实时控制。

需要注意的事项
必须闭环控制，不能只靠开环 ESC
滑移感知依赖编码器反馈，自适应扭矩依赖电流环或扭矩环。普通航模 ESC 只能做开环速度控制，无法精确估计滑移率，也难以实现稳定的扭矩分配。
车体速度估计是难点
滑移率计算需要知道“车体实际速度”，但越野环境中 GPS 可能不稳定，视觉里程计也可能受光照和扬尘影响。可先用多轮编码器融合、IMU 加速度积分和运动学约束做初步估计，再逐步引入更鲁棒的车速观测器。
扭矩分配要考虑转向与横摆耦合
差速越野机器人打滑时不仅会丢失牵引，还容易出现跑偏、甩尾或原地旋转。扭矩分配不能只抑制空转，还要结合 IMU 横摆角速度修正左右扭矩差，否则机器人可能“不滑了，但方向歪了”。
控制频率和计算资源要匹配
FOC 电流环通常需要较高频率，滑移估计和扭矩分配也应保持毫秒级响应。如果所有算法都挤在 Arduino Uno 上，控制周期会抖动，IMU 滤波和电机闭环都会受影响。建议把电机闭环放在下位 MCU，路径规划和视觉放在上位机。
安全降级比性能更重要
当滑移率持续过高、IMU 数据异常、电机过流、编码器失联或电池欠压时，应进入降扭、限速或停机状态。扭矩变化也要加斜率限制，避免从“全力输出”瞬间切到“完全降扭”，造成机械冲击或姿态突变。

1、轮速偏差 + 电流突变双条件滑移检测（基础感知层）
适用场景：户外巡检、野外勘探等场景，差速底盘在泥泞、碎石地面行驶时，单侧轮因摩擦力不足而空转。本案例通过“轮速偏差 + 电流突变”双条件判断滑移，避免单一指标误判。

/**
 * 差速越野机器人 - 轮速偏差 + 电流突变滑移检测
 * 硬件假设：Arduino Mega, 2×BLDC(SimpleFOC力矩模式), 编码器, 电流传感器
 * 核心：滑移判定需同时满足轮速偏差超阈值 + 电流突变
 */

#include <SimpleFOC.h>

BLDCMotor motorL(7);
BLDCMotor motorR(7);
Encoder encL(2, 4, 2048);
Encoder encR(3, 5, 2048);

// 底盘参数
const float WHEEL_RADIUS = 0.08;  // 轮半径 (m)
const float WHEEL_TRACK = 0.45;   // 轮距 (m)

// 目标参考速度（由上层运动规划给出）
float referenceSpeed = 0.3;  // m/s

// 滑移检测参数
const float SLIP_THRESHOLD = 0.35;      // 轮速偏差阈值 (35%)
const float CURRENT_CHANGE_THRESHOLD = 0.5; // 电流突变阈值 (A)

// 状态变量
float wheelSpeedL = 0, wheelSpeedR = 0;
float currentL = 0, currentR = 0;
float slipL = 0, slipR = 0;
float targetCurrentL = 0.5, targetCurrentR = 0.5;

unsigned long lastRead = 0;

float readWheelSpeed(int side) {
  if (side == 0) return motorL.shaft_velocity * WHEEL_RADIUS;
  else return motorR.shaft_velocity * WHEEL_RADIUS;
}

// 滑移度计算：结合轮速偏差与电流突变
float calculateSlip(int wheelIdx, float currentWheelSpeed, float current) {
  float speedError = referenceSpeed - currentWheelSpeed;
  float currentChange = fabs(current - (wheelIdx == 0 ? targetCurrentL : targetCurrentR));
  
  // 滑移判定：轮速偏差超阈值且电流突变（负载突变）
  if (referenceSpeed > 0.01 && 
      (speedError / referenceSpeed) > SLIP_THRESHOLD && 
      currentChange > CURRENT_CHANGE_THRESHOLD) {
    return fmin(1.0, speedError / referenceSpeed);  // 滑移度归一化
  }
  return 0.0;
}

void setup() {
  Serial.begin(115200);
  
  motorL.linkSensor(&encL);
  motorR.linkSensor(&encR);
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  
  // 力矩控制模式（滑移控制的前提）
  motorL.controller = MotionControlType::torque;
  motorR.controller = MotionControlType::torque;
  
  Serial.println("滑移检测就绪");
}

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();
  
  unsigned long now = millis();
  if (now - lastRead > 50) {  // 20Hz更新
    lastRead = now;
    
    // 读取轮速和电流
    wheelSpeedL = readWheelSpeed(0);
    wheelSpeedR = readWheelSpeed(1);
    
    // 简化：从SimpleFOC获取电流（需配置电流传感器）
    currentL = motorL.current_sp;
    currentR = motorR.current_sp;
    
    // 计算滑移度
    slipL = calculateSlip(0, wheelSpeedL, currentL);
    slipR = calculateSlip(1, wheelSpeedR, currentR);
    
    // 输出诊断
    Serial.print("SpeedL:"); Serial.print(wheelSpeedL);
    Serial.print(" SpeedR:"); Serial.print(wheelSpeedR);
    Serial.print(" SlipL:"); Serial.print(slipL);
    Serial.print(" SlipR:"); Serial.println(slipR);
  }
  
  // 基础扭矩输出
  motorL.move(targetCurrentL);
  motorR.move(targetCurrentR);
  
  delay(10);
}

要点：该代码实现了双条件滑移判定——仅当轮速偏差超过 35% 且 电流发生突变（超过 0.5A）时，才判定为滑移。这避免了以下误判场景：正常加速时轮速短暂偏离参考值（但电流平稳），或单轮经过小石子时电流瞬时波动（但轮速偏差不大）。滑移度归一化到 0~1，为后续扭矩重分配提供量化依据。

2、动态扭矩重分配 + 打滑轮限扭 + 接地轮增益（自适应控制层）
适用场景：碎石、沙地等摩擦力不均地形，单侧轮打滑时自动降低其扭矩、将对侧扭矩增益，维持整体牵引力。这是“感知-决策-执行”闭环的核心执行层。

/**
 * 差速越野机器人 - 动态扭矩重分配
 * 核心：检测到滑移 -> 降低打滑轮扭矩 -> 增益接地轮扭矩
 */

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

BLDCMotor motorL(7);
BLDCMotor motorR(7);

// 滑移检测（沿用案例一的逻辑）
extern float slipL, slipR;

// 扭矩分配参数
float baseCurrent = 0.8;        // 基础扭矩电流 (A)
float slipReductionFactor = 0.3; // 打滑轮扭矩降至 30%
float gripBoostFactor = 1.5;     // 接地轮扭矩增益至 150%
float maxCurrent = 2.0;          // 最大电流限幅

// 目标电流
float targetCurrentL = 0, targetCurrentR = 0;

void computeTorqueAllocation() {
  // 基础分配
  float baseL = baseCurrent;
  float baseR = baseCurrent;
  
  // 滑移响应
  if (slipL > 0.1) {
    // 左轮打滑：降低左轮扭矩，增益右轮
    baseL *= (1.0 - slipL * (1.0 - slipReductionFactor));
    baseR *= gripBoostFactor;
  }
  
  if (slipR > 0.1) {
    // 右轮打滑：降低右轮扭矩，增益左轮
    baseR *= (1.0 - slipR * (1.0 - slipReductionFactor));
    baseL *= gripBoostFactor;
  }
  
  // 双侧同时打滑（如冰面）：整体限扭防飞车
  if (slipL > 0.1 && slipR > 0.1) {
    float avgSlip = (slipL + slipR) / 2.0;
    baseL = baseCurrent * (1.0 - avgSlip * 0.8);
    baseR = baseCurrent * (1.0 - avgSlip * 0.8);
  }
  
  // 限幅
  targetCurrentL = constrain(baseL, 0, maxCurrent);
  targetCurrentR = constrain(baseR, 0, maxCurrent);
}

void setup() {
  Serial.begin(115200);
  // ... 电机初始化（力矩模式）
}

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 滑移检测（简化，实际应调用案例一的检测逻辑）
  // ...
  
  computeTorqueAllocation();
  
  motorL.move(targetCurrentL);
  motorR.move(targetCurrentR);
  
  Serial.print("IqL:"); Serial.print(targetCurrentL);
  Serial.print(" IqR:"); Serial.print(targetCurrentR);
  Serial.print(" SlipL:"); Serial.print(slipL);
  Serial.print(" SlipR:"); Serial.println(slipR);
  
  delay(20);
}

要点：该代码实现了基于滑移度的动态扭矩重分配。核心逻辑：打滑轮扭矩降至 30%（slipReductionFactor），接地轮扭矩增益至 150%（gripBoostFactor），实现“将扭矩从空转轮转移到有抓地力的轮”。双侧同时打滑时进入“整体限扭”模式，防止全机空转飞车。搜索结果指出，这种策略类似于汽车的电子限滑差速器（EDL），是差速越野机器人的核心控制逻辑。

3、多模态地形识别 + 扭矩策略切换（智能决策层）
适用场景：地形在碎石、泥泞、沙地、硬地之间切换时，滑移特性截然不同。本案例通过振动传感器、轮速波动和 IMU 数据识别地形类型，匹配对应的扭矩控制策略。

/**
 * 差速越野机器人 - 多模态地形识别 + 扭矩策略切换
 * 参考：基于轮速波动、滑移率变化、IMU振动数据构建地形识别模型
 * 输出地形类型（碎石、泥泞、沙地、硬地），匹配预设扭矩策略
 */

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

BLDCMotor motorL(7);
BLDCMotor motorR(7);
MPU6050 mpu;

int vibrationPin = A0;  // 振动传感器

// 地形类型
enum TerrainType { HARD, GRAVEL, MUD, SAND };
TerrainType currentTerrain = HARD;

// 地形参数配置 [滑移阈值, 扭矩补偿, 最大扭矩, 最小扭矩]
float terrainParams[4][4] = {
  {0.2, 0.25, 2.5, 0.5},   // 硬地：滑移阈值0.2，扭矩补偿0.25，最大扭矩2.5
  {0.3, 0.35, 2.2, 0.6},   // 碎石：滑移阈值0.3，扭矩补偿0.35
  {0.4, 0.45, 2.0, 0.7},   // 泥泞：滑移阈值0.4，扭矩补偿0.45
  {0.5, 0.55, 1.8, 0.8}    // 沙地：滑移阈值0.5，扭矩补偿0.55
};

// 地形识别参数
float vibrationValue = 0;
float memSlip[2] = {0};
float slipThreshold = 0.35;
float torqueCompensation = 0.25;
float maxTorque = 2.5;

void setup() {
  Serial.begin(115200);
  mpu.initialize();
  
  // 电机初始化（力矩模式）
  motorL.controller = MotionControlType::torque;
  motorR.controller = MotionControlType::torque;
}

// 地形识别：基于振动 + 滑移率变化
TerrainType identifyTerrain() {
  vibrationValue = analogRead(vibrationPin);
  
  // 简化：振动值 + 滑移率记忆判断地形
  if (vibrationValue > 300) return GRAVEL;      // 高振动 -> 碎石
  if (vibrationValue > 150) return MUD;         // 中振动 -> 泥泞
  if (vibrationValue > 80) return SAND;         // 低振动 + 高滑移 -> 沙地
  return HARD;                                   // 低振动 -> 硬地
}

void updateTorqueStrategy() {
  currentTerrain = identifyTerrain();
  
  // 根据地形切换参数
  slipThreshold = terrainParams[currentTerrain][0];
  torqueCompensation = terrainParams[currentTerrain][1];
  maxTorque = terrainParams[currentTerrain][2];
  
  Serial.print("Terrain:"); Serial.print(currentTerrain);
  Serial.print(" Vibration:"); Serial.print(vibrationValue);
  Serial.print(" MaxTorque:"); Serial.println(maxTorque);
}

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 地形识别（低频更新，100ms）
  static unsigned long lastTerrainCheck = 0;
  if (millis() - lastTerrainCheck > 100) {
    lastTerrainCheck = millis();
    updateTorqueStrategy();
  }
  
  // 扭矩输出（使用当前地形策略）
  float targetCurrent = 0.5 * (1.0 + torqueCompensation);
  targetCurrent = constrain(targetCurrent, 0.2, maxTorque);
  
  motorL.move(targetCurrent);
  motorR.move(targetCurrent);
  
  delay(20);
}

要点：该代码实现了地形感知与扭矩策略的联动。不同地形的滑移特性差异巨大：硬地滑移阈值 0.2（稍有偏差即判定滑移），沙地滑移阈值 0.5（需要更大偏差才判定）。振动传感器用于辅助识别地形粗糙度，IMU 可用于检测坡度。搜索结果中的多模态地形识别模型基于轮速波动、滑移率变化、IMU 振动数据构建，输出地形类型后匹配预设扭矩策略。

要点解读

1. 滑移检测不能只看轮速偏差，必须结合电流/扭矩信息。 轮速偏差法在以下场景会误判：机器人正常加速时，驱动轮转速短暂低于参考值（因为电机需要克服惯性）；下坡时轮速高于参考值（重力加速）。引入电流突变作为第二判据，可以有效区分“真实打滑”（电流因负载消失而突降）和“正常动态”（电流平稳变化）。搜索结果中的滑移检测机理明确列出了“转速偏差法 + 电流突变法 + IMU佐证”的多源融合策略。

2. 扭矩重分配的核心是“转移”而非“削减”，但必须设置下限。 打滑轮扭矩降至 30% 是“削减”，对侧轮扭矩增益至 150% 是“转移”。但如果只削减不转移，机器人整体牵引力会下降。关键工程细节：接地轮扭矩增益不能无限大，必须受 maxCurrent 限幅约束，否则可能烧毁电机或驱动器。搜索结果强调“分配器输出常加 T = constrain(T, -Tmax, Tmax)”。

3. 双侧同时打滑是最危险的场景，必须进入“整体限扭”模式。 冰面、湿滑瓷砖等场景下，两侧轮可能同时失去抓地力。如果继续输出大扭矩，电机空转飞车，机器人可能因反扭矩而翻滚。搜索结果指出：“若全部轮均近似同等打滑（如冰面）→ 整体限扭 + 降速，防全机空转飞车”。这是安全底线。

4. 滑移控制的底层前提是 FOC 力矩模式，而非速度模式。 速度模式下，控制器会不断增大电流试图达到目标速度，打滑时电流飙升，反而加剧空转。力矩模式下，控制器直接指定输出力矩，打滑时电流不会失控，为滑移检测和扭矩重分配提供了可控的执行环境。搜索结果明确指出“要在 FOC 扭矩/电流环下运行（如 SimpleFOC 的 MotionControlType::torque）”。

5. 地形识别与扭矩策略的联动是“分级响应”的高级形态。 硬地滑移阈值 0.2、沙地 0.5，这不是简单的参数调整，而是对不同地形物理特性的建模。沙地的低摩擦意味着“允许更大的轮速偏差而不判定为异常”，否则机器人会在正常沙地行驶中不断触发滑移响应。搜索结果中的多模态地形识别模型（基于轮速波动、滑移率变化、IMU振动）为这种联动提供了框架。

4、通用越野滑移感知+基础自适应扭矩程序（核心：滑移检测+对称扭矩补偿）
适用场景：草地、碎石路、砂石路面等常规越野场景，解决起步打滑、转向滑移问题，提升行驶稳定性。

核心逻辑
通过编码器与轮速换算，对比左右轮理论与实际转速，识别滑移状态
检测到滑移时，对称提升双轮扭矩，同时微调转速平衡，减少动力流失
平滑过渡扭矩调整，避免瞬间动力突变加剧滑移

// 通用越野滑移感知+自适应扭矩控制程序
// 硬件：双BLDC电机、双编码器、电流采样、PWM驱动
// 功能：滑移检测、扭矩自适应调整、稳定行驶

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte PWM_LEFT = 9, DIR_LEFT = 8;
const byte PWM_RIGHT = 10, DIR_RIGHT = 11;
const byte ENC_LEFT_A = 2, ENC_LEFT_B = 3;   // 左编码器中断
const byte ENC_RIGHT_A = 4, ENC_RIGHT_B = 5; // 右编码器中断
const byte CURRENT_PIN = A0;  // 电流采样

// ========== 滑移与扭矩变量 ==========
long encLeft = 0, encRight = 0;
long lastEncLeft = 0, lastEncRight = 0;
int targetLeft = 120, targetRight = 120;
int baseTorque = 100;
float slipThreshold = 20;  // 滑移判定阈值
bool slipActive = false;
unsigned long lastTime = 0;

// ========== 函数声明 ==========
void readEncoders(unsigned long dt);
float calculateSlip();
void adjustTorque(float slip);
void driveMotors(int leftPWM, int rightPWM);

void setup() {
  Serial.begin(115200);
  Serial.println("通用越野自适应扭矩系统启动");

  pinMode(PWM_LEFT, OUTPUT); pinMode(DIR_LEFT, OUTPUT);
  pinMode(PWM_RIGHT, OUTPUT); pinMode(DIR_RIGHT, OUTPUT);
   pinMode(CURRENT_PIN, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENC_LEFT_A), [](){ if(digitalRead(ENC_LEFT_A)) encLeft++; }, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC_RIGHT_A), [](){ if(digitalRead(ENC_RIGHT_A)) encRight++; }, RISING);
}

void loop() {
  unsigned long dt = millis() - lastTime;
  if (dt < 20) return;
  lastTime = millis();

  // 读取编码器，计算实际轮速
  readEncoders(dt);
  int actualLeft = (encLeft - lastEncLeft);
  int actualRight = (encRight - lastEncRight);

  // 计算滑移率
  float slip = calculateSlip();
  bool slipActive = slip > slipThreshold;

  // 自适应调整扭矩
  int currentTorque = baseTorque;
  if (slipActive) {
    currentTorque += 40; // 打滑时提升扭矩
  }
  int leftPWM = targetLeft + currentTorque;
  int rightPWM = targetRight + currentTorque);

  // 差速微调，平衡左右轮动力
  if (slipActive) {
    leftPWM = constrain(leftPWM, 0, 200);
    rightPWM = constrain(rightPWM, 0, 200);
  }

  driveMotors(leftPWM, rightPWM);
  lastEncLeft = encLeft;  lastEncRight = encRight;
  Serial.print("滑移率：");Serial.print(slip, 1);
  Serial.print(" 扭矩：");Serial.println(currentTorque);
  delay(20);
}

// 读取编码器，换算轮速
void readEncoders(unsigned long dt) {}

// 计算滑移率（左右轮差异判定）
float calculateSlip() {
  int actualLeft = (encLeft - lastEncLeft);
  int actualRight = (encRight - lastEncRight);
  float avgSpeed = (actualLeft + actualRight) / 2.0;
  if (avgSpeed == 0) return 0;
  return abs(actualLeft - actualRight) / avgSpeed * 100.0;
}

// 自适应扭矩调整
void adjustTorque(float slip) {}

// 驱动电机
void driveMotors(int leftPWM, int rightPWM) {
  leftPWM = constrain(leftPWM, 0, 200);
  rightPWM = constrain(rightPWM, 0, 200);
  digitalWrite(DIR_LEFT, HIGH);  digitalWrite(DIR_RIGHT, HIGH);
  analogWrite(PWM_LEFT, leftPWM);  analogWrite(PWM_RIGHT, rightPWM);
}

适用场景优化
加入坡度传感器，上下坡时提前提升基础扭矩，减少打滑风险
采用电流采样作为滑移辅助判据，结合编码器双重检测，提升识别精度
优化扭矩提升幅度，设置滑移率区间，分级调整扭矩，避免动力过剩

5、陡坡攀爬滑移感知+动态扭矩分配程序（核心：爬坡抗滑移+定向补偿）
适用场景：山地、陡坡越野，重点解决上坡打滑、动力不足问题，保障攀爬稳定性。

核心逻辑
通过倾角传感器识别坡度，结合轮速差异识别攀爬滑移状态
针对攀爬滑移，非均匀分配扭矩，增大抓地侧动力，平衡攀爬姿态
自适应调整扭矩输出，应对坡度变化与砂石路面打滑，防止溜坡

// 陡坡攀爬滑移感知+动态扭矩控制程序
// 硬件：双BLDC、编码器、MPU6050倾角、电流采样
// 功能：坡度识别、攀爬滑移检测、动态扭矩分配

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte PWM[2] = {9,10}, DIR[2]={8,11};
const byte ENC[2]={2,4};
const byte IMU_SDA=6, IMU_SCL=7;
const byte CURRENT_PIN=A0;

// 步数阈值：底盘上下相差不得超过20步
#define HARDNESS_SLOPE_DEG 20;
#define SLIP_DEG 5

// ========== 变量 ==========
long enc[2]={0,0}, lastEnc[2]={0,0};
int target[2]={80,80};
int baseTorque = 120;
int slope = 0;itoa(enc[i], last[i], 10);

// ========== 函数声明 ==========
int getSlope();
float getClimbSlip();
void dynamicTorque(int slope, float slip);
void driveMotor(uint8_t idx, int pwm);

void setup() {
  Serial.begin(115200);
  Serial.println("陡坡攀爬自适应系统启动");

  for(uint8_t i=0;i<2;i++) {
pinMode(PWM[i],OUTPUT); pinMode(DIR[i],OUTPUT);
  attachInterrupt(digitalPinToInterrupt(ENC[i]), [i](){ enc[i]++; }, RISING);
  }
  Wire.begin();
}

void loop() {
  unsigned long dt=millis();
  // 读取坡度与轮速
  slope = getSlope();
  long actual[2];
  for(uint8_t i=0;i<2;i++) actual[i] = enc[i]-lastEnc[i];
  float slip = getClimbSlip()), 50f);
    slip = slip > 18 ? 18 : slip;

  // 动态扭矩分配
  int lTorque, rTorque;
  if (slip > 5) {
    // 滑移时增大对应侧扭矩
    if (abs(actual[0]) > abs(actual[1])) { lTorque = baseTorque + 20; rTorque = baseTorque; }
    else { rTorque = baseTorque + 20; lTorque = baseTorque; }
  } else {
    lTorque = rTorque = baseTorque;
  }

  driveMotor(0, target[0] + lTorque);
  driveMotor(1, target[1] + rTorque);

  lastEnc[0]=enc[0]; lastEnc[1]=enc[1];
  Serial.print("坡度：");Serial.print(slope);
  Serial.print(" 滑移率：");Serial.println(slip,1);
  delay(25);
}

// 读取坡度
int getSlope() {
  // 模拟MPU6050读取坡度（实际需解析IMU数据）
  return 25; // 模拟25度坡度
}

// 攀爬滑移检测，结合轮速的转矩差
float getClimbSlip() {
  long actualLeft = enc[0]-lastEnc[0];
  long actualRight = enc[1]-lastEnc[1];
  float avg = (actualLeft + actualRight)/2.0;
  return avg == 0 ? 0 : abs(actualLeft-actualRight)/avg*100.0;
}

// 动态扭矩调整逻辑
void dynamicTorque(int slope, float slip) {}
// 驱动电机
void driveMotor(uint8_t idx, int pwm) {
  pwm = constrain(pwm,0,200);
  digitalWrite(DIR[idx], HIGH);
  analogWrite(PWM[idx], pwm);
}

适用场景优化
加入爬坡进度检测，越接近陡坡顶端，动态提升扭矩，保障冲顶成功
优化扭矩分配策略，根据坡度大小调整扭矩差额，坡度越陡补偿越大
加入溜坡检测，出现倒滑时紧急提升扭矩并锁止方向，防止倒退

6、泥泞脱困滑移感知+强力自适应扭矩程序（核心：脱困打滑抑制+极限拓矩）
适用场景：泥泞、沼泽、湿滑松软路面，机器人易陷入打滑，需快速检测并强力脱困。

核心逻辑
融合轮速、电机电流双重特征，精准识别泥泞深度打滑状态
打滑严重时，极限提升扭矩，同时交替差速输出，破坏粘附力实现脱困
脱困后平滑恢复常规扭矩，避免动力突变导致二次打滑

// 泥泞脱困滑移感知+强力扭矩控制程序
// 硬件：双BLDC、编码器、电流采样、倾角传感器
// 功能：泥泞打滑检测、强力脱困、防滑移控制

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte PWM[2]={9,10}, DIR[2]={8,11};
const byte ENC[2]={2,4};
const byte CURRENT_PIN=A0;
const byte IMU_PIN=A4;

// ========== 变量 ==========
long enc[2]={0,0}, lastEnc[2]={0,0};
int baseTorque=140;
int escapeTorque=200;
bool mudSlip=false;
unsigned long slipTimer=0;

// ========== 函数声明 ==========
bool detectMudSlip();
void escapeControl();
void driveMotor(uint8_t idx, int pwm);

void setup() {
  Serial.begin(115200);
  Serial.println("泥泞脱困自适应系统启动");

  for(uint8_t i=0;i<2;i++) {
pinMode(PWM[i], OUTPUT);    pinMode(DIR[i], OUTPUT);
  attachInterrupt(digitalPinToInterrupt(ENC[i]), [i](){ enc[i]++; }, RISING);
  }
  pinMode(CURRENT_PIN, INPUT);
}

void loop() {
  unsigned long dt=millis();
  // 持续检测泥泞滑移
  mudSlip = detectMudSlip();

  if (mudSlip) {
    // 脱困控制，交替强扭矩
    slipTimer += dt;
    if (slipTimer > 300) {
      escapeControl();
      slipTimer = 0;
    }
  } else {
    // 脱困成功，恢复常规扭矩
    driveMotor(0, baseTorque);
    driveMotor(1, baseTorque);
  }

  lastEnc[0]=enc[0]; lastEnc[1]=enc[1];
  Serial.print("泥泞打滑：");Serial.print(mudSlip);
  delay(20);
}

// 泥泞滑移检测，轮速低+电流大
bool detectMudSlip() {
  int actualLeft = enc[0]-lastEnc[0];
  int actualRight = enc[1]-lastEnc[1]; int currentVal = analogRead(CURRENT_PIN);
  // 轮速极低且电流超过阈值，判定泥泞打滑
  return (actualLeft < 5 && actualRight < 5) && currentVal > 700;
}

// 脱困控制，交替强扭矩破坏粘附
void escapeControl() {
  // 左强右弱，交替输出，模拟差速脱困
  driveMotor(0, escapeTorque);
  driveMotor(1, baseTorque);
  delay(150);
  driveMotor(0, baseTorque);
  driveMotor(1, escapeTorque);
}

// 驱动电机
void driveMotor(uint8_t idx, int pwm) {
  pwm = constrain(pwm, 0, 200);
  digitalWrite(DIR[idx], HIGH);
  analogWrite(PWM[idx], pwm);
}

适用场景优化
加入泥泞厚度识别，不同深度匹配不同扭矩强度与交替频率
优化脱困策略，结合前后摆动、差速组合，提升脱困成功率
设置脱困上限，超时未脱困自动停机，防止电机过载烧毁

要点解读
要点1：滑移感知需多参数融合，精准识别工况
单一参数无法准确判定滑移，多源数据融合是自适应控制的前提。
编码器+电流双检测：编码器测轮速，判断转速异常；电流采样测负载，识别电机过载，两者结合区分空转、打滑、负载过大
地形特征匹配：结合倾角、路面材质（碎石/泥泞）预设滑移阈值，提升不同地形的识别精度
动态阈值调整：根据行驶速度、负载大小动态调整滑移判定阈值，避免误判或漏判
要点2：自适应扭矩需分级调整，兼顾动力与安全
扭矩调整不能盲目增大，需分级平滑控制，避免动力过剩加剧滑移。
分级扭矩策略：按滑移程度分为轻微、中等、严重，对应小幅补偿、中幅提升、极限输出，精准匹配需求
平滑过渡控制：扭矩变化采用渐变算法，避免瞬间突增导致车轮空转或车身顿挫
扭矩上限保护：设置电机最大扭矩阈值，防止过载烧毁，保障设备安全
要点3：差速协同分配，提升越野通过性
差速控制是越野机器人脱困、转向的核心，需结合滑移状态动态分配。
非对称扭矩分配：滑移时重点补偿抓地力弱的一侧，平衡左右轮动力，减少空转损耗
交替动态输出：泥泞、深坑场景，采用左右交替强扭矩，破坏路面粘附力，快速脱困
转向自适应：差速转向时同步调整扭矩，避免转向滑移失控，提升越野机动性
要点4：闭环反馈控制，保障行驶稳定性
自适应控制需实时反馈、动态修正，抵消地形、负载的动态干扰。
实时轮速闭环：持续采集编码器数据，实时修正扭矩输出，保证实际速度贴合目标值
滑移动态修正：滑移状态变化时，快速调整扭矩分配，避免持续打滑导致陷车
急停保护联动：出现严重打滑、电机过热等异常，及时降速或停机，防止事故扩大
要点5：场景化适配优化，强化特殊地形能力
不同越野地形的滑移特性不同，需针对场景定制控制策略。
场景专属策略：针对陡坡、泥泞、碎石等地形，定制差异化的滑移检测与扭矩控制逻辑
自适应学习优化：记录不同地形的成功控制参数，优化扭矩调整规律，提升后续适配性
应急脱困模式：内置多种脱困算法（差速、摆动、强扭矩组合），应对极端陷车场景

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