该方案的核心特点是利用CPG网络生成节律性步态信号并通过传感器反馈动态调制抬腿高度与步长，结合BLDC电机的高动态响应实现非结构化地形的稳定行走；主要适用于四足/多足巡检、搜救、农业及教育科研等场景；实际部署需重点解决Arduino算力瓶颈、CPG参数调优难度、传感器可靠性及电源管理等问题。

一、主要特点
CPG步态引擎——从"硬编码步态"到"生物启发的自适应节律"
CPG（Central Pattern Generator，中枢模式发生器）源自生物体脊髓中的神经振荡网络，能够在没有外部节律性输入的情况下自主产生稳定的周期性运动信号。在机器人控制中，CPG的核心价值在于：
自组织节律生成：CPG网络由多个耦合的非线性振荡器组成，每个振荡器对应一条腿的关节运动。振荡器之间通过耦合参数（相位差、耦合强度）建立协调关系，自动生成如trot（对角步态）、walk（慢步）、bound（跳跃步态）等多种步态模式，无需为每种步态单独编写硬编码轨迹。
参数化调制能力：CPG的输出信号具有明确的物理含义——频率控制步速、振幅控制步长、相位差控制腿间协调关系。通过调整这些参数，可以在不同步态之间实现无缝平滑切换，而非传统方法中的"停车→切换→重启"。
双层网络架构：先进的CPG设计采用双层结构——主单元（Master Unit）负责控制髋关节（第1关节），从单元（Slave Unit）负责控制膝关节和踝关节（第2、3关节）。主单元接收IMU的偏航角（yaw）反馈，从单元接收俯仰角（pitch）反馈，形成层级化的姿态适应机制。
地形自适应——自适应抬腿高度与步长
这是CPG步态引擎与传感器反馈深度融合的核心体现。传统固定步态机器人在遇到台阶、碎石或斜坡时，要么绊倒要么打滑。自适应机制通过以下路径实现：
抬腿高度自适应：当足端力传感器或IMU检测到前方地形升高（如上坡、台阶），CPG网络中的反馈参数（ offset_goffset g）立即增大，驱动振荡器输出更高振幅的信号，使腿部抬升高度自动增加，防止脚尖磕碰障碍。反之，在平坦地面降低抬腿高度以节省能耗。
步长自适应：IMU检测到坡度变化时，CPG网络自动调整振荡频率和振幅的组合——上坡时减小步长（缩短步幅防打滑）、增加步高（防绊倒）；下坡时适当增大步长、降低步高，保持重心稳定。
触地反馈闭环：足底接触传感器（FSR）实时检测每只脚的触地时刻。当某只脚提前触地（说明地面比预期高）或延迟触地（说明地面比预期低/悬空），CPG网络立即调整该腿的摆动相位和幅度，实现"试探性落脚"。
BLDC驱动——高动态执行保障
BLDC无刷电机配合FOC（磁场定向控制）为CPG步态引擎提供了关键的执行层支撑：
高带宽力矩响应：FOC电流环带宽可达千赫兹级，能够在CPG输出的每个控制周期（通常20ms，即50Hz）内精确跟踪目标关节角度和力矩，确保步态信号的忠实执行。
力矩-位置混合控制：在支撑相（stance phase），BLDC工作于力矩控制模式，根据机身姿态和负载分布动态调整各腿的支撑力；在摆动相（swing phase），切换为位置控制模式，精确跟踪CPG生成的抬腿轨迹。这种混合控制策略兼顾了承载能力和运动灵活性。
低速平稳性：BLDC的正弦波驱动消除了有刷电机的齿槽效应和电刷噪声，在低速爬行步态下仍能保持平稳输出，避免因电机抖动导致的步态失稳。
传感器融合与反馈机制
CPG步态引擎的自适应能力完全依赖于多源传感器反馈：
IMU（MPU6050/ICM20948）：提供机身roll/pitch/yaw三轴姿态角和角速度，作为CPG网络的全局反馈信号。当姿态角偏离稳定范围（如pitch超过±8°），CPG自动调整参数恢复平衡。
足端力传感器（FSR）：检测每只脚的触地状态和接触力大小，用于判断支撑相/摆动相的实际切换时刻，修正CPG的理论相位。
超声波/激光雷达：前向障碍物检测，当检测到障碍物高度超过阈值（如15cm），触发"跨越步态"——CPG网络临时切换为高抬腿、大步幅的特殊模式。

二、典型应用场景
四足/多足巡检机器人
这是CPG步态引擎最核心的应用场景。变电站、管道廊道、矿山巷道等工业环境地形复杂（台阶、沟槽、碎石），轮式机器人难以通行。
自适应优势：CPG步态引擎使机器人能够根据地面起伏自动调整步高和步长，无需预先建模地形。例如，在管道廊道中遇到法兰凸起时，机器人自动抬高对应腿的抬腿高度跨越障碍，其余腿保持正常步态。
BLDC优势：FOC控制提供精确的力矩输出，确保机器人在倾斜管道上仍能保持稳定的支撑力分配。
搜救与灾难响应机器人
地震废墟、坍塌建筑等环境中，地形极度不规则且不可预测。
CPG优势：CPG网络的局部支撑性使得机器人在某条腿被瓦砾卡住时，其余腿的步态不受影响，系统自动调整被卡腿的相位和幅度尝试脱困，而非整体停机。
自适应抬腿：面对不同高度的废墟缝隙，机器人自动调整抬腿高度进行试探性穿越。
农业与野外探测机器人
农田、林地、沙地等非结构化环境中，地面松软度和起伏变化剧烈。
步长自适应：在松软沙地中，CPG自动缩短步长、降低步速，减少足端对地面的冲击力，防止陷入。在硬质地面恢复大步长、快步速，提高行进效率。
BLDC再生制动：下坡时BLDC工作于发电模式，回馈能量延长续航。
教育与科研平台
CPG步态引擎是机器人学、神经科学和仿生工程交叉领域的经典教学案例。
算法验证：Arduino生态提供了丰富的开源库（如SimpleFOC），便于快速搭建CPG网络并验证不同耦合参数对步态的影响。
竞赛应用：在RoboMaster、全国大学生机器人大赛等赛事中，CPG步态引擎为四足机器人提供了灵活的运动能力。

三、需要注意的事项
Arduino算力瓶颈是首要限制
CPG网络涉及多个耦合非线性微分方程的实时求解（如二阶Matsuoka振荡器模型），对算力和浮点运算能力要求较高。
平台选型：Arduino Uno（AVR，16MHz，无FPU）几乎无法胜任。必须选用ESP32-S3（双核240MHz，带FPU）、STM32F4/H7或Arduino Portenta H7等高性能平台。
算法轻量化：使用查表法（LUT）替代实时三角函数计算；将CPG参数量化为定点数（如Q15格式）；控制频率不低于50Hz以保证步态实时性。
主从架构：将CPG网络的全局参数优化放在上位机（如树莓派），Arduino仅负责单步振荡器的实时积分和BLDC指令下发。
CPG参数调优是最大工程挑战
CPG网络的性能高度依赖于耦合参数的精确设定。参数不当会导致步态不稳定、振荡发散或响应迟钝。
分步调参策略：先固定单腿CPG单元，调通单腿的振荡频率和振幅；再逐步增加耦合腿数，调通双腿、四腿的相位协调。
参数敏感性分析：对每个耦合参数进行扫参实验，确定稳定域边界，避免参数落入不稳定区域。
在线自适应：引入模糊逻辑或强化学习，让CPG参数能够根据地形反馈在线微调，而非依赖离线固定参数。
传感器可靠性与冗余设计
CPG步态引擎的自适应能力完全依赖传感器反馈，传感器失效将导致步态崩溃。
IMU漂移：MEMS陀螺仪存在零偏漂移，长时间运行后姿态估计失准。必须通过加速度计和磁力计进行互补滤波或扩展卡尔曼滤波（EKF）校正。
足端传感器防护：FSR力传感器在泥泞、粉尘环境中易失效。需加装防水防尘密封罩，并设计基于电机电流估算触地力的备用方案（当电流突增时判定为触地）。
传感器故障检测：设计传感器健康监控机制，当某传感器数据异常（如IMU读数突变、FSR持续为零）时，自动切换到降级步态模式（如降低速度、增大稳定裕度）。
BLDC参数匹配与FOC调参
CPG步态引擎对BLDC的动态响应一致性要求极高：
电机一致性：同一机器人上的所有BLDC电机应选用同批次产品，并精确测量电机极对数、内阻、电感等参数，写入FOC驱动板。
PID整定：FOC的电流环、速度环、位置环PID参数必须精细整定。电流环带宽应≥1kHz，速度环响应时间应<10ms，确保能忠实跟踪CPG的高频指令。
力矩限幅：必须设置关节力矩的硬性上限，防止CPG输出异常信号导致电机过载或机械结构损坏。MPC求解失败时需平滑回退至阻抗控制模式。
电源管理与续航
多足机器人的BLDC电机数量多（通常8-16个），瞬时功耗大。
电源隔离：BLDC动力电源（12V/24V）与MCU逻辑电源（3.3V/5V）必须物理隔离，仅单点共地，防止电机换相噪声导致MCU复位。
电容滤波：电源入口并联大容量电解电容（>1000μF）吸收瞬态电流尖峰。
低功耗步态：在待机或低速巡检模式下，CPG自动降低振荡频率和振幅，减少BLDC功耗。
电磁兼容（EMC）
BLDC高频PWM开关噪声易干扰IMU、编码器等模拟信号，导致CPG反馈信号失真。
布线规范：电机相线与传感器信号线正交走线，避免平行走线。
屏蔽设计：传感器线缆使用屏蔽线，屏蔽层单端接地。
时序隔离：在电机静止或低负载时段进行IMU采样，避开PWM驱动的峰值干扰期。

1、基础 CPG 相位振荡器与 Trot 步态生成
这个案例实现最核心的 CPG 引擎，使用简化的相位振荡器模型控制四条腿的髋关节，产生稳定的对角步态（Trot gait），并对步态频率进行基础调节。

// 案例一：基础CPG振荡器与Trot步态 (基于ESP32 + SimpleFOC)
// 功能：生成稳定的对角步态，频率可调，作为上层自适应控制的基础

#include <SimpleFOC.h>

// 定义8个关节电机 (4条腿，每条腿髋关节+膝关节)
BLDCMotor hipMotors[4] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};
BLDCMotor kneeMotors[4] = {BLDCMotor(7), BLDCMotor(7), BLDCMotor(7), BLDCMotor(7)};
// 对应的驱动器和编码器初始化略...

// CPG核心参数
struct CPGParams {
  float frequency;    // 步态频率 (Hz)
  float amplitude;    // 摆动幅度 (弧度)
  float phase[4];     // 各腿相位偏移 (弧度)
} cpg;

// 腿部索引: 0=左前(LF), 1=右前(RF), 2=左后(LH), 3=右后(RH)
// Trot步态相位差: 对角腿同相 (LF与RH同相, RF与LH同相), 对角之间相差PI
const float TROT_PHASE[4] = {0.0, PI, PI, 0.0}; 

float oscillatorState[4] = {0.0, 0.0, 0.0, 0.0}; // 各腿当前相位

void setupCPG() {
  cpg.frequency = 1.0;   // 1Hz步态
  cpg.amplitude = 0.4;   // 约23度摆动
  for (int i = 0; i < 4; i++) {
    cpg.phase[i] = TROT_PHASE[i];
  }
  // 电机初始化及FOC配置略...
}

void loopCPG() {
  static unsigned long lastTime = micros();
  unsigned long currentTime = micros();
  float dt = (currentTime - lastTime) / 1000000.0f;
  lastTime = currentTime;

  // 1. 更新振荡器状态 (相位累加)
  for (int i = 0; i < 4; i++) {
    oscillatorState[i] += 2.0f * PI * cpg.frequency * dt;
    if (oscillatorState[i] > 2.0f * PI) oscillatorState[i] -= 2.0f * PI;
  }

  // 2. 计算髋关节目标角度 (正弦摆动)
  float hipAngles[4];
  for (int i = 0; i < 4; i++) {
    // 应用相位偏移，生成各腿协调的摆动
    float phaseShifted = oscillatorState[i] + cpg.phase[i];
    hipAngles[i] = cpg.amplitude * sin(phaseShifted);
  }

  // 3. 膝关节跟随髋关节做简单的屈伸 (简化的逆运动学)
  //    实际项目中需要完整的IK计算
  float kneeAngles[4];
  for (int i = 0; i < 4; i++) {
    // 膝关节角度随髋关节摆动而反向变化，模拟抬腿和落地
    kneeAngles[i] = -0.5f * hipAngles[i] + 0.3f; 
  }

  // 4. 下发给电机 (由SimpleFOC执行速度/位置闭环)
  for (int i = 0; i < 4; i++) {
    hipMotors[i].target = hipAngles[i];
    kneeMotors[i].target = kneeAngles[i];
  }

  // 注意: SimpleFOC的 loopFOC() 和 move() 需在更高速的中断或主循环中调用
}

2、基于 IMU 反馈的地形自适应（抬腿高度/步长调节）
在案例一的基础上，引入 IMU 姿态数据和足端触地检测，实现抬腿高度和步长的动态调整。这体现了核心的“地形自适应”思想。

// 案例二：基于IMU反馈的自适应步态 (抬腿高度/步长)
// 功能：根据机身倾斜度自动调整步高和步长，提高斜坡/崎岖地形的通过性

#include <SimpleFOC.h>
#include <MPU6050_tockn.h> // 常用的MPU6050库

MPU6050 mpu6050(Wire);

// ... (电机定义同案例一) ...

// 自适应参数结构体
struct AdaptiveParams {
  float baseStepHeight;    // 基础抬腿高度 (弧度)
  float maxStepHeight;     // 最大抬腿高度
  float baseStepLength;    // 基础步长 (弧度)
  float pitchCompensation; // 俯仰补偿系数
} adapt;

// 状态变量
float currentPitch = 0.0f;      // 机身俯仰角 (度)
bool footContact[4] = {0,0,0,0}; // 足端触地标志 (由电流环或接触传感器获得)

void setupAdaptive() {
  adapt.baseStepHeight = 0.15f;
  adapt.maxStepHeight = 0.35f;
  adapt.baseStepLength = 0.3f;
  adapt.pitchCompensation = 0.005f; // 每度俯仰补偿系数

  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
}

void loopAdaptive() {
  // 1. 读取IMU姿态
  mpu6050.update();
  currentPitch = mpu6050.getAngleX(); // 假设X轴为俯仰方向

  // 2. 读取足端触地状态 (通过FOC的电流环Iq分量)
  for (int i = 0; i < 4; i++) {
    // 当关节电机力矩 (电流) 突然增大且位置几乎不变时，判定为触地
    // 此处简化为模拟值
    footContact[i] = (hipMotors[i].current_q > 1.5f); 
  }

  // 3. 动态调整CPG参数 (核心自适应逻辑)
  // 上坡时 (正俯仰): 增加前腿步高，减小步长，防止踢到坡面
  // 下坡时 (负俯仰): 增加后腿步高
  float stepHeightOffset = 0.0f;
  float stepLengthOffset = 0.0f;
  
  if (currentPitch > 5.0f) { // 上坡
    stepHeightOffset = adapt.pitchCompensation * currentPitch;
    stepLengthOffset = -adapt.pitchCompensation * currentPitch * 0.5f;
  } else if (currentPitch < -5.0f) { // 下坡
    stepHeightOffset = -adapt.pitchCompensation * currentPitch;
    stepLengthOffset = adapt.pitchCompensation * currentPitch * 0.5f;
  }

  // 限幅处理
  float targetStepHeight = constrain(adapt.baseStepHeight + stepHeightOffset, 
                                      0.1f, adapt.maxStepHeight);
  float targetStepLength = constrain(adapt.baseStepLength + stepLengthOffset, 
                                      0.15f, 0.5f);

  // 4. 将调整后的步高和步长映射到CPG振幅和关节角度
  //    例如: 步高对应膝关节摆动幅度，步长对应髋关节摆动幅度
  cpg.amplitude = targetStepLength; // 髋关节摆幅决定步长
  // 膝关节的摆幅需要根据步高独立控制，此处简化为线性关系
  float kneeSwingAmp = targetStepHeight * 1.5f; 

  // 5. 执行步态更新 (同案例一，但使用更新后的cpg.amplitude和kneeSwingAmp)
  // ...
}

3、多传感器融合与地形类型识别驱动的步态库切换
这是一个更完整的实现，它结合多种传感器（超声波、IMU、力矩）识别地形类型（平地、斜坡、台阶），然后调用预设的“最优步态库”，并动态调整参数，实现更高层面的地形自适应。

// 案例三：地形类型识别驱动的步态库切换 (基于Arduino Mega)
// 功能：识别平地/斜坡/台阶，切换不同的步态模式，并精细调整步高步长

#include <SimpleFOC.h>
#include <MPU6050_tockn.h>

// ... (电机定义同案例一，但使用Arduino Mega的更多引脚) ...

// 地形类型枚举
enum TerrainType { FLAT, SLOPE, STEP };
TerrainType currentTerrain = FLAT;

// 步态库参数集
struct GaitProfile {
  float stepLength;   
  float stepHeight;   
  float phaseDiff;    // 相位差，Trot为180°
  float kneeGain;     // 膝关节跟随系数
} gaitLibrary[3] = {
  {0.25f, 0.12f, 180.0f, 0.6f},  // FLAT: 标准步态
  {0.20f, 0.18f, 160.0f, 0.8f},  // SLOPE: 步长缩短，步高增加
  {0.15f, 0.25f, 150.0f, 1.0f}   // STEP: 步长更短，步高更高
};

GaitProfile currentGait = gaitLibrary[0];

// 传感器变量
float slopeAngle = 0.0f;
int obstacleHeight = 0;        // cm

void setup() {
  Serial.begin(115200);
  // 初始化IMU、超声波传感器、关节电机等...
  // 电机初始化及FOC配置略...
}

void loop() {
  // 1. 地形识别 (多传感器融合决策)
  currentTerrain = identifyTerrain();

  // 2. 根据地形选择并微调步态参数
  switch (currentTerrain) {
    case FLAT:
      currentGait = gaitLibrary[0];
      // 平地上可适当增加步长提高速度
      currentGait.stepLength += 0.03f;
      break;
    case SLOPE:
      currentGait = gaitLibrary[1];
      // 根据坡度大小进一步微调
      currentGait.stepHeight += slopeAngle * 0.002f;
      break;
    case STEP:
      currentGait = gaitLibrary[2];
      // 根据台阶高度调整步高
      currentGait.stepHeight = constrain(obstacleHeight * 0.8f, 0.15f, 0.35f);
      break;
  }

  // 3. 执行步态更新 (使用currentGait参数)
  executeGait(currentGait);

  delay(20); // 50Hz控制周期
}

// 地形识别函数 (示意)
TerrainType identifyTerrain() {
  // 读取IMU俯仰角
  slopeAngle = mpu6050.getAngleX();
  
  // 读取前方超声波传感器 (检测台阶/障碍物)
  int frontDist = readUltrasonic(A0);
  obstacleHeight = calculateHeight(frontDist); // 简化的高度计算

  // 决策逻辑
  if (abs(slopeAngle) > 15.0f) {
    return SLOPE;
  } else if (obstacleHeight > 5) {
    return STEP;
  } else {
    return FLAT;
  }
}

void executeGait(GaitProfile gait) {
  // 基于CPG或状态机，使用gait.stepLength, gait.stepHeight等参数
  // 生成各关节目标角度，并通过SimpleFOC下发
  // 这部分与案例一类似，但参数来源变为gait结构体
  // ...
}

要点解读

1. CPG是自适应性的“软骨架”
   CPG（中枢模式发生器）模拟了生物脊髓中的神经回路，它无需复杂的环境模型，仅靠几个耦合的振荡器就能产生稳定的节律运动。它的核心价值在于极限环特性，即当足端被外力（如石头）干扰时，振荡器相位会自动“拉回”到稳定状态，这使得机器人在复杂地形下具有天然的鲁棒性，比纯轨迹跟踪更抗扰动。CPG的参数（频率、幅值、相位差）构成了步态的“基因”，地形自适应就是对这些“基因”的在线调优。

2. 地形感知是自适应的“眼睛和皮肤”
   要实现自适应，机器人必须能“感知”地形。低成本方案主要依赖三类传感器：
   IMU（内耳）：感知机身姿态（俯仰/横滚），用于判断上下坡。
   足端触地力/电流环（皮肤）：通过BLDC FOC的电流环（Iq）来检测足端是否触地以及负载大小，这是一种巧妙的“虚拟力传感器”，无需额外硬件。
   前方测距（眼睛）：超声波或激光雷达预判前方地形起伏，用于提前调整步态。

3. 自适应策略：从“步态库切换”到“连续参数调节”
   地形自适应有两种主要实现方式：
   离散切换：预先设定“平地步态”、“斜坡步态”、“台阶步态”等参数集，根据识别结果直接切换。实现简单，但切换可能不够平滑。
   连续调节：像案例二那样，将坡度、起伏度作为连续变量，通过函数映射（如P控制）实时调整步高、步长。这种方式更自然、柔顺，但对传感器噪声敏感，需要良好滤波。

4. 算力是Arduino平台的硬约束，需合理分配
   实现完整的CPG+地形自适应+FOC对算力要求极高。建议[ citation:3]：
   “大脑+小脑”架构：用树莓派等负责视觉、路径规划等高层次任务；用ESP32/STM32专职运行CPG和FOC控制算法。
   优化技巧：逆运动学（IK）计算量大，应使用几何解析法而非数值迭代；三角函数可用查表法替代，并避免使用delay()，采用定时器中断保证实时性。

5. BLDC+FOC是实现柔顺力控的关键
   与普通舵机的位置控制不同，BLDC配合FOC（磁场定向控制）可以实现力矩（电流）闭环。这使得在支撑相，关节可以表现为“弹簧-阻尼”系统：当地面冲击导致腿长变化时，电机不是强行对抗，而是通过力矩控制“让位”来吸收能量，实现阻抗控制。这是实现柔顺行走、避免硬碰硬损坏和打滑的核心硬件基础。

4、室内外小型四足巡检机器人（CPG正弦步态+阶梯地形自适应）
适用场景：社区、厂房等室内外混合场景，需跨越矮台阶、门槛，核心需求是稳定节律性行走，遇台阶时自动抬腿，保持巡检连续性。

核心逻辑：CPG采用四神经元正弦振荡器，每腿对应独立振荡器，通过耦合保证四足相位差（0°、90°、180°、270°）；地形检测用红外传感器，当检测到前方障碍时，抬腿高度自适应增加，同时步长小幅优化，避障后恢复基础步态。

/* 小型四足巡检机器人：CPG正弦步态+阶梯自适应
   硬件：Arduino Mega + 4×BLDC关节电机（腿部） + 4×红外避障传感器
   核心：CPG振荡器生成节律，地形触发抬腿自适应，关节位置闭环控制
   参数：基础步频0.5Hz、基础抬腿高度30mm、自适应抬腿增量20mm、最大抬腿50mm
*/
#include <SimpleFOC.h>
#include <math.h>

// --- CPG核心参数 ---
const float STEP_FREQ = 0.5;   // 步频（Hz）
const float BASE_HEIGHT = 30;  // 基础抬腿高度（mm）
const float ADAPT_HEIGHT = 20; // 自适应增量（mm）
const float MAX_HEIGHT = 50;   // 最大抬腿高度（mm）
const float BASE_STEP = 100;   // 基础步长（mm）
const float ADAPT_STEP = 20;   // 自适应步长增量（mm）
const float COUPLING = 0.8;    // 振荡器耦合系数

// --- 四足CPG结构体（每腿对应一个振荡器） ---
struct CPGOscillator {
  float phase;      // 当前相位（0~2π）
  float amplitude;  // 抬腿幅度
  float baseline;   // 基础关节角度
  BLDCMotor motor;  // 驱动腿部关节的BLDC电机
};
CPGOscillator legs[4];

// --- 地形检测传感器 ---
#define SENSOR_LEG1 2
#define SENSOR_LEG2 3
#define SENSOR_LEG3 4
#define SENSOR_LEG4 5
const int OBSTACLE_THRESH = 300; // 障碍检测阈值（红外传感器输出值）

// --- 全局状态 ---
float currentTime = 0;
float stepCycleTime = 0;
bool isObstacle[4] = {false}; // 各腿前方是否检测到障碍

void setup() {
  Serial.begin(115200);
  initCPG();
  initMotors();
  initSensors();
  Serial.println("四足巡检机器人：CPG步态启动");
}

void loop() {
  updateCPG();
  checkTerrain();
  driveLegs();
  delay(10); // 控制周期10ms，保证步态连贯
}

// 初始化CPG振荡器（设置初始相位与耦合）
void initCPG() {
  for (int i = 0; i < 4; i++) {
    legs[i].phase = i * M_PI / 2; // 四足相位差90°，正弦步态
    legs[i].amplitude = 1.0;
    legs[i].baseline = 45; // 关节基础角度（度）
  }
}

// 初始化BLDC关节电机（位置闭环）
void initMotors() {
  int pins[4] = {6, 7, 8, 9}; // 4个关节电机的PWM引脚
  for (int i = 0; i < 4; i++) {
    legs[i].motor.controller = MotionControlType::angle;
    legs[i].motor.init();
    legs[i].motor.initFOC();
    legs[i].motor.target = 0;
    pinMode(pins[i], OUTPUT);
    legs[i].motor.driver.pwmPin = pins[i];
  }
}

// 初始化红外传感器
void initSensors() {
  pinMode(SENSOR_LEG1, INPUT);
  pinMode(SENSOR_LEG2, INPUT);
  pinMode(SENSOR_LEG3, INPUT);
  pinMode(SENSOR_LEG4, INPUT);
}

// CPG相位更新（核心振荡器方程）
void updateCPG() {
  currentTime += 0.01; // 时间增量10ms
  stepCycleTime += 0.01;
  if (stepCycleTime > 1.0 / STEP_FREQ) {
    stepCycleTime = 0;
  }
  // 简化的CPG相位更新：正弦振荡+耦合
  for (int i = 0; i < 4; i++) {
    // 自身振荡
    legs[i].phase += 2 * M_PI * STEP_FREQ * 0.01;
    if (legs[i].phase > 2 * M_PI) legs[i].phase -= 2 * M_PI;
    // 相邻耦合（保证相位差稳定）
    if (i < 3) legs[i].phase += COUPLING * (legs[i+1].phase - legs[i].phase) * 0.01;
    if (i > 0) legs[i].phase += COUPLING * (legs[i-1].phase - legs[i].phase) * 0.01;
  }
}

// 地形检测：判断是否需抬腿
void checkTerrain() {
  for (int i = 0; i < 4; i++) {
    int sensorVal = analogRead(pins[i]);
    isObstacle[i] = (sensorVal > OBSTACLE_THRESH);
  }
}

// 驱动腿部关节：自适应抬腿高度与步长
void driveLegs() {
  for (int i = 0; i < 4; i++) {
    // CPG相位映射为抬腿高度
    float sinVal = sin(legs[i].phase);
    // 自适应抬腿高度：有障碍时增加
    float targetHeight = BASE_HEIGHT + (isObstacle[i] ? ADAPT_HEIGHT : 0);
    targetHeight = constrain(targetHeight, 0, MAX_HEIGHT);
    // 目标抬腿高度映射为关节角度（简化：抬腿对应关节角度增大）
    float jointAngle = legs[i].baseline + targetHeight * sinVal * 0.5;
    jointAngle = constrain(jointAngle, 20, 70); // 关节角度限制

    // 自适应步长：有障碍时小幅增加
    float targetStep = BASE_STEP + (isObstacle[i] ? ADAPT_STEP : 0);
    // 步长映射为关节摆动幅度微调
    jointAngle += sinVal * targetStep * 0.01;

    legs[i].motor.target = jointAngle;
    legs[i].motor.loopFOC();
  }
}

5、户外六足勘探机器人（CPG环状网络步态+凹凸地形自适应）
适用场景：户外山地、泥泞等复杂凹凸地形，需稳定通过碎石、坑洼，核心需求是六足交替支撑，遇凹凸时单腿自适应抬腿高度，保证机身平稳。

核心逻辑：CPG采用6神经元环状振荡器，形成六足交替支撑的对角步态；每足配备地形检测传感器，当单腿探测到凹陷时，立即抬升对应腿的高度，同时通过CPG相位调整，保持机身姿态稳定，避免侧翻。

/* 六足勘探机器人：CPG环状步态+凹凸地形自适应
   硬件：ESP32 + 6×BLDC关节电机 + 6×地形检测传感器（模拟输入）
   核心：环状CPG生成六足步态，单腿自适应应对凹凸，机身姿态稳定控制
   参数：基础步频0.3Hz、基础抬腿40mm、最大抬腿80mm、步长自适应增量15mm
*/
#include <SimpleFOC.h>
#include <math.h>

// --- CPG环状网络参数 ---
const float STEP_FREQ = 0.3;
const float BASE_HEIGHT = 40;
const float MAX_HEIGHT = 80;
const float BASE_STEP = 120;
const float ADAPT_STEP = 15;
const float COUPLING = 1.0; // 环状耦合强度

// --- 六足CPG结构体 ---
struct HexCPG {
  float phase;
  float amplitude;
  float targetAngle;
  BLDCMotor motor;
  int terrainSensor; // 地形检测传感器引脚
  bool isPit;        // 是否探测到凹陷
};
HexCPG legs[6];

// --- 机身姿态参数 ---
float bodyPitch = 0, bodyRoll = 0;

void setup() {
  Serial.begin(115200);
  initHexCPG();
  initHexMotors();
  initTerrainSensors();
  Serial.println("六足勘探机器人：CPG环状步态启动");
}

void loop() {
  updateHexCPG();
  detectTerrain();
  adaptGait();
  driveHexLegs();
  delay(15); // 15ms控制周期，兼顾算力与步态连贯
}

// 初始化六足CPG环状网络（相位差60°）
void initHexCPG() {
  for (int i = 0; i < 6; i++) {
    legs[i].phase = i * M_PI / 3; // 环状相位差60°
    legs[i].amplitude = 1.0;
  }
}

// 初始化6个BLDC关节电机
void initHexMotors() {
  int pins[6] = {32, 33, 25, 26, 27, 28};
  for (int i = 0; i < 6; i++) {
    legs[i].motor.controller = MotionControlType::angle;
    legs[i].motor.init();
    legs[i].motor.initFOC();
    legs[i].motor.driver.pwmPin = pins[i];
  }
}

// 初始化地形传感器
void initTerrainSensors() {
  int sensorPins[6] = {34, 35, 36, 37, 38, 39};
  for (int i = 0; i < 6; i++) {
    legs[i].terrainSensor = sensorPins[i];
    pinMode(sensorPins[i], INPUT);
  }
}

// 更新环状CPG相位
void updateHexCPG() {
  float dt = 0.015; // 时间步长15ms
  for (int i = 0; i < 6; i++) {
    // 自身振荡
    legs[i].phase += 2 * M_PI * STEP_FREQ * dt;
    if (legs[i].phase > 2 * M_PI) legs[i].phase -= 2 * M_PI;
    // 环状耦合：与相邻神经元耦合
    int next = (i + 1) % 6;
    int prev = (i - 1 + 6) % 6;
    legs[i].phase += COUPLING * (legs[next].phase + legs[prev].phase - 2 * legs[i].phase) * dt;
  }
}

// 检测地形：判断是否为凹陷
void detectTerrain() {
  const int PIT_THRESH = 200;
  for (int i = 0; i < 6; i++) {
    int sensorVal = analogRead(legs[i].terrainSensor);
    // 传感器值低代表凹陷（假设传感器在坑洼处输出减小）
    legs[i].isPit = (sensorVal < PIT_THRESH);
  }
}

// 自适应步态调整：抬腿高度+步长+机身姿态补偿
void adaptGait() {
  for (int i = 0; i < 6; i++) {
    // 自适应抬腿高度
    float height = BASE_HEIGHT;
    if (legs[i].isPit) {
      height = MAX_HEIGHT; // 探测到凹陷，抬腿到最大高度
    } else if (sin(legs[i].phase) > 0.5) {
      height = BASE_HEIGHT * 1.2; // 抬腿中期，适度增加高度
    }
    // 步长自适应：凹陷处增加步长跨越障碍
    float step = BASE_STEP + (legs[i].isPit ? ADAPT_STEP : 0);
    legs[i].targetAngle = height * 0.8 + step * sin(legs[i].phase) * 0.2;
  }
  // 机身姿态补偿：通过多腿相位调整稳定机身
  bodyPitch = (legs[0].isPit + legs[3].isPit - legs[1].isPit - legs[4].isPit) * 2;
  bodyRoll = (legs[2].isPit + legs[5].isPit - legs[0].isPit - legs[3].isPit) * 2;
  // 相位微调补偿姿态
  for (int i = 0; i < 6; i++) {
    legs[i].phase += bodyPitch * 0.001;
    legs[i].phase += bodyRoll * 0.001;
  }
}

// 驱动六足关节
void driveHexLegs() {
  for (int i = 0; i < 6; i++) {
    legs[i].motor.target = legs[i].targetAngle;
    legs[i].motor.loopFOC();
  }
}

6、抢险救灾双足机器人（CPG双足步态+复杂障碍自适应）
适用场景：灾后废墟等极端环境，需跨越高障碍、绕过瓦砾，核心需求是双足拟人步态，遇高障碍时自适应增加抬腿高度与步长，遇狭窄空间时缩小步长，保持平衡。

核心逻辑：CPG采用双神经元振荡器控制双足，通过左右腿相位差180°实现交替行走；前向传感器检测障碍高度，侧向传感器检测狭窄空间，自适应调整抬腿高度、步长，同时通过CPG耦合调整重心，防止失衡。

/* 抢险救灾双足机器人：CPG双足步态+复杂障碍自适应
   硬件：Arduino Mega + 4×BLDC关节（髋/膝） + 前向距离传感器 + 侧向传感器
   核心：双CPG控制双足步态，障碍驱动抬腿/步长自适应，重心动态补偿
   参数：基础步频0.4Hz、基础抬腿50mm、最大抬腿120mm、步长调节范围80~150mm
*/
#include <SimpleFOC.h>
#include <math.h>

// --- CPG双足参数 ---
const float STEP_FREQ = 0.4;
const float BASE_HEIGHT = 50;
const float MAX_HEIGHT = 120;
const float MIN_STEP = 80;
const float MAX_STEP = 150;
const float BASE_STEP = 100;
const float PHASE_COUPLING = 0.9;

// --- 双足CPG结构体 ---
struct BipedCPG {
  float phase;
  float hipAngle;
  float kneeAngle;
  BLDCMotor hipMotor;
  BLDCMotor kneeMotor;
  int frontSensor;
  int sideSensor;
  float obstacleHeight;
  bool isNarrow;
};
BipedCPG leftLeg, rightLeg;

// --- 平衡控制参数 ---
float centerOfGravity = 0;

void setup() {
  Serial.begin(115200);
  initBipedCPG();
  initBipedMotors();
  initSensors();
  Serial.println("双足抢险机器人：CPG步态启动");
}

void loop() {
  updateBipedCPG();
  detectObstacles();
  adaptBipedGait();
  driveBipedJoints();
  balanceControl();
  delay(12); // 12ms控制周期，保证双足步态流畅
}

// 初始化双足CPG（相位差180°）
void initBipedCPG() {
  leftLeg.phase = 0;
  rightLeg.phase = M_PI; // 左右腿相位差180°，交替行走
}

// 初始化髋/膝关节电机
void initBipedMotors() {
  // 左腿
  leftLeg.hipMotor.controller = MotionControlType::angle;
  leftLeg.hipMotor.init();
  leftLeg.hipMotor.initFOC();
  leftLeg.hipMotor.driver.pwmPin = 6;

  leftLeg.kneeMotor.controller = MotionControlType::angle;
  leftLeg.kneeMotor.init();
  leftLeg.kneeMotor.initFOC();
  leftLeg.kneeMotor.driver.pwmPin = 7;

  // 右腿
  rightLeg.hipMotor.controller = MotionControlType::angle;
  rightLeg.hipMotor.init();
  rightLeg.hipMotor.initFOC();
  rightLeg.hipMotor.driver.pwmPin = 8;

  rightLeg.kneeMotor.controller = MotionControlType::angle;
  rightLeg.kneeMotor.init();
  rightLeg.kneeMotor.initFOC();
  rightLeg.kneeMotor.driver.pwmPin = 9;
}

// 初始化传感器
void initSensors() {
  leftLeg.frontSensor = A0;
  leftLeg.sideSensor = A1;
  rightLeg.frontSensor = A2;
  rightLeg.sideSensor = A3;
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
}

// 更新双足CPG相位（耦合保证相位稳定）
void updateBipedCPG() {
  float dt = 0.012;
  // 自身振荡
  leftLeg.phase += 2 * M_PI * STEP_FREQ * dt;
  rightLeg.phase += 2 * M_PI * STEP_FREQ * dt;
  if (leftLeg.phase > 2 * M_PI) leftLeg.phase -= 2 * M_PI;
  if (rightLeg.phase > 2 * M_PI) rightLeg.phase -= 2 * M_PI;
  // 相位耦合：保持180°相位差
  float phaseError = (rightLeg.phase - leftLeg.phase) - M_PI;
  leftLeg.phase += PHASE_COUPLING * phaseError * dt;
  rightLeg.phase -= PHASE_COUPLING * phaseError * dt;
}

// 检测障碍：高度与空间狭窄度
void detectObstacles() {
  const int HEIGHT_THRESH = 500;
  const int NARROW_THRESH = 600;

  // 前向传感器：模拟障碍高度
  leftLeg.obstacleHeight = analogRead(leftLeg.frontSensor);
  rightLeg.obstacleHeight = analogRead(rightLeg.frontSensor);

  // 侧向传感器：判断是否狭窄
  leftLeg.isNarrow = (analogRead(leftLeg.sideSensor) > NARROW_THRESH);
  rightLeg.isNarrow = (analogRead(rightLeg.sideSensor) > NARROW_THRESH);
}

// 双足步态自适应：抬腿高度、步长调整
void adaptBipedGait() {
  // 左腿自适应
  float leftHeight = BASE_HEIGHT;
  if (leftLeg.obstacleHeight > HEIGHT_THRESH) {
    // 障碍越高，抬腿越高（线性映射）
    leftHeight = map(leftLeg.obstacleHeight, HEIGHT_THRESH, 1023, BASE_HEIGHT, MAX_HEIGHT);
    leftHeight = constrain(leftHeight, BASE_HEIGHT, MAX_HEIGHT);
  }
  float leftStep = BASE_STEP;
  if (leftLeg.isNarrow) {
    leftStep = MIN_STEP; // 狭窄空间缩小步长
  } else if (leftLeg.obstacleHeight > HEIGHT_THRESH) {
    leftStep = map(leftLeg.obstacleHeight, HEIGHT_THRESH, 1023, BASE_STEP, MAX_STEP);
  }

  // 右腿自适应
  float rightHeight = BASE_HEIGHT;
  if (rightLeg.obstacleHeight > HEIGHT_THRESH) {
    rightHeight = map(rightLeg.obstacleHeight, HEIGHT_THRESH, 1023, BASE_HEIGHT, MAX_HEIGHT);
    rightHeight = constrain(rightHeight, BASE_HEIGHT, MAX_HEIGHT);
  }
  float rightStep = BASE_STEP;
  if (rightLeg.isNarrow) {
    rightStep = MIN_STEP;
  } else if (rightLeg.obstacleHeight > HEIGHT_THRESH) {
    rightStep = map(rightLeg.obstacleHeight, HEIGHT_THRESH, 1023, BASE_STEP, MAX_STEP);
  }

  // 相位映射为关节角度
  leftLeg.hipAngle = 30 + leftHeight * 0.3 * sin(leftLeg.phase);
  leftLeg.kneeAngle = 45 - leftHeight * 0.2 * sin(leftLeg.phase);
  leftLeg.hipAngle += leftStep * 0.02 * cos(leftLeg.phase);

  rightLeg.hipAngle = 30 + rightHeight * 0.3 * sin(rightLeg.phase);
  rightLeg.kneeAngle = 45 - rightHeight * 0.2 * sin(rightLeg.phase);
  rightLeg.hipAngle += rightStep * 0.02 * cos(rightLeg.phase);
}

// 驱动关节电机
void driveBipedJoints() {
  leftLeg.hipMotor.target = leftLeg.hipAngle;
  leftLeg.hipMotor.loopFOC();
  leftLeg.kneeMotor.target = leftLeg.kneeAngle;
  leftLeg.kneeMotor.loopFOC();

  rightLeg.hipMotor.target = rightLeg.hipAngle;
  rightLeg.hipMotor.loopFOC();
  rightLeg.kneeMotor.target = rightLeg.kneeAngle;
  rightLeg.kneeMotor.loopFOC();
}

// 重心平衡控制：通过CPG相位微调
void balanceControl() {
  // 重心偏移通过调整非支撑腿的相位实现
  float stanceLeg = (sin(leftLeg.phase) < 0) ? 0 : 1; // 0=左支撑，1=右支撑
  if (stanceLeg == 0) { // 左腿支撑，微调右腿相位稳定重心
    rightLeg.phase += 0.01 * (-centerOfGravity);
  } else {
    leftLeg.phase += 0.01 * (centerOfGravity);
  }
}

要点解读

1. CPG振荡器设计：节律性与鲁棒性的生物启发核心
   CPG是步态引擎的“大脑”，其振荡器设计直接决定步态稳定性与抗干扰能力，核心要点如下：
   振荡器结构适配足数：四足采用正弦振荡器+固定相位差，六足采用环状耦合振荡器，双足采用双神经元相位耦合，确保足间交替支撑，形成稳定节律，无需上位机发送位置指令；
   相位同步与自适应：通过神经元间耦合系数调节相位同步，案例中四足耦合系数0.8、双足0.9，既保证相位稳定，又允许小幅度自适应调整，避免步态紊乱；
   低依赖精确建模：CPG无需复杂逆运动学计算，仅需将振荡器相位映射为关节角度，大幅降低嵌入式平台算力压力，适配Arduino等资源受限设备。
2. 地形自适应策略：传感器驱动的动态参数映射
   自适应抬腿高度与步长的核心是传感器实时感知地形→动态映射为步态参数，关键在于精准感知与参数映射：
   多传感器协同感知：四足用红外检测障碍，六足用模拟传感器检测凹凸，双足用距离传感器检测高度+侧向传感器检测狭窄，覆盖不同地形特征，避免单一传感器漏检；
   参数自适应映射规则：抬腿高度与障碍高度线性映射，步长与障碍大小、空间狭窄度联动调整，双足案例中狭窄空间缩小步长，障碍处增大步长，兼顾跨越能力与通过性；
   参数边界约束：所有自适应参数均设置上下限，如最大抬腿高度、最小步长，避免参数突变导致关节卡滞或机器人失衡，保障步态平稳过渡。
3. BLDC闭环控制：步态执行的精准保障
   BLDC是步态执行的“肌肉”，需配合CPG输出实现精准关节运动，核心要点在于闭环控制与关节协同：
   闭环控制模式适配：案例均采用位置闭环（角度控制），将CPG输出的目标角度直接作为BLDC目标，SimpleFOC库实现FOC闭环控制，抑制转矩脉动，保证关节运动平稳；
   电机参数与控制周期匹配：Arduino控制周期设置为10~15ms，与BLDC电机响应速度匹配，避免控制周期过短导致电机过载，过长导致步态卡顿；
   多关节同步驱动：所有关节电机在同一控制周期内执行loopFOC()，保证多关节运动同步，避免因异步导致的步态偏差，确保抬腿、迈步动作连贯。
4. 步态稳定性控制：自适应与平衡的动态平衡
   复杂地形下，自适应参数调整易打破机器人平衡，需通过动态平衡控制弥补，核心要点如下：
   机身姿态实时补偿：六足案例通过多腿障碍检测计算机身俯仰、横滚角，微调CPG相位补偿；双足案例通过重心偏移调整非支撑腿相位，避免自适应调整导致失衡；
   支撑与摆动腿动态识别：通过CPG相位判断支撑腿与摆动腿，自适应参数仅作用于摆动腿，保证支撑腿稳定，避免因支撑腿姿态突变导致侧翻；
   相位滞后校正：当自适应参数调整导致相位轻微滞后时，通过耦合系数微调相位，确保足间交替支撑关系不变，维持步态连续性。
5. 嵌入式资源适配：工程落地的可行性保障
   Arduino平台资源有限，需在算法复杂度与硬件算力间权衡，核心要点在于轻量化与可靠性设计：
   算法轻量化设计：CPG采用简化的线性振荡器方程，避免复杂非线性耦合计算；传感器数据处理采用阈值判断，避免滤波算法带来的算力开销，适配Arduino算力；
   数据存储与调用优化：所有参数采用常量存储，避免动态数组占用RAM；关节角度采用单精度浮点数，平衡精度与存储开销；
   控制周期与中断优先级：控制周期固定为10~15ms，通过delay()实现简单定时，若采用中断需优先保障电机闭环控制的中断优先级，避免中断冲突导致电机失控，保障工程落地的可靠性。

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