“Arduino BLDC 之巡检机器人动态越野路况 + 智能步态 + 避障协同”可以理解为：机器人先用视觉、IMU、超声/激光、编码器、足端或轮端力觉感知地形与障碍，再把“地形类型、坡度、障碍高度、可通行区域”映射为步态参数或运动模态；在局部避障、速度约束和姿态稳定的协同下，由 BLDC FOC 完成柔顺执行。它既适用于四足/六足仿生机器人，也适用于轮腿复合巡检平台。
一、主要特点

1. 地形感知是多源融合的，不是单传感器判定
   越野路况通常包括碎石、泥地、台阶、沟渠、斜坡、金属格栅、电缆沟盖板等。单一超声或视觉容易受光照、反光、粉尘、镜面反射影响；IMU 能反映坡度和冲击，但不能判断前方障碍形状；电机电流能反映负载，但无法区分地面类型。工程上更稳妥的是把视觉/深度、IMU、超声/激光、编码器、电流、足端触觉等做时间同步和置信度融合。
2. 智能步态本质是“地形到运动参数的自适应映射”
   步态自适应通常包含几层：
   步态模式切换：平地步态、爬坡步态、台阶步态、碎石步态、试探步态；
   参数连续调制：步长、步高、步频、支撑相时长、机身高度、相位差随坡度/障碍高度渐变；
   CPG 或状态机生成：CPG 适合平滑切换和对角/三角步态生成；FSM + 预定义轨迹更适合资源受限平台；
   逆运动学映射：把足端轨迹或轮腿姿态转换到各关节目标角度；
   触地确认：通过足端压力、电流突变、编码器速度变化判断是否真实触地，再进入支撑相。
3. 避障与步态必须协同，不能各算各的
   如果避障只输出“左转/右转”，而步态仍按平地参数运行，机器人可能在转向时打滑或失稳。更合理的协同是：避障决定“能不能走、往哪走、走多快”，步态决定“怎么走、抬多高、支撑多久、重心如何迁移”。例如前方有低矮障碍时，避障模块降低速度并缩小转向半径，步态模块提高抬腿高度、缩短步长、增加支撑腿数量。
4. 轮腿复合平台需要模态切换策略
   轮腿巡检机器人在硬化路面可用轮式高速巡航；遇到台阶、沟渠、碎石或门槛时，再切换到腿式或轮腿混合越障。模态切换需要明确触发条件，例如坡度阈值、障碍高度、底盘离地间隙、支撑稳定性、地面可通行性评分，并加入迟滞，避免在临界地形频繁抖动。
5. BLDC FOC 是柔顺越障的执行基础
   越野越障时关节会频繁承受冲击、堵转和负载突变。BLDC 配合 FOC 可实现位置、速度、电流/力矩多环控制，使足端触地更柔顺、转向更平稳、越障时扭矩响应更快。相比普通 ESC 开环驱动，FOC 更适合低转速高扭矩、频繁微调的越野场景。
6. 分层架构更适合落地
   完整系统通常分为：
   感知层：相机、IMU、超声/激光、编码器、电流、足端触觉；
   决策层：地形分类、障碍检测、可通行区域评估、模态/步态选择；
   控制层：CPG/FSM 步态生成、逆运动学、姿态补偿、DWA/局部规划；
   执行层：BLDC FOC 电流环、速度环、位置环、故障保护。
   ATmega328P 可用于单腿关节控制、编码器采样、简单 FSM 步态和 FOC 执行；但视觉地形识别、多传感器融合、CPG 网络、完整逆运动学和局部规划更适合放在 ESP32-S3、STM32H7、Teensy 4.1 或上位机。
   二、应用场景
7. 变电站/配电房巡检
   地面可能有电缆沟盖板、门槛、金属格栅、油渍和狭窄通道。机器人需要识别可通行区域，避免滑入缝隙，并在设备间低速稳定移动。
8. 光伏/农业园区巡检
   草地、泥地、碎石路、田埂和斜坡较多，轮式平台容易打滑或托底。四足或轮腿平台可通过步态自适应保持机身稳定，同时完成设备状态采集。
9. 厂区管廊与地下通道巡检
   环境光照弱、地面不平、管道和支架密集。机器人需要在狭窄空间内绕行，并在台阶、坡道、积水区域保持通过性。
10. 灾后搜救与废墟穿越
    地面存在瓦砾、倾斜板、缝隙和视觉盲区。此时视觉可能失效，需要依赖触觉、电流、IMU 和试探步态进行低速越障。
11. 校园/园区安防巡逻
    铺装路面与绿化带交替出现，行人、宠物、车辆、路缘石共存。系统需要在高速巡航与低速越障之间平滑切换，并保证动态避障安全。
12. 科研与竞赛平台
    适合验证 CPG 步态、地形分类、IMU 姿态补偿、轮腿模态切换、DWA 避障、FOC 力矩控制和多传感器融合。
    三、需要注意的事项
13. 地形分类必须有置信度，不能只看一次测量
    超声、视觉、IMU 都可能出现瞬时误检。应使用滑动窗口、多传感器投票或卡尔曼滤波，避免因为一帧噪声就把平地误判为台阶，从而触发不必要的步态切换。
14. 步态切换要平滑，不能硬跳变
    从平地切到爬坡或台阶时，步长、步高、步频、机身高度应渐变。硬切换会导致重心突变、支撑多边形破坏，甚至摔倒。
15. 支撑稳定性优先于通行速度
    越障时应保证足够数量的支撑腿或稳定支撑多边形。摆动相抬腿过高会增加能耗和冲击，过低则容易磕碰；应根据障碍高度加上安全余量动态调整。
16. 视觉落足点必须用本体感知二次确认
    视觉可能把松软地面、湿滑台阶或玻璃误判为可通行区域。落足前应结合足端触觉、电流变化、编码器速度和 IMU 冲击进行验证。
17. 避障硬优先级高于步态执行
    无论步态多么适配当前地形，只要前方出现行人、车辆、悬崖、深沟或不可通行障碍，都应先减速、停步或绕行。算法层面的协同不能替代防撞条、悬崖传感器和急停保护。
18. BLDC 必须做电流、温度和编码器保护
    越野越障时关节容易短时堵转。应设置 q 轴电流上限、过温保护、编码器失联检测、姿态异常停机和故障降级策略，避免电机烧毁或机身倾覆。
19. 控制周期要稳定，避免被视觉或通信阻塞
    FOC 电流环、关节位置环和步态相位更新需要确定性周期。视觉识别、地图更新和远程通信应放在独立任务或上位机，底层 MCU 优先保证电机闭环实时性。
20. 轮腿平台要防止模态抖动
    如果障碍高度在切换阈值附近波动，机器人可能在轮式和腿式之间反复切换。应加入迟滞、最小保持时间和地形稳定性评分，确保模态切换后能稳定执行一段时间。
21. 标定环节不能省略
    机械零位、IMU 安装姿态、足端触地阈值、轮径、轮距、腿长、编码器方向都需要标定。否则逆运动学、姿态补偿和落足点规划都会产生系统性偏差。
22. 8 位 Arduino 更适合做执行层，不建议承担完整越野感知
    ATmega328P 可用于 BLDC FOC 执行、编码器采样、IMU 读取、简单步态状态机和基础避障；但视觉地形识别、CPG 网络、逆运动学、DWA 规划、轮腿模态决策更适合由高性能 MCU 或上位机承担。

整体来看，这个架构的关键是：感知层判断“地形和障碍是什么”，决策层决定“用什么步态和速度”，协同层保证“避障、姿态、重心、模态不冲突”，执行层由 BLDC FOC 把越障动作柔顺落地。做原型时，建议先从“IMU 坡度检测 + 固定步态库 + BLDC 关节闭环”开始，再加入“超声/视觉障碍检测 → 步态参数自适应 → 避障协同 → 触地确认 → 轮腿模态切换”。

4、四驱轮式巡检机器人——地形分类 + 步态库渐变切换 + 避障协同
适用场景：园区或野外巡检的四驱 BLDC 底盘，需要根据碎石、斜坡、浅沟等连续变化地形自动调整速度与扭矩分配，同时避开临时堆放的设备或人员。核心逻辑是“感知地形类型 → 匹配步态库 → 避障逻辑叠加”。

#include <SimpleFOC.h>

// 四驱 BLDC 配置
BLDCMotor motorFL(7), motorFR(7), motorRL(7), motorRR(7);
BLDCDriver3PWM driverFL(9,10,11,8), driverFR(3,5,6,7);
BLDCDriver3PWM driverRL(44,45,46,43), driverRR(41,42,40,39);
Encoder encFL(18,19,2048), encFR(20,21,2048);
Encoder encRL(22,23,2048), encRR(24,25,2048);

// 传感器
const int SONAR_FRONT_TRIG = 30, SONAR_FRONT_ECHO = 31;
const int SONAR_DOWN_TRIG  = 32, SONAR_DOWN_ECHO  = 33;
const int IMU_SDA = 34, IMU_SCL = 35;

// 地形类型
enum TerrainType { ROAD_SLOPE, ROAD_GRAVEL, ROAD_DITCH, ROAD_FLAT };
TerrainType currentTerrain = ROAD_FLAT;

// 步态库：速度、左右偏置、转向修正
struct GaitParams {
  float baseSpeed;      // rad/s
  float leftOffset;     // 左轮速度偏移
  float rightOffset;    // 右轮速度偏移
  float steerAdjust;    // 转向修正系数
};
GaitParams gaitLib[4] = {
  {2.5, 0.0, 0.0, 0.3},    // 斜坡：定向校正
  {3.0, -0.2, 0.2, 0.0},   // 碎石：左右微调
  {1.5, -0.5, 0.5, 0.2},   // 浅沟：大偏移+低速
  {4.0, 0.0, 0.0, 0.0}     // 平地：全速
};
GaitParams currentGait = gaitLib[3];

float frontDist = 999, groundDist = 999;
unsigned long lastSonarRead = 0;

float readSonar(int trig, int echo) {
  digitalWrite(trig, LOW); delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long dur = pulseIn(echo, HIGH, 20000);
  if (dur == 0) return 999;
  return dur * 0.0343 / 2.0; // cm
}

void initMotor(BLDCMotor &m, BLDCDriver3PWM &d, Encoder &e) {
  e.init(); m.linkSensor(&e);
  d.voltage_power_supply = 24; d.init();
  m.linkDriver(&d);
  m.controller = MotionControlType::velocity;
  m.init(); m.initFOC();
}

void setup() {
  Serial.begin(115200);
  pinMode(SONAR_FRONT_TRIG, OUTPUT); pinMode(SONAR_FRONT_ECHO, INPUT);
  pinMode(SONAR_DOWN_TRIG, OUTPUT);  pinMode(SONAR_DOWN_ECHO, INPUT);

  initMotor(motorFL, driverFL, encFL);
  initMotor(motorFR, driverFR, encFR);
  initMotor(motorRL, driverRL, encRL);
  initMotor(motorRR, driverRR, encRR);
}

TerrainType classifyTerrain() {
  // 基于向下超声波的地面波动 + 前方障碍距离
  if (groundDist < 8.0) return ROAD_DITCH;       // 地面距离异常近 → 浅沟或凸起
  if (frontDist < 40.0 && groundDist < 15.0) return ROAD_GRAVEL;
  if (frontDist > 80.0 && groundDist > 25.0) return ROAD_FLAT;
  return ROAD_SLOPE;
}

void applyGait(TerrainType target) {
  // 渐变过渡，避免硬切换冲击
  GaitParams &from = currentGait;
  GaitParams &to = gaitLib[target];
  float alpha = 0.15; // 每周期过渡 15%

  from.baseSpeed   += (to.baseSpeed   - from.baseSpeed)   * alpha;
  from.leftOffset  += (to.leftOffset  - from.leftOffset)  * alpha;
  from.rightOffset += (to.rightOffset - from.rightOffset) * alpha;
  from.steerAdjust += (to.steerAdjust - from.steerAdjust) * alpha;

  if (abs(from.baseSpeed - to.baseSpeed) < 0.05) currentTerrain = target;
}

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

  // 传感器读取（10Hz）
  if (millis() - lastSonarRead > 100) {
    lastSonarRead = millis();
    frontDist  = readSonar(SONAR_FRONT_TRIG, SONAR_FRONT_ECHO);
    groundDist = readSonar(SONAR_DOWN_TRIG, SONAR_DOWN_ECHO);

    TerrainType detected = classifyTerrain();
    if (detected != currentTerrain) applyGait(detected);
  }

  // 避障叠加：前方障碍减速/转向
  float obstacleScale = 1.0;
  float steerBias = 0.0;
  if (frontDist < 30.0) {
    obstacleScale = 0.3;
    steerBias = (frontDist < 20.0) ? 0.8 : 0.3;
  }

  // 四驱执行
  float base = currentGait.baseSpeed * obstacleScale;
  float vL = base + currentGait.leftOffset;
  float vR = base + currentGait.rightOffset;
  float steer = steerBias * currentGait.steerAdjust;

  motorFL.move((vL - steer) / 0.05);
  motorRL.move((vL - steer) / 0.05);
  motorFR.move((vR + steer) / 0.05);
  motorRR.move((vR + steer) / 0.05);

  delay(20);
}

解读：该案例直接映射了搜索结果中“动态越野路况+智能步态+避障协同”的代码结构：gaitLib 数组对应斜坡/碎石/浅沟/平地的步态参数，applyGait() 采用渐变过渡而非硬切换，避障逻辑以 obstacleScale 和 steerBias 的形式叠加到步态输出上，而非独立抢占。BLDC FOC 的四轮独立速度环确保渐变过程中各轮转矩平稳，避免地形切换时的顿挫。

2、四足巡检机器人——足端力触觉反馈 + 步频自适应 + 避障相位修正
适用场景：四足构型的巡检机器人在碎石、坑洼地形中行走，需要根据足端触地冲击力实时调整步频，防止硬地高频冲击损坏关节或松软地面陷足。避障逻辑通过超声波触发相位提前切换。

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

// 四足 8 关节 BLDC
BLDCMotor joints[8] = {
  BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7),
  BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)
};
BLDCDriver3PWM drivers[8] = {
  BLDCDriver3PWM(2,3,4,5), BLDCDriver3PWM(6,7,8,9),
  BLDCDriver3PWM(10,11,12,13), BLDCDriver3PWM(14,15,16,17),
  BLDCDriver3PWM(22,23,24,25), BLDCDriver3PWM(26,27,28,29),
  BLDCDriver3PWM(30,31,32,33), BLDCDriver3PWM(34,35,36,37)
};
Encoder encs[8] = {
  Encoder(18,19,2048), Encoder(20,21,2048),
  Encoder(38,39,2048), Encoder(40,41,2048),
  Encoder(42,43,2048), Encoder(44,45,2048),
  Encoder(46,47,2048), Encoder(48,49,2048)
};

// 足端力传感器（模拟）
const int FORCE_PINS[4] = {A0, A1, A2, A3};
float footForces[4] = {0};

// IMU
MPU6050 imu;
float pitchAngle = 0;

// 避障超声波
#define TRIG_F 50
#define ECHO_F 51
float frontDist = 999;

// 步态参数
float stepFreq = 1.2;          // 步频 Hz
const float MIN_FREQ = 0.6;
const float MAX_FREQ = 2.0;
const float FORCE_THRESHOLD = 15.0; // 冲击力阈值
float phaseAngle = 0;
int currentPhase = 0;           // 0=左前+右后，1=右前+左后
float phaseCorrection = 0;      // 避障触发的相位提前量

float readSonar() {
  digitalWrite(TRIG_F, LOW); delayMicroseconds(2);
  digitalWrite(TRIG_F, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG_F, LOW);
  long dur = pulseIn(ECHO_F, HIGH, 15000);
  return (dur == 0) ? 999 : dur * 0.0343 / 2.0;
}

void setup() {
  Serial.begin(115200);
  Wire.begin();
  imu.initialize();
  imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2G);
  imu.setFullScaleGyroRange(MPU6050_GYRO_FS_250DPS);
  pinMode(TRIG_F, OUTPUT); pinMode(ECHO_F, INPUT);

  for (int i = 0; i < 8; i++) {
    encs[i].init();
    joints[i].linkSensor(&encs[i]);
    drivers[i].voltage_power_supply = 12;
    drivers[i].init();
    joints[i].linkDriver(&drivers[i]);
    joints[i].controller = MotionControlType::angle;
    joints[i].init(); joints[i].initFOC();
  }
}

void loop() {
  for (int i = 0; i < 8; i++) joints[i].loopFOC();

  // 足端力读取
  float maxImpact = 0;
  for (int i = 0; i < 4; i++) {
    footForces[i] = analogRead(FORCE_PINS[i]) * 0.1;
    footForces[i] = constrain(footForces[i], 0, 50);
    if (footForces[i] > maxImpact) maxImpact = footForces[i];
  }

  // IMU 俯仰
  int16_t ax, ay, az;
  imu.getAcceleration(&ax, &ay, &az);
  pitchAngle = atan2(ay, az) * 180 / PI;

  // 避障检测：前方近距障碍 → 相位提前 + 步频降低
  frontDist = readSonar();
  if (frontDist < 25.0) {
    phaseCorrection = 0.3;  // 提前切换相位，准备转向/越障
    stepFreq = max(MIN_FREQ, stepFreq - 0.1);
  } else {
    phaseCorrection *= 0.9; // 逐渐恢复
  }

  // 触觉反馈：冲击力大 → 降低步频提升稳定性；冲击小 → 可提升步频
  if (maxImpact > FORCE_THRESHOLD) {
    stepFreq = max(MIN_FREQ, stepFreq - 0.05);
  } else {
    stepFreq = min(MAX_FREQ, stepFreq + 0.02);
  }

  // 基于俯仰角修正相位
  float tiltCorrection = pitchAngle * 0.005;
  phaseAngle += (2 * PI * stepFreq * 0.02) + phaseCorrection + tiltCorrection;

  if (phaseAngle >= PI) {
    phaseAngle -= PI;
    currentPhase = 1 - currentPhase;
  }

  // TODO: 根据 currentPhase 和步态参数生成 8 关节目标角度
  // 此处省略逆运动学解算，实际需根据机械结构计算各关节 angle

  delay(20);
}

解读：该案例借鉴了搜索结果中四足机器人“足端力反馈调整步频 + IMU 俯仰修正相位”的控制逻辑。maxImpact 超过阈值时降低步频，使足端有更长时间吸收冲击，避免硬地高频振动损坏关节；避障超声波检测到近距障碍时，phaseCorrection 人为提前相位切换，使机器人更早进入转向或越障姿态。BLDC 的 FOC 角度环在此场景中需要配合逆运动学，Arduino 算力有限时可使用预计算查表法降低实时解算负担。

3、轮足混合构型——滚动窗口地形规划 + 动态避障 + 步态执行协同
适用场景：轮足混合巡检机器人（轮式移动 + 可抬升腿部越障），需要在滚动窗口内同时处理地形可通行性评估和动态障碍物避让，输出轮速与抬腿动作的协同指令。

#include <SimpleFOC.h>
#include <vector>

// 轮驱 BLDC
BLDCMotor wheelL(7), wheelR(7);
BLDCDriver3PWM wDriverL(9,10,11,8), wDriverR(3,5,6,7);
Encoder wEncL(18,19,2048), wEncR(20,21,2048);

// 抬腿关节 BLDC
BLDCMotor legL(7), legR(7);
BLDCDriver3PWM lDriverL(22,23,24,25), lDriverR(26,27,28,29);
Encoder lEncL(30,31,2048), lEncR(32,33,2048);

// 滚动窗口参数
const float WINDOW_RADIUS = 3.0;  // m
const unsigned long PLAN_CYCLE = 2000; // ms
unsigned long lastPlan = 0;

// 传感器
#define SONAR_F 40
#define SONAR_L 41
#define SONAR_R 42
float distF = 999, distL = 999, distR = 999;

// 地形障碍物（简化：窗口内网格）
struct GridCell {
  bool occupied;
  float cost; // 地形代价：0=平坦，1=粗糙
};
const int GRID_RES = 10;
GridCell grid[GRID_RES][GRID_RES];

// 当前执行
float targetWheelSpeed = 0;
float targetLegLift = 0; // 0=放下，1=抬起
bool obstacleDetected = false;

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

void setup() {
  Serial.begin(115200);
  pinMode(SONAR_F, OUTPUT);
  // 简化：三路超声波共用 trig 的配置略，实际需独立引脚

  // 轮驱初始化
  wEncL.init(); wEncR.init();
  wheelL.linkSensor(&wEncL); wheelR.linkSensor(&wEncR);
  wDriverL.voltage_power_supply = 24; wDriverR.voltage_power_supply = 24;
  wDriverL.init(); wDriverR.init();
  wheelL.linkDriver(&wDriverL); wheelR.linkDriver(&wDriverR);
  wheelL.controller = MotionControlType::velocity;
  wheelR.controller = MotionControlType::velocity;
  wheelL.init(); wheelL.initFOC();
  wheelR.init(); wheelR.initFOC();

  // 抬腿初始化
  lEncL.init(); lEncR.init();
  legL.linkSensor(&lEncL); legR.linkSensor(&lEncR);
  lDriverL.voltage_power_supply = 24; lDriverR.voltage_power_supply = 24;
  lDriverL.init(); lDriverR.init();
  legL.linkDriver(&lDriverL); legR.linkDriver(&lDriverR);
  legL.controller = MotionControlType::angle;
  legR.controller = MotionControlType::angle;
  legL.init(); legL.initFOC();
  legR.init(); legR.initFOC();
}

void updateGrid() {
  // 模拟：根据超声波填充窗口网格
  for (int i = 0; i < GRID_RES; i++)
    for (int j = 0; j < GRID_RES; j++)
      grid[i][j] = {false, 0.0};
  // 前方近距 → 标记为障碍
  if (distF < 1.5) {
    int center = GRID_RES / 2;
    grid[center][center + 1].occupied = true;
  }
  // 侧方近距 → 标记粗糙
  if (distL < 0.8 || distR < 0.8) {
    grid[GRID_RES/2][GRID_RES/2].cost = 1.0;
  }
}

void planPath() {
  // 简化：窗口内搜索最优局部目标
  obstacleDetected = (distF < 1.0) || (distL < 0.5) || (distR < 0.5);

  if (obstacleDetected) {
    targetWheelSpeed = 0.3;
    // 前方障碍 → 抬腿越障；侧方障碍 → 转向
    if (distF < 1.0 && distL > 0.8) {
      targetLegLift = 1.0;  // 抬腿准备越障
    } else if (distL < 0.5) {
      // 左侧近 → 右转
      targetWheelSpeed = 0.2;
    }
  } else {
    targetWheelSpeed = 1.2;
    targetLegLift = 0.0;
  }
}

void loop() {
  wheelL.loopFOC(); wheelR.loopFOC();
  legL.loopFOC(); legR.loopFOC();

  // 传感器更新
  distF = readSonar(SONAR_F, SONAR_F + 10); // 实际需正确 echo 引脚
  // distL, distR 略

  // 滚动窗口重规划
  if (millis() - lastPlan > PLAN_CYCLE) {
    lastPlan = millis();
    updateGrid();
    planPath();
  }

  // 执行：轮速 + 抬腿
  wheelL.move(targetWheelSpeed / 0.05);
  wheelR.move(targetWheelSpeed / 0.05);

  // 抬腿角度目标（0=放下，1.2 rad=抬起）
  float legTarget = targetLegLift * 1.2;
  legL.move(legTarget);
  legR.move(legTarget);

  delay(20);
}

解读：该案例融合了搜索结果中“滚动窗口地形规划”与四足越障的“抬腿执行”逻辑。滚动窗口每 2 秒更新一次网格，将动态障碍物与地形代价统一标记；planPath() 根据障碍方向决定“抬腿越障”还是“转向绕行”，而非将所有障碍统一处理为绕行。BLDC 在此架构中承担双重角色：轮驱负责平面移动，抬腿关节负责垂直越障，两者的 FOC 环独立运行但由同一决策层协同调度。

要点解读

1. 地形识别与避障在决策层应“统一表示、分级处理”，而非两套独立系统
   越野巡检的核心矛盾在于：地形特征（碎石、沟渠）是“可通行但需要调整步态”，障碍物（设备、人员）是“不可通行或需要绕行”。案例三的网格表示将两者统一为 GridCell {occupied, cost}：occupied 对应不可通行障碍，cost 对应需要降速或调整步态的粗糙地形。这样避障与步态调度共享同一感知地图，避免“避障系统说前方有障碍、步态系统说地形可通行”的冲突。搜索结果中滚动窗口方案将移动障碍物与不可通行地形统一标记为障碍物并按优先级处理，也是同一设计思路。

2. 步态切换必须渐变，硬切换的冲击可能损坏关节或导致失稳
   案例一的 applyGait() 采用 alpha=0.15 的每周期插值过渡，而非直接赋值。四驱轮式底盘在碎石→平地的硬切换会导致轮速突变，轮胎打滑或电机过流；四足构型在步频突变时可能因惯性导致机身俯仰超调。搜索结果中四足步态库切换的讨论同样强调“平滑过渡机制”是避免机身抖动或倾覆的关键。Arduino 主循环频率有限时，渐变过渡的 alpha 值不宜过大（建议 0.1-0.2），否则过渡时间过长，响应跟不上地形变化。

3. 触觉反馈（足端力/电流）在越野场景中的价值高于纯视觉预判
   案例二的 maxImpact 阈值逻辑来自搜索结果中“足端力传感器感知触地冲击力，实时调整步频”的方案。视觉或超声波可以“预判”前方是碎石，但无法知道足端实际踩上去的冲击有多硬。触觉反馈提供的是确认信号：如果视觉判断为平地但足端力峰值超阈值，说明实际地面比预期更硬或有隐藏凸起，应立即降步频。轮式底盘虽无足端力，但可通过电机电流波动间接判断地面粗糙度，逻辑等价。

4. 避障在巡检机器人中应作为“叠加修正”而非“独立抢占”，除非距离极近
   案例一将避障输出为 obstacleScale（速度缩放）和 steerBias（转向偏置），叠加到步态输出上；案例三在 distF < 1.0m 时才触发抬腿越障的独立动作。这种分级设计的理由是：巡检机器人的主要任务是“沿路径巡检”，频繁的避障抢占会导致步态频繁中断、巡检效率低下。只有当障碍物进入“必须立即处理”的距离（如 <1m 或 <30cm）时，避障才应完全接管输出。搜索结果中电力巡检的分层架构同样采用“超声波硬优先级仅在近距时干预”的设计。

5. Arduino 算力约束决定了“步态库查表 + 轻量分类”是唯一可行的实时方案
   四足逆运动学实时解算、遗传算法路径规划在 Arduino 上均不可行。搜索结果中的滚动窗口方案虽然提及“遗传算法”，但实际实现被简化为 10×10 网格的简化搜索；四足步态切换的核心也被描述为“预先构建步态库，运行时查表+微调”。案例一的 gaitLib 数组和案例三的简化网格搜索都是对这一约束的回应。若需要更复杂的地形分类（如基于 IMU 频谱分析的粗糙度识别），应将特征提取后的简单阈值判断放在 Arduino 端，复杂分类交由协处理器完成并下发地形标签。

4、地形特征提取 + 步态参数库切换（轮腿式巡检车）
适用硬件：ESP32 + SimpleFOC 差速/轮腿底盘（2~4×BLDC）+ 编码器 + 九轴 IMU + 前向 TOF + 电流采样。
核心思路：越野不能靠单一传感器判断路况，而要提取一组特征（粗糙度、坡度、滑移率、阻力波动）→ 归一化成地形等级 0~3 → 查参数库。等级是唯一的中间量，所有执行参数（速度、加速度、悬挂刚度、避障阈值）都由它派生。

/*
 * 案例4：地形特征 -> 等级(0~3) -> 参数库 -> 执行
 * 特征：RMS(垂直加速度) / pitch / 滑移率 / 电流波动
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

BLDCMotor wheelL(7), wheelR(7);
BLDCDriver3PWM drvL(9,10,11,8), drvR(22,23,24,25);
Encoder encL(2,3,2048), encR(32,33,2048);
void onL(){ encL.handleA(); } void onR(){ encR.handleA(); }
MPU6050 imu;

const float R_WHEEL = 0.065f, TRACK = 0.34f;

// ---- 滑动窗口特征 ----
#define WIN 25                    // 25 × 20ms = 0.5s
float accBuf[WIN]; int ai = 0;
float curBuf[WIN]; int ci = 0;
float aRms = 0, iRipple = 0;

void pushSample(float az, float iSum) {
  accBuf[ai] = az; ai = (ai+1)%WIN;
  curBuf[ci] = iSum; ci = (ci+1)%WIN;
  float s = 0;
  for (int k=0;k<WIN;k++) s += accBuf[k]*accBuf[k];
  aRms = sqrtf(s/WIN);
  float mn=1e9, mx=-1e9;
  for (int k=0;k<WIN;k++){ mn=min(mn,curBuf[k]); mx=max(mx,curBuf[k]); }
  iRipple = mx - mn;
}

// ---- 滑移率：指令速度 vs 里程速度 ----
float slipRatio(float vCmd, float vOdo) {
  if (vCmd < 0.03f) return 0;
  return constrain((vCmd - vOdo) / vCmd, 0.0f, 1.0f);
}

// ---- 地形等级（带迟滞，防抖）----
int grade = 0;
int gradeOf(float rms, float pitchAbs, float slip) {
  float score = 0;
  score += (rms > 3.0f) ? 2.0f : (rms > 1.5f) ? 1.0f : 0.0f;
  score += (pitchAbs > 12.0f) ? 1.5f : (pitchAbs > 6.0f) ? 0.8f : 0.0f;
  score += (slip > 0.30f) ? 2.0f : (slip > 0.12f) ? 1.0f : 0.0f;
  if (score >= 4.0f) return 3;      // 极崎岖 / 大坡度 / 强打滑
  if (score >= 2.2f) return 2;      // 崎岖
  if (score >= 0.9f) return 1;      // 轻微起伏
  return 0;                         // 平整
}
// 迟滞：升级快、降级慢（路况恶化要立刻反应，好转要稳住）
int smoothGrade(int cur, int raw, unsigned long &sinceMs) {
  if (raw > cur) { sinceMs = millis(); return raw; }
  if (raw < cur && millis() - sinceMs > 2000) { sinceMs = millis(); return raw; }
  return cur;
}

// ---- 参数库：地形等级 -> 执行参数 ----
struct GaitParams {
  float vmax;      // 速度上限 m/s
  float amax;      // 加速度上限 m/s²
  float stiff;     // 悬挂/腿部刚度系数（0.3~1.0）
  float obsStop;   // 避障停止距离 m
  float obsSlow;   // 避障减速距离 m
};
const GaitParams LIB[4] = {
  {0.90f, 0.80f, 1.00f, 0.30f, 0.90f},   // 0 平整：快、硬、近距避障
  {0.65f, 0.55f, 0.80f, 0.40f, 1.10f},   // 1 起伏：略软
  {0.40f, 0.30f, 0.55f, 0.50f, 1.30f},   // 2 崎岖：慢、软、远距避障
  {0.22f, 0.15f, 0.35f, 0.60f, 1.50f},   // 3 极差：龟速、最软、最保守
};

const int TRIG=6, ECHO=7;
float readUS() {
  digitalWrite(TRIG, LOW); delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10); digitalWrite(TRIG, LOW);
  long d = pulseIn(ECHO, HIGH);
  return (d>0) ? d*0.034f/2.0f/100.0f : 9.0f;
}

float px=0, py=0, pth=0, vCmd=0;
unsigned long gradeT0 = 0;

void setup() {
  encL.init(); encL.enableInterrupts(onL);
  encR.init(); encR.enableInterrupts(onR);
  wheelL.linkSensor(&encL); wheelR.linkSensor(&encR);
  wheelL.controller = wheelR.controller = MotionControlType::velocity;
  wheelL.PID_velocity.P = 0.35f; wheelL.PID_velocity.I = 4.0f;
  wheelR.PID_velocity.P = 0.35f; wheelR.PID_velocity.I = 4.0f;
  wheelL.voltage_limit = wheelR.voltage_limit = 12.0f;
  wheelL.current_limit = wheelR.current_limit = 5.0f;
  wheelL.init(); wheelR.init();
  drvL.voltage_power_supply = drvR.voltage_power_supply = 12.0f;
  drvL.init(); drvR.init();
  wheelL.linkDriver(&drvL); wheelR.linkDriver(&drvR);
  Wire.begin(); imu.initialize();
  pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);
  gradeT0 = millis();
  Serial.begin(115200);
}

void loop() {
  // 1) 感知
  imu.update();
  float az = imu.getAccelerationZ() / 9.81f;              // g
  float pitchAbs = fabsf(imu.getPitch());
  float vl = wheelL.shaft_velocity * R_WHEEL;
  float vr = wheelR.shaft_velocity * R_WHEEL;
  float vOdo = 0.5f*(vl+vr);
  float iSum = fabsf(wheelL.current) + fabsf(wheelR.current);

  pushSample(az, iSum);
  float slip = slipRatio(vCmd, vOdo);

  // 2) 地形等级（特征融合 + 迟滞）
  int raw = gradeOf(aRms, pitchAbs, slip);
  // 电流波动大也是"路面碎"的证据，直接抬一级
  if (iRipple > 2.0f && raw < 2) raw++;
  grade = smoothGrade(grade, raw, gradeT0);

  // 3) 参数 + 避障
  const GaitParams &P = LIB[grade];
  float dF = readUS();
  float v = P.vmax;
  if (dF < P.obsStop)      v = 0;
  else if (dF < P.obsSlow) v = P.vmax * (dF - P.obsStop) / (P.obsSlow - P.obsStop);

  // 4) 加速度限幅（越野必须限制冲击，不只是限速）
  static float vPrev = 0;
  float dv = constrain(v - vPrev, -P.amax*0.02f, P.amax*0.02f);
  vCmd = vPrev + dv; vPrev = vCmd;

  // 5) 执行（stiff 可映射到悬挂舵机/腿部预压，此处示意为增益缩放）
  wheelL.PID_velocity.P = 0.35f * (0.6f + 0.4f*P.stiff);
  wheelR.PID_velocity.P = 0.35f * (0.6f + 0.4f*P.stiff);
  wheelL.loopFOC(); wheelR.loopFOC();
  wheelL.move(vCmd/R_WHEEL); wheelR.move(vCmd/R_WHEEL);

  Serial.print("g="); Serial.print(grade);
  Serial.print(" rms="); Serial.print(aRms,2);
  Serial.print(" slip="); Serial.print(slip,2);
  Serial.print(" v="); Serial.println(vCmd,2);
  delay(20);
}

关键点：grade 是唯一的中间变量，所有参数从 LIB[grade] 查表，任何新传感器只能改进“等级判定”，不改执行层——这是越野参数化的可维护性所在。

5、FreeRTOS 三层 + DWA 与地形可通行性双向耦合
适用硬件：ESP32 双核 + 差速 BLDC + IMU + 前向/侧向 TOF/超声。
核心思路：避障和越野不能各算各的。地形 → 速度窗口（正向约束）：DWA 的 vmax/amax 由地形等级限制；避障 → 地形权重（反向约束）：前方障碍近时，地形评估更保守（同一段路，被堵住时不允许“赌一把冲过去”）。这是真正意义上的“协同算法”。

/*
 * 案例5：地形层(10Hz) - DWA层(20Hz) - 执行层(1kHz)
 * 耦合：地形限制 DWA 窗口；障碍距离反向加权地形代价
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor wheelL(7), wheelR(7);
// 驱动/编码器初始化同案例一（略）
MPU6050 imu;
SemaphoreHandle_t mux;
const float R_WHEEL=0.065f, TRACK=0.34f, R_ROBOT=0.22f;

// ---- 地形层输出 ----
volatile int   s_grade = 0;
volatile float s_vmax = 0.9f, s_amax = 0.8f;
volatile float s_slip = 0;
// ---- 感知 ----
volatile float s_dF = 9, s_dL = 9, s_dR = 9;
volatile float s_v = 0, s_w = 0;
// ---- 执行 ----
volatile float c_v = 0, c_w = 0;
volatile bool  c_estop = false;

// 地形等级 -> 动力学上限（含打滑修正）
void gradeToLimits(int g, float slip, float &vmax, float &amax) {
  const float VT[4] = {0.90f, 0.65f, 0.40f, 0.22f};
  const float AT[4] = {0.80f, 0.55f, 0.30f, 0.15f};
  vmax = VT[g]; amax = AT[g];
  vmax *= (1.0f - 0.6f * constrain(slip, 0, 1));   // 打滑直接削速度
  amax *= (1.0f - 0.4f * constrain(slip, 0, 1));
  vmax = max(vmax, 0.10f);                          // 保底可蠕动
}

// ---- 任务1：地形层 10Hz ----
void taskTerrain(void*) {
  TickType_t last = xTaskGetTickCount();
  static float buf[10]; static int i = 0;
  float vCmdPrev = 0;
  for (;;) {
    imu.update();
    float az = imu.getAccelerationZ()/9.81f;
    buf[i] = az; i = (i+1)%10;
    float s=0; for(int k=0;k<10;k++) s += buf[k]*buf[k];
    float rms = sqrtf(s/10);

    xSemaphoreTake(mux, portMAX_DELAY);
    float vOdo = s_v;
    xSemaphoreGive(mux);

    float slip = (vCmdPrev > 0.05f) ? constrain((vCmdPrev-vOdo)/vCmdPrev,0,1) : 0;

    float score = ((rms>3.0f)?2.0f:(rms>1.5f)?1.0f:0)
                + ((fabsf(imu.getPitch())>12)?1.5f:(fabsf(imu.getPitch())>6)?0.8f:0)
                + ((slip>0.30f)?2.0f:(slip>0.12f)?1.0f:0);
    int raw = (score>=4)?3:(score>=2.2f)?2:(score>=0.9f)?1:0;

    int g = s_grade;
    g = (raw > g) ? raw : (raw < g && (millis()%3000<10)) ? raw : g;  // 升快降慢
    float vmax, amax; gradeToLimits(g, slip, vmax, amax);

    xSemaphoreTake(mux, portMAX_DELAY);
    s_grade = g; s_vmax = vmax; s_amax = amax; s_slip = slip;
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(100));
  }
}

// ---- 任务2：DWA 层 20Hz（地形-避障双向耦合）----
void taskDWA(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    int g; float vmax, amax, dF, vCur, wCur;
    xSemaphoreTake(mux, portMAX_DELAY);
    g=s_grade; vmax=s_vmax; amax=s_amax; dF=s_dF; vCur=s_v; wCur=s_w;
    xSemaphoreGive(mux);

    // 【反向耦合】障碍越近，地形判定越保守：可用速度再打折
    float caution = constrain(1.0f - dF/1.2f, 0.0f, 0.6f);
    float vCap = vmax * (1.0f - 0.7f*caution);

    // 动态窗口（含地形加速度上限）
    float vLo = max(-vCap, vCur - amax*0.05f);
    float vHi = min( vCap, vCur + amax*0.05f);
    float wHi = 1.4f * (1.0f - 0.4f*caution);

    float bestV=0, bestW=0, bestS=-1e9f;
    for (float v=vLo; v<=vHi+1e-6f; v+=0.05f) {
      // 可停性：刹车距离（用地形加速度，不是名义值）
      if ((v*v)/(2*max(amax,0.05f)) + R_ROBOT >= dF) continue;
      for (float w=-wHi; w<=wHi+1e-6f; w+=0.10f) {
        float clear; float sc = evalTraj(v, w, clear);   // 仿真打分（同前案例）
        if (sc < -1e8f) continue;
        // 【正向耦合】地形等级直接进入代价：崎岖时更看重余量
        sc -= 0.6f * g * (1.0f - constrain(clear/1.0f,0,1));
        if (sc > bestS) { bestS=sc; bestV=v; bestW=w; }
      }
    }
    if (bestS < -1e8f) { bestV=0; bestW=0; }

    xSemaphoreTake(mux, portMAX_DELAY);
    c_v = bestV; c_w = bestW;
    c_estop = (bestS < -1e8f);
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(50));
  }
}

// ---- 任务3：执行 1kHz（含地形加速度限幅）----
void taskExec(void*) {
  TickType_t last = xTaskGetTickCount();
  float vC=0, wC=0;
  for (;;) {
    float v,w,amax; bool es;
    xSemaphoreTake(mux, portMAX_DELAY);
    v=c_v; w=c_w; es=c_estop; amax=s_amax;
    xSemaphoreGive(mux);
    if (es) { v=0; w=0; }
    vC += constrain(v - vC, -amax*0.001f, amax*0.001f);
    wC += constrain(w - wC, -2.5f*0.001f, 2.5f*0.001f);
    float vL=vC-wC*TRACK*0.5f, vR=vC+wC*TRACK*0.5f;
    wheelL.loopFOC(); wheelR.loopFOC();
    wheelL.move(vL/R_WHEEL); wheelR.move(vR/R_WHEEL);

    xSemaphoreTake(mux, portMAX_DELAY);
    s_v = vC; s_w = wC;
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
  }
}

void setup() {
  // 电机初始化同案例一（略）
  Wire.begin(); imu.initialize();
  mux = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(taskTerrain, "terr", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskDWA,     "dwa",  4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskExec,    "exec", 4096, NULL, 3, NULL, 1);
}
void loop() { vTaskDelay(portMAX_DELAY); }

双向耦合的两个方向都很重要：地形只单向限制速度（正向）会漏掉“障碍+烂路同时出现”的最危险工况；而障碍只单向避让（反向）会出现在松软地面上急打方向导致侧滑。两者同时进入窗口与代价，才是“协同”。

6、四足越野 —— 落脚点评估 + 步态切换 + 滑移降级
适用硬件：ESP32 + 8×SimpleFOC BLDC（4 腿 × 髋/膝）+ 编码器 + IMU + 4×足端接触（或电流触地）。
核心思路：足式越野的“智能”体现在落脚点选择而非抬腿动作。每步为摆动腿生成 3 个候选落点，用代价函数（粗糙度 + 坡度 + 障碍距离 + 偏离理想位置）选优；同时按地形等级在 walk（静态稳定）/ trot（动态高效）间切换，检测到滑移立即降级到 walk 并缩短步幅。

/*
 * 案例6：落脚点候选评估 + walk/trot 切换 + 滑移降级
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

BLDCMotor hip[4], knee[4];
// 驱动/编码器初始化同四足案例（略）
MPU6050 imu;

const float BODY_H = 0.16f, L1 = 0.12f, L2 = 0.14f;
const float STRIDE_N = 0.10f, LIFT_N = 0.05f;

// ---- 步态定义 ----
enum Gait { G_WALK, G_TROT, G_CRAWL };   // walk: 单腿摆; trot: 对角双腿; crawl: 极慢三腿支撑
Gait gait = G_WALK;
const float PHASE_WALK[4] = {0.00f, 0.50f, 0.25f, 0.75f};   // 错开
const float PHASE_TROT[4] = {0.00f, 0.50f, 0.50f, 0.00f};   // 对角同步
const float DUTY_WALK = 0.75f, DUTY_TROT = 0.50f;

// ---- 落脚点候选 ----
struct Cand { float x, y; float cost; };
float terrainRough = 0;      // 0..1 由 IMU 垂直加速度 RMS 归一
float slopePitch = 0;        // 坡度
float obsDist = 9;           // 前方障碍

// 代价：粗糙 + 坡度 + 障碍 + 偏离理想 + 越界
float footCost(float cx, float cy) {
  float c = 0;
  // 粗糙度：越靠前越未知，用保守估计
  c += 2.0f * terrainRough * (1.0f + 0.3f*fabsf(cy));
  // 坡度：上坡侧落脚更难
  c += 1.2f * fabsf(slopePitch) / 20.0f * (cy > 0 ? 1.0f : 0.6f);
  // 障碍：靠近障碍的落点有风险
  c += 1.5f * constrain(1.0f - obsDist/0.6f, 0, 1) * (cx > 0 ? 1.0f : 0.3f);
  // 偏离理想位置（横向偏移越大越不利）
  c += 0.8f * fabsf(cy) / 0.06f;
  // 越界（超过腿长可达）
  float r = sqrtf(cx*cx + cy*cy);
  if (r > 0.24f) c += 100.0f;
  return c;
}

// 为某腿选落点：三个候选（正中 / 左偏 / 右偏）
void pickFoot(int leg, float idealX, float idealY, float &ox, float &oy) {
  Cand cs[3];
  float offs[3] = {0.0f, -0.05f, 0.05f};
  for (int i=0;i<3;i++) {
    cs[i].x = idealX;
    cs[i].y = idealY + offs[i];
    cs[i].cost = footCost(cs[i].x, cs[i].y);
  }
  int b = 0; for (int i=1;i<3;i++) if (cs[i].cost < cs[b].cost) b = i;
  ox = cs[b].x; oy = cs[b].y;
}

// ---- 2 连杆 IK（同四足案例）----
void ikFoot(float x, float z, float &qH, float &qK) {
  float d = min(sqrtf(x*x+z*z), L1+L2-0.005f);
  qK = acosf(constrain((L1*L1+L2*L2-d*d)/(2*L1*L2), -1, 1));
  float alpha = atan2f(z, x);
  float beta  = acosf(constrain((L1*L1+d*d-L2*L2)/(2*L1*d), -1, 1));
  qH = alpha - beta;
}

// ---- 滑移检测：触地后足端位移 vs 机身位移 ----
float prevFootX[4] = {0}; float slipScore[4] = {0};
const float SLIP_TH = 0.02f;               // 触地脚相对机身漂移阈值
bool slipDetected = false;

void checkSlip() {
  slipDetected = false;
  for (int i=0;i<4;i++) {
    // 支撑腿的期望足端位置应从后向前匀速移动，实际漂移过大 = 滑
    float drift = fabsf(hip[i].shaft_angle - prevFootX[i]);
    prevFootX[i] = hip[i].shaft_angle;
    if (drift > SLIP_TH && fabsf(hip[i].shaft_velocity) < 2.0f) {
      slipScore[i] += 0.1f;
    } else slipScore[i] = max(0.0f, slipScore[i] - 0.02f);
    if (slipScore[i] > 0.4f) slipDetected = true;
  }
}

// ---- 步态切换 ----
unsigned long gaitT0 = 0;
void updateGait() {
  float az = fabsf(imu.getAccelerationZ()/9.81f - 1.0f);
  terrainRough = constrain(az*2.0f, 0, 1);
  slopePitch = imu.getPitch();
  checkSlip();

  Gait want = gait;
  if (slipDetected)                 want = G_CRAWL;   // 滑移：最保守
  else if (terrainRough > 0.5f || fabsf(slopePitch) > 10.0f) want = G_WALK;
  else if (terrainRough < 0.15f && fabsf(slopePitch) < 4.0f) want = G_TROT;

  // 升级（保守化）立即；降级（激进）需稳定 2s
  int rank[3] = {2, 1, 0};   // crawl/walk/trot 保守度
  if (rank[want] > rank[gait]) { gait = want; gaitT0 = millis(); }
  else if (rank[want] < rank[gait] && millis() - gaitT0 > 2000) { gait = want; gaitT0 = millis(); }
}

void setup() {
  // 八关节初始化同四足案例（位置环 + current_limit）
  Wire.begin(); imu.initialize();
  gaitT0 = millis();
  Serial.begin(115200);
}

void loop() {
  imu.update();
  updateGait();

  // ---- 步态参数由等级派生 ----
  const float *PH;
  float duty, stride, lift, period;
  switch (gait) {
    case G_TROT:  PH = PHASE_TROT; duty = DUTY_TROT; stride = 0.12f; lift = 0.05f; period = 0.60f; break;
    case G_WALK:  PH = PHASE_WALK; duty = DUTY_WALK; stride = 0.09f; lift = 0.07f; period = 0.90f; break;
    default:      PH = PHASE_WALK; duty = 0.85f;     stride = 0.05f; lift = 0.09f; period = 1.40f; break;
  }

  static unsigned long t0 = millis();
  float s = fmodf((millis()-t0)/1000.0f/period, 1.0f);

  for (int leg=0; leg<4; leg++) {
    float t = fmodf(s - PH[leg] + 1.0f, 1.0f);
    bool swing = (t < 1.0f - duty);
    float fx, fz;
    if (swing) {
      float u = t / (1.0f - duty);                 // 摆动归一化 0..1
      float idealX = stride * (u - 0.5f);
      float idealY = 0.0f;
      float ox, oy;
      pickFoot(leg, idealX, idealY, ox, oy);       // 落脚点评估
      fx = ox;
      fz = BODY_H - lift * sinf(PI*u);             // 抬腿
      if (u > 0.85f && fz < BODY_H - 0.01f) fz += 0.01f;   // 落地前软着陆缓冲
    } else {
      fx = -stride*0.5f + stride*0.5f*((t-(1.0f-duty))/duty);
      fz = BODY_H;
      // 滑移时支撑腿主动回拉，稳定机身
      if (slipDetected) fx -= 0.005f;
    }
    float qH, qK; ikFoot(fx, fz, qH, qK);
    hip[leg].move(qH); knee[leg].move(qK);
  }

  Serial.print("gait="); Serial.print(gait);
  Serial.print(" rough="); Serial.print(terrainRough,2);
  Serial.print(" slip="); Serial.println(slipDetected);
  delay(20);
}

关键点：pickFoot 的三个候选只在横向 ±5cm 内选——越野时落点修正必须小步幅、高频次，一次挪大反而更容易踩空；G_CRAWL 的 duty 0.85 意味着任意时刻至少三腿支撑，是滑移时唯一还能站稳的步态。

要点解读
一、越野的中间变量是“地形等级”，不是直接调参数
如果代码里到处是 if (rms > 3) v = 0.4 这样的散点判断，参数会迅速失控：改一个阈值要动十几处。正确结构是先把多源特征（垂直加速度 RMS、坡度、滑移率、电流波动）压成一个带迟滞的地形等级（案例一 grade 0~3），再让所有执行参数从一张表派生（速度、加速度、刚度、避障距离）。这样调参只需改表，加传感器只需改判级，两者解耦。这也是越野与平地最大的工程差异：平地可以写死参数，越野必须参数化。

二、越野必须限“加速度”，不只是限“速度”
平地上限速就够了，越野上限速远远不够——同样的 0.4 m/s，在碎石上以 0.8 m/s² 加速会打滑、冲击悬挂、甚至翻车；以 0.15 m/s² 加速则稳。所以案例一、二的参数表里都同时有 vmax 和 amax，且 amax 随地形等级下降得更快（0.8 → 0.15，5 倍降幅，而速度只降 4 倍）。加速度限幅还必须放在执行层（案例二 taskExec 里的 vC += constrain(…)），因为上层规划出来的目标速度可能一步跳变，只有执行层才能保证输出连续。

三、避障与步态必须双向耦合，单向都漏工况
两个方向缺一不可：

正向（地形→避障）：地形差时 DWA 的速度窗口必须收窄、可停性判据必须用地形实际加速度而非名义值（案例二 (vv)/(2amax)）。用名义 0.8 算刹车距离，在松土上会算出“能停住”但实际刹不住。
反向（避障→地形）：前方障碍近时，同一段烂路的风险陡然上升（没有绕行空间），必须临时提高地形保守度（案例二 caution 直接削 vCap）。
只做正向，会出现“贴着障碍在烂路上试图急停”；只做反向，会出现“在松软地面为躲避障碍急打方向而侧滑”。协同的价值就在这两个方向的乘积。

四、滑移/触地是越野唯一可信的真值，模型不可信
轮式在松土、足式在碎石上，运动学模型（轮速→位移、关节角→足端位置）都会失真。唯一的真值反馈是实际位移与预期不符：轮式的滑移率（案例一）、足式的支撑腿漂移（案例三 checkSlip）。检测到滑移后的正确响应有三步：① 立即降速/降步幅；② 降级步态（trot→walk→crawl，案例三）；③ 提高地形等级（把“打滑”当作路况变差的证据，案例一 score += 2.0）。绝不能做的是“加大电流试图冲过去”——那只会让滑移变成翻车。

五、分层与保守降级：每一层都要能独立地把机器人带回安全状态
越野场景的失效模式多且相互叠加，所以架构上要求：地形层（10Hz，慢但看得远）、规划层（20Hz，DWA 算窗口）、执行层（1kHz，纯跟踪 + 加速度限幅）三层节奏分离；每层都有自己的保守退路——DWA 无可行解 → 刹停（c_estop）；滑移检测 → crawl 步态；地形等级最高 → 龟速蠕动；传感器失效 → 按最高等级处理。越野的“智能”不体现在多会走，而体现在多会承认走不了：能主动降级到 crawl、能停下来等人接管，比勉强冲过去安全一个量级。

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