Arduino BLDC之户外机器人崎岖地形自适应识别+动态步态切换的核心，是把“地形感知—步态决策—运动学重规划—BLDC关节伺服”压进一个硬实时闭环，使机器人在碎石、斜坡、台阶、泥泞等非结构化地面中，从单一固定步态升级为多模态自主切换。 相比传统固定步态机器人，它强调“先识别地形，再选择步态”，而不是靠人工遥控或预设模式切换。以下从主要特点、应用场景与注意事项展开。

主要特点
. 多源地形自适应识别
自适应识别不是简单判断“有没有障碍”，而是区分地面类型、坡度、台阶高度、松软程度和可通行性。典型方案采用“远—中—近”三层感知：远距用激光雷达或深度相机识别可通行区域；中距用超声波/ToF判断前方障碍尺寸；近距用IMU、关节编码器、电机电流和足端触地检测判断地面硬度与落足稳定性。
在嵌入式平台上，IMU仍承担核心姿态估计任务。通过互补滤波或轻量级卡尔曼滤波融合加速度计与陀螺仪，可实时获得机身俯仰角、横滚角和角速度；再结合关节电流波动、足端下沉量和支撑腿姿态，判断当前是否处于斜坡、软土、台阶边缘或单侧悬空状态。
. 动态步态切换机制
动态步态切换的本质是“根据地形改变运动时序”。平坦地面可采用小跑或轮式巡航，提高效率；遇到碎石、台阶或斜坡时切换为爬行步态或高步高步态，保证至少三足或双对角足稳定支撑；遇到沟壑或较高障碍时，再触发跨越步态，提高抬腿高度和支撑相时长。
在Arduino/ESP32这类资源受限平台上，更推荐采用“有限状态机 + 预定义步态库”的方式。状态机负责判断当前处于Walk、Trot、Climb、Cross或Stop模式；步态库提供不同步态的相位、步长、步高和支撑/摆动时间。切换时不应突变，而应通过相位插值和平滑过渡，避免机身重心剧烈偏移。
. 逆运动学与足端轨迹规划
每条腿通常包含髋关节、大腿、小腿等2—3个自由度。动态步态切换需要把“足端目标轨迹”反解为各关节目标角度。支撑相负责推动机身前进，摆动相负责抬腿、前跨和落足。
在嵌入式实现中，逆运动解算应尽量简化：可用几何解析法、查表法或CORDIC加速三角函数运算；足端轨迹常采用摆线、椭圆或贝塞尔曲线，保证抬腿平滑、落足柔和。对于崎岖地形，落足点不应固定，而应根据IMU姿态和前方地形微调，避免落在低附着力或悬空区域。
. BLDC关节伺服与柔顺力控
BLDC相比普通舵机的优势在于高扭矩密度、低发热和可闭环力控。但普通航模ESC只能做开环速度控制，无法完成精确步态；必须采用带编码器的BLDC伺服方案，并配合FOC实现位置环、速度环和电流环闭环。
在崎岖地形中，柔顺控制尤为关键。当足端意外触碰到石块或陷入松软地面时，系统应通过阻抗控制或电流限幅让关节“顺势吸收冲击”，而不是硬性顶住。这样既能保护减速器和结构件，也能提高落足稳定性。下坡或制动时，BLDC还可进入再生制动模式，回收部分能量。
. 分层实时控制架构
这类系统不适合把所有逻辑塞进一个loop。更合理的架构是：
上层负责地形识别、步态模式选择和路径决策；
中层负责步态时序、足端轨迹和逆运动解算；
底层负责关节位置/速度/电流闭环、IMU滤波和安全保护。
步态更新频率通常建议在50Hz左右，关节伺服需更高，IMU姿态闭环也应保持高频响应。Arduino Uno/Nano等8位MCU难以独立完成全部任务，更适合作为轻量原型或下位执行节点；复杂方案建议采用ESP32、STM32F4/F7或“上位机+下位机”分层架构。

应用场景
野外巡检与地质勘探：在草地、碎石路、田埂、坡道等环境中执行图像采集、气体检测或土壤采样。动态步态可避免单侧悬空或打滑导致倾覆。
灾害救援与废墟探测：地震、坍塌现场存在瓦砾、台阶、狭小通道和不稳定地面。机器人可通过地形识别选择爬行或跨越步态，降低人工遥控负担。
农业与林业监测：农田垄沟、林地落叶层、泥泞地面都会造成轮式底盘打滑。四足或轮腿复合构型能根据地面软硬调整步高和支撑策略。
电力/矿山/隧道巡检：变电站碎石区、矿山巷道、隧道检修道常有不平整地面和局部积水。自适应步态可维持机身姿态稳定，提高传感器数据质量。
高校教学与科研验证：用于验证步态规划、地形分类、IMU融合、FOC伺服和嵌入式实时控制。可作为从Arduino原型向STM32/ROS2平台迁移的过渡载体。

需要注意的事项
不要高估Arduino的单芯片算力
地形识别、步态切换、逆运动学和四路以上BLDC伺服同时运行时，Arduino Uno/Nano极易成为瓶颈。建议至少使用ESP32、STM32F4或双MCU主从架构；若引入激光雷达、深度相机或强化学习策略，应把感知和规划放到上位机。
步态切换必须平滑，禁止相位突变
从Walk切到Trot、从平地切到爬坡时，如果直接替换步态参数，容易造成重心跳变和机身侧翻。切换前应完成相位对齐、步长渐变和支撑腿过渡，并在IMU倾角超限时暂停切换。
地形识别不能只靠单一传感器
仅靠超声波容易误判低矮障碍；仅靠IMU无法区分机身振动与真实坡度；仅靠视觉在强光、扬尘、夜间会失效。更可靠的做法是融合IMU、关节电流、编码器、足端触地信息和近距距离传感器。
BLDC关节必须具备位置/电流反馈
没有编码器就无法做精确步态；没有电流采样就难以实现柔顺控制和堵转保护。普通ESC不适合直接用于多足关节控制，应选用支持FOC、位置环和电流环的无刷伺服驱动方案。
安全保护优先级高于运动性能
倾角超限、通信丢失、关节堵转、电池欠压、IMU异常或足端长时间悬空时，应立即进入降级或停机状态。运动指令应加限幅和斜率限制，避免步态切换瞬间产生冲击。户外场景还需做好防水、防尘、减震和电磁隔离。

1、IMU坡度 + 超声波台阶识别 + 三档步态库切换
适用场景：户外巡检、校园巡逻等结构化地形场景，机器人通过IMU俯仰角识别斜坡，超声波检测前方台阶或障碍物高度，在平地/斜坡/台阶三种预定义步态间自动切换。

/**
 * 户外机器人 - 地形识别 + 三档步态库切换
 * 硬件假设：Arduino Mega, 8×BLDC关节电机(SimpleFOC), MPU6050 IMU, 超声波×2
 * 核心：IMU俯仰角 -> 斜坡判断；超声波高度 -> 台阶判断；步态库参数切换
 */

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

MPU6050 imu;
int ultrasonicPins[2] = {A0, A1};

// 四足关节电机（8个关节，简化为8个BLDCMotor声明）
BLDCMotor joints[8] = {
  BLDCMotor(2, 3, 4), BLDCMotor(5, 6, 7),
  BLDCMotor(8, 9, 10), BLDCMotor(11, 12, 13),
  BLDCMotor(14, 15, 16), BLDCMotor(17, 18, 19),
  BLDCMotor(20, 21, 22), BLDCMotor(23, 24, 25)
};

// 步态库：平地/斜坡/台阶三档
struct GaitParams {
  float stepLength;    // 步长
  float stepHeight;    // 抬腿高度
  float phaseDiff;     // 相位差
  float jointAngleMax; // 关节最大角度
} gaitLibrary[3] = {
  {15.0f, 8.0f, 180.0f, 0.52f},   // 平地：大步低抬
  {12.0f, 10.0f, 160.0f, 0.58f},  // 斜坡：减步长、增步高
  {10.0f, 15.0f, 150.0f, 0.65f}   // 台阶：小步高抬
};

GaitParams currentGait = gaitLibrary[0];
int currentTerrain = 0;

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 < 8; i++) {
    joints[i].init();
    joints[i].initFOC();
  }
}

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

  // 读取 IMU 获取俯仰角（坡度）
  int16_t ax, ay, az, gx, gy, gz;
  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float pitch = atan2(ay, az) * 180 / PI;

  // 超声波测距（前方障碍/台阶高度，简化）
  int frontDist = analogRead(ultrasonicPins[0]) / 10;
  int obstacleHeight = 0;
  if (frontDist < 30 && frontDist > 5) obstacleHeight = frontDist / 5;

  // 地形识别：台阶 > 斜坡 > 平地
  if (obstacleHeight >= 10) {
    currentTerrain = 2; // 台阶
  } else if (abs(pitch) > 15.0) {
    currentTerrain = 1; // 斜坡
  } else {
    currentTerrain = 0; // 平地
  }

  // 切换到对应步态
  currentGait = gaitLibrary[currentTerrain];

  // 斜坡微调：上坡增加后腿步高，下坡反之
  float stepHeightOffset = 0;
  if (pitch > 5.0) {
    stepHeightOffset = pitch * 0.05;
  } else if (pitch < -5.0) {
    stepHeightOffset = pitch * 0.03;
  }

  float targetStepHeight = constrain(
    currentGait.stepHeight + stepHeightOffset, 5.0, 25.0);

  Serial.print("Terrain:"); Serial.print(currentTerrain);
  Serial.print(" Pitch:"); Serial.print(pitch);
  Serial.print(" StepH:"); Serial.println(targetStepHeight);

  delay(50);
}

要点：该代码实现了基于IMU与超声波融合的三档地形分类。IMU的俯仰角（pitch）直接反映机器人机身相对于水平面的倾斜，是识别斜坡的核心依据。超声波用于检测前方障碍物高度，当高度超过阈值时判定为台阶。步态库中的三个参数集分别针对平地（大步低抬，追求速度）、斜坡（减步长防打滑、增步高防绊倒）、台阶（小步高抬，确保跨越）优化。

2、足端力反馈 + 触地检测 + 动态步频调整
适用场景：碎石、泥地、草地等非结构化地形，足端触地力变化剧烈。通过足端力传感器检测触地冲击，结合IMU姿态动态调整步频，避免刚性碰撞或打滑。

/**
 * 户外机器人 - 足端力反馈 + 触地检测 + 动态步频
 * 硬件假设：ESP32 + 4路BLDC关节, MPU6050, 足端压力传感器×4
 * 核心：足端力检测触地 -> 动态调整步频 -> 崎岖地形稳定性
 */

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

BLDCMotor joints[8] = {
  BLDCMotor(2, 3, 4), BLDCMotor(5, 6, 7),
  BLDCMotor(8, 9, 10), BLDCMotor(11, 12, 13),
  BLDCMotor(14, 15, 16), BLDCMotor(17, 18, 19),
  BLDCMotor(20, 21, 22), BLDCMotor(23, 24, 25)
};

MPU6050 imu;
int pressurePins[4] = {A0, A1, A2, A3}; // 足端压力传感器

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

// 地形状态
float pitchAngle = 0;
float footForces[4] = {0};
bool footContact[4] = {false, false, false, false};

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

  for (int i = 0; i < 8; i++) {
    joints[i].init();
    joints[i].initFOC();
  }
}

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

// 动态调整步频
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姿态辅助：倾斜时降低步频
  int16_t ax, ay, az;
  imu.getAcceleration(&ax, &ay, &az);
  pitchAngle = atan2(ay, az) * 180 / PI;

  if (abs(pitchAngle) > 15.0) {
    stepFreq = max(minStepFreq, stepFreq - 0.05);
  }
}

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

  readFootForces();
  adaptStepFrequency();

  // 简化：将步频映射到关节摆动速度
  float jointSpeed = stepFreq * 2.0;
  for (int i = 0; i < 8; i++) {
    joints[i].move(jointSpeed * sin(millis() / 1000.0 * stepFreq * PI));
  }

  Serial.print("Pitch:"); Serial.print(pitchAngle);
  Serial.print(" MaxForce:"); Serial.print(footForces[0]);
  Serial.print(" Freq:"); Serial.println(stepFreq);

  delay(20);
}

要点：该案例体现了基于足端力反馈的动态步频调整。搜索结果指出，足端力传感器感知触地冲击力可实时调整步频，同时结合机身俯仰角动态修正步态相位。冲击力过大（硬地/坑洼）时降低步频提升稳定性，冲击力平稳（平整地面）时提高步频提升速度。IMU倾斜时进一步降频，防止坡面失衡。这种“触觉感知”策略不依赖昂贵传感器，是低成本户外机器人的务实选择。

3、地面粗糙度分析 + 地形分类 + 悬挂/步态模式切换
适用场景：非结构化野外环境，机器人通过向下传感器分析地面波动性，自动分类地形类型（平坦/粗糙/松软），并切换悬挂刚度或步态参数。

/**
 * 户外机器人 - 地面粗糙度分析 + 地形分类 + 模式切换
 * 硬件假设：Arduino Mega, BLDC底盘, 超声波(向下), 可调悬挂(PWM控制)
 * 核心：地面波动检测 -> 地形分类 -> 悬挂/步态模式切换
 */

#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       // 松软泥地
};

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

// 地面波动分析（方差分析）
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);
}

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

要点解读

1. 地形识别的传感器选择应遵循“IMU管坡度、测距管台阶、触觉管粗糙度”的分工原则。 IMU的俯仰角是识别斜坡最直接、最可靠的信号，不受地面纹理或光照影响。超声波或激光测距用于检测前方障碍物高度，判断是否为台阶或障碍物。足端力传感器或地面波动检测则提供“触觉感知”，弥补视觉/测距在松软地面和粗糙度判断上的不足。单一传感器无法覆盖所有地形类型，融合是必然选择。

2. 步态库切换与动态参数调整是“离散”与“连续”两种互补策略。 步态库切换（案例一）适合地形类型明确可分的场景——平地、斜坡、台阶三类，参数切换直接有效。动态参数调整（案例二）适合地形连续变化的场景——从平缓坡到陡坡的过渡，步频、步高等参数应平滑变化而非跳变。搜索结果指出，上坡时增加步高防绊倒、减小步长防打滑；下坡时适当增大步长、降低步高保持重心稳定。实际系统中，两者可以结合：用步态库定义“基准步态”，用动态参数调整实现“段内微调”。

3. 抬腿高度是地形适应性的第一杠杆，步频是第二杠杆。 搜索结果明确指出：崎岖地形应增大抬腿高度、降低步频以提升稳定性。抬腿高度不够会导致脚尖磕碰障碍物，步频过高则在凹凸地面上容易失去平衡。BLDC配合FOC的高带宽力矩响应使抬腿高度能够在每个控制周期（20ms）内精确调整，这是BLDC相比舵机的核心优势之一。足端力反馈进一步提供了“触地确认”，当足端冲击力过大时自动降频。

4. 触觉反馈是“从感知到执行”的最后一环，也是低成本方案的关键。 视觉预判可能因光照或纹理误判，IMU坡度可能受加速度干扰，但足端力传感器提供的是物理接触的真实性验证。搜索结果指出，足端触地检测可通过BLDC的FOC电流环（Iq）实时监测——当Iq突然增大且编码器位置停滞时，判定为足端触地，用于切换摆动相与支撑相。这种“虚拟力传感器”方案无需额外硬件，适合Arduino级别的算力约束。

5. Arduino级平台应聚焦“执行层+简单感知层”，复杂地形分类和高级步态规划交给上位机。 视觉地形分类（如区分草地、碎石、木板）需要图像处理算力，Arduino无法胜任。合理的分层架构是：树莓派/Jetson运行视觉分类器或学习型步态选择器（如KAIST的APT-RL技术可在15.5小时仿真数据上训练，使机器人自主选择行走、奔跑、跳跃策略），通过串口向Arduino发送“地形类型标签”和“步态指令”；Arduino负责IMU读取、触觉检测、步态参数执行和BLDC关节控制。SimpleFOC库为BLDC关节提供了完整的FOC控制能力，使Arduino能够胜任执行层的实时控制任务。

4、多传感器融合崎岖地形识别+两驱自适应步态程序（核心：轻量感知+稳定步态）
适用场景：野外日常巡检、田间巡查等场景，地形以碎石、缓坡、坑洼为主，要求行走稳定、功耗低。

核心逻辑
通过多方向测距与机身倾角传感器，识别地形起伏与坡度
基于地形参数，切换平地、缓坡、坑洼三种步态，调整电机转速
闭环控制电机，保证车身平稳，适配不同地形行走需求

// 户外轻量崎岖地形巡检程序
// 硬件：BLDC电机（双轮驱动）、超声波/红外测距、MPU6050倾角、PWM控制
// 功能：地形识别、步态切换、电机驱动，适配日常野外巡检

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte OBS_FRONT = 2;    // 前方测距
const byte OBS_UNDER = A0;   // 底部测距（识别坑洼）
const byte IMU_SDA = 4, IMU_SCL = 5; // 倾角模块
const byte MOTOR_L_PWM = 9, MOTOR_L_DIR = 8;
const byte MOTOR_R_PWM = 10, MOTOR_R_DIR = 11;

// ========== 地形与步态定义 ==========
#define TERRAIN_FLAT 0
#define TERRAIN_SLOPE 1
#define TERRAIN_PITFALL 2
uint8_t currentTerrain = TERRAIN_FLAT;

// 步态参数：速度+扭矩补偿
struct TerrainGait {
  uint16_t speed;
  int torqueOffset;
};
TerrainGait gLib[3] = {
  {150, 0},   // 平地：正常速度
  {100, 20},  // 缓坡：降速+增大扭矩
  {60, 40}    // 坑洼：低速+强扭矩
};
TerrainGait currentGait = gLib[0];

// ========== 函数声明 ==========
uint8_t getTerrainType();
void switchGait();
void driveMotor(int leftPWM, int rightPWM);

void setup() {
  Serial.begin(115200);
  Serial.println("轻量巡检地形自适应系统启动");

  pinMode(OBS_FRONT, INPUT_PULLUP);
  pinMode(OBS_UNDER, INPUT_PULLUP);
  pinMode(MOTOR_L_PWM, OUTPUT); pinMode(MOTOR_L_DIR, OUTPUT);
  pinMode(MOTOR_R_PWM, OUTPUT); pinMode(MOTOR_R_DIR, OUTPUT);
  Wire.begin(); // 初始化IMU
}

void loop() {
  uint8_t terrain = getTerrainType();
  if (terrain != currentTerrain) {
    currentTerrain = terrain;
    switchGait();
  }

  // 按当前步态驱动电机
  int leftPWM = currentGait.speed + currentGait.torqueOffset;
  int rightPWM = currentGait.speed + currentGait.torqueOffset;
  driveMotor(leftPWM, rightPWM);

  Serial.print("地形：");Serial.print(currentTerrain);
  Serial.print(" 速度：");Serial.println(currentGait.speed);
  delay(30);
}

// 地形识别：front距离+底部信号+倾角
uint8_t getTerrainType() {
  uint16_t frontDist = map(analogRead(OBS_FRONT), 0, 1023, 0, 120);
  uint16_t underDist = map(analogRead(OBS_UNDER), 0, 1023, 0, 50);
  // 模拟IMU获取倾角（简化数值，实际需解析IMU数据）
  int pitchAngle = 15;

  if (underDist < 20) {
    return TERRAIN_PITFALL;  // 底部距离小，判定坑洼
  } else if (pitchAngle >= 10 || frontDist < 40) {
    return TERRAIN_SLOPE;   // 坡度大/前方障碍，判定缓坡
  } else {
    return TERRAIN_FLAT;    // 参数正常，判定平地
  }
}

// 切换对应步态
void switchGait() {
  currentGait = gLib[currentTerrain];
  Serial.print("步态切换：");Serial.println(currentTerrain);
}

// 电机差速驱动
void driveMotor(int leftPWM, int rightPWM) {
  leftPWM = constrain(leftPWM, 0, 200);
  rightPWM = constrain(rightPWM, 0, 200);
  digitalWrite(MOTOR_L_DIR, HIGH);
  digitalWrite(MOTOR_R_DIR, HIGH);
  analogWrite(MOTOR_L_PWM, leftPWM);
  analogWrite(MOTOR_R_PWM, rightPWM);
}

适用场景优化
替换超声波传感器为激光雷达，提升复杂地形识别精度
加入电池电压检测，动态补偿电机功率，避免电量不足动力下降
优化步态参数，结合地形陡峭程度，分级调整扭矩，提升爬坡能力

5、复杂崎岖地形+四驱自适应步态+倾角校正程序（核心：全地形适配+平稳校正）
适用场景：山地、矿区等极端崎岖地形巡检，需四驱动力，自动校正车身姿态，防止侧翻。

核心逻辑
融合前/左/右测距+三轴倾角，识别陡峭、碎石堆、深坑等复杂地形
切换四驱步态，独立控制四个BLDC电机，匹配各轮地形受力
基于倾角数据，实时调整电机输出，校正车身姿态，保障平稳行走

// 复杂崎岖地形四驱巡检程序
// 硬件：四BLDC电机（分动）、多方向测距、三轴IMU、PWM驱动
// 功能：复杂地形识别、四驱步态、姿态校正，适配山地巡检

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte OBS[4] = {2,3,4,A0}; // 前、左、右、底测距
const byte IMU_SDA = 6, IMU_SCL =7;
const byte PWM_PINS[4] = {9,10,11,12};
const byte DIR_PINS[4] = {8,7,6,5};

// ========== 地形与步态定义 ==========
#define TERRAIN_STEEP 0
#define TERRAIN_GRAVEL 1
#define TERRAIN_DEPTH 2
#define TERRAIN_FLAT 3
uint8_t terrain = TERRAIN_FLAT;

struct QuadGait {
  uint16_t baseSpeed;
  int tiltCompensate; // 倾角补偿值
  int torqueBoost;   // 扭矩提升
};
QuadGait gLib[4] = {
  {80, 30, 50},   // 陡坡：低速+强补偿+高扭矩
  {100, 20, 30},   // 碎石：中速+中补偿
  {50, 40, 60},   // 深坑：超低速+强补偿
  {150, 0, 0}     // 平地：高速+无补偿
};
QuadGait currentGait = gLib[3];

// ========== 变量 ==========
int pitch=0, roll=0;

// ========== 函数声明 ==========
uint8_t classifyTerrain();
void applyGait();
void correctPose();

void setup() {
  Serial.begin(115200);
  Serial.println("复杂地形四驱巡检系统启动");

  for (uint8_t i =0; i<4; i++) {pinMode(PWM_PINS[i], OUTPUT); pinMode(DIR_PINS[i], OUTPUT);}
  for (byte i=0; i<4; i++) pinMode(OBS[i], INPUT_PULLUP);
  Wire.begin();
}

void loop() {
  terrain = classifyTerrain();
  if (terrain != currentGait.terrainTag) {
    currentGait = gLib[terrain];
  }

  // 姿态校正
  correctPose();

  // 四驱驱动
  for (uint8_t i=0; i<4; i++) {
    int pwm = currentGait.baseSpeed + currentGait.torqueBoost;
    digitalWrite(DIR_PINS[i], HIGH);
    analogWrite(PWM_PINS[i], pwm);
  }

  Serial.print("地形：");Serial.print(terrain);
  Serial.print(" 倾角补偿：");Serial.println(currentGait.tiltCompensate);
  delay(25);
}

// 分类复杂地形
uint8_t classifyTerrain() {
  uint16_t dist[4];
  for (uint8_t i=0; i<4; i++) {dist[i] = map(analogRead(OBS[i]),0,1023,0,100);}
  // 模拟IMU读取坡度与侧倾（实际需解析数据）
  pitch = 30; roll =10;

  if (dist[1] <30 || dist[2]<30) return TERRAIN_GRAVEL; // 左右障碍，碎石
  if (dist[3] <15) return TERRAIN_DEPTH; // 底部距离小，深坑
  if (pitch >=20) return TERRAIN_STEEP;  // 坡度大，陡坡
  return TERRAIN_FLAT;
}

// 姿态校正，抵消倾角
void correctPose() {
  int leftTilt = roll * 2;
  int rightTilt = -roll *2;
  // 左右电机补偿侧倾
  analogWrite(PWM_PINS[1], currentGait.baseSpeed + currentGait.torqueBoost + leftTilt);
  analogWrite(PWM_PINS[2], currentGait.baseSpeed + currentGait.torqueBoost + rightTilt);
}

适用场景优化
加入石头、树枝识别，提前避障，避免陷车
实时采集电机电流，判断负载，动态调整扭矩，防止电机过载
优化姿态校正算法，采用PID控制，提升车身平稳性，减少颠簸

6、动态越野路况+智能步态+避障协同程序（核心：动态识别+主动避障）
适用场景：野外自由巡检、应急勘察等场景，地形连续变化，需自动避障并自适应调整步态。

核心逻辑
连续采集多方向地形数据，识别动态越野路况（连续坡道、碎石路、浅沟）
步态随地形实时渐变切换，避免硬切换导致的冲击
结合避障逻辑，自动调整行驶方向与速度，实现安全巡检

// 动态越野路况自适应巡检程序
// 硬件：多传感器、四驱BLDC、视觉模块（模拟）、PWM控制
// 功能：动态地形识别、渐变步态、避障协同，适配自由巡检

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte SENSOR_N = 8; // 前后左右8组测距
const byte IMU_PIN =A4;
const byte PWM_PINS[4] = {9,10,11,12};
const byte DIR_PINS[4] = {8,7,6,5};

// ========== 地形定义 ==========
#define DYNAMIC_ROAD 0   // 连续斜坡
#define DYNAMIC_GRAVEL 1 // 碎石路
#define DYNAMIC_DITCH 2  // 浅沟
uint8_t currentRoad = DYNAMIC_GRAVEL;

// ========== 动态步态 ==========
struct DynGait {
  uint16_t speed;
  int leftOffset;
  int rightOffset;
  int steerAdjust;
};
DynGait gLib[3] = {
  {90, 0, 0, 10},   // 斜坡：定向校正
  {120, -5, 5, 0},  // 碎石：左右微调
  {60, -15, 15, 5}  // 浅沟：大偏移+避障
};
DynGait currentGait = gLib[1];

// ========== 函数声明 ==========
uint8_t detectDynamicRoad();
void transitionGait();
void obstacleAvoid();

void setup() {
  Serial.begin(115200);
  Serial.println("动态越野巡检系统启动");

  for (uint8_t i=0; i<4; i++) {
pinMode(PWM_PINS[i], OUTPUT); pinMode(DIR_PINS[i], OUTPUT);
  }
  for (byte i=0; i<SENSOR_N; i++) pinMode(i+2, INPUT_PULLUP);
}

void loop() {
  uint8_t road = detectDynamicRoad();
  if (road != currentRoad) {
    transitionGait(road);
    currentRoad = road;
  }

  // 避障校正
  obstacleAvoid();

  // 驱动电机
  for (uint8_t i=0; i<4; i++) {
    int pwm = currentGait.speed;
    digitalWrite(DIR_PINS[i], HIGH);
    if (i == 1) pwm += currentGait.leftOffset;
    if (i == 2) pwm += currentGait.rightOffset;
    analogWrite(PWM_PINS[i], constrain(pwm,0,200));
  }

  Serial.print("路况：");Serial.print(currentRoad);
  Serial.print(" 转向校正：");Serial.println(currentGait.steerAdjust);
  delay(20);
}

// 检测动态路况
uint8_t detectDynamicRoad() {
  uint16_t frontAvg=0, leftAvg=0, rightAvg=0, bottomAvg=0;
  for (byte i=0; i<SENSOR_N; i++) {
    uint16_t d = map(analogRead(i+2), 0,1023,0,100);
    if (i<2) frontAvg+=d;
    if (i>=2 && i<4) leftAvg+=d;
    if (i>=4 && i<6) rightAvg+=d;
    if (i>=6) bottomAvg+=d;
  }
  frontAvg/=2; leftAvg/=2; rightAvg/=2; bottomAvg/=2;

  // 判断逻辑
  if (frontAvg <25 || leftAvg <25 || rightAvg <25) return DYNAMIC_DITCH;
  if (abs(leftAvg-rightAvg) >30) return DYNAMIC_ROAD;
  return DYNAMIC_GRAVEL;
}

// 渐变切换步态
void transitionGait(uint8_t road) {
  currentGait = gLib[road];
  Serial.print("路况适配，步态渐变：");Serial.println(road);
}

// 避障协同
void obstacleAvoid() {
  uint16_t frontVal = map(analogRead(2),0,1023,0,100);
  if (frontVal <20) {
    // 前方障碍，左转避让
    digitalWrite(DIR_PINS[2], LOW);
    digitalWrite(DIR_PINS[1], HIGH);
  } else {
    digitalWrite(DIR_PINS[2], HIGH);
    digitalWrite(DIR_PINS[1], HIGH);
  }
}

适用场景优化
升级视觉识别，提前检测大障碍物，规划绕行路径
加入里程计定位，结合地形数据，实现巡检轨迹记录与实况标注
实现步态无级渐变，根据地形起伏平滑调整参数，减少行走颠簸

要点解读
要点1：地形感知必须多源融合，保障识别准确率
户外崎岖地形复杂多变，单一传感器无法覆盖所有路况，多传感器融合是稳定识别的基础。
传感器搭配：测距、倾角、电流、视觉多类型传感器搭配，分别捕捉距离、姿态、负载、障碍信息，全面覆盖地形特征
数据冗余设计：多通道重复采集关键数据，通过对比校验减少误判，避免单一传感器故障导致识别错误
特征精准提取：针对不同地形提取核心特征，比如距、坡度、起伏度，简化识别算法，提升恶劣环境下的识别稳定性
要点2：步态需分类适配，精准匹配地形特性
不同地形的受力与行走需求不同，步态必须针对性设计，才能保障行走效率与安全。
步态分类设计：按地形类型（平地、坡、坑、碎石）定制专属步态，每类步态匹配对应速度、扭矩、校正参数
参数关联逻辑：地形难走程度越高，速度越低、扭矩越大、姿态校正越强，保证机器人在复杂地形有足够的通过性
动态参数优化：结合实时负载、路况变化，动态微调步态参数，避免参数固化导致适配不足
要点3：步态切换要平滑渐变，避免硬切换失稳
崎岖地形行走时，步态硬切换会造成机身冲击，甚至侧翻，平滑过渡是稳定性的关键。
渐变过渡算法：采用数值插值、参数逐渐调整的方式，让前后步态参数缓慢变化，消除切换瞬间的冲击
过渡时长适配：不同地形差异越大，过渡时间越长，保证电机输出平稳衔接，防止动力突变打滑
过渡校验：切换过程中实时监测机身姿态，若出现剧烈晃动，暂停切换并自动回退稳定参数
要点4：闭环控制+姿态校正，保障车身动态平稳
地形起伏会造成车身晃动，闭环控制与姿态校正是维持平稳的核心手段。
实时反馈控制：结合倾角、电机电流、轮速反馈，实时调整电机输出，抵消地形带来的扰动，保持行驶稳定
姿态主动校正：通过左右电机、前后驱动差速控制，主动抵消坡度与侧倾，保持车身水平，降低侧翻风险
协同控制：多电机协调发力，根据地形受力分配动力，避免单轮过载，提升整体行走稳定性
要点5：方案需兼顾低功耗与高可靠性，适配户外长期作业
户外巡检周期长、环境恶劣，低功耗和高可靠性是方案落地的前提。
低功耗优化：优化传感器采集与算法逻辑，空闲时段降低采样频率，减少无效计算，延长续航时间
抗干扰设计：针对户外高温、低温、扬尘、电磁干扰，对传感器与电路做防护，保障设备稳定运行
容错机制：加入传感器故障检测、电机异常保护、地形误判自恢复，遇到异常自动调整，避免巡检中断

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