Arduino BLDC 之“视觉识别台阶高度 + 预规划越障步态”，本质上是“深度/视觉感知台阶几何 → 显式台阶参数提取 → 落足点与步态参数预规划 → BLDC 关节位置/力矩闭环执行”的前馈越障控制链：视觉先回答“台阶多高、多深、在哪里”，步态规划再回答“每条腿抬多高、跨多远、何时落足”，最后由 BLDC 伺服把轨迹转化为稳定、低冲击的爬阶动作。 与纯本体感知越障不同，预规划强调“看见台阶后再调整步态”，而不是等足端撞上台阶才被动补偿。

主要特点

1. 视觉台阶识别需要输出“显式几何参数”，而不是只给一张深度图
   对嵌入式步态规划来说，最有用的不是原始点云，而是台阶高度、台阶深度、台阶宽度、首级台阶距离、台阶走向角等可解释参数。 这些参数可以直接进入步态规划器，例如根据台阶高度确定抬腿高度，根据台阶深度确定落足点前后位置，根据走向角调整机身偏航。若只使用“前方有障碍”这类粗分类，步态规划仍缺乏足够约束。
2. 预规划步态强调“摆动相提前适配”，避免足端硬撞台阶立面
   普通平地步态在遇到台阶时，摆动腿容易撞到台阶立面或落足点过浅。预规划需要提前调整摆动相轨迹：抬腿高度应高于台阶高度加上安全余量，前伸距离应保证足端能落在台阶踏面安全区域内，落足速度应在接近踏面时降低，以减少冲击。 对于四足机器人，通常还要保证至少三条腿或稳定支撑多边形维持机身平衡，再让摆动腿跨越台阶。
3. 落足点选择必须结合机器人自身运动学可达域
   视觉给出的“可落足区域”不一定都能被机器人真正踩到。预规划需要把相机坐标系下的台阶参数转换到机身坐标系，再通过逆运动学判断足端是否能到达。 若落足点过远、过近、过偏或超出关节角度范围，即使视觉认为“有平面”，也不能直接采用。此时应调整机身位置、偏航角或步态相位，使落足点进入可达域。
4. BLDC FOC 提供柔顺执行能力，降低踏面冲击
   台阶越障过程中，足端从空中落到踏面时会产生明显冲击。若采用纯位置控制，关节容易硬性撞击；若引入力矩限幅、阻抗控制或柔顺位置跟踪，足端触地瞬间可以吸收部分冲击。 BLDC 配合 FOC 电流环能实现低转矩脉动和较快力矩响应，比开环 ESC 更适合这种需要精确角度、同时又要求柔顺触地的场景。
5. 分层架构更适合 Arduino/ESP32 平台落地
   完整系统建议分为四层：
   感知层：RGB-D/ToF 相机、IMU、关节编码器、足端触地检测。
   几何层：台阶检测、高度/深度估计、踏面分割、落足区域提取。
   规划层：步态模式选择、抬腿高度/步长/相位规划、落足点可达性校验。
   执行层：BLDC FOC 位置/速度/电流闭环、逆运动学、姿态补偿、安全保护。
   Arduino Uno/Nano 更适合承担单关节 BLDC 控制或简单步态执行；若要做实时台阶识别、逆运动学和多关节 FOC，建议使用 ESP32-S3、Teensy 4.1、STM32 或上位机协同。

应用场景
室内楼梯配送/巡检机器人：楼道、仓库、机房等环境中存在固定台阶，视觉预规划可避免盲目攀爬。
灾后搜救机器人：废墟中常出现破损台阶、落差平台和瓦砾堆，视觉识别台阶高度有助于选择跨越或绕行。
野外四足平台：田埂、石阶、堤岸、路缘石等离散地形需要精确落足点规划。
教育与科研实验平台：用于验证台阶几何估计、落足点规划、步态参数调制和 BLDC 关节柔顺控制。
人形/双足机器人爬阶验证：双足支撑面更小，对台阶高度、落足深度和质心控制更敏感，预规划价值更高。

需要注意的事项
视觉测高必须考虑相机外参和机身姿态
台阶高度不能只靠图像像素估算。相机安装角度、机身俯仰角、地面倾斜都会影响测量结果。应结合 IMU 姿态和相机标定，将深度点云转换到统一坐标系后再计算台阶高度。
台阶边缘容易误判，需加入踏面面积和深度连续性约束
单级台阶的踏面可能较窄，边缘处深度跳变明显。若只检测高度，可能把踏面边缘误判为可落足点。应检查踏面是否足够宽、是否连续、是否位于足端可达范围内。
预规划不能替代触地检测
视觉只能预测“应该落在哪里”，不能保证“实际是否踩稳”。台阶表面可能有灰尘、油污、松动木板或破损边缘。执行阶段仍需通过足端力、关节电流、编码器突变或 IMU 冲击信号确认触地，并在异常时快速抬腿或停机。
台阶越障时步态切换必须平滑
从平地切到爬阶步态时，抬腿高度、步长、机身高度和相位差应渐变，不能突变。否则支撑腿受力会瞬间变化，容易引起机身俯仰或侧翻。
BLDC 关节需做好堵转保护和电流限幅
爬阶过程中，足端可能磕碰台阶立面或陷入踏面边缘，关节会短时承受较大负载。应设置电流上限、温度监控和姿态异常降级策略，防止电机过热或减速器损坏。
Arduino 平台不宜同时承担视觉识别和多关节实时控制
台阶识别、点云处理、逆运动学和 FOC 闭环对算力要求较高。更合理的做法是上位机或高性能 MCU 负责视觉与几何估计，下位机负责步态执行和 BLDC 闭环。
落足点应保留安全余量
足端不应正好落在台阶边缘，而应向踏面内侧偏移一定距离，例如根据足端尺寸、定位误差和机身晃动留出 1~3 厘米余量。否则轻微定位偏差就可能导致足端滑出台阶。
下行台阶比上行更需要重心控制
下台阶时，机身容易前倾，足端触地瞬间也可能产生较大冲击。应提前降低重心、缩短步长、提高支撑腿数量，并在足端触地后缓慢转移重心，避免前翻。

1、超声波+IMU 融合测高 + 步态库切换（入门级）
场景说明：面向室内结构化台阶环境（楼梯间、门槛），用超声波测距传感器检测前方台阶高度，IMU 判断机身姿态，根据台阶高度从预定义步态库中选取对应步态参数。计算量小，适合 Arduino Mega/ESP32。

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

MPU6050 imu;
int ultrasonicPins[2] = {A0, A1};  // 前方双超声波（测距+测高）

// 四足关节电机（8 个关节：4 髋 + 4 膝）
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;   // 步长 cm
  float stepHeight;   // 步高 cm（足端离地高度）
  float phaseDiff;    // 相位差（度）
  float jointAngleMax;// 最大关节角度 rad
} 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();
  }
  Serial.println("台阶感知步态库就绪");
}

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

  // 步态参数映射到关节角度（简化正弦步态）
  static unsigned long lastGaitUpdate = 0;
  if (millis() - lastGaitUpdate > 20) {
    lastGaitUpdate = millis();
    executeGait(currentGait, targetStepHeight);
  }
}

void executeGait(GaitParams& gait, float stepH) {
  static float phase = 0;
  phase += 0.05f;
  if (phase > 2 * PI) phase -= 2 * PI;

  for (int leg = 0; leg < 4; leg++) {
    float legPhase = phase + (leg * gait.phaseDiff * PI / 180.0f);
    // 对角步态：0/3同相，1/2反相
    if (leg == 1 || leg == 2) legPhase += PI;

    // 髋关节：前后摆动
    float hipAngle = gait.jointAngleMax * sin(legPhase);
    // 膝关节：抬腿（仅摆动相）
    float kneeAngle = 0;
    if (sin(legPhase) > 0) {
      kneeAngle = (stepH / 20.0f) * sin(legPhase) * 0.5f;
    }

    joints[leg * 2].move(hipAngle);
    joints[leg * 2 + 1].move(kneeAngle);
  }
}

代码要点：通过超声波测距间接估算台阶高度（距离/5 的简化映射），IMU 俯仰角辅助判断坡度。步态库以结构体数组形式预存三套参数，根据地形识别结果直接索引切换 。斜坡场景下对步高做线性补偿，避免前腿绊倒或后腿蹬空。

2、深度相机台阶高度提取 + 贝塞尔足端轨迹预规划（进阶）
场景说明：使用 Intel RealSense 或轻量级 ToF 深度相机获取台阶高度，预规划贝塞尔曲线足端轨迹，确保抬腿轨迹在起点和落点处速度平滑、方向可控。适合算力较强的平台（ESP32-S3/Teensy 4.1）。

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

// ===== 四足BLDC（每腿2关节：髋+膝）=====
BLDCMotor motors[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)
};

// ===== 深度数据接口（模拟：实际通过UART/I2C读取相机）=====
struct StepInfo {
  float height;      // 台阶高度 (m)
  float distance;    // 台阶前沿距离 (m)
  bool  valid;       // 数据有效
} stepData = {0, 0, false};

// ===== 预规划参数 =====
float stepHeight = 0.15f;   // 预规划台阶高度 (m)
float stepLength = 0.20f;   // 步长 (m)
float liftClearance = 0.05f;// 抬腿净空 (m)

// ===== 贝塞尔轨迹点 =====
struct Point3D { float x, y, z; };

// 预计算足端轨迹（摆动相）
Point3D swingTrajectory[11];  // 11个路径点
int trajIndex = 0;

void setup() {
  Serial.begin(115200);
  for (int i = 0; i < 8; i++) {
    motors[i].init();
    motors[i].initFOC();
    motors[i].controller = MotionControlType::angle;
  }
  Serial.println("视觉预规划越障就绪");
}

void loop() {
  // ===== 1. 视觉感知：读取台阶高度 =====
  readDepthCamera();

  // ===== 2. 预规划：生成贝塞尔足端轨迹 =====
  if (stepData.valid && stepData.height > 0.05f) {
    planSwingTrajectory(stepData.height, stepData.distance);
  }

  // ===== 3. 执行：跟踪轨迹 =====
  for (int i = 0; i < 8; i++) motors[i].loopFOC();
  executeTrajectory();
}

void readDepthCamera() {
  // 模拟深度相机输出（实际通过UART读取）
  // 真实场景：提取ROI内台阶高度中值
  static unsigned long lastRead = 0;
  if (millis() - lastRead > 100) {
    lastRead = millis();
    // 示例：台阶高15cm，距离0.5m
    stepData.height = 0.15f;
    stepData.distance = 0.50f;
    stepData.valid = true;
  }
}

void planSwingTrajectory(float hStep, float dist) {
  // 足端起点（当前支撑位置）
  Point3D P0 = {0, 0, 0};

  // 通过点：台阶顶部（预规划落点）
  Point3D P3 = {dist, 0, hStep};

  // 控制点1：垂直抬腿（确保足够净空）
  Point3D P1 = {0, 0, hStep + liftClearance};

  // 控制点2：水平接近落点上方
  Point3D P2 = {dist, 0, hStep + liftClearance};

  // 三次贝塞尔采样：B(t) = (1-t)³P0 + 3(1-t)²tP1 + 3(1-t)t²P2 + t³P3
  for (int i = 0; i <= 10; i++) {
    float t = i / 10.0f;
    float u = 1.0f - t;
    float b0 = u*u*u, b1 = 3*u*u*t, b2 = 3*u*t*t, b3 = t*t*t;

    swingTrajectory[i].x = b0*P0.x + b1*P1.x + b2*P2.x + b3*P3.x;
    swingTrajectory[i].y = b0*P0.y + b1*P1.y + b2*P2.y + b3*P3.y;
    swingTrajectory[i].z = b0*P0.z + b1*P1.z + b2*P2.z + b3*P3.z;
  }
  trajIndex = 0;
  Serial.print("轨迹已规划，台阶高度: ");
  Serial.println(hStep * 100);
}

void executeTrajectory() {
  static unsigned long lastStep = 0;
  if (millis() - lastStep < 50) return;  // 每50ms推进一个轨迹点
  lastStep = millis();

  if (trajIndex > 10) return;

  Point3D target = swingTrajectory[trajIndex];
  trajIndex++;

  // 逆运动学：足端位置 → 关节角度（简化二连杆模型）
  float hipAngle, kneeAngle;
  footToJointAngles(target.x, target.z, hipAngle, kneeAngle);

  // 应用到前左腿（leg 0）
  motors[0].move(hipAngle);
  motors[1].move(kneeAngle);

  Serial.print("轨迹点 ");
  Serial.print(trajIndex);
  Serial.print(": x=");
  Serial.print(target.x, 3);
  Serial.print(" z=");
  Serial.println(target.z, 3);
}

void footToJointAngles(float x, float z, float& hip, float& knee) {
  // 简化二连杆逆运动学（腿长L1=L2=0.15m）
  float L1 = 0.15f, L2 = 0.15f;
  float dist = sqrt(x*x + z*z);
  dist = constrain(dist, 0.01f, L1 + L2 - 0.01f);

  // 余弦定理求膝关节角
  float cosKnee = (L1*L1 + L2*L2 - dist*dist) / (2*L1*L2);
  cosKnee = constrain(cosKnee, -1.0f, 1.0f);
  knee = PI - acos(cosKnee);

  // 髋关节角
  float alpha = atan2(x, z);
  float cosBeta = (L1*L1 + dist*dist - L2*L2) / (2*L1*dist);
  cosBeta = constrain(cosBeta, -1.0f, 1.0f);
  float beta = acos(cosBeta);
  hip = alpha + beta;
}

代码要点：贝塞尔曲线控制点 P1 设为垂直抬腿方向，确保足端先垂直上升获得净空，再水平接近落点上方，最后垂直下降触地——这种“垂直上升-水平过渡-垂直下降”的策略能有效防止台阶边缘绊脚和触地滑移 。逆运动学用余弦定理简化二连杆解算，避免实时浮点三角函数瓶颈。

3、视觉+触觉融合 + 越障状态机 + 预规划补偿（工业级）
场景说明：视觉提供台阶高度先验信息用于预规划，触觉（足端力传感器）提供落点确认与打滑检测。预规划阶段先生成标称轨迹，执行中根据触觉反馈实时补偿轨迹落点，形成“视觉预规划 + 触觉修正”的闭环。适合高性能平台（Jetson + Arduino 从站架构）。

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

MPU6050 imu;

// ===== 四足BLDC =====
BLDCMotor motors[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)
};

// ===== 足端力传感器 =====
int forcePins[4] = {A0, A1, A2, A3};
float footForces[4] = {0};

// ===== 越障状态机 =====
enum StairState {
  APPROACH,      // 接近台阶
  PRE_PLAN,      // 预规划轨迹
  LIFT_LEG,      // 抬腿
  SWING,         // 摆动越障
  LAND_CHECK,    // 落点确认
  BODY_SHIFT,    // 重心转移
  STANCE         // 支撑
} state = APPROACH;

// ===== 视觉台阶信息 =====
struct StepVision {
  float height;
  float distance;
  bool  detected;
} vision = {0, 0, false};

// ===== 预规划轨迹（标称）=====
struct TrajPoint { float x, z; };
TrajPoint nominalTraj[11];
TrajPoint compensatedTraj[11];
int trajIdx = 0;

// ===== 触觉修正参数 =====
float heightCorrection = 0;  // 高度补偿
float slipDetection = 0;     // 打滑检测

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

  for (int i = 0; i < 8; i++) {
    motors[i].init();
    motors[i].initFOC();
    motors[i].controller = MotionControlType::angle;
  }
  Serial.println("视觉+触觉融合越障就绪");
}

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

  // ===== 1. 感知层 =====
  readVision();
  readFootForces();
  readIMU();

  // ===== 2. 调度层：状态机 =====
  switch (state) {
    case APPROACH:
      if (vision.detected && vision.height > 0.05f) {
        state = PRE_PLAN;
        Serial.println("检测到台阶，进入预规划");
      }
      break;

    case PRE_PLAN:
      planNominalTrajectory(vision.height);
      state = LIFT_LEG;
      break;

    case LIFT_LEG:
      // 垂直抬腿阶段
      executeLiftPhase();
      if (trajIdx > 2) state = SWING;
      break;

    case SWING:
      // 摆动越障，实时触觉补偿
      executeSwingPhase();
      if (trajIdx > 9) state = LAND_CHECK;
      break;

    case LAND_CHECK:
      // 落点确认：力传感器检测触地
      if (checkLanding()) {
        state = BODY_SHIFT;
      } else {
        // 未触地：微调落点高度
        heightCorrection -= 0.005f;
        Serial.print("落点补偿: ");
        Serial.println(heightCorrection);
      }
      break;

    case BODY_SHIFT:
      // 重心转移，准备下一腿
      delay(200);
      state = APPROACH;
      vision.detected = false;
      break;
  }
}

void readVision() {
  // 模拟深度相机/视觉识别输出
  static unsigned long lastRead = 0;
  if (millis() - lastRead > 200) {
    lastRead = millis();
    vision.height = 0.15f;
    vision.distance = 0.40f;
    vision.detected = true;
  }
}

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

void readIMU() {
  static unsigned long lastIMU = 0;
  if (millis() - lastIMU > 50) {
    lastIMU = millis();
    int16_t ax, ay, az;
    imu.getAcceleration(&ax, &ay, &az);
    // 俯仰角用于机身姿态补偿
  }
}

void planNominalTrajectory(float hStep) {
  // 标称轨迹控制点
  float liftClearance = 0.06f;
  float dist = vision.distance;

  // 贝塞尔控制点
  float P0[2] = {0, 0};
  float P1[2] = {0, hStep + liftClearance};           // 垂直抬腿
  float P2[2] = {dist, hStep + liftClearance};        // 水平过渡
  float P3[2] = {dist, hStep};                         // 落点

  for (int i = 0; i <= 10; i++) {
    float t = i / 10.0f;
    float u = 1.0f - t;
    float b0 = u*u*u, b1 = 3*u*u*t, b2 = 3*u*t*t, b3 = t*t*t;

    nominalTraj[i].x = b0*P0[0] + b1*P1[0] + b2*P2[0] + b3*P3[0];
    nominalTraj[i].z = b0*P0[1] + b1*P1[1] + b2*P2[1] + b3*P3[1];

    // 触觉补偿后的轨迹
    compensatedTraj[i].x = nominalTraj[i].x;
    compensatedTraj[i].z = nominalTraj[i].z + heightCorrection;
  }
  trajIdx = 0;
}

void executeLiftPhase() {
  static unsigned long lastStep = 0;
  if (millis() - lastStep < 80) return;
  lastStep = millis();

  if (trajIdx > 10) return;
  float hip, knee;
  footToJointAngles(nominalTraj[trajIdx].x, nominalTraj[trajIdx].z, hip, knee);
  motors[0].move(hip);
  motors[1].move(knee);
  trajIdx++;
}

void executeSwingPhase() {
  static unsigned long lastStep = 0;
  if (millis() - lastStep < 60) return;
  lastStep = millis();

  if (trajIdx > 10) return;
  float hip, knee;
  footToJointAngles(compensatedTraj[trajIdx].x, compensatedTraj[trajIdx].z, hip, knee);
  motors[0].move(hip);
  motors[1].move(knee);
  trajIdx++;
}

bool checkLanding() {
  // 足端力超过阈值判定触地
  return footForces[0] > 150;
}

void footToJointAngles(float x, float z, float& hip, float& knee) {
  float L1 = 0.15f, L2 = 0.15f;
  float dist = sqrt(x*x + z*z);
  dist = constrain(dist, 0.01f, L1 + L2 - 0.01f);

  float cosKnee = (L1*L1 + L2*L2 - dist*dist) / (2*L1*L2);
  cosKnee = constrain(cosKnee, -1.0f, 1.0f);
  knee = PI - acos(cosKnee);

  float alpha = atan2(x, z);
  float cosBeta = (L1*L1 + dist*dist - L2*L2) / (2*L1*dist);
  cosBeta = constrain(cosBeta, -1.0f, 1.0f);
  float beta = acos(cosBeta);
  hip = alpha + beta;
}

代码要点：状态机将越障分解为 APPROACH→PRE_PLAN→LIFT_LEG→SWING→LAND_CHECK→BODY_SHIFT 六个阶段，每阶段有明确的进入/退出条件 。视觉预规划生成标称轨迹（nominalTraj），触觉修正生成补偿轨迹（compensatedTraj）——如果落点检测失败（力传感器未触发），则逐步降低 heightCorrection 微调落点高度，直至触地成功。

要点解读

1. 视觉测高的本质是“先验信息”，而非“精确控制依据”

视觉识别台阶高度受光照、纹理、相机标定精度影响，很难达到毫米级精度 。在工程实践中，视觉的作用是提供台阶高度的粗估值（±2-3cm 精度），用于预规划步态参数（抬腿高度、步长）。真正的落点确认必须依赖触觉（足端力传感器或关节电流突变）。案例三的“视觉预规划 + 触觉修正”双闭环架构是稳健的工程方案。

2. 预规划越障步态的核心是“垂直上升-水平过渡-垂直下降”三段式

台阶越障的足端轨迹必须满足：抬腿阶段垂直上升（避免台阶边缘绊脚），过渡阶段水平前移（减少不必要的垂直运动），落地阶段垂直下降（防止触地滑移）。贝塞尔曲线通过控制点设计天然满足这一需求：P1 设在起点正上方，P2 设在落点正上方，P0→P1 和 P2→P3 都是近似垂直的。相比简单的正弦轨迹，贝塞尔轨迹的起落速度更可控。

3. 步态库 + 参数化是算力受限平台的务实选择

Arduino 平台的浮点运算能力有限，实时生成复杂轨迹不现实。预定义步态库（如案例一的结构体数组）将“地形类型→步态参数”的映射固化为查表操作，运行时只需做简单索引和插值 。台阶越障场景下，步态参数的关键调整方向是：减小步长（避免前腿过早触地）、增加抬腿高度（确保越过台阶边缘）、降低步频（增加支撑时间）。

4. 逆运动学必须采用简化解析解，避免通用数值求解

在 Arduino 上实现通用逆运动学（如雅可比伪逆）计算量过大。案例二和案例三采用二连杆余弦定理解析解，仅需几次 acos/atan2 调用即可完成足端位置到关节角度的转换 。前提是腿部构型满足二连杆假设（大腿+小腿，膝关节单自由度）。如果腿部有冗余自由度，可采用预计算查表法：离线生成足端空间到关节空间的映射表，运行时插值。

5. 状态机是越障调度的骨架，触觉反馈是状态转换的“金标准”

台阶越障涉及多个物理阶段的顺序切换：接近→抬腿→摆动→触地→支撑。每个阶段的转换必须基于可靠的状态确认信号，而非简单的时间延迟 。触觉信号（足端力或关节电流）是判断“是否真正触地”的唯一可靠依据——视觉只能告诉机器人“台阶在那里”，触觉才能告诉机器人“我踩上去了”。案例三的 LAND_CHECK 状态用 footForces[0] > 150 作为触地确认条件，并在未触地时持续微调落点高度，这是防止踏空或踩偏的关键机制。

4、室内单级台阶视觉识别+预规划越障步态
适用场景：室内、平整路面的单级台阶，精准识别台阶高度，预规划轮足协同的越障步态。

#include <SimpleFOC.h>
SimpleFOCMotor2 frontLift = SimpleFOCMotor2(2,3,4,A0,A1); // 前轮悬挂抬升关节
SimpleFOCMotor2 wheelL = SimpleFOCMotor2(5,6,7,A2,A3);    // 左驱动轮
SimpleFOCMotor2 wheelR = SimpleFOCMotor2(8,9,10,A4,A5);   // 右驱动轮
float step_height = 0; // 台阶高度，单位cm
int detect_step_height(){
  // 模拟视觉返回台阶高度，实际接OpenMV/深度相机接口
  if(openmv.find_step()) {
    float h = openmv.get_step_height(); // 0-30cm
    if(h >=1 && h <=30) { step_height = h; return 1; }
  }
  return 0;
}

// 预规划越障步态参数
void preplan_gait(){
  float target_lift = step_height * 0.15f; // 抬轮高度与台阶高度线性正相关
  float target_speed = (step_height >15) ? 0.4f : 0.6f; // 台阶越高，速度越慢
  frontLift.set(target_lift,1.0f);
  wheelL.setVelocity(target_speed * 120);
  wheelR.setVelocity(target_speed * 120);
}

void setup(){
  Serial.begin(9600);
  frontLift.init(); wheelL.init(); wheelR.init();
  frontLift.controller=FOC_VELOCITY;
  wheelL.controller=FOC_VELOCITY; wheelR.controller=FOC_VELOCITY;
}

void loop(){
  if(detect_step_height()){
    preplan_gait();
  } else {
    frontLift.set(0,0); // 无台阶，恢复正常
    wheelL.setVelocity(80);
    wheelR.setVelocity(80);
  }
  delay(10);
}

5、户外高低差视觉识别+动态预规划越障
适用场景：户外复杂路面的不规则高低差，结合视觉识别高度与地形，动态预规划越障步态。

#include <SimpleFOC.h>
SceneClassifier; // 视觉地形分类
SimpleFOCMotor2 hip[2] = {SimpleFOCMotor2(2,3,4,A0,A1), SimpleFOCMotor2(5,6,7,A2,A3)}; // 左右髋关节
SimpleFOCMotor2 wheelL = SimpleFOCMotor2(8,9,10,A4,A5);
SimpleFOCMotor2 wheelR = SimpleFOCMotor2(11,12,13,A6,A7);
float height_diff = 0;
int terrain_type = 0; // 0:平缓 1:低丘 2:高坎
int detect_terrain_and_height(){
  sceneClassifier.update();
  terrain_type = sceneClassifier.get_terrain_type();
  height_diff = sceneClassifier.get_height_diff(); // 识别高低差高度
  return (terrain_type !=0 && height_diff>=1);
}

// 结合地形与高度预规划越障
void preplan_adapt_gait(){
  float lift_angle=0, step_amp=0, speed=0.5f;
  if(terrain_type==1){ // 低丘：小抬腿+正常步幅
    lift_angle = height_diff * 0.1f;
    step_amp =0.3f;
    speed =0.6f;
  } else if(terrain_type==2){ // 高坎：大抬腿+缩小步幅
    lift_angle = height_diff * 0.18f;
    step_amp =0.2f;
    speed =0.3f;
  }
  hip[0].set(lift_angle + step_amp ,1.0f); // 左髋抬升+前摆
  hip[1].set(lift_angle - step_amp ,1.0f); // 右髋保持
  wheelL.setVelocity(speed * 120);
  wheelR.setVelocity(speed * 120);
}

void loop(){
  if(detect_terrain_and_height()){
    preplan_adapt_gait();
  } else {
    hip[0].set(0,0);
    hip[1].set(0,0);
    wheelL.setVelocity(70);
    wheelR.setVelocity(70);
  }
  delay(10);
}

6、连续台阶视觉识别+预规划连续越障步态
适用场景：楼梯等连续台阶场景，识别多级台阶高度，预规划连续越障步态，实现平稳上下楼梯。

#include <SimpleFOC.h>
SimpleFOCMotor2 leg[4] = {/* 四足12个关节定义同前 */};
int step_count=0; // 连续台阶数量float step_height =0; int detect_stair_info(){
  if(openmv.find_stairs()){
    step_count = openmv.get_stair_count();
    step_height = openmv.get_avg_step_height();
    if(step_height>=1 && step_count<=30) return 1; 
  }
  return 0;
}

// 预规划连续越障步态：交替抬腿逐级越障
void preplan_continuous_gait(){
  float lift_height = step_height *0.16f; // 单级抬腿高度
  float cycle_time = 1.2f + step_height *0.01f; // 台阶越高，步态周期越长
  static float phase =0;
  phase += cycle_time / 10 *2*M_PI; // 按周期推进相位
  float diag_lift = lift_height * sin(phase); // 对角腿交替抬升
  leg[0].set(diag_lift,1.0f);      // 左前腿
  leg[6].set(diag_lift,1.0f);      // 右后腿
  leg[3].set(-diag_lift*0.5f,1.0f); // 右侧腿支撑补偿
}

void setup(){
  Serial.begin(9600);
  for(int i=0;i<12;i++){ leg[i/3].init(); leg[i/3].controller=FOC_POSITION; }
}

void loop(){
  if(detect_stair_info()){
    preplan_continuous_gait();
  } else {
    for(int i=0;i<12;i++) leg[i/3].set(0,0);
  }
  delay(10);
}

要点解读
视觉识别需优先保证精度，设置置信度过滤
台阶高度识别误差会导致越障失败，需采用深度相机优先识别（精度高）或单目+结构光辅助；需设置置信度阈值，低于80%时不进入越障模式，等待重新识别，避免误判导致步态失效。

预规划步态需匹配台阶高度，参数线性可调
抬腿高度、步幅、速度需与台阶高度线性相关，避免固定步态适配所有高度；台阶越高，抬腿高度越大、速度越慢、步幅越小，同时预留20%的安全余量，防止抬腿高度不足绊倒。

需结合地形融合规划，避免单一高度参数失配
户外场景高低差常伴随坡度、障碍，需结合地形类型调整步态：高坎时缩小步幅，低丘时增加速度；连续台阶需保证逐级发力，避免一次跨越多级台阶导致失稳。

需加入异常回退逻辑，保障越障安全
执行越障后若视觉检测到台阶仍存在（越障失败），需立刻减速回退到台阶边缘，重新启动规划；同时设置机身倾斜阈值，倾斜超过15°时立刻停止，防止倾覆。

步态执行需柔性过渡，减少规划与执行断层
预规划的参数不能瞬间输出，需设置渐变过渡逻辑，抬腿、速度在100-200ms内逐步调整到位，避免刚性冲击；同时加入力矩反馈，抬腿触地后根据压力调整支撑力，保证平稳落地。

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