Arduino BLDC 之户外机器人“足端力反馈 + 触地检测 + 动态步频”，本质上是“足端力/关节电流感知触地 → 触地相位切换 → 力反馈调制阻抗/步态参数 → 动态步频闭环”的自适应行走系统：足端力反馈提供地面交互量化信号，触地检测负责摆动相/支撑相切换，动态步频则根据冲击力、打滑、地形起伏和机身姿态实时调整行走节奏，使机器人在碎石、泥泞、台阶、斜坡等非结构化地形上既能走稳，又不会因固定步频而磕碰或失衡。 它的工程价值在于把步态从“按时间循环”升级为“按接触状态和受力状态循环”。

主要特点

1. 足端力反馈提供“地面交互”的直接量化信号
   足端力反馈可分为外置传感器方案和关节电流估算方案。外置方案常用薄膜压力传感器或微型多维力传感器，能直接检测垂直压力、侧向摩擦力和接触角度，适合判断触地、打滑、塌陷和地面凸起。 关节电流估算方案则通过 BLDC 的 FOC 电流环、编码器位置和动力学模型推算足端受力，无需额外足端传感器，但对摩擦补偿、机械背隙和模型标定要求较高。 在 Arduino/ESP32 级别平台上，更常见的是“薄膜压力传感器 + BLDC 电流辅助判断”的混合方案。
2. 触地检测是步态相位切换的关键触发器
   四足或双足机器人的每条腿都有摆动相和支撑相。固定时间步态在平地上可用，但在崎岖地形上容易提前落地或延迟抬腿。触地检测通过足端力阈值、关节电流突变、编码器速度变化、IMU 冲击信号和期望/实际足端位置偏差综合判断是否真正触地。 一旦检测到触地，系统应立即从摆动相切到支撑相，并降低足端下落速度，避免硬冲击；若检测到离地，则恢复摆动轨迹。
3. 动态步频让机器人根据地形实时“调速”
   动态步频不是简单改变电机转速，而是根据触地冲击力、滑移率、机身俯仰角和地形粗糙度调整步态周期。典型策略包括：
   足端冲击力过大时降低步频，减少磕碰和机身震荡；
   地面松软或打滑时降低步频、缩短步长，提高支撑稳定性；
   地面平整且触地稳定时提高步频，提升移动效率；
   上坡时增加抬腿高度、减小步长，防止绊倒；
   下坡时降低重心、控制步频，防止前冲失稳。
   步频调整应带迟滞和平滑滤波，避免在边界地形上频繁跳变。
4. 力反馈与阻抗控制结合，可实现柔顺落足
   足端落地瞬间如果采用纯位置控制，容易产生较大冲击。引入阻抗控制后，足端在接近地面时表现为“虚拟弹簧-阻尼”系统：刚度决定落足硬度，阻尼决定冲击吸收能力。 当检测到触地冲击力偏大时，可临时降低刚度、提高阻尼，使足端更柔顺；当确认支撑稳定后，再提高刚度以维持机身姿态。BLDC 的 FOC 电流环能提供低转矩脉动和高带宽力矩输出，是实现这种柔顺落足的执行基础。
5. 分层实时架构更适合嵌入式平台
   推荐分为四层：
   感知层：足端压力/力传感器、BLDC 电流采样、编码器、IMU。
   检测层：触地判定、离地判定、打滑检测、冲击力估计。
   决策层：动态步频计算、步长/步高调整、阻抗参数调制。
   执行层：BLDC FOC 关节控制、步态相位切换、姿态补偿。
   Arduino Uno/Nano 适合做单腿触地检测和简单步频调节；若要同时运行多腿力反馈、触地相位切换和动态步频，建议使用 ESP32-S3、Teensy 4.1 或 STM32，并把复杂地形分类交给上位机。

应用场景
野外巡检机器人：碎石路、草地、田埂和坑洼地面较多，动态步频可减少磕碰和打滑。
矿区与隧道巡检：地面起伏大、可能有积水和松散碎石，触地检测能避免虚支撑。
灾后搜救：瓦砾堆、台阶和缝隙环境中，足端力反馈可判断落足点是否可靠。
农业与林业移动平台：泥地、落叶层和垄沟会导致足端下沉，动态步频可提升通过性。
教育与科研平台：用于验证足端力反馈、触地相位切换、阻抗控制和动态步频算法。

需要注意的事项
薄膜压力传感器只能反映局部压力，不能直接等同于三维地面反力
单轴压力传感器适合判断触地和大致受力，但难以准确区分侧向滑移和法向压力。若要做更可靠的打滑检测，需要结合编码器速度、IMU 加速度和关节电流。
关节电流估算触地需要做好摩擦和重力补偿
BLDC 电流受减速器摩擦、连杆重力和惯性影响较大。如果不做补偿，静止负载也可能被误判为触地，导致相位切换错误。
触地检测必须带迟滞和滤波
足端在粗糙地面上可能出现“触地—离地—再触地”的抖动。应设置力阈值迟滞、时间窗口滤波和置信度判断，避免步态相位频繁翻转。
动态步频不能突变
步频变化会影响摆动轨迹、支撑时长和机身重心转移。切换时应限制最大变化率，并同步调整步长、抬腿高度和相位差，防止机身俯仰或侧倾。
FOC 控制周期和传感器采样周期需要匹配
足端力反馈和触地检测属于毫秒级事件，若采样太慢会错过落地瞬间；若控制周期不稳定，则阻抗调制会出现抖动。关键闭环应放在硬件定时器中断中运行。
电源干扰会影响力反馈精度
多路 BLDC 同时换相时电流波动较大，可能干扰 ADC 采样的足端力信号和电流采样信号。建议控制电源与动力电源隔离，敏感模拟信号使用屏蔽线，并加入滤波电容。
打滑检测需要区分“真实滑移”和“正常支撑变形”
地面轻微压缩、轮胎/脚垫形变都会引起位置偏差。应同时观察足端力、关节电流、编码器速度和机身加速度，而不是仅凭单一阈值判断。
Arduino 平台应控制算法复杂度
多腿力反馈、触地检测、动态步频和 BLDC FOC 同时运行对 8 位 MCU 压力较大。更稳妥的做法是让 Arduino/Teensy/STM32 负责实时执行层，视觉地形分类和高层步态规划交给上位机。

1、基于足端压力与IMU融合的碎石地形自适应步频控制
场景说明：面向碎石、泥地、草地等非结构化地形，足端触地力变化剧烈。通过足端压力传感器检测触地冲击，结合IMU姿态动态调整步频——冲击大则降频避震，地面平整则升频提效，实现稳定穿越不摔倒。

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

// ===== 硬件定义 =====
MPU6050 imu;
int pressurePins[4] = {A0, A1, A2, A3};  // 四足压力传感器

// 四足BLDC电机（简化：每腿单电机）
BLDCMotor motors[4] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};
BLDCDriver3PWM drivers[4] = {
  BLDCDriver3PWM(9, 10, 11, 8),
  BLDCDriver3PWM(3, 5, 6, 4),
  BLDCDriver3PWM(44, 45, 46, 43),
  BLDCDriver3PWM(22, 23, 24, 21)
};

// ===== 自适应步频参数 =====
float baseStepFreq = 1.2f;      // 基础步频 Hz
float minStepFreq = 0.6f;       // 最小步频（软地形）
float maxStepFreq = 2.0f;       // 最大步频（硬平地）
float forceThreshold = 15.0f;   // 冲击力阈值
float stepFreq = 1.2f;

// ===== 触地检测状态 =====
float footForces[4] = {0};
bool footContact[4] = {false};
float pitchAngle = 0;

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

  for (int i = 0; i < 4; i++) {
    motors[i].linkDriver(&drivers[i]);
    motors[i].init();
    motors[i].initFOC();
  }
}

void loop() {
  // ===== 1. 感知层：足端力 + IMU姿态 =====
  readFootForces();
  readIMU();

  // ===== 2. 调度层：触地判断 + 动态步频调整 =====
  detectFootContact();
  adaptStepFrequency();

  // ===== 3. 执行层：步频映射到关节摆动 =====
  executeGait(stepFreq);

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

void readFootForces() {
  for (int i = 0; i < 4; i++) {
    footForces[i] = analogRead(pressurePins[i]);
    footContact[i] = footForces[i] > 100;  // 简化触地判断
  }
}

void readIMU() {
  int16_t ax, ay, az;
  imu.getAcceleration(&ax, &ay, &az);
  pitchAngle = atan2(ay, az) * 180.0 / PI;
}

void detectFootContact() {
  // 触地检测：压力突变判断
  static float lastForce[4] = {0};
  for (int i = 0; i < 4; i++) {
    float delta = footForces[i] - lastForce[i];
    if (delta > 50) {
      footContact[i] = true;  // 触地瞬间
    } else if (footForces[i] < 30) {
      footContact[i] = false; // 离地
    }
    lastForce[i] = footForces[i];
  }
}

void adaptStepFrequency() {
  // 检测最大触地冲击力
  float maxForce = 0;
  for (int i = 0; i < 4; i++) {
    if (footForces[i] > maxForce) maxForce = footForces[i];
  }

  // 冲击力过大 -> 硬地/坑洼 -> 降低步频
  if (maxForce > forceThreshold * 3) {
    stepFreq = max(minStepFreq, stepFreq - 0.1);
  }
  // 冲击力平稳 -> 平整地面 -> 提高步频
  else if (maxForce < forceThreshold) {
    stepFreq = min(maxStepFreq, stepFreq + 0.05);
  }

  // IMU辅助：倾斜时降低步频
  if (abs(pitchAngle) > 15.0) {
    stepFreq = max(minStepFreq, stepFreq - 0.05);
  }
}

void executeGait(float freq) {
  // 简化：步频映射到关节目标角度振荡
  static unsigned long lastTime = 0;
  float period = 1000.0 / freq;
  float phase = (millis() % (unsigned long)period) / period * 2 * PI;

  for (int i = 0; i < 4; i++) {
    // 对角步态：腿0/3同相，腿1/2反相
    float legPhase = (i == 0 || i == 3) ? phase : phase + PI;
    float targetAngle = 30.0 * sin(legPhase);  // 简化关节目标
    motors[i].move(targetAngle);
  }
}

代码要点：感知层用四路压力传感器读取足端力，IMU获取俯仰角。调度层通过力突变判断触地，再根据最大冲击力分档调整步频——冲击大则降频（避震），冲击小则升频（提效）。执行层将步频映射为关节角度正弦振荡。

2、基于BLDC电流环的“虚拟力传感器”触地检测与步频调制
场景说明：不额外安装足端力传感器，利用BLDC FOC的电流环（Iq）作为“虚拟力传感器”——触地瞬间电流突变，据此检测触地并调制步频。适合对成本敏感、但已有BLDC驱动器的户外机器人。

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

// ===== 四足BLDC（带电流采样） =====
BLDCMotor motors[4] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};
BLDCDriver3PWM drivers[4] = {
  BLDCDriver3PWM(9, 10, 11, 8),
  BLDCDriver3PWM(3, 5, 6, 4),
  BLDCDriver3PWM(44, 45, 46, 43),
  BLDCDriver3PWM(22, 23, 24, 21)
};

MPU6050 imu;

// ===== 触地检测参数 =====
float currentBaseline[4] = {0};   // 各腿空载电流基准
float currentThreshold[4] = {0};  // 触地电流阈值
bool footContact[4] = {false};
float currentHistory[4][5] = {0}; // 历史电流（用于突变检测）
int historyIdx = 0;

// ===== 动态步频 =====
float stepFreq = 1.2f;
float minFreq = 0.6f, maxFreq = 2.0f;

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

  for (int i = 0; i < 4; i++) {
    motors[i].linkDriver(&drivers[i]);
    motors[i].init();
    motors[i].initFOC();
    // 空载标定：悬空运行，记录电流基线
    motors[i].move(0);
    delay(500);
    currentBaseline[i] = motors[i].current.q;
    currentThreshold[i] = currentBaseline[i] * 3.0;  // 阈值=3倍基线
  }
}

void loop() {
  // ===== 1. 感知：读取BLDC电流（虚拟力） =====
  readMotorCurrents();

  // ===== 2. 调度：电流突变触地检测 =====
  detectContactByCurrent();

  // ===== 3. 动态步频调整 =====
  adaptFrequencyByContact();

  // ===== 4. 执行步态 =====
  executeGait(stepFreq);

  for (int i = 0; i < 4; i++) motors[i].loopFOC();
}

void readMotorCurrents() {
  for (int i = 0; i < 4; i++) {
    currentHistory[i][historyIdx] = motors[i].current.q;
  }
  historyIdx = (historyIdx + 1) % 5;
}

void detectContactByCurrent() {
  for (int i = 0; i < 4; i++) {
    float current = motors[i].current.q;
    // 电流突然增大 -> 触地
    if (current > currentThreshold[i] && !footContact[i]) {
      footContact[i] = true;
    }
    // 电流回落 -> 离地
    else if (current < currentBaseline[i] * 1.5 && footContact[i]) {
      footContact[i] = false;
    }
  }
}

void adaptFrequencyByContact() {
  // 统计当前触地腿数
  int contactCount = 0;
  for (int i = 0; i < 4; i++) {
    if (footContact[i]) contactCount++;
  }

  // 多腿触地 -> 稳定支撑 -> 可提速
  if (contactCount >= 3) {
    stepFreq = min(maxFreq, stepFreq + 0.05);
  }
  // 单腿或零腿触地 -> 不稳定 -> 降速
  else if (contactCount <= 1) {
    stepFreq = max(minFreq, stepFreq - 0.1);
  }
}

void executeGait(float freq) {
  static unsigned long lastUpdate = 0;
  if (millis() - lastUpdate < 10) return;
  lastUpdate = millis();

  float period = 1000.0 / freq;
  float phase = (millis() % (unsigned long)period) / period * 2 * PI;

  for (int i = 0; i < 4; i++) {
    float legPhase = (i == 0 || i == 3) ? phase : phase + PI;
    float targetAngle = 25.0 * sin(legPhase);
    motors[i].move(targetAngle);
  }
}

代码要点：感知层用motors[i].current.q读取BLDC的Q轴电流作为“虚拟力传感器”。调度层通过电流突变判断触地，统计触地腿数来决策步频——多腿触地（≥3）则加速，少腿触地（≤1）则减速。执行层同案例一。初始化时需悬空标定电流基线。

3、力反馈 + 触地 + 步频三参数联调的自适应崎岖地形步态
场景说明：融合足端力反馈、触地状态机、动态步频三个参数，实现“一脚一策”——每腿根据自身触地状态和受力情况独立调整摆动速度，同时全局步频根据地形崎岖度动态缩放。适合负载变化大、地形连续变化的户外巡检场景。

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

// ===== 硬件 =====
MPU6050 imu;
int pressurePins[4] = {A0, A1, A2, A3};

BLDCMotor motors[4] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};
BLDCDriver3PWM drivers[4] = {
  BLDCDriver3PWM(9, 10, 11, 8),
  BLDCDriver3PWM(3, 5, 6, 4),
  BLDCDriver3PWM(44, 45, 46, 43),
  BLDCDriver3PWM(22, 23, 24, 21)
};

// ===== 足端状态机 =====
enum FootState { SWING, TOUCHDOWN, STANCE, LIFTOFF };
FootState footState[4] = {SWING, SWING, SWING, SWING};
float footForces[4] = {0};
float forceAtTouchdown[4] = {0};

// ===== 全局步频 =====
float globalStepFreq = 1.2f;
float terrainRoughness = 0;  // 地形崎岖度 0~1

// ===== 每腿步频调制 =====
float legFreq[4] = {1.2f, 1.2f, 1.2f, 1.2f};

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

  for (int i = 0; i < 4; i++) {
    motors[i].linkDriver(&drivers[i]);
    motors[i].init();
    motors[i].initFOC();
  }
}

void loop() {
  // ===== 1. 感知 =====
  readSensors();

  // ===== 2. 调度：状态机 + 步频联调 =====
  updateFootStateMachine();
  computeTerrainRoughness();
  adaptLegFrequencies();

  // ===== 3. 执行 =====
  executeAdaptiveGait();

  for (int i = 0; i < 4; i++) motors[i].loopFOC();
}

void readSensors() {
  for (int i = 0; i < 4; i++) {
    footForces[i] = analogRead(pressurePins[i]);
  }
}

void updateFootStateMachine() {
  for (int i = 0; i < 4; i++) {
    float force = footForces[i];

    switch (footState[i]) {
      case SWING:
        // 触地检测：力超过阈值
        if (force > 150) {
          footState[i] = TOUCHDOWN;
          forceAtTouchdown[i] = force;
        }
        break;

      case TOUCHDOWN:
        // 记录冲击力，进入支撑
        forceAtTouchdown[i] = max(forceAtTouchdown[i], force);
        footState[i] = STANCE;
        break;

      case STANCE:
        // 支撑相：力持续
        if (force < 30) {
          footState[i] = LIFTOFF;
        }
        break;

      case LIFTOFF:
        // 离地：进入摆动相
        footState[i] = SWING;
        forceAtTouchdown[i] = 0;
        break;
    }
  }
}

void computeTerrainRoughness() {
  // 崎岖度 = 各腿触地冲击力的变异系数
  float sum = 0, sumSq = 0;
  int count = 0;
  for (int i = 0; i < 4; i++) {
    if (footState[i] == STANCE || footState[i] == TOUCHDOWN) {
      sum += forceAtTouchdown[i];
      sumSq += forceAtTouchdown[i] * forceAtTouchdown[i];
      count++;
    }
  }
  if (count > 1) {
    float mean = sum / count;
    float variance = sumSq / count - mean * mean;
    terrainRoughness = constrain(sqrt(variance) / mean, 0, 1);
  }
}

void adaptLegFrequencies() {
  // 全局步频：崎岖度越高，步频越低
  globalStepFreq = maxFreq - (maxFreq - minFreq) * terrainRoughness;
  globalStepFreq = constrain(globalStepFreq, 0.6f, 2.0f);

  // 每腿微调：支撑相腿可稍慢，摆动相腿可稍快
  for (int i = 0; i < 4; i++) {
    if (footState[i] == STANCE) {
      legFreq[i] = globalStepFreq * 0.9;  // 支撑腿慢一点
    } else {
      legFreq[i] = globalStepFreq * 1.1;  // 摆动腿快一点
    }
  }
}

void executeAdaptiveGait() {
  static unsigned long lastTime = 0;
  if (millis() - lastTime < 10) return;
  lastTime = millis();

  for (int i = 0; i < 4; i++) {
    float period = 1000.0 / legFreq[i];
    float phase = (millis() % (unsigned long)period) / period * 2 * PI;

    // 摆动相才输出运动，支撑相保持
    if (footState[i] == SWING) {
      float targetAngle = 30.0 * sin(phase);
      motors[i].move(targetAngle);
    } else {
      // 支撑相：保持当前角度，提供支撑力
      motors[i].move(motors[i].shaft_angle);
    }
  }
}

代码要点：感知层读取四路足端压力和IMU。调度层包含两个核心：足端状态机（SWING→TOUCHDOWN→STANCE→LIFTOFF）逐腿判断触地/离地；地形崎岖度通过触地冲击力的变异系数计算，崎岖度越高则全局步频越低。执行层支持每腿独立步频微调——摆动腿稍快、支撑腿稍慢。

要点解读

1. 触地检测的本质是“力/电流的突变识别”，而非绝对阈值

足端从悬空到触地的瞬间，压力传感器读数或BLDC电流Iq会发生阶跃式上升。案例一用delta > 50判断突变，案例二用current > threshold判断，案例三用状态机在SWING相检测力超阈值。相比单纯比较绝对力值，突变检测能有效区分“缓慢加载”与“真实触地”。

2. 足端力反馈是步频决策的最直接依据

冲击力大小直接反映地面硬度：软地（草地/泥地）冲击小，足端容易下陷，应适当降低步频以增加支撑时间；硬地（碎石/岩石）冲击大，应降低步频以吸收振动。案例一用“最大冲击力分档”实现，案例三用“冲击力变异系数”量化崎岖度，都是将力信号转化为步频指令的有效方式。

3. 动态步频需要“全局基准 + 每腿微调”两层结构

全局步频决定整体运动节奏，但四足的触地状态不同——支撑腿需要更稳，摆动腿需要更快。案例三的legFreq[i]设计体现了这一思想：支撑相腿频率×0.9，摆动相腿频率×1.1，在保持步态协调的同时让每腿适应自身负载。

4. BLDC的电流环可作为“虚拟足端力传感器”

对于已采用SimpleFOC驱动的机器人，电机电流Iq与输出力矩近似成正比。触地时负载突增，电流随之上升。案例二完全不使用额外压力传感器，仅通过motors[i].current.q实现触地检测，降低了硬件成本和机械复杂度。但需注意：空载电流基线必须逐腿标定，否则阈值不准。

5. 触地状态机是步态调度的核心骨架

三个案例的共同结构是“SWING→触地检测→STANCE→离地检测→SWING”。案例三将其显式实现为enum FootState。状态机的价值在于：只有摆动相才执行运动控制，支撑相则保持位置或输出支撑力矩，避免支撑腿误动作导致机身失稳。状态机的转换条件（力阈值、时间约束）需要根据实际硬件标定。

4、足端力反馈+触地检测+平地动态步频闭环
适用场景：平地、缓坡等规则地形，通过足端力判断触地状态，根据足端受力大小动态调节步频，提升行走效率与稳定性。

#include <SimpleFOC.h>
SimpleFOCMotor2 joints[12] = {/* 12个关节定义同四足机器人前案 */};
int touch_force[4] = {0};  // 4足端FSR压力，0-100
bool foot_ground[4] = {false}; // 足端触地标志
float base_step_freq = 1.0f; // 基础步频，Hz
float dynamic_step_freq = 1.0f;

// 感知层：足端力采集与触地检测
void detect_foot_touch(){
  for(int i=0;i<4;i++){
    touch_force[i] = analogRead(A0+i) / 10.23f; // 0-1023映射为0-100
    foot_ground[i] = (touch_force[i] > 15) ? true : false; // 触地阈值
  }
}

// 调度层：根据足端力反馈动态调整步频
void adjust_step_freq(){
  int contact_num = 0;
  int total_force = 0;
  for(int i=0;i<4;i++){
    if(foot_ground[i]){
      contact_num++;
      total_force += touch_force[i];
    }
  }
  int avg_force = (contact_num > 0) ? total_force / contact_num : 0;
  // 受力越大，步频适当降低，提升稳定性
  if(avg_force > 60) dynamic_step_freq = base_step_freq * 0.7f;
  else if(avg_force > 30) dynamic_step_freq = base_step_freq * 0.9f;
  else dynamic_step_freq = base_step_freq; // 受力正常，全速行走
}

// 执行层：按动态步频执行步态
void execute_gait(){
  static float phase = 0;
  phase += dynamic_step_freq * 0.0628f; // 2π对应一个步态周期
  if(phase > 2 * M_PI) phase -= 2 * M_PI;
  // 交替抬腿，步频越快，相位推进越快
  joints[0].set(0.4f * sin(phase));
  joints[6].set(0.4f * sin(phase + M_PI));
}

void loop(){
  detect_foot_touch();
  adjust_step_freq();
  execute_gait();
  delay(10);
}

5、足端力反馈+地形自适应步频调节
适用场景：碎石路、草地、沙地等不规则户外地形，结合足端力与地形变化动态调节步频，适应复杂环境。

#include <SimpleFOC.h>
#include <Adafruit_MPU6050.h>
Adafruit_MPU6050 mpu;
SimpleFOCMotor2 joints[12] = {/* 定义同前 */};
int touch_force[4] = {0};
bool foot_ground[4] = {false};
float terrain_roughness = 0;
float dynamic_step_freq = 0.8f;

// 感知层：足端力+振动融合识别地形
void detect_terrain_and_touch(){
  for(int i=0;i<4;i++){
    touch_force[i] = analogRead(A0+i) / 10.23f;
    foot_ground[i] = (touch_force[i] > 20) ? true : false;
  }
  // 振动幅值反映地形粗糙度
  sensors_event_t a;
  mpu.getEvent(&a);
  float vibra = sqrtf(a.acceleration.x*a.acceleration.x + a.acceleration.y*a.acceleration.y + a.acceleration.z*a.acceleration.z);
  terrain_roughness = vibra / 9.8f; // 归一化地形粗糙度
}

// 调度层：结合地形与足端力调步频
void adapt_step_freq(){
  int untouch_count = 0;
  for(int i=0;i<4;i++) if(!foot_ground[i]) untouch_count++;
  // 路面起伏大+触地脚少，降低步频防失稳
  if(terrain_roughness > 1.2f || untouch_count >= 2){
    dynamic_step_freq = 0.4f;
  } else if(terrain_roughness > 0.8f || untouch_count ==1){
    dynamic_step_freq = 0.7f;
  } else {
float avg_force = (touch_force[0]+touch_force[1]+touch_force[2]+touch_force[3])/4;
    dynamic_step_freq = (avg_force > 50) ? 0.9f : 1.2f;
  }
}

// 执行层：步频适配，抬腿高度随地形变化
void execute_gait(){
  static float phase = 0;
  phase += dynamic_step_freq * 0.0628f;
  float lift_height = 0.4f + terrain_roughness * 0.5f; // 颠簸路面抬腿更高
  joints[0].set(lift_height * sin(phase));
  joints[6].set(lift_height * sin(phase + M_PI));
}

void loop(){
  detect_terrain_and_touch();
  adapt_step_freq();
  execute_gait();
  delay(10);
}

6、足端力反馈+故障容错步频重构
适用场景：足端传感器或关节故障，需要根据剩余触地反馈重构步态，动态调整步频保证机器人不倾倒。

#include <SimpleFOC.h>
SimpleFOCMotor2 joints[12] = {/* 定义同前 */};
int touch_force[4] = {0};
bool foot_ground[4] = {false};
bool sensor_fault = false;
float dynamic_step_freq = 0.5f;

// 感知层：传感器故障检测
void detect_sensor_fault(){
  int valid_count = 0;
  for(int i=0;i<4;i++){
    touch_force[i] = analogRead(A0+i) / 10.23f;
    // 连续3次超过量程或为0，判定传感器故障
    static int zero_cnt[4] = {0}, over_cnt[4] = {0};
    if(touch_force[i] == 0) zero_cnt[i]++; else zero_cnt[i] = 0;
    if(touch_force[i] == 100) over_cnt[i]++; else over_cnt[i] = 0;
    if(zero_cnt[i] >=3 || over_cnt[i]>=3){ foot_ground[i] = false; continue; }
    foot_ground[i] = (touch_force[i] >15) ? true : false;
    valid_count++;
  }
  sensor_fault = (valid_count < 4);
}

// 调度层：故障时重构步频与步态
void replan_for_fault(){
  if(!sensor_fault){ dynamic_step_freq = 1.0f; return; }
int valid_foot[4], val_cnt =0;
  for(int i=0;i<4;i++) if(well(i)) valid_foot[val_cnt++] = i;
  // 有效触地足越少，步频越低
  if(val_cnt == 3) dynamic_step_freq = 0.7f;
  else if(val_cnt == 2) dynamic_step_freq = 0.4f;
  else dynamic_step_freq = 0.2f; // 仅1足触地，慢速调整
}

// 执行层：步态重构，仅驱动有效关节
void execute_adaptive_gait(){
  static float phase = 0;
  phase += dynamic_step_freq * 0.0628f;
  for(int i=0;i<12;i++){
    if(fault(i)) continue; // 故障关节停机
    float angle = 0.4f * sin(phase + (i<6 ? 0 : M_PI));
    joints[i].set(angle, 0.8f); // 降低速度，减小冲击
  }
}

void loop(){
  detect_sensor_fault();
  replan_for_fault();
  execute_adaptive_gait();
  delay(10);
}

要点解读
足端力需做滤波与归一化，提升触地检测准确率
原始力传感器数据包含高频噪声和瞬时尖峰，需采用滑动平均滤波（窗口5-10个采样点），再将力值映射为0-1的归一化值；触地阈值需提前校准，避免因传感器灵敏度差异导致误判。

步频动态调节需匹配运动状态，平衡效率与稳定
步频与足端受力、地形粗糙度呈负相关：足端受力越大（承重越多）、地形越崎岖，步频越低，减少足端冲击；平地轻载时步频适当提高，提升行走效率；同时需设置步频上下限，避免过高导致步态失稳。

触地检测需支持故障识别，避免步态失效
需监测传感器数据的异常值（持续为0或满量程），判定传感器故障；同时结合关节电流判断触地：足端触地后关节电流会上升，双重验证提升触地检测可靠性，避免因传感器问题导致步态错乱。

步态执行需加入缓冲机制，避免切换冲击
步频变化时需采用渐变过渡（每次调整幅度不超过0.2Hz），不能用瞬间切换，防止步态突变导致机器人倾倒；同时加入足端落地缓冲，触地瞬间降低关节速度，减少冲击力。

故障容错需遵循重心约束，优先保障机身稳定
传感器或关节故障时，需根据有效触地足数量调整步频：3足有效时维持中等步频，2足有效时降低步频，仅1足有效时慢速调整；同时停止故障关节运动，避免额外力矩导致机身晃动，保证安全。

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