该方案的核心特点是利用中枢模式发生器（CPG）网络生成具有节律性和相位耦合特性的双足步态，结合BLDC电机配合FOC算法实现的阻抗控制与反射机制，使机器人在触地瞬间能像生物肌肉一样吸收冲击并自适应未知地形；主要适用于地震废墟搜救、灾后现场勘探、科研验证等场景；实际部署需重点解决Arduino算力瓶颈、高频控制环实时性、多源传感器融合延迟及CPG参数整定等问题。

一、主要特点
CPG双足步态——从“刚性插值”到“仿生节律”
传统双足步态常采用预定义的轨迹插值（如正弦波、贝塞尔曲线），在复杂地形下缺乏灵活性。CPG（Central Pattern Generator）是一种受生物神经系统启发的控制方法，通过耦合的非线性振荡器网络生成节律性运动信号。
节律生成与相位耦合：CPG网络由多个相互耦合的振荡器组成，每个振荡器对应一个关节（如髋、膝、踝）。通过调整振荡器之间的相位差，可以生成行走、跑步、转弯等多种步态，且步态切换平滑自然。
反射机制（Reflex）：CPG网络可以接收来自传感器的反馈信号（如足底压力、IMU姿态），动态调整振荡器的频率和振幅。例如，当检测到前方有台阶时，自动抬高摆动腿的落足点；当检测到地面倾斜时，调整机身姿态保持平衡。
计算高效：相比复杂的逆向动力学解算，CPG网络通常由简单的微分方程组构成，计算量较小，适合在资源受限的嵌入式平台上实时运行。
BLDC+FOC柔顺力控——从“位置硬控”到“阻抗控制”
复杂障碍自适应的核心在于“触地”瞬间的处理。如果仅依靠位置控制，足端会以刚性方式撞击地面，不仅容易损坏机械结构，还会导致机身剧烈震动。
虚拟模型控制（VMC）：结合FOC（磁场定向控制）算法，BLDC电机可以实现精细的力矩闭环控制。系统可以在足端与期望位置之间建立一个“虚拟弹簧-阻尼”模型。当足端触地时，电机不再死板地追踪位置，而是像生物肌肉一样产生顺应外力的弹性位移，吸收冲击能量。
高动态响应：BLDC电机的电磁时间常数极小，配合FOC的高频电流环（通常>1kHz），能够毫秒级响应地形变化带来的负载突变，确保在松软地面或斜坡上不打滑、不陷落。
能量回馈：在下坡或制动时，BLDC可工作于发电模式，将动能回馈至电池，延长续航。
复杂障碍自适应——从“盲走”到“主动感知”
触地检测：通过监测电机电流（力矩）的突变或足底压力传感器，实时判断触地时刻。一旦检测到触地，立即切换控制模式（从摆动相的位置控制切换到支撑相的力/阻抗控制）。
落足点修正：结合IMU（惯性测量单元）和机身姿态，实时调整CPG网络的输出参数。例如，当检测到前方有台阶时，自动抬高落足点；当检测到地面倾斜时，调整落足角度，确保足端与地面平行接触。
地形预测：通过记录历史落足点信息，利用圆弧模型等算法预测地形变化趋势，提前规划下一步的足端轨迹，提升运动连续性。
分层控制架构
规划层（CPG网络）：负责生成具有节律性的关节角度或力矩指令。
控制层（FOC+阻抗控制）：负责将CPG指令转化为电机PWM信号，并处理触地冲击。
感知层（IMU+电流反馈）：负责提供机身姿态和触地状态反馈。

二、典型应用场景
地震废墟搜救
在地震、爆炸等灾难后的废墟中，轮式和履带式机器人移动受限，而双足机器人有望穿越这种高度非结构化的环境。
CPG优势：通过调整CPG网络的相位和振幅，机器人可以实现高抬腿跨越废墟、深坑，甚至在单脚支撑于不稳定支点上时也能快速调整姿态。
柔顺触地优势：在坍塌物上行走时，阻抗控制能有效缓冲意外碰撞，保护机身结构。
灾后现场勘探
在火灾、化学泄漏等灾难现场，机器人需深入核心区进行环境探测。
地形自适应：机器人能根据地形起伏自动调整步高和落足点，保持机身水平，确保搭载的传感器平台稳定。
抗冲击能力：在碎石或松软地面上，阻抗控制能像“弹簧”一样吸收不平地面的冲击，防止足端打滑或陷入。
科研与教育验证
作为高校机器人学、嵌入式控制课程的实验平台，用于验证CPG步态生成、FOC驱动、阻抗控制等前沿技术。

三、需要注意的事项
Arduino算力瓶颈与架构分工
CPG网络计算、逆向运动学解算、FOC算法同时运行对算力要求极高。
平台选型：经典8位Arduino（如Uno）几乎无法胜任。必须选用ESP32-S3（双核240MHz）、STM32H7或Arduino Portenta H7等高性能平台。
主从架构：建议采用“上位机+下位机”架构。上位机（如树莓派、Jetson或高性能Arduino）负责CPG网络生成、逆向运动学解算和步态规划；下位机（如专用FOC驱动板）负责高频电流环控制和电机换相。
控制频率与实时性
复杂障碍自适应要求极高的实时性，尤其是触地瞬间的力矩响应。
高频控制环：FOC的电流环频率建议≥1kHz，速度环和位置环≥500Hz。CPG网络的更新频率应与控制环匹配（如100Hz~500Hz），避免轨迹跟踪滞后。
通信延迟：上位机与下位机之间的通信（如CAN、SPI）延迟必须控制在毫秒级，否则会导致力矩指令滞后，引发机身抖动。
触地检测的准确性与延迟
触地检测是切换控制模式的关键，误判会导致摔倒。
多源融合：仅靠电流突变检测触地容易受干扰（如电机堵转）。建议融合足底压力传感器、IMU加速度突变等多源信息，提高检测鲁棒性。
延迟补偿：传感器采样、滤波、通信都会引入延迟。需在算法中加入延迟补偿机制，预测触地时刻，提前切换控制模式。
CPG参数整定
CPG网络的参数（如振荡器频率、耦合强度）直接决定步态性能。
参数选择：需根据步长、步高、运动速度动态调整。例如，高速运动时需增大抬腿高度，避免足端刮地。
连续性约束：在多步态切换时（如从行走切换到跑步），需确保前后两段CPG输出在连接点处的速度和加速度连续，避免冲击。
电机力矩响应与一致性
阻抗控制的效果高度依赖电机的力矩响应性能。
电机选型：选用低齿槽效应、高扭矩密度的BLDC电机，确保低速下的力矩平稳性。
参数一致性：同一机器人上的所有电机参数（极对数、内阻、电感）必须一致，否则会导致各腿力矩输出不均，机身倾斜。
电磁兼容（EMC）与电源管理
多电机高频PWM驱动会产生严重的电磁干扰。
电源隔离：动力电源与逻辑电源必须物理隔离，使用独立DC-DC模块。
滤波设计：电源入口并联大容量电解电容和高频陶瓷电容，吸收电压尖峰。
布线规范：强电与弱电严格分开，编码器信号线使用屏蔽线。

1、基础CPG双足步态与感知反馈调节
此案例演示了如何用一个简化的CPG模型生成双足行走的节律信号，并引入足端力或IMU的感知反馈，实现抬腿高度的自适应调节。

#include <SimpleFOC.h>

// 假设已定义双足6个关节的BLDC电机及驱动器 (略)

// CPG参数：使用正弦振荡器模拟髋、膝关节的运动规律
float phase = 0.0;           // 步态相位 (0~2PI)
float frequency = 2.0;       // 步频 (rad/s)
float amplitudeHip = 30.0;   // 髋关节振幅 (度)
float amplitudeKnee = 25.0;  // 膝关节振幅 (度)
float phaseDiff = PI / 2;    // 髋膝相位差
float footForce = 0.0;       // 足底力传感器值
float baseLiftHeight = 20.0; // 基础抬腿高度 (度)

void setup() {
  Serial.begin(115200);
  // 初始化电机、传感器...
  pinMode(A0, INPUT); // 假设足底力传感器在A0
}

void loop() {
  // 1. 更新CPG相位
  phase += frequency * 0.01; // 假设循环周期10ms
  if (phase > 2 * PI) phase -= 2 * PI;

  // 2. 读取足底力反馈，实现抬腿高度自适应
  footForce = analogRead(A0) / 1023.0 * 5.0; // 模拟力值读取
  // 若左足在摆动相提前触地 (力>阈值)，下次抬腿略增高
  if (footForce > 2.0 && phase < PI) {
    baseLiftHeight = min(30.0, baseLiftHeight + 0.5); 
  } else if (phase > PI) {
    // 右足摆动时同理
  }

  // 3. 生成左右腿的关节角度
  // 左腿: 髋关节 sin(phase), 膝关节 sin(phase + phaseDiff)
  float leftHipAngle = 90 + amplitudeHip * sin(phase);
  float leftKneeAngle = 90 + amplitudeKnee * sin(phase + phaseDiff) + baseLiftHeight * 0.3;
  // 右腿与左腿相位差180度
  float rightHipAngle = 90 + amplitudeHip * sin(phase + PI);
  float rightKneeAngle = 90 + amplitudeKnee * sin(phase + PI + phaseDiff) + baseLiftHeight * 0.3;

  // 4. 执行关节角度 (通过逆运动学或直接位置控制)
  // 假设有 setJointAngle(leg, joint, angle) 函数
  setJointAngle(LEFT_LEG, HIP, leftHipAngle);
  setJointAngle(LEFT_LEG, KNEE, leftKneeAngle);
  setJointAngle(RIGHT_LEG, HIP, rightHipAngle);
  setJointAngle(RIGHT_LEG, KNEE, rightKneeAngle);

  delay(10);
}

2、基于有限状态机的障碍自适应步态切换
当超声波或视觉传感器检测到障碍物时，双足机器人需要进行步态切换，如从“行走”模式切换到“跨越”模式。此案例展示了如何使用有限状态机（FSM）来实现这一逻辑。

#include <NewPing.h>
// 假设电机驱动代码同案例一 (略)

#define TRIG_PIN 7
#define ECHO_PIN 8
#define MAX_DIST 100
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DIST);

enum GaitState { NORMAL_WALK, CLIMB_STEP, OBSTACLE_AVOID };
GaitState currentState = NORMAL_WALK;
int obstacleDist = 0;

void setup() {
  Serial.begin(115200);
  // 初始化电机...
}

void loop() {
  // 1. 传感器检测
  obstacleDist = sonar.ping_cm();

  // 2. 状态切换逻辑
  if (obstacleDist > 0 && obstacleDist < 20 && currentState == NORMAL_WALK) {
    currentState = CLIMB_STEP; // 检测到近距障碍，进入爬坡/跨越步态
    Serial.println("State: CLIMB_STEP");
  } else if (obstacleDist > 30 && currentState == CLIMB_STEP) {
    currentState = NORMAL_WALK; // 已跨过障碍，恢复正常行走
    Serial.println("State: NORMAL_WALK");
  }

  // 3. 根据状态执行不同步态
  switch (currentState) {
    case NORMAL_WALK:
      performNormalGait(); // 调用案例一中的常规CPG步态
      break;
    case CLIMB_STEP:
      performClimbGait(); // 高抬腿、步幅缩短的“跨越”步态
      break;
    // case OBSTACLE_AVOID: ...
  }
  delay(50);
}

// 示例：爬坡/跨越步态（高抬腿、小步幅）
void performClimbGait() {
  static float phase = 0;
  phase += 0.1;
  // 提高膝关节振幅（抬腿更高），减小髋关节振幅（步幅缩短）
  float highLiftKnee = 45.0; 
  float shortStepHip = 15.0; 
  // ... 类似案例一生成关节角度，但使用上述修改后的参数 ...
}

3、融合IMU的姿态稳定与地形补偿
在复杂地形上，通过MPU6050等IMU实时检测机身姿态，并动态补偿关节角度以维持ZMP（零力矩点）稳定，是双足机器人平衡的关键。

#include <MPU6050.h>
// 假设电机驱动代码同案例一 (略)

MPU6050 mpu;
float pitch, roll;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  // 初始化电机...
}

void loop() {
  // 1. 读取IMU数据
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  // 简单的互补滤波计算俯仰角 (pitch) 和横滚角 (roll)
  pitch = atan2(ay, az) * 180 / PI; 
  roll = atan2(-ax, az) * 180 / PI;

  // 2. 根据机身倾斜，调整支撑腿的踝关节或髋关节角度进行补偿
  // 例如：机身向前倾斜(pitch>0)，支撑腿踝关节略微背屈以稳定重心
  float ankleCompensation = -pitch * 0.5; // 比例系数
  // 假设有函数 setAnkleAngle(leg, angle) 作用到踝关节BLDC电机

  // 3. 执行步态 (案例一或案例二的步态生成逻辑)
  // ... 在生成各关节目标角度后，叠加补偿值 ...
  // float leftAnkleTarget = baseAnkleAngle + ankleCompensation;
  // setJointAngle(LEFT_LEG, ANKLE, leftAnkleTarget);

  delay(10);
}

要点解读
CPG降低步态规划维度：CPG的本质是参数化的节律发生器。你只需调节频率（控制步速）、振幅（控制步长/步高）和相位差（控制腿间协调）等少数参数，即可生成行走、跑步、跨越等多样化的步态模式，无需为每个动作硬编码复杂轨迹。这在应对复杂环境时非常高效。

地形自适应依赖多传感器融合：“自适应”的核心是闭合感知-运动环路。系统必须融合多种传感器信息：足底力/触地传感器用于判断支撑与摆动相的实际切换，修正CPG相位；IMU姿态数据用于平衡控制，通过调整关节角度维持ZMP在支撑多边形内；超声波/视觉传感器用于前向障碍感知，触发避障或跨越步态。

步态切换的平滑性：在抢险救灾场景中，机器人频繁遭遇碎石、台阶等，需要在“行走”、“跨越”、“爬坡”等模式间切换。生硬的切换会导致摔倒。一个关键设计是让CPG网络的反馈参数（如偏移量offset或振幅）能连续、平滑地过渡，而非“停车→切换→重启”。这要求控制状态机设计良好，并配合插值算法。

BLDC与FOC是动态平衡的执行基石：双足机器人维持动态平衡，需要在毫秒级时间内精确输出关节力矩来调整姿态。BLDC电机配合FOC（磁场定向控制）能提供千赫兹级带宽的电流环响应，实现对关节力矩的精确伺服控制，这是舵机等传统执行器难以做到的。尤其在支撑相切换为力矩控制，摆动相切换为位置控制的混合模式下，BLDC的优势更为突出。

警惕算力瓶颈：完整的CPG网络、逆运动学解算和多传感器融合对微控制器算力要求极高。在Arduino平台上，8位MCU（如Uno）极易成为瓶颈。工程上建议采用分布式架构：使用ESP32、树莓派或Jetson Nano作为“大脑”，负责复杂的步态规划与决策，而将Arduino作为“小脑”，专注于执行底层的关节伺服控制。

4、废墟狭窄通道行走——CPG基础双足步态+通道约束自适应
适用场景：地震废墟的狭窄通道、坍塌缝隙等宽度受限场景，双足机器人需保持CPG自发步态的同时，自适应通道宽度，避免关节碰撞障碍物，维持稳定行走。

核心逻辑：采用2自由度CPG模型（髋关节+膝关节，各1个振荡器），通过相位差控制双足交替运动；结合超声波传感器实时检测通道宽度，动态调整CPG步态的关节摆幅，同时嵌入防碰撞约束，当通道宽度不足时缩小步幅，保证关节不碰撞通道壁。

/* 废墟狭窄通道行走：CPG双足步态+通道约束自适应
   核心：CPG振荡器自激生成步态，超声波检测通道宽度，动态缩放步幅
   硬件：Arduino Mega + 4×BLDC + 2×超声波 + 4×编码器
*/
#include <SimpleFOC.h>

// --- CPG模型参数：2自由度/足，双足共4个振荡器 ---
#define CPG_OSC_COUNT 4  // 振荡器总数：左髋(0)、左膝(1)、右髋(2)、右膝(3)
struct Oscillator {
  float phase;      // 当前相位（0~2π，步态循环标识）
  float freq;       // 振荡频率（Hz，控制步态速度）
  float amplitude;  // 输出摆幅（rad，控制步幅大小）
  float phaseShift; // 耦合相位差（双足交替的核心：髋关节相位差π）
};
Oscillator oscillators[CPG_OSC_COUNT];

// --- 通道约束与步态参数 ---
#define MIN_CHANNEL_WIDTH 300  // 最小通道宽度（mm，触发步幅缩小）
#define MAX_AMPLITUDE_HIP 0.5  // 髋关节最大摆幅（rad，约28.6°）
#define MAX_AMPLITUDE_KNEE 0.8  // 膝关节最大摆幅（rad，约45.8°）
float leftWallDist, rightWallDist;  // 左右通道壁距离（mm）
float targetChannelWidth;           // 目标通道宽度（左距+右距）
float adaptiveAmplitudeScale = 1.0; // 步幅缩放系数

// --- BLDC电机与关节配置 ---
BLDCMotor jointMotors[4]; // 4个关节电机：0=左髋，1=左膝，2=右髋，3=右膝
float jointCurrentAngles[4] = {0}; // 关节当前角度

// --- 超声波引脚定义 ---
#define LEFT_ULTRA_TRIG 2
#define LEFT_ULTRA_ECHO 3
#define RIGHT_ULTRA_TRIG 4
#define RIGHT_ULTRA_ECHO 5

void setup() {
  Serial.begin(115200);
  initCPG();
  initMotors();
  initSensors();
  Serial.println("废墟通道CPG步态启动");
}

void loop() {
  // 1. 感知通道宽度：超声波获取左右壁距离
  detectChannelWidth();
  // 2. 自适应调整CPG：根据通道宽度计算步幅缩放系数
  adjustCPGAmplitude();
  // 3. CPG自激更新：推进相位，生成步态目标
  updateCPG();
  // 4. 关节闭环控制：CPG目标→电机执行
  controlJoints();
  // 5. 步态周期监控：确保相位同步
  monitorGaitCycle();
}

// 初始化CPG振荡器：设定初始频率、相位与耦合关系
void initCPG() {
  // 基础振荡参数：步态周期约1.5Hz（行走速度适配废墟场景）
  float baseFreq = 1.5;
  // 左髋振荡器：基础相位0
  oscillators[0].freq = baseFreq;
  oscillators[0].phase = 0;
  oscillators[0].amplitude = MAX_AMPLITUDE_HIP;
  // 左膝振荡器：与左髋耦合，相位滞后0.5π（保证抬腿时膝盖弯曲）
  oscillators[1].freq = baseFreq;
  oscillators[1].phase = 0;
  oscillators[1].amplitude = MAX_AMPLITUDE_KNEE;
  oscillators[1].phaseShift = 0.5 * PI;
  // 右髋振荡器：与左髋相位差π（双足交替核心）
  oscillators[2].freq = baseFreq;
  oscillators[2].phase = PI;
  oscillators[2].amplitude = MAX_AMPLITUDE_HIP;
  // 右膝振荡器：与右髋相位差0.5π
  oscillators[3].freq = baseFreq;
  oscillators[3].phase = PI;
  oscillators[3].amplitude = MAX_AMPLITUDE_KNEE;
  oscillators[3].phaseShift = 0.5 * PI;
}

// 初始化BLDC关节电机（编码器闭环控制）
void initMotors() {
  // 电机引脚与编码器定义（简化：仅示例核心配置，需根据实际硬件调整引脚）
  int motorPins[4][3] = {{9,10,11}, {12,13,14}, {15,16,17}, {18,19,20}}; // 每电机3个PWM引脚
  int encoderPins[4][2] = {{2,3}, {4,5}, {6,7}, {8,9}}; // 每编码器2个引脚

  for (int i = 0; i < 4; i++) {
    jointMotors[i].linkDriver(new BLDCDriver3PWM(motorPins[i][0], motorPins[i][1], motorPins[i][2]));
    jointMotors[i].linkSensor(new Encoder(encoderPins[i][0], encoderPins[i][1]));
    jointMotors[i].controller = MotionControlType::angle;
    jointMotors[i].init();
    jointMotors[i].initFOC();
    jointMotors[i].target = 0;
  }
}

// 初始化超声波传感器
void initSensors() {
  pinMode(LEFT_ULTRA_TRIG, OUTPUT);
  pinMode(LEFT_ULTRA_ECHO, INPUT);
  pinMode(RIGHT_ULTRA_TRIG, OUTPUT);
  pinMode(RIGHT_ULTRA_ECHO, INPUT);
}

// 检测通道宽度：通过超声波计算左右壁距离
void detectChannelWidth() {
  leftWallDist = getUltrasonicDistance(LEFT_ULTRA_TRIG, LEFT_ULTRA_ECHO);
  rightWallDist = getUltrasonicDistance(RIGHT_ULTRA_TRIG, RIGHT_ULTRA_ECHO);
  // 通道宽度=左距+机身宽度+右距，假设机身宽度200mm
  targetChannelWidth = leftWallDist + rightWallDist + 200;
  Serial.printf("通道宽度：%dmm（左距%d，右距%d）\n", (int)targetChannelWidth, (int)leftWallDist, (int)rightWallDist);
}

// 超声波测距函数
float getUltrasonicDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  float distance = pulseIn(echoPin, HIGH) / 58.0; // mm
  return constrain(distance, 50, 800); // 过滤无效距离
}

// 根据通道宽度自适应调整CPG步幅
void adjustCPGAmplitude() {
  // 通道越窄，步幅越小：目标宽度<最小通道宽度时，缩放系数减小
  if (targetChannelWidth < MIN_CHANNEL_WIDTH * 2) { // 机身宽度+最小活动空间
    adaptiveAmplitudeScale = map(targetChannelWidth, MIN_CHANNEL_WIDTH * 2, 800, 0.3, 1.0);
    adaptiveAmplitudeScale = constrain(adaptiveAmplitudeScale, 0.3, 1.0);
  } else {
    adaptiveAmplitudeScale = 1.0;
  }

  // 更新各振荡器摆幅：髋关节与膝关节同比例缩放
  oscillators[0].amplitude = MAX_AMPLITUDE_HIP * adaptiveAmplitudeScale;
  oscillators[1].amplitude = MAX_AMPLITUDE_KNEE * adaptiveAmplitudeScale;
  oscillators[2].amplitude = MAX_AMPLITUDE_HIP * adaptiveAmplitudeScale;
  oscillators[3].amplitude = MAX_AMPLITUDE_KNEE * adaptiveAmplitudeScale;

  Serial.printf("步幅缩放系数：%.2f\n", adaptiveAmplitudeScale);
}

// 更新CPG相位：自激振荡推进，同时保持耦合关系
void updateCPG() {
  float dt = 0.01; // 时间步长：10ms，与控制周期同步
  for (int i = 0; i < CPG_OSC_COUNT; i++) {
    // 相位自推进：dθ/dt = 2πf（频率决定步态速度）
    oscillators[i].phase += 2 * PI * oscillators[i].freq * dt;
    // 相位归一化：保持在0~2π
    if (oscillators[i].phase >= 2 * PI) oscillators[i].phase -= 2 * PI;
    if (oscillators[i].phase < 0) oscillators[i].phase += 2 * PI;
  }
}

// 关节控制：将CPG相位转换为关节目标角度，驱动电机
void controlJoints() {
  for (int i = 0; i < 4; i++) {
    // CPG目标角度：正弦函数映射相位→角度，髋关节与膝关节角度映射不同
    float targetAngle;
    if (i == 0 || i == 2) { // 髋关节（0=左髋，2=右髋）
      // 髋关节角度：正弦波，幅度=振荡器摆幅
      targetAngle = oscillators[i].amplitude * sin(oscillators[i].phase);
      // 左右髋互补，右髋相位差π，自动保证交替
    } else { // 膝关节（1=左膝，3=右膝）
      // 膝关节角度：仅在抬腿阶段弯曲（相位在0~π时为正角度，π~2π时为0）
      if (oscillators[i].phase < PI) {
        targetAngle = oscillators[i].amplitude * sin(oscillators[i].phase - oscillators[i].phaseShift);
        targetAngle = max(0, targetAngle); // 膝关节角度≥0（不反弯）
      } else {
        targetAngle = 0; // 支撑阶段膝关节伸直
      }
    }
    // 关节角度约束：避免超限
    if (i == 0 || i == 2) targetAngle = constrain(targetAngle, -MAX_AMPLITUDE_HIP, MAX_AMPLITUDE_HIP);
    else targetAngle = constrain(targetAngle, 0, MAX_AMPLITUDE_KNEE);

    // 闭环控制：电机目标角度=当前角度+目标角度偏移
    jointMotors[i].target = jointCurrentAngles[i] + targetAngle;
    jointMotors[i].loopFOC();
    jointCurrentAngles[i] = jointMotors[i].shaftAngle; // 更新当前角度
  }
}

// 步态周期监控：确保双足相位差稳定（误差<0.1π）
void monitorGaitCycle() {
  float phaseDiff = fabs(oscillators[0].phase - oscillators[2].phase);
  if (phaseDiff < PI - 0.1 || phaseDiff > PI + 0.1) {
    // 相位偏差超限，强制同步：右髋相位=左髋相位+π
    oscillators[2].phase = oscillators[0].phase + PI;
    oscillators[3].phase = oscillators[2].phase;
    Serial.println("相位同步校正");
  }
}

5、石块障碍跨越——CPG越障步态自适应+障碍识别
适用场景：抢险现场的散落石块、小型障碍（高度50-150mm）跨越，双足机器人需在CPG基础步态上，自适应调整抬腿高度与关节角度，实现障碍跨越，同时保证落地稳定性。

核心逻辑：在CPG双足步态基础上，增加“障碍识别-步态切换”机制：通过激光雷达识别障碍高度与位置，当检测到障碍时，实时调整CPG振荡器的摆幅与相位，提高抬腿高度，同时调整膝关节弯曲时机，保证足端越过障碍，落地后自动恢复基础步态。

/* 石块障碍跨越：CPG越障步态+障碍识别自适应
   核心：激光雷达识别障碍，实时调整CPG摆幅（抬腿高度）与相位，跨越后恢复
   硬件：Arduino Due + 4×BLDC + 激光雷达 + 超声波 + 3×IMU + 4×编码器 + 压力传感器
*/
#include <SimpleFOC.h>
#include <NewPing.h> // 超声波库（辅助测障）

// --- CPG参数与越障状态 ---
#define CPG_OSC_COUNT 4
struct Oscillator {
  float phase;
  float freq;
  float amplitude;
  float phaseShift;
};
Oscillator oscillators[CPG_OSC_COUNT];

// 障碍参数
float obstacleHeight = 0;   // 障碍高度（mm）
float obstacleDistance = 0; // 障碍距离（mm）
bool obstacleDetected = false; // 是否检测到障碍
bool inObstacleMode = false;   // 是否处于越障模式
float baseAmplitude = 0.5;     // 基础髋关节摆幅
float obstacleLiftScale = 1.0; // 越障抬腿缩放系数

// --- 传感器与状态 ---
#define LIDAR_PIN 2 // 激光雷达数据引脚（假设串口输入，简化处理）
#define ULTRA_TRIG 3
#define ULTRA_ECHO 4
#define PRESSURE_PIN A0 // 足端压力传感器
bool footTouched = false; // 足端是否触地

// --- BLDC电机与关节 ---
BLDCMotor jointMotors[4];
float jointCurrentAngles[4] = {0};

void setup() {
  Serial.begin(115200);
  initCPG();
  initMotors();
  initSensors();
  Serial.println("石块越障CPG步态启动");
}

void loop() {
  // 1. 障碍识别：激光雷达+超声波融合检测
  detectObstacle();
  // 2. CPG越障自适应：根据障碍调整步态参数
  adaptCPGForObstacle();
  // 3. CPG更新与关节控制
  updateCPG();
  controlJoints();
  // 4. 落地检测：压力传感器判断落地，切换回基础步态
  checkLanding();
}

// 初始化CPG：基础参数与越障预留的初始摆幅
void initCPG() {
  float baseFreq = 1.2; // 越障时略慢，保证稳定
  oscillators[0].freq = baseFreq; oscillators[0].phase = 0; oscillators[0].amplitude = baseAmplitude;
  oscillators[1].freq = baseFreq; oscillators[1].phase = 0; oscillators[1].amplitude = 0.8; oscillators[1].phaseShift = 0.5*PI;
  oscillators[2].freq = baseFreq; oscillators[2].phase = PI; oscillators[2].amplitude = baseAmplitude;
  oscillators[3].freq = baseFreq; oscillators[3].phase = PI; oscillators[3].amplitude = 0.8; oscillators[3].phaseShift = 0.5*PI;
}

// 初始化电机（同案例1，引脚需根据Due调整）
void initMotors() {
  int motorPins[4][3] = {{3,4,5}, {6,7,8}, {9,10,11}, {12,13,14}};
  int encoderPins[4][2] = {{2,3}, {4,5}, {6,7}, {8,9}};
  for (int i = 0; i < 4; i++) {
    jointMotors[i].linkDriver(new BLDCDriver3PWM(motorPins[i][0], motorPins[i][1], motorPins[i][2]));
    jointMotors[i].linkSensor(new Encoder(encoderPins[i][0], encoderPins[i][1]));
    jointMotors[i].controller = MotionControlType::angle;
    jointMotors[i].init();
    jointMotors[i].initFOC();
  }
}

// 初始化传感器
void initSensors() {
  pinMode(ULTRA_TRIG, OUTPUT);
  pinMode(ULTRA_ECHO, INPUT);
  pinMode(PRESSURE_PIN, INPUT);
  // 激光雷达：假设串口接收数据，初始配置（简化）
  Serial1.begin(9600); // 激光雷达串口
}

// 障碍检测：激光雷达（障碍距离/高度）+超声波（辅助验证）
void detectObstacle() {
  // 简化：从串口读取激光雷达数据（格式：高度,距离，如“100,500”）
  if (Serial1.available() > 0) {
    String data = Serial1.readStringUntil('\n');
    int comma = data.indexOf(',');
    if (comma != -1) {
      obstacleHeight = data.substring(0, comma).toFloat();
      obstacleDistance = data.substring(comma+1).toFloat();
      // 验证障碍：高度≥50mm且距离≤1000mm，视为有效障碍
      if (obstacleHeight >= 50 && obstacleDistance <= 1000) {
        obstacleDetected = true;
        inObstacleMode = true; // 进入越障模式
        Serial.printf("检测到障碍：高%dmm，距%dmm\n", (int)obstacleHeight, (int)obstacleDistance);
      }
    }
  }
  // 超声波辅助测量障碍高度（弥补雷达盲区）
  if (!obstacleDetected) {
    float ultraDist = getUltrasonicDistance(ULTRA_TRIG, ULTRA_ECHO);
    if (ultraDist < 300 && ultraDist > 50) {
      obstacleHeight = ultraDist;
      obstacleDistance = 200; // 假设障碍在前方200mm
      obstacleDetected = true;
      inObstacleMode = true;
    }
  }
}

// 根据障碍调整CPG参数：核心是提高抬腿高度（增大摆幅）
void adaptCPGForObstacle() {
  if (inObstacleMode) {
    // 抬腿高度与障碍高度正相关：障碍越高，抬腿幅度越大
    // 公式：抬腿高度=障碍高度+安全余量（30mm），摆幅缩放系数=（目标高度/基础高度）
    float targetLiftHeight = obstacleHeight + 30; // 安全余量
    obstacleLiftScale = targetLiftHeight / 80; // 基础抬腿高度80mm
    obstacleLiftScale = constrain(obstacleLiftScale, 1.0, 2.5); // 缩放系数上限2.5（防止关节超限）

    // 更新髋关节摆幅（决定抬腿高度的核心参数）
    oscillators[0].amplitude = baseAmplitude * obstacleLiftScale;
    oscillators[2].amplitude = baseAmplitude * obstacleLiftScale;
    // 膝关节摆幅同步增大，保证抬腿时膝盖充分弯曲
    oscillators[1].amplitude = 0.8 * obstacleLiftScale;
    oscillators[3].amplitude = 0.8 * obstacleLiftScale;
    // 调整相位：让抬腿动作提前，预留越障时间
    float phaseAdvance = 0.2 * PI; // 相位提前0.2π
    oscillators[0].phase += phaseAdvance;
    oscillators[2].phase += phaseAdvance;

    Serial.printf("越障模式：摆幅缩放%.2f，抬腿高度%dmm\n", obstacleLiftScale, (int)targetLiftHeight);
  } else {
    // 恢复基础步态
    oscillators[0].amplitude = baseAmplitude;
    oscillators[2].amplitude = baseAmplitude;
    oscillators[1].amplitude = 0.8;
    oscillators[3].amplitude = 0.8;
  }
}

// 更新CPG相位（同案例1，新增越障时频率微调）
void updateCPG() {
  float dt = 0.01;
  // 越障时略降频率，保证越障动作充分完成
  float currentFreq = inObstacleMode ? 1.0 : 1.2;
  for (int i = 0; i < CPG_OSC_COUNT; i++) {
    oscillators[i].phase += 2 * PI * currentFreq * dt;
    if (oscillators[i].phase >= 2 * PI) oscillators[i].phase -= 2 * PI;
    if (oscillators[i].phase < 0) oscillators[i].phase += 2 * PI;
  }
}

// 关节控制：越障时增加膝关节弯曲阈值，保证充分抬腿
void controlJoints() {
  for (int i = 0; i < 4; i++) {
    float targetAngle;
    if (i == 0 || i == 2) { // 髋关节
      targetAngle = oscillators[i].amplitude * sin(oscillators[i].phase);
      targetAngle = constrain(targetAngle, -oscillators[i].amplitude, oscillators[i].amplitude);
    } else { // 膝关节
      // 越障模式下，膝关节弯曲时机提前（相位<PI时均有一定角度）
      if (inObstacleMode) {
        targetAngle = oscillators[i].amplitude * sin(oscillators[i].phase - oscillators[i].phaseShift);
        targetAngle = constrain(targetAngle, 0, oscillators[i].amplitude);
      } else {
        // 基础模式：仅抬腿时弯曲
        if (oscillators[i].phase < PI) {
          targetAngle = oscillators[i].amplitude * sin(oscillators[i].phase - oscillators[i].phaseShift);
          targetAngle = max(0, targetAngle);
        } else {
          targetAngle = 0;
        }
      }
    }
    jointMotors[i].target = jointCurrentAngles[i] + targetAngle;
    jointMotors[i].loopFOC();
    jointCurrentAngles[i] = jointMotors[i].shaftAngle;
  }
}

// 落地检测：压力传感器判断足端触地，触发越障模式退出
void checkLanding() {
  // 读取足端压力（简化：假设A0为左足压力，数值>500为触地）
  int pressureValue = analogRead(PRESSURE_PIN);
  footTouched = (pressureValue > 500);

  if (inObstacleMode && footTouched) {
    // 检测到落地，延迟1个步态周期后退出越障模式
    delay(1500); // 1.5秒，约1个步态周期
    obstacleDetected = false;
    inObstacleMode = false;
    obstacleLiftScale = 1.0;
    Serial.println("障碍跨越完成，恢复基础步态");
  }
}

6、不稳定斜坡行走——CPG抗扰动步态+坡面自适应
适用场景：山体滑坡、泥石流后的不稳定斜坡（坡度10-30°），双足机器人需在CPG步态基础上，实时感知坡面角度，自适应调整CPG相位与关节角度，维持机身平衡，防止侧翻。

核心逻辑：通过IMU实时检测机身俯仰角与横滚角，将坡面角度信息反馈至CPG模型，调整振荡器的相位分布（使上坡侧关节提前发力）与关节角度补偿（抵消坡面倾斜带来的力矩），同时通过CPG的相位耦合维持双足交替，实现斜坡稳定行走。

/* 不稳定斜坡行走：CPG抗扰动步态+坡面自适应
   核心：IMU检测坡面角度，调整CPG相位与关节角度补偿，维持机身平衡
   硬件：Arduino Mega + 4×BLDC + MPU6050 + 扭矩传感器 + 压力传感器 + 4×编码器
*/
#include <SimpleFOC.h>
#include <MPU6050.h>

// --- CPG参数与坡面补偿 ---
#define CPG_OSC_COUNT 4
struct Oscillator {
  float phase;
  float freq;
  float amplitude;
  float phaseShift;
};
Oscillator oscillators[CPG_OSC_COUNT];

// 坡面参数
float slopePitch = 0;   // 坡面俯仰角（°，前后倾斜）
float slopeRoll = 0;    // 坡面横滚角（°，左右倾斜）
float pitchCompensation = 0; // 俯仰角补偿量
float rollCompensation = 0;  // 横滚角补偿量

// --- 传感器与状态 ---
MPU6050 mpu;
float accelX, accelY, accelZ;
float gyroX, gyroY, gyroZ;
float filteredPitch = 0, filteredRoll = 0; // 滤波后的坡面角度
bool leftFootOnGround = false, rightFootOnGround = false; // 足端接地状态

// --- BLDC电机与关节 ---
BLDCMotor jointMotors[4];
float jointCurrentAngles[4] = {0};

void setup() {
  Serial.begin(115200);
  initCPG();
  initMotors();
  initSensors();
  Serial.println("斜坡抗扰动CPG步态启动");
}

void loop() {
  // 1. 坡面感知：IMU检测机身角度，计算坡面倾斜
  detectSlopeAngle();
  // 2. CPG补偿计算：根据坡面角度调整相位与补偿量
  calculateCPGCompensation();
  // 3. CPG更新与关节控制：融合补偿后的关节目标
  updateCPG();
  adaptiveJointControl();
  // 4. 平衡监控：横滚角超限，强制调整CPG相位
  monitorBalance();
}

// 初始化CPG：基础参数，预留坡面补偿的调整空间
void initCPG() {
  float baseFreq = 1.3; // 斜坡行走略加快频率，减少支撑时间，提升稳定性
  oscillators[0].freq = baseFreq; oscillators[0].phase = 0; oscillators[0].amplitude = 0.5;
  oscillators[1].freq = baseFreq; oscillators[1].phase = 0; oscillators[1].amplitude = 0.8; oscillators[1].phaseShift = 0.5*PI;
  oscillators[2].freq = baseFreq; oscillators[2].phase = PI; oscillators[2].amplitude = 0.5;
  oscillators[3].freq = baseFreq; oscillators[3].phase = PI; oscillators[3].amplitude = 0.8; oscillators[3].phaseShift = 0.5*PI;
}

// 初始化电机（同案例1）
void initMotors() {
  int motorPins[4][3] = {{9,10,11}, {12,13,14}, {15,16,17}, {18,19,20}};
  int encoderPins[4][2] = {{2,3}, {4,5}, {6,7}, {8,9}};
  for (int i = 0; i < 4; i++) {
    jointMotors[i].linkDriver(new BLDCDriver3PWM(motorPins[i][0], motorPins[i][1], motorPins[i][2]));
    jointMotors[i].linkSensor(new Encoder(encoderPins[i][0], encoderPins[i][1]));
    jointMotors[i].controller = MotionControlType::angle;
    jointMotors[i].init();
    jointMotors[i].initFOC();
  }
}

// 初始化传感器：IMU+压力传感器
void initSensors() {
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050连接失败，使用默认角度");
    filteredPitch = 0; filteredRoll = 0;
  }
  // 压力传感器：假设A0=左足，A1=右足
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
}

// 检测坡面角度：通过IMU加速度计计算俯仰/横滚角
void detectSlopeAngle() {
  mpu.update();
  accelX = mpu.getAccelerationX();
  accelY = mpu.getAccelerationY();
  accelZ = mpu.getAccelerationZ();

  // 计算原始角度：利用重力加速度在轴上的分量计算倾斜角
  slopePitch = atan2(accelX, accelZ) * 180 / PI; // 俯仰角（前后倾斜）
  slopeRoll = atan2(accelY, accelZ) * 180 / PI;   // 横滚角（左右倾斜）

  // 卡尔曼滤波简化：一阶低通滤波，减少振动噪声
  float alpha = 0.5; // 滤波系数，越小滤波效果越好
  filteredPitch = alpha * slopePitch + (1-alpha) * filteredPitch;
  filteredRoll = alpha * slopeRoll + (1-alpha) * filteredRoll;

  // 压力传感器检测足端接地
  leftFootOnGround = analogRead(A0) > 600;
  rightFootOnGround = analogRead(A1) > 600;

  Serial.printf("坡面角度：俯仰%.1f°，横滚%.1f°\n", filteredPitch, filteredRoll);
}

// 计算CPG补偿：根据坡面角度，调整相位偏移与关节补偿量
void calculateCPGCompensation() {
  // 1. 俯仰角补偿：上坡时，后髋关节提前发力，调整相位
  if (filteredPitch > 0) { // 上坡（机身前倾，坡面为上坡）
    // 后髋关节（假设为右髋？需根据机器人结构明确，此处简化为相位补偿）
    // 上坡时，后腿支撑阶段需提前发力，相位提前
    pitchCompensation = filteredPitch * 0.01; // 每度补偿0.01rad相位
    oscillators[2].phase += pitchCompensation; // 右髋相位提前
  } else if (filteredPitch < 0) { // 下坡
    oscillators[0].phase += abs(pitchCompensation); // 左髋相位提前（下坡时前腿支撑）
  }

  // 2. 横滚角补偿：坡面倾斜时，上侧关节增加支撑角度，抵消侧翻力矩
  rollCompensation = filteredRoll * 0.005; // 每度补偿0.005rad角度
  // 左侧倾斜（roll>0）：左髋增加支撑角度，右髋减少
  if (filteredRoll > 0) {
    oscillators[0].amplitude += rollCompensation;
    oscillators[2].amplitude -= rollCompensation;
  } else if (filteredRoll < 0) {
    oscillators[0].amplitude -= abs(rollCompensation);
    oscillators[2].amplitude += abs(rollCompensation);
  }

  // 限制补偿量，防止关节超限
  oscillators[0].amplitude = constrain(oscillators[0].amplitude, 0.3, 0.7);
  oscillators[2].amplitude = constrain(oscillators[2].amplitude, 0.3, 0.7);
}

// 更新CPG相位（同前，融合补偿后的参数）
void updateCPG() {
  float dt = 0.01;
  for (int i = 0; i < CPG_OSC_COUNT; i++) {
    oscillators[i].phase += 2 * PI * oscillators[i].freq * dt;
    if (oscillators[i].phase >= 2 * PI) oscillators[i].phase -= 2 * PI;
    if (oscillators[i].phase < 0) oscillators[i].phase += 2 * PI;
  }
}

// 自适应关节控制：融合坡面补偿后的关节目标角度
void adaptiveJointControl() {
  for (int i = 0; i < 4; i++) {
    float targetAngle;
    if (i == 0 || i == 2) { // 髋关节
      // 基础CPG目标+坡面补偿角度（直接叠加补偿量，抵消坡面倾斜）
      targetAngle = oscillators[i].amplitude * sin(oscillators[i].phase);
      // 俯仰角补偿：上坡时后髋关节增加支撑角度，防止机身前倾
      if (i == 2 && filteredPitch > 0) targetAngle += pitchCompensation * 2;
      if (i == 0 && filteredPitch < 0) targetAngle += abs(pitchCompensation) * 2;
      // 横滚角补偿：倾斜侧髋关节增加角度，维持平衡
      if (filteredRoll > 0 && i == 0) targetAngle += rollCompensation * 3;
      if (filteredRoll < 0 && i == 2) targetAngle += abs(rollCompensation) * 3;
      // 角度约束
      targetAngle = constrain(targetAngle, -0.7, 0.7);
    } else { // 膝关节
      // 坡面行走时，支撑阶段膝关节适当弯曲，增加稳定性
      if ((i == 1 && leftFootOnGround) || (i == 3 && rightFootOnGround)) {
        // 支撑足膝关节微弯，抵消坡面载荷
        targetAngle = 0.1 + abs(filteredPitch) * 0.002;
      } else {
        // 摆动足按CPG生成角度
        targetAngle = oscillators[i].amplitude * sin(oscillators[i].phase - oscillators[i].phaseShift);
        targetAngle = max(0, targetAngle);
      }
      targetAngle = constrain(targetAngle, 0, 1.0);
    }

    jointMotors[i].target = jointCurrentAngles[i] + targetAngle;
    jointMotors[i].loopFOC();
    jointCurrentAngles[i] = jointMotors[i].shaftAngle;
  }
}

// 平衡监控：横滚角超限（>15°），强制调整CPG相位恢复平衡
void monitorBalance() {
  if (abs(filteredRoll) > 15) {
    Serial.println("横滚角超限，启动平衡恢复");
    // 强制将倾斜侧髋关节相位提前，快速调整支撑角度
    if (filteredRoll > 0) {
      oscillators[0].phase += 0.5; // 左髋相位提前，增加左侧支撑
      oscillators[0].amplitude = 0.6; // 增大左髋摆幅
    } else {
      oscillators[2].phase += 0.5; // 右髋相位提前
      oscillators[2].amplitude = 0.6;
    }
    // 恢复后重置补偿量
    delay(500);
    rollCompensation = 0;
    filteredRoll = 0;
  }
}

要点解读

1. CPG模型的轻量化与步态适配：平衡算法复杂度与Arduino算力
   CPG是双足步态的生物启发核心，但Arduino算力有限，必须采用轻量化建模策略，才能保证实时性，这是落地的前提：
   振荡器精简：核心振荡器仅保留相位、频率、振幅三个核心参数，避免复杂的神经元互连模型，案例中2-4个振荡器即可覆盖双足关节，单周期计算量仅数十次浮点运算，适配Arduino Mega/Due的处理能力。
   相位耦合简化：利用三角函数替代复杂的相位耦合方程，通过预设相位差实现双足交替，案例1中左右髋相位差π，既保证交替性，又极大降低计算负载，避免复杂的相位同步算法。
   参数与场景强绑定：不同抢险场景的步态需求不同，狭窄通道需小步幅、越障需高抬腿、斜坡需抗扰动，需针对性配置CPG参数，而非通用模型，案例通过动态调整振幅、相位提前量，实现场景适配，避免参数冗余。
2. 多传感器融合的实时环境感知：精准与鲁棒的平衡
   抢险场景环境动态多变，单一传感器无法应对，多传感器融合的核心是互补冗余+数据滤波，确保感知精准且鲁棒：
   传感器功能互补：案例中根据场景需求组合传感器——狭窄通道用超声波测距，越障用激光雷达+超声波，斜坡用IMU+压力传感器，避免了超声波测距范围小、激光雷达成本高、IMU易受振动干扰的局限，实现功能互补。
   数据滤波与容错：对传感器数据做轻量化滤波，如斜坡场景用一阶低通滤波抑制振动噪声，越障场景用阈值判断过滤无效数据；同时引入多传感器交叉验证，如激光雷达与超声波共同确认障碍，避免单一传感器失效导致的误判。
   感知与控制的周期匹配：传感器采样周期与CPG控制周期同步，避免感知滞后，案例中采用10ms控制周期，传感器同步采样，确保障碍检测、坡面角度计算能实时反馈至CPG调整，不出现明显延迟。
3. 步态自适应的动态调整机制：从被动响应到主动适配
   抢险场景的核心是应对不确定性，步态自适应不能依赖预编程，必须实现动态闭环调整，让CPG根据环境实时变化：
   参数动态调整逻辑：核心是基于环境感知数据，动态调整CPG的关键参数——振幅决定步幅、相位决定发力时机、频率决定步速。案例1中根据通道宽度缩放振幅，案例2中根据障碍高度提升振幅并提前相位，案例3中根据坡面角度补偿相位，均是“感知-计算-调整”的闭环，无需人工干预。
   状态切换与平滑过渡：步态切换（如基础步态→越障步态）不能生硬突变，否则会导致关节冲击，案例2中设置相位提前量、频率微调，实现步态的平滑过渡；切换回基础步态时设置延迟，避免频繁切换，保证稳定性。
   环境特征的量化映射：将环境参数（通道宽度、障碍高度、坡面角度）量化为CPG参数的调整系数，形成可复用的映射关系，如障碍高度与振幅缩放系数的线性映射，既保证自适应的合理性，又避免复杂的自适应算法，适配Arduino算力。
4. BLDC驱动的高精度闭环控制：从关节控制到步态稳定
   CPG生成的是理想步态，需通过BLDC电机精准执行，而抢险场景的振动、冲击易导致关节偏差，必须依赖高精度闭环控制，保证步态落地的精准性：
   编码器角度闭环优先：案例中均采用增量式编码器实现关节角度闭环，通过SimpleFOC库实现PID位置控制，消除机械间隙、负载变化带来的偏差，保证关节角度严格跟踪CPG目标，避免步态畸变。
   扭矩与电流的限制适配：抢险场景常伴随突发载荷，需对BLDC的电流与扭矩做限制，防止过载烧毁电机。案例中通过调整PWM占空比、限制目标电流实现扭矩限制，如越障时适当提高扭矩限制，支撑阶段降低扭矩，平衡负载与过载风险。
   关节协同与相位同步：双足步态依赖关节协同，案例中通过CPG相位同步机制，实时校正左右髋的相位差，避免双足同步运动导致失衡；同时通过关节角度约束，防止关节超限，保护机械结构。
5. 抢险场景的可靠性设计：鲁棒性优先于性能
   抢险救灾的核心诉求是可靠运行，代码设计需优先保证鲁棒性，而非追求高参数性能，这是与常规机器人开发的核心差异：
   状态监测与故障容错：案例中嵌入多种状态监测——相位同步监测、平衡监测、电机过载监测，一旦检测到异常，立即触发校正或停机，如斜坡横滚角超限启动平衡恢复，防止侧翻；同时对传感器失效做容错处理，如IMU失效时使用默认角度，避免系统崩溃。
   机械结构与参数的约束保护：通过代码约束关节角度范围、电机扭矩极限，防止极端环境下机械结构损坏；步态参数的调整均在机械安全范围内，如越障振幅上限2.5倍基础值，避免关节超限，实现软件保护机械。
   低延迟与高响应优先：抢险场景中，障碍、坡面的变化需快速响应，代码采用轻量化算法、固定周期控制、精简通信协议，确保从感知到控制的延迟控制在10ms以内，保证机器人能快速应对突发环境变化，而非追求高精度的复杂算法。

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