Arduino BLDC之野外巡检自适应地形四轮独立调平机器人的核心在于构建“多源姿态感知 → 独立悬挂主动调节 → 四轮力矩动态分配 → BLDC精准执行”的完整闭环，将传统被动式减震升级为主动式底盘调平，在算力受限的嵌入式平台上实现野外复杂地形下的高稳定性、高通过性巡检作业。 该方案解决了野外环境中地面不平导致传感器抖动、机身倾斜影响检测精度、单轮悬空打滑导致动力丢失等核心痛点。以下从主要特点、应用场景及注意事项三个维度进行专业解析。

主要特点

1. 多源姿态感知层：从“盲走”到“全向感知”
   这是系统的“平衡觉”，负责实时监测机身姿态和地形起伏，为独立调平提供决策依据。
   IMU主导的姿态解算：采用MPU6050/ICM-20948等六轴/九轴IMU，通过互补滤波或扩展卡尔曼滤波（EKF）融合陀螺仪的高频动态响应与加速度计的低频重力参考，实时解算出机身的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。融合算法有效抑制陀螺仪积分漂移，同时克服纯加速度计在加减速时的动态误差。
   四轮独立位移/力反馈：在每个车轮的悬挂机构上安装电位计或霍尔位移传感器，实时监测各轮的压缩量和悬挂行程；进阶方案中采用薄膜压力传感器或应变片直接测量各轮接地压力，为力矩分配提供精确输入。
   前瞻地形扫描：通过前置超声波阵列或单线激光雷达扫描前方路面轮廓，提前识别坑洼、石块、斜坡等障碍，为主动调平预留反应时间（通常≥0.3秒）。
2. 独立悬挂与主动调平机构
   这是系统的“四肢”，通过机械结构+电机驱动实现每个车轮的独立高度调节。
   主动悬挂架构：区别于传统被动弹簧减震，每个车轮配备独立的直线推杆电机或旋转舵机+连杆机构，可主动调节车轮相对于底盘的高度。架构分为两类：
   并联式：每个车轮独立驱动，互不干扰，控制简单但结构复杂。
   串联摇臂式（H型平衡摇臂）：通过摇臂机构耦合对角车轮，机械上自动平衡两侧高度差，减少控制自由度，适合低速巡检场景。
   调平控制策略：基于IMU反馈的Pitch/Roll角，通过PID控制器计算每个车轮的目标高度修正量。例如，当检测到机身右倾（Roll>0）时，右侧车轮主动下降、左侧车轮主动上升，直至Roll角回归零位。控制周期通常≥50Hz，确保快速响应地形变化。
   被动减震辅助：在主动悬挂基础上保留弹簧或橡胶缓冲元件，吸收高频路面震动，降低主动电机的负载和功耗。
3. 四轮力矩动态分配
   这是系统的“动力中枢”，根据各轮接地状态和附着力智能分配驱动力，避免打滑和动力浪费。
   附着率估计：通过编码器反馈的各轮转速与IMU推算的机身速度对比，实时估计每个车轮的滑移率。滑移率过大表明附着力不足（如泥泞、沙地），需降低该轮扭矩。
   加权最小二乘分配：基于各轮接地压力（或滑移率倒数）作为权重，通过二次规划实时求解最优力矩分配方案。例如，当左前轮悬空时，自动将驱动力转移至其他三轮；当爬坡时，增加后轮扭矩比例、减少前轮扭矩，防止翻车。
   差速转向协同：在转向过程中，内侧车轮减速、外侧车轮加速，同时结合调平机构补偿离心力导致的机身倾斜，确保转弯稳定性。
4. BLDC高动态执行与FOC力控
   这是系统的“肌肉”，负责将调平和驱动指令转化为精准、柔顺的电机动作。
   FOC磁场定向控制：BLDC电机配合FOC算法实现电流环（转矩控制）、速度环、位置环的三环闭环。FOC消除换相阶跃，转矩纹波降低70%以上，确保机器人在低速巡检和精准调平时无抖动。
   阻抗控制与冲击吸收：当车轮触地或遭遇冲击时，系统通过阻抗控制使悬挂像“弹簧-阻尼器”一样吸收能量，避免刚性结构损坏。阻抗参数（刚度、阻尼）可根据地形动态调整：硬地面提高刚度提升响应速度，软地面降低刚度增加柔顺性。
   再生制动与能量回收：在下坡或减速工况下，BLDC电机进入发电模式，将动能回馈至电池，不仅提升能源利用率，也为紧急制动提供冗余安全保障。

应用场景
该系统凭借其高稳定性和全地形适应性，主要应用于以下场景：
电力变电站/输电线路巡检：变电站户外环境包含草地、碎石、电缆沟盖板等复杂地形，且地面可能不平整。四轮独立调平确保搭载的红外热像仪、可见光相机始终保持水平，拍摄清晰的设备图像；力矩动态分配防止在松软地面打滑，确保巡检任务连续执行。
石油/化工园区管道巡检：化工园区地面可能包含油污、积水、砂石，且存在防爆要求。BLDC电机无火花特性满足防爆标准；独立调平确保气体传感器、摄像头稳定工作，准确检测泄漏和异常。
矿山/隧道内部探测：矿山巷道和隧道内部地面崎岖不平，可能存在积水和碎石。四轮独立调平确保机器人不侧翻、不卡死；力矩动态分配在湿滑地面自动降低打滑轮扭矩，提升通过性。
农业温室/农田监测：农田垄沟、温室大棚内地面为土壤或地膜，软硬不均且存在坡度。独立调平确保搭载的多光谱相机、土壤传感器保持正确姿态，采集准确数据；BLDC电机的高扭矩使其能轻松穿越泥泞地面。
高校教育与科研验证平台：作为机器人学、自动控制、车辆工程课程的实验平台，学生可以通过该项目深入理解悬挂系统动力学、姿态控制、力矩分配、FOC控制等核心概念，是学习移动机器人底盘设计的理想载体。

需要注意的事项
在实现该系统时，需重点关注以下技术细节与工程红线：

1. MCU选型与分层架构（首要挑战）
   算力瓶颈：标准Arduino Uno（ATmega328P, 16MHz）无法独立运行IMU融合+调平PID+力矩分配+FOC。强烈建议采用“上位机+下位机”分层架构：上位机（如Raspberry Pi 4B/5、Jetson Nano）运行ROS2，负责SLAM建图、路径规划、地形识别等复杂计算；下位机（如ESP32-S3、STM32F4/F7、Teensy 4.1）作为底层控制器，负责BLDC电机FOC控制、IMU数据读取、悬挂调平PID及紧急保护逻辑执行。
   控制周期：FOC电流环需≥1kHz（建议10-20kHz），悬挂调平PID更新频率≥100Hz，力矩分配频率≥50Hz，IMU融合频率≥200Hz。所有控制回路必须使用硬件定时器中断，严禁使用delay()函数。
   通信延迟：上下位机之间通过串口或CAN总线通信，控制指令传输延迟应<10ms，否则会导致调平响应迟缓、机身晃动。
2. 机械结构设计与悬挂选型
   悬挂行程与刚度：悬挂行程需根据预期地形起伏确定，通常取50-150mm。行程过短无法适应大坑洼，过长则增加结构重量和控制难度。弹簧刚度需通过仿真或实验确定，确保在满载情况下仍有足够的调节余量。
   推杆电机选型：主动悬挂推杆电机需具备高扭矩、高响应速度和位置反馈功能。推荐使用带编码器的直流推杆电机或无刷直线电机，推力需≥50N（根据机器人重量计算）。
   防护等级：野外环境存在雨水、灰尘、泥浆，所有运动部件（推杆、连杆、轴承）需达到IP65以上防护等级，防止异物侵入导致卡死或磨损。
3. IMU安装与姿态解算精度
   安装位置：IMU必须刚性固定在底盘重心附近，并加硅胶减震垫以隔离电机和路面高频振动。安装前需进行零偏校准和轴对齐，否则姿态解算会出现系统性误差。
   滤波算法调参：互补滤波的融合系数α或EKF的过程噪声/测量噪声协方差矩阵需通过实验精细标定。α过大导致姿态响应迟缓，α过小导致噪声放大。建议通过串口实时输出原始数据、融合角度和控制输出，在上位机绘制曲线辅助调参。
   磁干扰补偿：若使用九轴IMU（含磁力计），需注意电机和动力线产生的磁场干扰。建议将磁力计远离电机安装，或在软件中进行硬铁/软铁补偿。
4. 力矩分配算法的稳定性
   权重系数标定：力矩分配中的权重系数（如接地压力、滑移率倒数）需通过实验标定，确保在不同地形下都能合理分配扭矩。权重系数不合理会导致某些车轮过载打滑，其他车轮动力浪费。
   分配平滑性：力矩分配结果需经过限幅和斜率限制，避免扭矩突变导致机身冲击。建议采用一阶低通滤波器平滑分配结果，截止频率取5-10Hz。
   失效保护：当某个车轮传感器失效（如编码器断线）时，系统应自动将该轮扭矩降为零，并将驱动力重新分配至其他车轮，确保机器人不会失控。
5. 严格的电源管理与电磁兼容（EMC）
   隔离供电：BLDC电机在启停和调平过程中会产生极大的电流冲击和高频电磁噪声，极易导致主控复位或IMU数据跳变。必须采用隔离DC-DC模块为控制电路独立供电，并在电机驱动端并联大容量低ESR电容（1000μF-4700μF）吸收反电动势。
   信号屏蔽：IMU、编码器、位移传感器等敏感信号线需使用屏蔽双绞线，并远离动力线布线，PCB布局时注意AGND与DGND单点连接。
   电池选型与BMS：推荐24V/36V高倍率锂电池组，容量≥10Ah，并配备电池管理系统（BMS）防止过充过放。野外巡检需支持快充或热插拔双电池方案，减少待机时间。
6. 安全冗余与失效保护
   多层保护机制：系统需具备堵转保护、缺相保护、过流保护、过温保护、欠压保护、倾角超限保护等多重安全机制。故障发生时，应立即停机并记录故障码。
   硬件急停：必须设置独立的硬件急停电路（如物理急停按钮），采用中断级处理，可强切电机使能（EN），确保在系统失控时能瞬间切断动力。
   翻车检测与自救：当IMU检测到机身倾角超过安全阈值（如45°）时，判定为翻车风险，立即停机并发送报警。进阶方案中，可设计自主翻身功能，通过悬挂机构协同运动将机身恢复至正常姿态。

1、基于IMU姿态反馈的4Z轴独立调平（丝杠/直线执行器方案）
适用场景：野外巡检机器人需要在斜坡或不平地面上保持机身水平，以确保传感器（相机、激光雷达）的视场稳定。采用四角独立丝杠或直线执行器，通过IMU的pitch/roll反馈控制各轴升降，实现“最低点跟踪”调平。

/**
 * 野外巡检机器人 - 四轴独立调平（IMU + 丝杠/直线执行器）
 * 硬件假设：Arduino Mega, MPU6050, 4路直线执行器(通过H桥驱动), BLDC底盘驱动
 * 核心：IMU姿态 -> 四轴高度解算 -> 独立PID -> 执行器输出
 * 参考：RM2026四轴底座调平算法[citation:2], 丘陵农机四轮自适应底盘[citation:7][citation:18]
 */

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

MPU6050 imu;

// 直线执行器控制引脚（每个轴：方向+使能/PWM）
const int ACTUATOR_DIR[4] = {2, 4, 7, 8};   // FL, FR, BL, BR
const int ACTUATOR_PWM[4] = {3, 5, 9, 10};

// 底盘几何参数 (cm)
const float WHEELBASE = 40.0;   // 前后轴距
const float TRACK_WIDTH = 30.0; // 左右轮距

// 调平目标
float targetPitch = 0.0;  // 目标俯仰角 (度)
float targetRoll = 0.0;   // 目标横滚角 (度)

// PID参数（简化位置式）
float Kp = 8.0, Ki = 0.5, Kd = 2.0;
float integral[4] = {0, 0, 0, 0};
float lastError[4] = {0, 0, 0, 0};

// 执行器限位
const int PWM_MIN = 0;
const int PWM_MAX = 255;

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++) {
    pinMode(ACTUATOR_DIR[i], OUTPUT);
    pinMode(ACTUATOR_PWM[i], OUTPUT);
    analogWrite(ACTUATOR_PWM[i], 0);
  }
}

// 读取IMU姿态角（简化互补滤波）
void readAttitude(float &pitch, float &roll) {
  int16_t ax, ay, az, gx, gy, gz;
  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  static float pitchEst = 0, rollEst = 0;
  const float alpha = 0.98;

  float accPitch = atan2(-ax, sqrt((long)ay*ay + (long)az*az)) * 180.0 / PI;
  float accRoll  = atan2(ay, az) * 180.0 / PI;

  // 陀螺积分（简化，仅示意）
  float dt = 0.01;
  pitchEst = alpha * (pitchEst + gx / 131.0 * dt) + (1 - alpha) * accPitch;
  rollEst  = alpha * (rollEst  + gy / 131.0 * dt) + (1 - alpha) * accRoll;

  pitch = pitchEst;
  roll  = rollEst;
}

// 根据目标姿态计算四轴目标高度（最低点跟踪策略）
// 参考：丘陵农机四轮自适应底盘采用"跟踪最低固定点平面"策略[citation:7][citation:18]
void computeTargetHeights(float pitch, float roll, float targetH[4]) {
  // 将pitch/roll偏差映射为四角高度差
  // 简化模型：前左 + 前右 + 后左 + 后右
  // 俯仰偏差影响前后，横滚偏差影响左右
  float pitchOffset = (pitch - targetPitch) * (WHEELBASE / 2.0) * PI / 180.0;
  float rollOffset  = (roll - targetRoll) * (TRACK_WIDTH / 2.0) * PI / 180.0;

  // 四角高度目标（相对基准）
  // FL: +pitchOffset + rollOffset, FR: +pitchOffset - rollOffset
  // BL: -pitchOffset + rollOffset, BR: -pitchOffset - rollOffset
  targetH[0] =  pitchOffset + rollOffset;  // 前左
  targetH[1] =  pitchOffset - rollOffset;  // 前右
  targetH[2] = -pitchOffset + rollOffset;  // 后左
  targetH[3] = -pitchOffset - rollOffset;  // 后右
}

// 简化：用执行器PWM占空比近似高度控制（实际需位置反馈）
void setActuator(int axis, float control) {
  control = constrain(control, -1.0, 1.0);
  digitalWrite(ACTUATOR_DIR[axis], control >= 0 ? HIGH : LOW);
  int pwm = (int)(abs(control) * PWM_MAX);
  analogWrite(ACTUATOR_PWM[axis], pwm);
}

void loop() {
  float pitch, roll;
  readAttitude(pitch, roll);

  float targetH[4];
  computeTargetHeights(pitch, roll, targetH);

  // 独立PID控制各轴
  for (int i = 0; i < 4; i++) {
    float error = targetH[i];  // 目标高度偏差（简化：当前位置视为0）

    integral[i] += error * 0.01;
    integral[i] = constrain(integral[i], -50, 50);

    float derivative = (error - lastError[i]) / 0.01;
    float output = Kp * error + Ki * integral[i] + Kd * derivative;
    lastError[i] = error;

    setActuator(i, output);
  }

  Serial.print("Pitch:"); Serial.print(pitch);
  Serial.print(" Roll:"); Serial.print(roll);
  Serial.print(" FL:"); Serial.print(targetH[0]);
  Serial.print(" FR:"); Serial.print(targetH[1]);
  Serial.print(" BL:"); Serial.print(targetH[2]);
  Serial.print(" BR:"); Serial.println(targetH[3]);

  delay(10);
}

要点：该代码实现了基于IMU姿态反馈的四轴独立调平。核心策略是“最低点跟踪”——将底盘倾斜角度映射为四角的高度差目标，前左/前右/后左/后右各自独立调节。RM2026的调平方案中强调：调平目标不是要求IMU读数为零，而是接近配置中的目标偏置。实际部署中，执行器需要位置反馈（编码器或限位开关）才能实现精确的“高度闭环”，否则只能用PWM占空比近似控制升降速度。

2、超声波地形预判 + 自适应底盘高度调节（被动悬挂+主动升降）
适用场景：野外巡检机器人前方存在台阶、沟壑或凸起时，超声波传感器提前检测地面高度变化，主动调节底盘离地间隙，避免底盘托底。参考ASME论文中“双传感器系统分类障碍并触发高度调节或路径重规划”的思路。

/**
 * 野外巡检机器人 - 超声波地形预判 + 底盘高度自适应
 * 硬件假设：Arduino Mega, 超声波×2(前向+向下), BLDC底盘, 直线执行器×4(升降)
 * 核心：前向超声波预判障碍高度 -> 向下超声波测量离地间隙 -> 高度调节决策
 * 参考：ASME IDETC-CIE 2025 高度可调AGV自适应悬挂[citation:1]
 */

#include <SimpleFOC.h>

// BLDC底盘电机
BLDCMotor motorL(5);
BLDCMotor motorR(6);

// 超声波引脚
#define TRIG_FRONT 7
#define ECHO_FRONT 8
#define TRIG_GROUND 9
#define ECHO_GROUND 10

// 升降执行器
const int LIFT_DIR[4] = {22, 24, 26, 28};
const int LIFT_PWM[4] = {23, 25, 27, 29};

// 离地间隙阈值 (cm)
const float MIN_CLEARANCE = 8.0;   // 最小离地间隙
const float MAX_CLEARANCE = 25.0;  // 最大离地间隙
const float TARGET_CLEARANCE = 15.0; // 目标离地间隙

// 障碍预判距离
const float LOOKAHEAD_DIST = 60.0; // 前向预判距离 (cm)

float frontDist = 999;
float groundDist = 999;
float currentClearance = 15.0;

unsigned long lastLiftTime = 0;
const unsigned long LIFT_INTERVAL = 200; // 升降调节间隔

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

void setAllLift(float direction) {
  for (int i = 0; i < 4; i++) {
    digitalWrite(LIFT_DIR[i], direction >= 0 ? HIGH : LOW);
    analogWrite(LIFT_PWM[i], (int)(abs(direction) * 200));
  }
}

void stopAllLift() {
  for (int i = 0; i < 4; i++) {
    analogWrite(LIFT_PWM[i], 0);
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(TRIG_FRONT, OUTPUT); pinMode(ECHO_FRONT, INPUT);
  pinMode(TRIG_GROUND, OUTPUT); pinMode(ECHO_GROUND, INPUT);
  for (int i = 0; i < 4; i++) {
    pinMode(LIFT_DIR[i], OUTPUT);
    pinMode(LIFT_PWM[i], OUTPUT);
  }

  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
}

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

  // 1. 感知
  frontDist = measureDistance(TRIG_FRONT, ECHO_FRONT);
  groundDist = measureDistance(TRIG_GROUND, ECHO_GROUND);

  // 向下超声波测量的是传感器到地面的距离
  // 实际离地间隙 = 传感器安装高度 - 测量距离（简化：直接用测量值）
  currentClearance = groundDist;

  // 2. 决策：前向障碍物高度判断
  bool needLift = false;
  float liftDirection = 0;

  if (frontDist < LOOKAHEAD_DIST && frontDist > 5.0) {
    // 前方有障碍物，判断是否需要升高底盘
    // 简化：如果前方距离持续小于预判距离，尝试升高
    static int obstacleCount = 0;
    if (frontDist < 30.0) obstacleCount++;
    else obstacleCount = max(0, obstacleCount - 1);

    if (obstacleCount > 3) {
      needLift = true;
      liftDirection = 1.0; // 升高
    }
  }

  // 3. 离地间隙闭环
  if (currentClearance < MIN_CLEARANCE) {
    // 离地间隙过低，强制升高
    needLift = true;
    liftDirection = 1.0;
  } else if (currentClearance > MAX_CLEARANCE) {
    // 离地间隙过高，降低
    liftDirection = -1.0;
  } else if (needLift && currentClearance < TARGET_CLEARANCE) {
    liftDirection = 1.0;
  } else if (needLift && currentClearance >= TARGET_CLEARANCE) {
    liftDirection = 0; // 已达到目标，停止
  }

  // 4. 执行（带间隔控制，防止频繁启停）
  unsigned long now = millis();
  if (now - lastLiftTime > LIFT_INTERVAL) {
    if (liftDirection != 0) {
      setAllLift(liftDirection);
      lastLiftTime = now;
    } else {
      stopAllLift();
    }
  }

  // 5. 底盘运动（简化：根据前方距离调速）
  float speed = 3.0;
  if (frontDist < 40.0) speed = 1.5;
  if (frontDist < 20.0) speed = 0;

  motorL.move(speed);
  motorR.move(speed);

  Serial.print("Front:"); Serial.print(frontDist);
  Serial.print(" Clearance:"); Serial.print(currentClearance);
  Serial.print(" LiftDir:"); Serial.println(liftDirection);

  delay(50);
}

要点：该代码体现了“预判-调节”的地形适应策略。前向超声波用于提前检测障碍物，向下超声波用于监测实际离地间隙。ASME论文指出，双传感器系统的核心价值在于分类障碍物并触发高度调节或路径重规划。代码中加入了“障碍物持续计数”机制——只有前方距离持续小于阈值超过3个周期，才判定为真实障碍物而非噪声。升降执行器的间隔控制（200ms）防止了频繁启停对机械结构的冲击。

3、地形分类 + 步态/悬挂模式切换（自适应轮腿/悬挂刚度调节）
参考搜索结果：野外巡检机器人在不同地形上需要不同的悬挂特性。在硬质路面需要刚性悬挂保证操控，在碎石/松软地面需要柔性悬挂吸收振动。本案例通过向下超声波检测地面波动性，自动切换悬挂刚度或轮腿模式。

/**
 * 野外巡检机器人 - 地形分类 + 悬挂模式切换
 * 硬件假设：Arduino Mega, 超声波(向下), BLDC底盘, 可调悬挂(通过PWM控制阻尼/刚度)
 * 核心：地面波动检测 -> 地形分类 -> 悬挂模式切换
 * 参考：Arduino自适应地形机器人可变形轮设计[citation:17], 四轮自适应底盘[citation:7]
 */

#include <SimpleFOC.h>

BLDCMotor motorL(5);
BLDCMotor motorR(6);

#define TRIG_GROUND 7
#define ECHO_GROUND 8
#define SUSPENSION_PWM 9  // 悬挂刚度调节（简化）

// 地形类型
enum TerrainType {
  TERRAIN_FLAT,      // 平坦硬质
  TERRAIN_ROUGH,     // 粗糙碎石
  TERRAIN_SOFT,      // 松软泥地
  TERRAIN_SLOPE      // 斜坡（需结合IMU）
};

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;

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;
}

// 地面波动分析（参考：滑动平均+变化量检测[citation:17]）
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 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(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;

  setSuspension(SUSPENSION_MEDIUM);
}

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

  // 1. 地面波动分析
  float variance = analyzeGroundVariance();

  // 2. 地形分类
  // 波动大 -> 粗糙地面；波动小 -> 平坦
  // 距离值大且稳定 -> 可能悬空或软地
  if (variance > TOLERANCE * 2) {
    currentTerrain = TERRAIN_ROUGH;
  } else if (variance > TOLERANCE) {
    currentTerrain = TERRAIN_SOFT;
  } else {
    currentTerrain = TERRAIN_FLAT;
  }

  // 3. 悬挂模式切换
  int suspensionMode;
  float speedFactor;

  switch (currentTerrain) {
    case TERRAIN_FLAT:
      suspensionMode = SUSPENSION_HARD;
      speedFactor = 1.0;   // 正常速度
      break;
    case TERRAIN_ROUGH:
      suspensionMode = SUSPENSION_SOFT;
      speedFactor = 0.6;   // 降速
      break;
    case TERRAIN_SOFT:
      suspensionMode = SUSPENSION_MEDIUM;
      speedFactor = 0.5;
      break;
    default:
      suspensionMode = SUSPENSION_MEDIUM;
      speedFactor = 0.7;
  }

  setSuspension(suspensionMode);

  // 4. 底盘运动
  float baseSpeed = 3.0;
  motorL.move(baseSpeed * speedFactor);
  motorR.move(baseSpeed * speedFactor);

  Serial.print("Terrain:"); Serial.print(currentTerrain);
  Serial.print(" Variance:"); Serial.print(variance);
  Serial.print(" Suspension:"); Serial.print(suspensionMode);
  Serial.print(" SpeedFactor:"); Serial.println(speedFactor);

  delay(50);
}

要点：该案例的核心是基于地面波动性的地形分类。参考搜索结果中Arduino自适应地形机器人的方法：使用向下超声波检测地面高度，通过滑动平均滤波消除噪声，计算时间窗口内的变化量来判断地面类型。波动大（变化量超过容忍度）判定为粗糙地面，波动小判定为平坦。悬挂模式根据地形类型切换——硬质路面用刚性悬挂保证操控精度，碎石地用柔性悬挂吸收冲击，松软地面用中等刚度防止陷车。代码中的“容忍度”参数需要根据实际机器人轮径和超声波安装高度进行实验校准。

要点解读

1. 四轮独立调平的核心是“最低点跟踪”而非“姿态归零”。 丘陵农机四轮自适应底盘的研究明确指出，调平策略应采用“跟踪最低固定点平面”——即以四个支撑点中最低的那个为基准，其他点向它看齐，而非强行让IMU读数为零。这样做的好处是：调平过程不会导致某个轮子悬空，保证所有轮子始终接触地面，牵引力最大化。RM2026的调平方案也强调“调平目标不是要求IMU原始读数为0，而是接近配置中的目标偏置”。

2. 调平执行器的最小数量是3个，但4个更稳定。 从几何学看，确定一个平面只需要3个支撑点。但搜索结果中的调平方案选择了4轴设计，原因是“3轴在大负载且负载不均的情况下不容易保持平衡，4轴更稳定”。野外巡检机器人的负载（传感器、电池、设备）往往偏置安装，4轴调平能更好地应对偏载，避免某个执行器过载。

3. 地形预判与离地间隙调节必须配合，否则会“托底”或“过度升高”。 ASME论文中的双传感器系统明确区分了“障碍物分类”和“高度调节触发”。前向传感器负责“预判”——在机器人到达障碍物之前判断是否需要升高底盘；向下传感器负责“监测”——实时确认实际离地间隙。只有前向预判而没有向下反馈，机器人可能升高过头或升高不足；只有向下反馈而没有前向预判，机器人会在接近障碍物时来不及调节。

4. 地面波动检测的“容忍度”参数需要现场校准，不能使用固定值。 Arduino自适应地形机器人的实现经验表明，轮子处于展开状态时，即使在平地上行驶，车身也会因爪子结构产生周期性起伏，导致超声波读数波动。解决方案是引入“容忍度”概念——用滑动平均滤波消除单次噪声，但容忍度阈值必须通过实验校准：让机器人在已知平地上行驶，记录超声波读数的最大波动范围，阈值应略大于该范围。

5. Arduino的合理角色是“调平执行层”，复杂的地形分类和路径规划应交给上位机。 野外巡检机器人的完整架构通常包括：上位机（Jetson/树莓派）处理语义分割、地形分类和全局路径规划；Arduino负责IMU姿态读取、执行器PID控制和底层BLDC驱动。SimpleFOC库为BLDC底盘提供了完整的FOC控制能力，包括编码器/霍尔传感器接口、电流检测和多种控制模式，使Arduino能够胜任执行层的实时控制任务。

4、四轮 BLDC 独立速度闭环 + SimpleFOC 基础底盘
这个案例负责把四个轮子的 BLDC 跑通，形成独立速度闭环底盘。后续调平、力矩分配、巡检任务都依赖这一层。

#include <SimpleFOC.h>

BLDCMotor motorFL(7);
BLDCMotor motorFR(7);
BLDCMotor motorRL(7);
BLDCMotor motorRR(7);

BLDCDriver3PWM driverFL(9, 10, 11, 8);
BLDCDriver3PWM driverFR(3, 5, 6, 7);
BLDCDriver3PWM driverRL(12, 13, 14, 15);
BLDCDriver3PWM driverRR(16, 17, 18, 19);

MagneticSensorI2C sensorFL = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensorFR = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensorRL = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensorRR = MagneticSensorI2C(AS5600_I2C);

float targetSpeedFL = 0;
float targetSpeedFR = 0;
float targetSpeedRL = 0;
float targetSpeedRR = 0;

void initWheel(BLDCMotor &motor, BLDCDriver3PWM &driver, Sensor &sensor) {
  sensor.init();
  motor.linkSensor(&sensor);
  driver.voltage_power_supply = 24;
  driver.init();
  motor.linkDriver(&driver);
  motor.controller = MotionControlType::velocity;
  motor.PID_velocity.P = 0.3f;
  motor.PID_velocity.I = 2.0f;
  motor.PID_velocity.D = 0.0f;
  motor.voltage_limit = 8;
  motor.velocity_limit = 15;
  motor.init();
  motor.initFOC();
}

void setup() {
  Serial.begin(115200);
  Wire.begin();

  initWheel(motorFL, driverFL, sensorFL);
  initWheel(motorFR, driverFR, sensorFR);
  initWheel(motorRL, driverRL, sensorRL);
  initWheel(motorRR, driverRR, sensorRR);
}

void loop() {
  motorFL.loopFOC();
  motorFR.loopFOC();
  motorRL.loopFOC();
  motorRR.loopFOC();

  motorFL.move(targetSpeedFL);
  motorFR.move(targetSpeedFR);
  motorRL.move(targetSpeedRL);
  motorRR.move(targetSpeedRR);
}

这个案例的重点是：四个轮子各自拥有传感器、驱动和 PID 参数，避免把四轮底盘当成两个差速轮处理。野外巡检场景下，独立闭环能为后续打滑补偿和调平执行提供更稳定的底层执行能力。

5、IMU 姿态感知 + 四轮独立调平 PID
这个案例在案例一基础上加入 IMU，用俯仰角和横滚角计算四轮高度补偿量，再通过悬挂推杆或关节位置环实现底盘调平。

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

MPU6050 mpu;

float pitch = 0;
float roll = 0;
float lastTime = 0;

float targetHeightFL = 0;
float targetHeightFR = 0;
float targetHeightRL = 0;
float targetHeightRR = 0;

float heightFL = 0;
float heightFR = 0;
float heightRL = 0;
float heightRR = 0;

float kpLevel = 1.2f;
float kdLevel = 0.15f;

void readIMU() {
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float dt = (millis() - lastTime) / 1000.0f;
  if (dt <= 0) dt = 0.01f;
  lastTime = millis();

  float accelPitch = atan2(ax, az) * RAD_TO_DEG;
  float accelRoll  = atan2(ay, az) * RAD_TO_DEG;

  float gyroPitchRate = gx / 131.0f;
  float gyroRollRate  = gy / 131.0f;

  pitch = 0.98f * (pitch + gyroPitchRate * dt) + 0.02f * accelPitch;
  roll  = 0.98f * (roll + gyroRollRate * dt) + 0.02f * accelRoll;
}

void computeLevelHeights() {
  float pitchCmd = constrain(pitch, -20, 20);
  float rollCmd  = constrain(roll, -20, 20);

  targetHeightFL = -pitchCmd * 0.5f - rollCmd * 0.5f;
  targetHeightFR = -pitchCmd * 0.5f + rollCmd * 0.5f;
  targetHeightRL =  pitchCmd * 0.5f - rollCmd * 0.5f;
  targetHeightRR =  pitchCmd * 0.5f + rollCmd * 0.5f;
}

float levelPID(float target, float current, float &lastError) {
  float error = target - current;
  float output = kpLevel * error - kdLevel * (error - lastError);
  lastError = error;
  return constrain(output, -10, 10);
}

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();

  if (!mpu.testConnection()) {
    while (1) delay(100);
  }
}

void loop() {
  readIMU();
  computeLevelHeights();

  static float lastErrorFL = 0, lastErrorFR = 0;
  static float lastErrorRL = 0, lastErrorRR = 0;

  float cmdFL = levelPID(targetHeightFL, heightFL, lastErrorFL);
  float cmdFR = levelPID(targetHeightFR, heightFR, lastErrorFR);
  float cmdRL = levelPID(targetHeightRL, heightRL, lastErrorRL);
  float cmdRR = levelPID(targetHeightRR, heightRR, lastErrorRR);

  // 这里应将 cmdFL/FR/RL/RR 发送给悬挂推杆或关节电机
  // 例如：suspensionFL.move(cmdFL);
}

这个案例的关键是：IMU 提供车身姿态，调平 PID 输出四个轮位的修正量。实际工程中，heightFL 等变量应来自悬挂位移传感器、电位计、编码器或推杆行程反馈；如果没有独立悬挂执行器，可退化为“扭矩补偿式调平”，即根据姿态微调四轮目标转速，维持机身稳定。

6、滑移检测 + 四轮力矩动态分配 + 巡检安全状态机
这个案例面向野外巡检的可靠性，重点处理打滑、单轮悬空、坡道起步和故障降级。它不追求完整二次规划求解，而是给出嵌入式平台上更容易落地的加权分配框架。

#include <SimpleFOC.h>

BLDCMotor motor[4];
BLDCDriver3PWM driver[4];
Sensor *sensor[4];

float wheelSpeed[4] = {0, 0, 0, 0};
float bodySpeedEst = 0;
float slipRatio[4] = {0, 0, 0, 0};
float weight[4] = {1, 1, 1, 1};
float torqueCmd[4] = {0, 0, 0, 0};

float totalTorque = 0.5f;
float steerTorque = 0.0f;

enum Mode { NORMAL, SLIP, LEVELING, STOP };
Mode currentMode = NORMAL;

void estimateBodySpeed() {
  float sum = 0;
  int valid = 0;
  for (int i = 0; i < 4; i++) {
    if (slipRatio[i] < 0.3f) {
      sum += wheelSpeed[i];
      valid++;
    }
  }
  bodySpeedEst = valid > 0 ? sum / valid : 0;
}

void updateSlipRatio() {
  for (int i = 0; i < 4; i++) {
    wheelSpeed[i] = sensor[i]->getVelocity();
    if (abs(bodySpeedEst) > 0.01f) {
      slipRatio[i] = abs(wheelSpeed[i] - bodySpeedEst) / abs(bodySpeedEst);
    } else {
      slipRatio[i] = 0;
    }
  }
}

void updateWeight() {
  for (int i = 0; i < 4; i++) {
    if (slipRatio[i] > 0.4f) {
      weight[i] = 0.1f;
    } else if (slipRatio[i] > 0.2f) {
      weight[i] = 0.5f;
    } else {
      weight[i] = 1.0f;
    }
  }
}

void distributeTorque() {
  float wSum = weight[0] + weight[1] + weight[2] + weight[3];
  if (wSum < 0.01f) wSum = 0.01f;

  float baseFL = (weight[0] / wSum) * totalTorque - steerTorque;
  float baseFR = (weight[1] / wSum) * totalTorque + steerTorque;
  float baseRL = (weight[2] / wSum) * totalTorque - steerTorque;
  float baseRR = (weight[3] / wSum) * totalTorque + steerTorque;

  torqueCmd[0] = constrain(baseFL, -1.0f, 1.0f);
  torqueCmd[1] = constrain(baseFR, -1.0f, 1.0f);
  torqueCmd[2] = constrain(baseRL, -1.0f, 1.0f);
  torqueCmd[3] = constrain(baseRR, -1.0f, 1.0f);
}

void updateMode() {
  float maxSlip = 0;
  for (int i = 0; i < 4; i++) {
    if (slipRatio[i] > maxSlip) maxSlip = slipRatio[i];
  }

  if (maxSlip > 0.5f) {
    currentMode = SLIP;
  } else if (abs(pitch) > 15 || abs(roll) > 15) {
    currentMode = LEVELING;
  } else {
    currentMode = NORMAL;
  }
}

void setup() {
  Serial.begin(115200);
  // 初始化四个电机、驱动和传感器
}

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

  updateSlipRatio();
  estimateBodySpeed();
  updateWeight();
  distributeTorque();
  updateMode();

  if (currentMode == STOP) {
    for (int i = 0; i < 4; i++) motor[i].move(0);
  } else {
    for (int i = 0; i < 4; i++) {
      motor[i].move(torqueCmd[i]);
    }
  }
}

这个案例的价值在于把“四轮独立驱动”从速度控制推进到力矩管理。野外巡检中，碎石、泥泞、斜坡都会让某个轮子突然失去附着力，动态分配能减少打滑扩大导致的姿态失控。

要点解读
分层架构优先于单循环堆叠
底层跑 FOC 电流/速度环，中层做调平 PID 和力矩分配，上层处理巡检任务和状态机。不要把所有逻辑塞进一个 loop，否则控制周期会不稳定，IMU 滤波和电机闭环都会受影响。
调平必须有姿态反馈和执行反馈
仅靠 IMU 的 Pitch/Roll 只能知道“车身歪了”，不知道“每个轮子实际抬了多少”。工程上应加入悬挂行程、推杆位置或关节编码器反馈，否则调平 PID 容易超调或持续震荡。
力矩分配要先抑制打滑，再追求速度
野外巡检的首要目标是稳定通过，而不是最快到达。滑移率估计、权重衰减、扭矩限幅和斜坡限变率都应保守设置，避免某个轮子突然空转导致机身横摆或侧倾。
SimpleFOC 更适合做底层执行，不建议在 Arduino Uno 上跑完整系统
四轮 BLDC、IMU 融合、悬挂控制和力矩分配对定时器、PWM 通道和中断响应要求较高。更推荐 ESP32、Teensy 4.x 或 STM32F4/F7 作为下位控制器，上位机再负责巡检路径、传感器数据融合和远程通信。
安全状态机应高于运动控制
倾角超限、电池欠压、电机过流、IMU 失联、悬挂卡滞都应进入 STOP 或降级模式。运动指令必须经过限幅和斜率限制，防止调平机构或 BLDC 在切换瞬间产生冲击。

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