Arduino BLDC 之野外巡检机器人——地形分类 + 悬挂模式切换，本质上是“IMU/视觉/触地感知融合识别地形 → 轻量分类器输出地形标签 → 悬挂刚度/阻尼/高度参数切换 → BLDC 位置/力矩闭环执行”的自适应底盘系统：机器人通过加速度计、陀螺仪、超声/ToF、视觉或足端触地信号判断当前处于平整路面、碎石坡地、沟坎或软土等状态，再自动切换悬挂的软硬度、行程和车身高度，使巡检平台在越野移动时兼顾通过性、稳定性和传感器姿态。 它的工程难点不在于单独跑通地形识别或单独驱动悬挂，而在于让“感知频率、分类置信度、悬挂切换平滑性、BLDC 力矩响应”形成稳定闭环，避免机器人在模式切换瞬间发生俯仰、侧倾或冲击。

主要特点

1. 地形分类采用多源特征融合，而非单一传感器判定
   平整路面、碎石路、泥泞地、台阶和斜坡在传感器上呈现不同特征：平整路面 IMU 垂向加速度波动小、车轮转速稳定；碎石路垂向加速度高频成分明显；泥泞地车轮打滑、编码器速度与 IMU 估计速度不一致；台阶或沟坎会出现超声/ToF 距离突变；斜坡则表现为持续俯仰角偏移。 在嵌入式平台上，可采用阈值法、决策树、轻量 SVM 或 TinyML 模型进行分类；若算力受限，也可用“IMU 振动能量 + 车轮滑移率 + 超声高度差”做规则融合。
2. 悬挂模式切换对应不同刚度、阻尼和车身高度
   典型模式可分为：
   巡航模式：悬挂较硬、行程短、重心低，适合平整路面高速移动，减少能量损耗。
   越野模式：悬挂变软、行程增大、阻尼提高，适合碎石、沟坎和起伏地形。
   爬坡/斜坡模式：前后悬挂高度差主动调整，保持机身俯仰角接近水平，防止传感器视场偏移。
   脱困模式：悬挂抬升、扭矩输出增强，用于跨越障碍或从坑洼中脱困。
   BLDC 执行悬挂调节时，可通过位置环控制推杆或丝杠行程，也可通过力矩环模拟可变刚度与阻尼，使悬挂在遇到冲击时像“弹簧-阻尼器”一样吸收能量。
3. BLDC 是悬挂执行层的高带宽载体
   相比传统被动弹簧或普通舵机，BLDC 配合 FOC 可实现更平滑的力矩输出和更精确的位置闭环。 在悬挂系统中，BLDC 可用于驱动电动推杆、丝杠、摇臂或主动减振器；电流环用于限制冲击力，速度环用于控制悬挂伸缩速率，位置环用于维持目标车身高度。若采用 SimpleFOC 等开源方案，可较快实现位置/速度/电流三环控制。
4. 切换必须带迟滞和平滑过渡
   地形分类结果若直接触发悬挂动作，容易出现频繁抖动：机器人在碎石与硬地交界处来回切换，悬挂不断伸缩，导致机身晃动、功耗上升和机械磨损。工程上通常引入状态迟滞、置信度阈值、时间滤波和输出斜率限制。例如连续 3~5 次判定为碎石路才切换至越野模式；切换时悬挂目标高度按斜坡函数渐变，而不是阶跃变化。
5. 分层实时架构更适合嵌入式落地
   推荐分为四层：
   感知层：IMU、超声/ToF、编码器、触地/压力传感器、可选视觉模块。
   分类层：特征提取、地形标签输出、置信度评估。
   决策层：悬挂模式选择、目标高度/刚度/阻尼计算、切换平滑控制。
   执行层：BLDC 悬挂电机位置/力矩闭环、行走电机驱动、安全保护。
   Arduino Uno/Nano 适合做轻量原型或单轴悬挂验证；若要同时运行多路 BLDC 闭环、IMU 滤波和地形分类，更推荐 ESP32-S3、Teensy 4.1 或 STM32，并把视觉与高层分类放到上位机。

应用场景
电力/变电站户外巡检：草地、碎石、电缆沟盖板和不平路面较多，悬挂切换可保持云台相机和红外传感器稳定。
矿山与隧道巡检：巷道地面崎岖、有积水和碎石，主动悬挂可提高车轮贴地性和机身稳定性。
农业与林业监测：田埂、垄沟、泥地和落叶层会导致底盘颠簸，悬挂模式切换可减少传感器抖动。
管道与厂区巡检：门槛、减速带、排水沟和油污路面常见，悬挂可根据障碍高度调整通过策略。
科研与教学平台：用于验证 IMU 地形特征提取、嵌入式分类器、BLDC 主动悬挂控制和模式切换安全逻辑。

需要注意的事项
地形分类不能只靠 IMU
IMU 能反映振动和坡度，但难以区分“路面粗糙”和“机器人自身加速”。应融合车轮编码器、超声/ToF 距离、触地传感器和视觉信息，提高分类可靠性。
悬挂切换频率必须受限
高频切换会让 BLDC 持续启停，造成电流尖峰、机械冲击和电池电压跌落。应设置最小切换间隔、状态迟滞和置信度门槛，避免在边界地形上反复跳变。
BLDC 悬挂执行器需具备位置反馈
若只有开环推杆，很难准确控制车身高度和悬挂行程。推荐使用带编码器、电位器或霍尔位移反馈的执行机构，并配合 FOC 位置环或速度环。
悬挂动作会改变重心和轮载荷
抬升一侧悬挂时，另一侧车轮可能减载甚至离地，进而影响驱动和制动。切换时应同步调整行走电机扭矩分配，并在检测到单侧离地时暂停大幅调平。
Arduino 平台要严格控制控制周期
IMU 融合、地形分类、悬挂 PID 和多路 BLDC 闭环同时运行时，8 位 MCU 容易成为瓶颈。关键闭环应放在硬件定时器中断中，复杂分类可降频运行或交给上位机。
电源隔离和 EMC 必须做好
悬挂 BLDC 和行走 BLDC 同时动作时电流变化剧烈，可能干扰 IMU 和传感器。控制电源与动力电源应隔离，敏感信号线使用屏蔽线，并在电机驱动端加入母线电容。
必须设置失效保护策略
当 IMU 异常、编码器失联、悬挂卡滞或倾角超限时，应自动进入降级模式：停止悬挂切换、降低行驶速度、锁定当前高度或停机。安全优先级应高于地形自适应性能。

1、超声波地面波动分析 + 悬挂刚度切换
通过向下超声波连续采样分析地面波动性，将地形分为平坦/粗糙/松软三类，切换悬挂刚度。

#include <SimpleFOC.h>

// ===== BLDC 驱动轮 =====
BLDCMotor motorL(7), motorR(7);

// ===== 传感器与执行器 =====
#define TRIG_GROUND 12
#define ECHO_GROUND 13
#define SUSPENSION_PWM 9   // 悬挂执行器（推杆/电磁阀）

// ===== 地形类型 =====
enum TerrainType {
  TERRAIN_FLAT,      // 平坦硬质
  TERRAIN_ROUGH,     // 粗糙碎石
  TERRAIN_SOFT       // 松软泥地
};

TerrainType currentTerrain = TERRAIN_FLAT;

// ===== 地面波动检测参数 =====
const int SAMPLE_COUNT = 10;
float samples[SAMPLE_COUNT];
int sampleIdx = 0;
float previousAvg = 0;
const float TOLERANCE = 3.0;   // 波动容忍度 (cm)

// ===== 悬挂模式 =====
const int SUSPENSION_HARD = 0;     // 高刚度
const int SUSPENSION_SOFT = 180;   // 低刚度
const int SUSPENSION_MEDIUM = 90;  // 中等刚度

void setup() {
  Serial.begin(115200);
  pinMode(TRIG_GROUND, OUTPUT);
  pinMode(ECHO_GROUND, INPUT);
  pinMode(SUSPENSION_PWM, OUTPUT);

  // 初始化采样缓冲
  for (int i = 0; i < SAMPLE_COUNT; i++) samples[i] = 15.0;
  previousAvg = 15.0;

  // 电机初始化
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;

  setSuspension(SUSPENSION_MEDIUM);
}

float measureDistance() {
  digitalWrite(TRIG_GROUND, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_GROUND, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_GROUND, LOW);
  long dur = pulseIn(ECHO_GROUND, HIGH, 25000);
  return (dur == 0) ? 999.0 : dur * 0.0343 / 2.0;
}

// 地面波动分析（方差分析）
float analyzeGroundVariance() {
  samples[sampleIdx] = measureDistance();
  sampleIdx = (sampleIdx + 1) % SAMPLE_COUNT;

  float sum = 0;
  for (int i = 0; i < SAMPLE_COUNT; i++) sum += samples[i];
  float avg = sum / SAMPLE_COUNT;

  float change = abs(avg - previousAvg);
  previousAvg = avg;

  return change;
}

void setSuspension(int mode) {
  analogWrite(SUSPENSION_PWM, mode);
}

void classifyTerrain(float variance) {
  if (variance < TOLERANCE) {
    currentTerrain = TERRAIN_FLAT;
    setSuspension(SUSPENSION_HARD);
  } else if (variance < TOLERANCE * 2.5) {
    currentTerrain = TERRAIN_ROUGH;
    setSuspension(SUSPENSION_MEDIUM);
  } else {
    currentTerrain = TERRAIN_SOFT;
    setSuspension(SUSPENSION_SOFT);
  }

  Serial.print("[地形] 波动=");
  Serial.print(variance);
  Serial.print(" 类型=");
  Serial.println(currentTerrain == TERRAIN_FLAT ? "平坦" :
                 currentTerrain == TERRAIN_ROUGH ? "粗糙" : "松软");
}

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

  float variance = analyzeGroundVariance();
  classifyTerrain(variance);

  motorL.move(3.0);
  motorR.move(3.0);

  delay(100);
}

来源：该结构参考了花雕学编程中“地面粗糙度分析+地形分类+悬挂模式切换”的框架，核心思路是通过向下超声波连续采样的方差判断地面波动性 。

2、IMU振动特征分类 + 悬挂阻抗动态调整
利用 IMU 的加速度数据提取振动特征，分类地形后动态调整悬挂的“虚拟刚度-阻尼”参数，而非简单的高低两档切换。

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

MPU6050 imu;

// ===== BLDC =====
BLDCMotor motorL(7), motorR(7);

// ===== 悬挂执行器 =====
#define SUSP_STIFFNESS_PWM 9   // 刚度调节
#define SUSP_DAMPING_PWM 10    // 阻尼调节

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

// ===== 地形参数（振动阈值/刚度/阻尼）=====
struct TerrainParams {
  float vibThreshold;    // 振动强度阈值
  float stiffness;       // 悬挂刚度 (0-255)
  float damping;         // 悬挂阻尼 (0-255)
};

TerrainParams terrainMap[4] = {
  {50.0,  200, 80},   // 硬地：高刚度、低阻尼
  {150.0, 140, 120},  // 碎石：中刚度、中阻尼
  {250.0, 80,  180},  // 泥泞：低刚度、高阻尼
  {350.0, 50,  220}   // 沙地：极低刚度、极高阻尼
};

// ===== 振动检测 =====
float vibrationRMS = 0;
const int VIB_WINDOW = 20;
float vibBuffer[VIB_WINDOW];
int vibIdx = 0;

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

  pinMode(SUSP_STIFFNESS_PWM, OUTPUT);
  pinMode(SUSP_DAMPING_PWM, OUTPUT);

  // 电机初始化从略
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

// 计算振动RMS（均方根）
void updateVibration() {
  int16_t ax, ay, az;
  imu.getAcceleration(&ax, &ay, &az);

  // 垂向加速度变化量
  float az_g = az / 16384.0;
  static float lastAz = 0;
  float deltaAz = abs(az_g - lastAz);
  lastAz = az_g;

  vibBuffer[vibIdx] = deltaAz;
  vibIdx = (vibIdx + 1) % VIB_WINDOW;

  float sumSq = 0;
  for (int i = 0; i < VIB_WINDOW; i++) {
    sumSq += vibBuffer[i] * vibBuffer[i];
  }
  vibrationRMS = sqrt(sumSq / VIB_WINDOW) * 1000;  // 放大便于比较
}

// 基于振动强度分类地形
void classifyTerrain() {
  if (vibrationRMS < terrainMap[0].vibThreshold) {
    currentTerrain = HARD;
  } else if (vibrationRMS < terrainMap[1].vibThreshold) {
    currentTerrain = GRAVEL;
  } else if (vibrationRMS < terrainMap[2].vibThreshold) {
    currentTerrain = MUD;
  } else {
    currentTerrain = SAND;
  }

  // 应用对应的悬挂参数
  analogWrite(SUSP_STIFFNESS_PWM, terrainMap[currentTerrain].stiffness);
  analogWrite(SUSP_DAMPING_PWM, terrainMap[currentTerrain].damping);

  Serial.print("[地形] 振动=");
  Serial.print(vibrationRMS);
  Serial.print(" 类型=");
  Serial.print(currentTerrain);
  Serial.print(" 刚度=");
  Serial.print(terrainMap[currentTerrain].stiffness);
  Serial.print(" 阻尼=");
  Serial.println(terrainMap[currentTerrain].damping);
}

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

  updateVibration();
  classifyTerrain();

  motorL.move(3.0);
  motorR.move(3.0);

  delay(50);
}

来源：该结构参考了 Arduino Blog 上 GRIP（Ground Recognition Intelligence Platform）项目的思路——利用 IMU 检测底盘振动来分类地形，以及学术研究中“振动RMS作为地形分类特征”的方法 。

3、多传感器融合 + 湿滑/不平坦复合地形悬挂决策
融合超声波地面检测、IMU振动和轮速滑移率，覆盖专利中定义的更细分的悬挂硬度区间，处理“不平坦且湿滑”的复合地形。

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

MPU6050 imu;

// ===== BLDC 驱动轮 =====
BLDCMotor motorL(7), motorR(7);

// ===== 悬挂执行器 =====
#define SUSP_PWM 9

// ===== 传感器 =====
#define TRIG_GROUND 12
#define ECHO_GROUND 13
#define VIBRATION_PIN A0

// ===== 悬挂硬度区间（参考专利定义）=====
enum SuspensionHardness {
  HARDNESS_VERY_HIGH = 0,    // 极高（搬运重物时）
  HARDNESS_HIGH = 60,        // 高（硬地快速行驶）
  HARDNESS_MEDIUM_HIGH = 100,// 中高（湿滑硬地）
  HARDNESS_MEDIUM = 140,     // 中（不平坦地面）
  HARDNESS_LOW = 180,        // 低（不平坦且湿滑）
  HARDNESS_VERY_LOW = 220    // 极低（大幅晃动时）
};

// ===== 运行状态 =====
struct RunInfo {
  float groundVariance;      // 地面波动
  float vibrationLevel;      // 振动强度
  float slipRatio;           // 滑移率
  bool isWet;                // 湿滑标志
} info = {0, 0, 0, false};

// 悬挂目标硬度
int targetHardness = HARDNESS_MEDIUM;

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

  pinMode(TRIG_GROUND, OUTPUT);
  pinMode(ECHO_GROUND, INPUT);
  pinMode(SUSP_PWM, OUTPUT);

  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();

  analogWrite(SUSP_PWM, HARDNESS_MEDIUM);
}

float measureGroundVariance() {
  static float prevDist = 15.0;
  digitalWrite(TRIG_GROUND, LOW); delayMicroseconds(2);
  digitalWrite(TRIG_GROUND, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG_GROUND, LOW);
  long dur = pulseIn(ECHO_GROUND, HIGH, 25000);
  float dist = (dur == 0) ? 999.0 : dur * 0.0343 / 2.0;

  float change = abs(dist - prevDist);
  prevDist = dist;
  return change;
}

float readVibration() {
  return analogRead(VIBRATION_PIN);
}

float computeSlipRatio() {
  // 理论速度 vs 实际速度的差异
  float cmdSpeed = 3.0;
  float actualSpeed = (motorL.shaft_velocity + motorR.shaft_velocity) / 2.0;
  if (cmdSpeed < 0.1) return 0;
  return constrain(abs(cmdSpeed - actualSpeed) / cmdSpeed, 0, 1);
}

// 融合决策悬挂硬度（参考专利的区间逻辑）
void decideSuspensionHardness() {
  info.groundVariance = measureGroundVariance();
  info.vibrationLevel = readVibration();
  info.slipRatio = computeSlipRatio();

  bool isUneven = (info.groundVariance > 3.0) || (info.vibrationLevel > 300);
  bool isSlippery = (info.slipRatio > 0.3);
  info.isWet = isSlippery;

  // 专利逻辑：湿滑 → 提高硬度；不平坦 → 降低硬度；两者兼有 → 中间值
  if (isUneven && isSlippery) {
    targetHardness = HARDNESS_LOW;      // 第四区间：不平坦且湿滑
  } else if (isSlippery) {
    targetHardness = HARDNESS_MEDIUM_HIGH;  // 第三区间：湿滑
  } else if (isUneven) {
    targetHardness = HARDNESS_MEDIUM;   // 第二区间：不平坦
  } else {
    targetHardness = HARDNESS_HIGH;     // 第一区间：正常硬地
  }

  // 大幅晃动 → 极低硬度（第五区间）
  if (info.vibrationLevel > 500) {
    targetHardness = HARDNESS_VERY_LOW;
  }

  analogWrite(SUSP_PWM, targetHardness);

  Serial.print("[决策] 波动="); Serial.print(info.groundVariance);
  Serial.print(" 振动="); Serial.print(info.vibrationLevel);
  Serial.print(" 滑移="); Serial.print(info.slipRatio);
  Serial.print(" → 硬度="); Serial.println(targetHardness);
}

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

  decideSuspensionHardness();

  motorL.move(3.0);
  motorR.move(3.0);

  delay(100);
}

来源：该结构参考了专利 CN113320347A 中“根据地面信息和姿态信息调整悬挂硬度区间”的逻辑，覆盖了湿滑、不平坦、两者兼有以及大幅摆动等多种工况 。

要点解读

1. 地形分类的传感器选择决定了“感知维度”
   超声波向下检测地面波动，本质是测量“几何高程变化”；IMU 检测振动，本质是测量“动力学响应”。两者互补：超声波对缓坡和大坑洼敏感，但对松软地面的识别有限；IMU 振动对碎石、草地、沙地的“质感”敏感，但无法区分“前方有坑”和“后方有坑”。野外巡检建议至少融合这两类信号，单一传感器容易误判 。

2. 悬挂切换的“目标”不是简单的硬/软，而是刚度-阻尼的协同变化
   硬地需要高刚度+低阻尼（响应快、吸收小振动）；湿滑地面需要高刚度（保持轮胎压力）；不平坦地面需要低刚度+高阻尼（吸收冲击、防止弹跳）；大幅晃动时则需要极低刚度让车身“浮”起来。案例二和案例三中的参数表体现了这种多维协同，而非单一旋钮的开关
   。

3. 专利中的“硬度区间”逻辑是工程化的宝贵参考
   专利 CN113320347A 定义了一套完整的映射：正常→第一区间，湿滑→第三区间（更高），不平坦→第二区间（更低），不平坦且湿滑→第四区间（介于二和三之间），大幅摆动→第五区间（最低）。这套逻辑的价值在于：它把“定性描述”变成了“可编程的决策表”，每个区间对应一个 PWM 值或执行器位置，在 Arduino 上可以直接用 if-else 实现。

4. Arduino 算力限制下，“查表+微调”比“在线优化”更务实
   学术研究中的地形分类常涉及 SVM、CNN、LSTM 等模型 。但 Arduino Uno/Mega 无法运行这些模型。可行的策略是：离线训练、在线查表。在 PC 上训练一个简单的阈值分类器或小型决策树，把结果固化为 if (vibration > X) terrain = GRAVEL 这样的规则，部署到 Arduino。GRIP 项目用 Edge Impulse 训练后部署到 UNO Q，是这条路径的参考 。

5. 悬挂执行器的“物理实现”决定控制精度上限
   搜索结果显示，野外悬挂有半主动（磁流变阻尼器、空气弹簧）和主动（推杆/直线电机）两条路线 。Arduino 环境下，最简单的方案是用带编码器的直流推杆或PWM 控制的电磁阀调节悬挂高度/阻尼。但需注意：推杆的响应速度远低于电信号，控制周期不能太快，且必须加入位置反馈（编码器或限位开关）才能实现案例一中的“取零点”和行程保护。华北电力大学的自适应悬挂用无刷减速电机驱动，配合“虚拟弹簧阻尼”算法，是 Arduino 平台可借鉴的方向 。

4、振动识别地形+悬挂刚度硬切换
适用场景：野外场景地形类型明确（土路、碎石路、硬质路面），通过振动特征识别地形，切换固定刚度的悬挂模式。

#include <SimpleFOC.h>
#include <Adafruit_MPU6050.h>
Adafruit_MPU6050 mpu;
SimpleFOCMotor2 driveL = SimpleFOCMotor2(2,3,4,A0,A1);  // 左驱动轮
SimpleFOCMotor2 driveR = SimpleFOCMotor2(5,6,7,A2,A3); // 右驱动轮
SimpleFOCMotor2 suspension[4] = {
  SimpleFOCMotor2(8,9,10,A4,A5), // 前左悬挂
  SimpleFOCMotor2(11,12,13,A6,A7), // 前右悬挂
  SimpleFOCMotor2(14,15,16,A8,A9), // 后左悬挂
  SimpleFOCMotor2(17,18,19,A10,A11) // 后右悬挂
};
float vibration_amp = 0; // 振动幅值
enum TERRAIN_TYPE { SOFT, MEDIUM, HARD }; // 软土路、碎石路、硬化路
enum SUSP_MODE { MODE_SOFT, MODE_MEDIUM, MODE_HARD };
SUSP_MODE current_mode = MODE_HARD;

// 振动特征提取：计算加速度标准差
void detect_terrain_by_vibration(){
  sensors_event_t a;
  mpu.getEvent(&a);
  static float acc_buf[16] = {0}, buf_ptr = 0;
  acc_buf[buf_ptr] = sqrtf(a.acceleration.x*a.acceleration.x + a.acceleration.y*a.acceleration.y + a.acceleration.z*a.acceleration.z);
  buf_ptr = (buf_ptr + 1) % 16;
  // 计算标准差作为振动幅值
  float sum = 0, sum2 = 0;
  for(int i=0;i<16;i++){ sum += acc_buf[i]; sum2 += acc_buf[i]*acc_buf[i]; }
  vibration_amp = sqrtf((sum2/16) - (sum/16)*(sum/16));
}

void classify_terrain(){
  if(vibration_amp < 0.8f) current_mode = MODE_SOFT; // 平整软土路
  else if(vibration_amp < 1.8f) current_mode = MODE_MEDIUM; // 碎石路
  else current_mode = MODE_HARD; // 硬化路面/颠簸路
}

void setup(){
  Serial.begin(9600);
  Wire.begin();
  mpu.begin();
  for(int i=0;i<4;i++){ suspension[i].init(); suspension[i].controller=FOC_POSITION; }
  driveL.init(); driveR.init();
  driveL.controller=FOC_VELOCITY; driveR.controller=FOC_VELOCITY;
}

void loop(){
  detect_terrain_by_vibration();
  classify_terrain();
  // 模式切换，硬悬挂支撑行程小，软悬挂行程大
  float target_height = 0;
  if(current_mode == MODE_SOFT) target_height = 8.0f; // 软悬挂：行程大，滤振
  else if(current_mode == MODE_MEDIUM) target_height = 5.0f;
  else target_height = 2.0f; // 硬悬挂：行程小，支撑稳定
  // 四轮悬挂同步切换
  for(int i=0;i<4;i++){
    float error = target_height - suspension[i].shaft_angle;
    float u = 1.0f * error + 0.02f * id(error);
    suspension[i].setVoltage(constrain(u,-6,6));
  }
  // 驱动轮匹配速度：颠簸路减速
  int speed = (current_mode==MODE_HARD)?80:((current_mode==MODE_MEDIUM)?120:150);
  driveL.setVelocity(speed); driveR.setVelocity(speed);
}

5、地形+姿态融合+悬挂动态调节
适用场景：斜坡、连续颠簸、起伏地形，需结合地形振动与机身姿态，动态调节四轮悬挂支撑高度，保证巡检稳定性。

#include <SimpleFOC.h>
#include <Adafruit_MPU6050.h>
Adafruit_MPU6050 mpu;
SimpleFOCMotor2 suspension[4] = {/* 定义同案例1 */};
float terrain_slope_x = 0, terrain_slope_y = 0;
float target_height[4] = {5.0f,5.0f,5.0f,5.0f};
float dynamic_gain = 0.5f;

// 融合振动与姿态，输出地形特征
void fuse_terrain_data(){
  sensors_event_t a;
  mpu.getEvent(&a);
  // 姿态解算：计算俯仰/横滚角
  terrain_slope_x = atan2f(a.acceleration.y, a.acceleration.z) * 180.0f / M_PI;
  terrain_slope_y = atan2f(-a.acceleration.x, a.acceleration.z) * 180.0f / M_PI;
  // 振动幅值同案例1计算，此处简化
  float vib = sqrtf(a.acceleration.x*a.acceleration.x + a.acceleration.y*a.acceleration.y) / 9.8f;
  // 颠簸越严重，增益越高，响应越快
  dynamic_gain = 0.3f + vib * 0.5f;
}

// 规划四轮悬挂目标高度，补偿坡度
void plan_suspension_target(){
  // 上坡时前轮抬高，下坡时后轮抬高，保证机身水平
  target_height[0] = 5.0f + terrain_slope_x * 0.1f; // 前左
  target_height[1] = 5.0f + terrain_slope_x * 0.1f; // 前右
  target_height[2] = 5.0f - terrain_slope_x * 0.1f; // 后左
  target_height[3] = 5.0f - terrain_slope_x * 0.1f; // 后右
  // 横滚补偿
  target_height[0] += terrain_slope_y * 0.05f;
  target_height[2] -= terrain_slope_y * 0.05f;
  // 行程限幅
  for(int i=0;i<4;i++) target_height[i] = constrain(target_height[i], 0.5f, 15.0f);
}

void loop(){
  fuse_terrain_data();
  plan_suspension_target();
  for(int i=0;i<4;i++){
    float error = target_height[i] - suspension[i].shaft_angle;
    float u = dynamic_gain * (1.5f * error + 0.03f * id(error));
    suspension[i].setVoltage(constrain(u,-6,6));
  }
}

6、悬挂故障容错+剩余轴降级调节
适用场景：野外复杂环境，单悬挂/传感器故障时，需用剩余悬挂重新分配支撑力，保证机器人不倾倒、不坠崖。

#include <SimpleFOC.h>
#include <Adafruit_MPU6050.h>
Adafruit_MPU6050 mpu;
SimpleFOCMotor2 suspension[4] = {/* 定义同案例1 */};
bool susp_fault[4] = {false,false,false,false};
float safe_height[4] = {5.0f,5.0f,5.0f,5.0f};
float fault_thresh_current = 14.0f; // 电流故障阈值
float fault_thresh_pos = 16.0f;    // 行程故障阈值

// 悬挂故障检测：电流/行程/温度异常
void detect_suspension_fault(){
  for(int i=0;i<4;i++){
    if(abs(suspension[i].current) > fault_thresh_current || suspension[i].shaft_angle > fault_thresh_pos){
      if(!susp_fault[i]){
        susp_fault[i] = true;
        suspension[i].setVoltage(0); // 故障轴停机
      }
    } else {
      // 连续10次无异常则恢复
      static int safe_cnt[4] = {0};
      safe_cnt[i]++;
      if(safe_cnt[i] > 10) { susp_fault[i] = false; safe_cnt[i]=0; }
    }
  }
}

// 故障后剩余悬挂目标高度重分配
void replan_suspension_target(){
  int fault_cnt = 0, fault_idx = -1;
  for(int i=0;i<4;i++){ if(susp_fault[i]) { fault_cnt++; fault_idx = i;} }
  // 单轴故障：提升对角轴，补偿支撑力
  if(fault_cnt == 1){
    // 前左故障，提升前右30%
    safe_height[0] = 0.0f;
    safe_height[1] = 15.0f; safe_height[2] = safe_height[3] = 5.0f;
  } else if(fault_cnt == 2){
// 交叉故障，剩余轴保持中等高度
    int valid[2], val_cnt=0;
    for(int i=0;i<4;i++) if(!susp_fault[i]) valid[val_cnt++]=i;
    safe_height[valid[0]] = 7.0f; safe_height[valid[1]] = 7.0f;
  } else {
    // 无故障正常规划
    for(int i=0;i<4;i++) safe_height[i] = 5.0f;
  }
  for(int i=0;i<4;i++) safe_height[i] = constrain(safe_height[i],0.5f,15.0f);
}

void loop(){
  detect_suspension_fault();
  replan_suspension_target();
  for(int i=0;i<4;i++){
    if(susp_fault[i]) continue;
    float error = safe_height[i] - suspension[i].shaft_angle;
    float u = 0.8f * error + 0.02f * id(error);
    suspension[i].setVoltage(constrain(u,-6,6));
  }
}

要点解读
地形分类需多特征融合，避免单一传感器误判
仅靠振动幅值容易因机器人自身抖动误判，需融合IMU姿态、轮速差、悬挂行程等多特征：平路振动小+机身水平，碎石路振动大+机身微晃，斜坡振动中等+姿态倾斜，分类准确率可提升40%以上。

悬挂切换需采用软切换（渐变调节），避免硬冲击
禁止瞬间切换目标行程，需设置1-2s的渐变过渡，逐步调整执行器电压，防止悬挂突然伸缩导致机身晃动，损坏搭载的巡检设备或造成图像模糊。

动态调节需匹配运动状态，兼顾滤振与稳定性
颠簸地形需提高增益加快响应，同时增大悬挂行程过滤高频振动；高速行驶时需降低增益避免超调，防止车身随悬挂反复震荡引发侧翻，低速巡检时则优先滤振提升画面质量。

故障容错需遵循重心补偿原则，优先保障不倾倒
单轴故障时需提升对角轴，保证重心落在剩余支撑面内；多轴故障时保持剩余轴同高，避免产生额外力矩；同时需设置故障恢复逻辑，连续无异常后自动重启故障轴，减少人工干预。

需结合驱动控制实现悬挂与行走协同
崎岖地形需降低行走速度，减少悬挂冲击；上坡时悬挂需补偿坡度保持机身水平，同时降低驱动扭矩防止打滑；下坡时需增大后悬挂行程，提升后轮抓地力，保证制动安全。

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