该方案的核心特点是利用贝塞尔曲线生成数学上连续可微的平滑足端轨迹，结合BLDC电机配合FOC算法实现的阻抗/力矩控制，使机器人在触地瞬间具备“虚拟弹簧”般的柔顺性，从而吸收冲击并自适应未知地形；主要适用于非结构化环境巡检、农业采摘、特种搜救等场景；实际部署需重点解决Arduino算力瓶颈、高频控制环实时性、足端触地检测延迟及电机力矩响应等问题。

一、主要特点
贝塞尔曲线足端轨迹——从“刚性插值”到“平滑运动”
传统足端轨迹规划常采用线性插值或简单的圆弧，在起点和终点处容易出现速度或加速度的突变（不连续），导致机器人运动抖动、冲击大。贝塞尔曲线通过控制点定义参数化曲线，具有独特的数学优势：
连续可微性：贝塞尔曲线在所有点上都是连续且可微的，其导数也是贝塞尔曲线。这意味着足端在起跳和落地时的速度、加速度可以平滑过渡，避免了刚性冲击，极大提升了运动的平稳性。
灵活性与仿生性：通过调整控制点的数量和位置（如二次、三次或五次贝塞尔曲线），可以灵活地拟合出类似动物奔跑时的足端轨迹（如高抬腿、低空滑行等），使机器人运动更加自然、高效。
计算高效：相比复杂的样条插值，贝塞尔曲线的计算量相对较小，适合在资源受限的嵌入式平台上实时生成。
BLDC+FOC柔顺力控——从“位置硬控”到“阻抗控制”
地形自适应的核心在于“触地”瞬间的处理。如果仅依靠位置控制，足端会以刚性方式撞击地面，不仅容易损坏机械结构，还会导致机身剧烈震动。
虚拟模型控制（VMC）：结合FOC（磁场定向控制）算法，BLDC电机可以实现精细的力矩闭环控制。系统可以在足端与期望位置之间建立一个“虚拟弹簧-阻尼”模型。当足端触地时，电机不再死板地追踪位置，而是像生物肌肉一样产生顺应外力的弹性位移，吸收冲击能量。
高动态响应：BLDC电机的电磁时间常数极小，配合FOC的高频电流环（通常>1kHz），能够毫秒级响应地形变化带来的负载突变，确保在松软地面或斜坡上不打滑、不陷落。
能量回馈：在下坡或制动时，BLDC可工作于发电模式，将动能回馈至电池，延长续航。
地形自适应触地——从“盲走”到“主动感知”
触地检测：通过监测电机电流（力矩）的突变或足底压力传感器，实时判断触地时刻。一旦检测到触地，立即切换控制模式（从摆动相的位置控制切换到支撑相的力/阻抗控制）。
落足点修正：结合IMU（惯性测量单元）和机身姿态，实时调整贝塞尔曲线的终点坐标。例如，当检测到前方有台阶时，自动抬高落足点；当检测到地面倾斜时，调整落足角度，确保足端与地面平行接触。
分层控制架构
规划层（贝塞尔曲线）：负责生成平滑的足端空间轨迹（X, Y, Z坐标随时间变化）。
控制层（FOC+阻抗控制）：负责将轨迹转化为关节力矩指令，并处理触地冲击。
感知层（IMU+电流反馈）：负责提供机身姿态和触地状态反馈。

二、典型应用场景
非结构化环境巡检
在光伏电站、农业大棚、变电站或城市下水道等场景中，地面往往包含草地、沙地、碎石及台阶。
贝塞尔曲线优势：平滑的轨迹规划使机器人在跨越障碍时重心波动小，不易倾覆。
柔顺触地优势：在碎石或松软地面上，阻抗控制能像“弹簧”一样吸收不平地面的冲击，防止足端打滑或陷入。
农业采摘与运输
果园、农田环境复杂，垄沟、杂草丛生。
地形自适应：机器人能根据地形起伏自动调整步高和落足点，保持机身水平，确保搭载的采摘机械臂或货物平台稳定。
低扰动：贝塞尔曲线的平滑运动减少了对农作物的机械扰动。
特种搜救与废墟探索
地震废墟、火灾现场等环境充满不确定性。
高越障能力：通过调整贝塞尔曲线的控制点，机器人可以实现高抬腿跨越废墟、深坑。
抗冲击能力：在坍塌物上行走时，柔顺控制能有效缓冲意外碰撞，保护机身结构。
科研与教育验证
作为高校机器人学、嵌入式控制课程的实验平台，用于验证运动学算法、FOC驱动、阻抗控制等前沿技术。

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

1、单腿贝塞尔轨迹生成与力反馈自适应
此案例聚焦单条腿的足端轨迹规划，使用三阶贝塞尔曲线生成平滑的抬腿和迈步动作，并结合足端力传感器实现触地自适应的闭环控制。

#include <SimpleFOC.h>

// 定义一条腿的三个关节电机 (髋、膝、踝)
BLDCMotor hip = BLDCMotor(7);
BLDCMotor knee = BLDCMotor(7);
BLDCMotor ankle = BLDCMotor(7);
BLDCDriver3PWM hip_driver = BLDCDriver3PWM(9, 10, 11);
BLDCDriver3PWM knee_driver = BLDCDriver3PWM(5, 6, 7);
BLDCDriver3PWM ankle_driver = BLDCDriver3PWM(3, 4, 8);

// 足端力传感器 (模拟输入)
int forceSensorPin = A0;
float footForce = 0.0;

// 贝塞尔曲线控制点 (足端在机器人坐标系下的位置)
// P0: 起点 (支撑相结束), P1, P2: 控制点, P3: 终点 (摆动相结束)
float P0[3] = {0.1, 0.0, -0.2};
float P1[3] = {0.15, 0.05, -0.1};
float P2[3] = {0.2, -0.05, -0.1};
float P3[3] = {0.25, 0.0, -0.2};

// 步态阶段参数
float phase = 0.0;        // 0~1, 摆动相
float stepHeight = 0.08;  // 目标抬腿高度 (m)
float stepLength = 0.15;  // 步长 (m)

void setup() {
  Serial.begin(115200);
  // 初始化电机及FOC...
  pinMode(forceSensorPin, INPUT);
}

// 三阶贝塞尔曲线插值计算
void bezierCubic(float t, float p0[3], float p1[3], float p2[3], float p3[3], float result[3]) {
  float u = 1 - t;
  float b0 = u * u * u;
  float b1 = 3 * u * u * t;
  float b2 = 3 * u * t * t;
  float b3 = t * t * t;
  for (int i = 0; i < 3; i++) {
    result[i] = b0 * p0[i] + b1 * p1[i] + b2 * p2[i] + b3 * p3[i];
  }
}

// 逆运动学：根据足端位置计算关节角度 (示例简化函数)
void inverseKinematics(float footPos[3], float* hipAngle, float* kneeAngle, float* ankleAngle) {
  // 在这里实现具体的运动学解算
  *hipAngle = 0.0;
  *kneeAngle = 0.0;
  *ankleAngle = 0.0;
}

void loop() {
  // 1. 读取足端力传感器
  footForce = analogRead(forceSensorPin) / 1023.0 * 5.0; // 假设转换为力 (N)

  // 2. 地形自适应触地逻辑
  // 若提前触地(力>阈值且处于摆动相)，下次降低步高
  if (phase < 0.5 && footForce > 2.0) {
    stepHeight = max(0.02, stepHeight - 0.005); // 降低步高，但有下限
  }
  // 若到摆动相结束仍未触地(力<阈值)，下次增加步高
  else if (phase >= 0.5 && footForce < 0.5) {
    stepHeight = min(0.15, stepHeight + 0.005); // 增加步高，但有上限
  }

  // 3. 实时更新贝塞尔曲线控制点高度
  P1[2] = -0.19 + (stepHeight - 0.08); // 根据stepHeight微调控制点高度
  P2[2] = -0.21 + (stepHeight - 0.08);

  // 4. 计算足端轨迹
  float footPos[3];
  float t = phase; // phase从0到1
  if (t < 1.0) {
    // 摆动相：使用贝塞尔曲线
    bezierCubic(t, P0, P1, P2, P3, footPos);
  } else {
    // 支撑相：足端向后滑动，模拟身体前进 (简化)
    footPos[0] = P3[0] - (t - 1.0) * stepLength;
    footPos[1] = 0;
    footPos[2] = P3[2];
    // 柔顺控制：根据触地力微调Z轴高度
    float forceError = footForce - 1.0; // 期望触地力为1N
    footPos[2] += forceError * 0.003;
  }

  // 5. 逆运动学求解并驱动关节电机
  float hipA, kneeA, ankleA;
  inverseKinematics(footPos, &hipA, &kneeA, &ankleA);
  hip.move(hipA);
  knee.move(kneeA);
  ankle.move(ankleA);

  // 6. 更新相位 (循环)
  phase += 0.02; // 步态频率控制
  if (phase > 1.0) phase = 0.0;

  delay(10); // 控制循环周期
  hip.loopFOC(); knee.loopFOC(); ankle.loopFOC();
}

代码要点：
贝塞尔轨迹平滑性：三阶贝塞尔曲线保证了足端位置、速度和加速度的连续，减少了机械冲击。
触地力闭环自适应：通过实时检测足端力，动态调整下一步的抬腿高度。提前触地则降低步高，延迟触地则增加步高，让机器人“感知”地面并自适应。
支撑相柔顺控制：在支撑相，根据触地力误差微调足端Z轴高度，表现如同弹簧，可吸收地面冲击。

2、四足机器人CPG步态 + 地形自适应抬腿高度与步长
此案例扩展至四足机器人，采用CPG（中枢模式发生器）生成节律性的对角步态，并融合IMU和足端力信息，实现步高和步长的地形自适应。

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

MPU6050 imu;
float pitchAngle = 0; // 机身俯仰角

// 四条腿的关节电机定义... (略)

// 步态与地形自适应参数
struct GaitParams {
  float stepLength;    // 步长 (cm)
  float stepHeight;    // 步高 (cm)
  float phaseDiff;     // 相位差 (度)
} gaitLibrary[3] = {
  {15.0, 8.0, 180.0},   // 平地步态
  {12.0, 10.0, 160.0},  // 斜坡步态: 步长减小, 步高增加
  {10.0, 15.0, 150.0}   // 台阶步态: 步长更小, 步高更高
};
GaitParams currentGait = gaitLibrary[0]; // 默认平地

// CPG振荡器状态 (简化)
float phase[4] = {0, PI, PI, 0}; // 对角步态相位

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

// 地形类型识别与步态切换
void updateGaitFromTerrain() {
  // 1. 通过IMU检测坡度
  float slopeAngle = fabs(pitchAngle) * 180.0 / PI;
  if (slopeAngle > 15.0) { // 大于15度为斜坡
    currentGait = gaitLibrary[1]; // 切换到斜坡步态
  } 
  // 2. 通过足端力或超声波检测台阶 (逻辑占位)
  else if (/* 检测到台阶 */) {
    currentGait = gaitLibrary[2]; // 切换到台阶步态
  } else {
    currentGait = gaitLibrary[0]; // 默认平地步态
  }
}

void loop() {
  // 1. 更新IMU和地形状态
  imu.getMotion6(...); // 获取IMU数据
  updateGaitFromTerrain();

  // 2. 更新CPG相位 (简化为固定频率)
  for (int i = 0; i < 4; i++) {
    phase[i] += 0.05; // 步频控制
    if (phase[i] > 2*PI) phase[i] -= 2*PI;
  }

  // 3. 为每条腿计算足端轨迹
  for (int leg = 0; leg < 4; leg++) {
    float footPos[2]; // 足端x, z位置
    float t = phase[leg] / (2*PI); // 将相位映射到0~1

    if (t < 0.5) { // 摆动相 (t: 0~0.5)
      // 利用贝塞尔曲线生成抬腿轨迹，参数来自currentGait
      // footPos = bezier(t, currentGait.stepLength, currentGait.stepHeight);
    } else {       // 支撑相 (t: 0.5~1)
      // 足端向后滑动，参数来自currentGait
      // footPos = slide(t, currentGait.stepLength);
    }

    // 4. 逆运动学求解并驱动关节
    // inverseKinematicsAndMove(leg, footPos);
  }

  delay(10);
}

代码要点：
CPG步态引擎：模仿生物神经振荡器，通过调整频率和相位差实现trot、walk等步态切换，抗干扰能力强。
地形类型识别：综合IMU（坡度）和传感器（台阶）数据，切换到预设的步态库（步长、步高参数不同），实现“模式化”自适应。
参数化地形适应：针对斜坡、台阶等地形，调整步态参数集，比单一反馈更直接有效。

3、足端轨迹与S型速度曲线融合规划
此案例关注轨迹的时间最优与平滑性。将贝塞尔几何路径与S型速度规划器结合，确保足端在精确跟踪轨迹的同时，速度与加速度连续，避免冲击。

// 伪代码概念，展示贝塞尔路径与S型速度规划的结合
class SmoothTrajectoryPlanner {
private:
  BezierCurve curve;      // 存储贝塞尔路径控制点
  SCurvePlanner speedPlanner; // S型速度规划器
  float elapsedTime, totalTime;

public:
  void setPath(BezierCurve path, float duration) {
    curve = path;
    totalTime = duration;
    elapsedTime = 0;
    speedPlanner.setTarget(1.0); // 目标进度为1
  }

  void update() {
    elapsedTime += 0.01; // 假设固定时间步长
    float progress = elapsedTime / totalTime;
    progress = constrain(progress, 0, 1.0);

    // 核心：用S型规划器平滑归一化进度，而非直接使用线性progress
    float smoothProgress = speedPlanner.update();

    // 根据平滑后的进度计算贝塞尔曲线上的点
    float pos[2];
    bezierCubic(smoothProgress, curve.P0, curve.P1, curve.P2, curve.P3, pos);

    // 将pos发送给逆运动学或驱动器...
  }
};

void loop() {
  planner.update();
  // 更新电机...
  delay(10);
}

代码要点：
路径与速度解耦：贝塞尔曲线负责定义“空间路径”（去哪儿），S型规划器负责定义“时间律”（多快去），两者结合实现了完整的平滑运动控制。
S型速度规划：通过限制加加速度（Jerk），使速度曲线呈S形，避免了梯形速度曲线在起止点的加速度突变，极大减少了机械冲击。
工程化封装：将规划器封装为类，代码复用性和可维护性更好，可独立优化速度和路径算法。

要点解读
贝塞尔曲线解决“路径连续性”：足式机器人足端轨迹若不平滑（如折线），会导致关节速度、加速度突变，产生冲击和振动。贝塞尔曲线天生具有凸包性和端点插值特性，能生成C²连续（加速度连续） 的路径，是保证运动平滑的基础。

地形自适应是“感知-规划-执行”的闭环：自适应不是单一功能，而是一个完整的闭环系统：足端力/IMU感知 -> 判断地形（步高/步长调整） -> 贝塞尔轨迹重规划 -> BLDC高精度执行。传感器融合（力觉+惯性）是关键。

S型速度曲线是“时间上的平滑”：贝塞尔曲线保证了空间路径的平滑，而S型速度规划保证了时间进度的平滑。两者结合才能实现无冲击的柔顺运动。只规划路径而不规划速度，仍会造成突然加减速。

BLDC与FOC是实现柔顺控制的执行基础：BLDC电机配合FOC算法，可提供高带宽的力矩响应和精准的位置/速度控制。这为在支撑相实现阻抗控制（表现如弹簧阻尼系统）和在摆动相实现精确轨迹跟踪提供了可能。

算法复杂性与实时性的平衡：Arduino算力有限，需注意算法简化（如减少贝塞尔曲线阶数）和控制周期分配。通常将FOC电流环放在高频中断，轨迹规划和逆运动学放在主循环（如50Hz-100Hz），以在功能和实时性间取得平衡。

4、基础四足机器人单足贝塞尔轨迹+固定地形触地
适用场景：平坦或微起伏地形（如室内地板、户外草坪）下，四足机器人实现稳定行走，单足采用贝塞尔曲线规划轨迹，简化触地控制，重点实现轨迹平滑性与基础步态稳定性。
核心逻辑：以单足为研究对象，建立“抬腿-摆动-落地”的贝塞尔轨迹（采用三次贝塞尔曲线，保证轨迹曲率连续），固定地形下预设触地角度与高度，通过BLDC电机驱动关节执行轨迹，结合编码器反馈闭环控制关节位置，避免轨迹偏差导致的落地冲击。

/* 基础四足单足：贝塞尔曲线足端轨迹 + 固定地形触地
   核心：三次贝塞尔曲线生成抬腿/摆动轨迹，预设触地角度，编码器闭环控制BLDC关节
   地形：平坦，仅用超声波辅助高度修正，简化触地逻辑
   硬件：Arduino Mega + 2×BLDC + HC-SR04 + 2×编码器
*/
#include <SimpleFOC.h>

// --- 贝塞尔曲线核心参数 ---
#define BEZIER_DEGREE 3  // 三次贝塞尔曲线
float bezierP0[2];      // 起点P0（足端落地点，关节角度计算）
float bezierP1[2];      // 控制点P1（抬腿阶段最高点，引导轨迹曲率）
float bezierP2[2];      // 控制点P2（摆动阶段引导点，保证落地姿态）
float bezierP3[2];      // 终点P3（足端下一次落地点）
float t = 0.0;          // 曲线参数（0~1，0=落地，1=下一次落地）

// --- 地形与触地参数（固定平坦地形，可微调） ---
#define TERRAIN_HEIGHT 50   // 预设地面高度（mm，超声波测量修正）
#define TOUCH_ANGLE 15      // 触地时关节角度（°，髋关节+膝关节组合）
#define BEZIER_STEP_TIME 50 // 轨迹执行步长时间（ms，控制移动速度）
#define MAX_SWING_HEIGHT 80 // 摆动阶段最大抬腿高度（mm）

// --- BLDC电机与编码器配置 ---
BLDCMotor motorHip, motorKnee; // 髋关节（横滚/偏航）、膝关节（弯曲）
float currentHipAngle = 0;     // 髋关节当前角度
float currentKneeAngle = 0;    // 膝关节当前角度
float targetHipAngle = 0;      // 髋关节目标角度
float targetKneeAngle = 0;     // 膝关节目标角度

// --- 超声波测距（地形高度感知） ---
#define TRIG_PIN 2
#define ECHO_PIN 3
float terrainHeight = 0;        // 实际地形高度（mm）

void setup() {
  Serial.begin(115200);
  initMotors();
  initSensors();
  Serial.println("单足贝塞尔轨迹+固定触地启动");
  // 初始化贝塞尔控制点：假设足端落地点为原点，抬腿最高点在正上方
  bezierP0[0] = 0.0; bezierP0[1] = 0.0;  // 落地点（x=0，y=0）
  bezierP1[0] = 30.0; bezierP1[1] = MAX_SWING_HEIGHT; // 控制点1（x=30，y=最大高度）
  bezierP2[0] = 60.0; bezierP2[1] = MAX_SWING_HEIGHT; // 控制点2（x=60，y=最大高度）
  bezierP3[0] = 90.0; bezierP3[1] = 0.0;  // 下一次落地点（x=90，y=0）
}

void loop() {
  // 1. 地形感知：获取实际地面高度
  terrainHeight = getTerrainHeight();
  // 2. 贝塞尔轨迹生成：根据t计算当前足端位置
  float footPos[2] = calculateBezierCurve(t);
  // 3. 关节角度转换：足端位置→髋关节/膝关节目标角度（简化逆运动学）
  convertFootToJoint(footPos, terrainHeight);
  // 4. BLDC闭环控制：执行关节角度
  executeJointMotion();
  // 5. 轨迹参数更新：控制移动速度
  updateTrajectoryParameter();
  // 6. 触地判断：t≈1时完成摆动，进入触地阶段，校正关节角度
  if (t >= 0.95) {
    correctTouchAngle();
  }
}

// 初始化BLDC电机与编码器
void initMotors() {
  // 髋关节：BLDC驱动，编码器反馈
  motorHip.linkDriver(new BLDCDriver3PWM(9, 10, 11));
  motorHip.linkSensor(new Encoder(2, 3));
  motorHip.controller = MotionControlType::angle;
  motorHip.init();
  motorHip.initFOC();
  motorHip.target = 0;

  // 膝关节：BLDC驱动，编码器反馈
  motorKnee.linkDriver(new BLDCDriver3PWM(12, 13, 14));
  motorKnee.linkSensor(new Encoder(4, 5));
  motorKnee.controller = MotionControlType::angle;
  motorKnee.init();
  motorKnee.initFOC();
  motorKnee.target = 0;
}

// 初始化传感器
void initSensors() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
}

// 超声波测地形高度
float getTerrainHeight() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  float distance = pulseIn(ECHO_PIN, HIGH) / 58.0; // 转换为mm
  // 结合预设地形，微调（平坦地形直接返回）
  return (distance < TERRAIN_HEIGHT * 0.8) ? TERRAIN_HEIGHT : distance;
}

// 计算三次贝塞尔曲线：返回足端位置（x,y）
float* calculateBezierCurve(float t) {
  // 三次贝塞尔公式：B(t) = (1-t)^3*P0 + 3(1-t)^2*t*P1 + 3(1-t)*t^2*P2 + t^3*P3
  float footX = pow(1-t, 3) * bezierP0[0] + 3 * pow(1-t, 2) * t * bezierP1[0] + 3 * (1-t) * pow(t, 2) * bezierP2[0] + pow(t, 3) * bezierP3[0];
  float footY = pow(1-t, 3) * bezierP0[1] + 3 * pow(1-t, 2) * t * bezierP1[1] + 3 * (1-t) * pow(t, 2) * bezierP2[1] + pow(t, 3) * bezierP3[1];
  // 存入静态数组返回（避免动态分配内存）
  static float footPos[2];
  footPos[0] = footX;
  footPos[1] = footY + terrainHeight; // 叠加地形高度
  return footPos;
}

// 足端位置→关节角度（简化逆运动学：假设腿长固定，髋关节控制水平位移，膝关节控制垂直高度）
void convertFootToJoint(float footPos[2], float terrainHeight) {
  // 假设腿长L=100mm，髋关节控制x方向，膝关节控制y方向
  float L = 100.0;
  // 髋关节目标角度：arcsin(footX/L)（简化，仅考虑水平位移）
  targetHipAngle = asin(footPos[0] / L) * (180.0 / PI);
  // 膝关节目标角度：90° - arccos((L - footY)/L)（简化，控制垂直高度）
  targetKneeAngle = 90.0 - acos((L - footPos[1]) / L) * (180.0 / PI);
  // 限制角度范围（防止关节超限）
  targetHipAngle = constrain(targetHipAngle, -45, 45);
  targetKneeAngle = constrain(targetKneeAngle, 0, 120);
}

// 执行关节运动：BLDC闭环控制
void executeJointMotion() {
  motorHip.target = targetHipAngle;
  motorKnee.target = targetKneeAngle;
  motorHip.loopFOC();
  motorKnee.loopFOC();
  // 记录当前角度
  currentHipAngle = motorHip.shaftAngle;
  currentKneeAngle = motorKnee.shaftAngle;
}

// 更新轨迹参数：控制移动速度
void updateTrajectoryParameter() {
  // t从0→1，控制一次摆动周期
  if (t < 1) {
    t += 0.02; // 每步增加0.02，总周期50步（50ms/步=2.5s，可根据需求调整）
  } else {
    t = 0;     // 完成一次轨迹，重置参数
  }
}

// 触地校正：t≈1时，确保关节角度符合触地要求，避免落地冲击
void correctTouchAngle() {
  // 校正髋关节角度至触地角度
  if (abs(currentHipAngle - TOUCH_ANGLE) > 1) {
    targetHipAngle = TOUCH_ANGLE;
  }
  // 校正膝关节角度至触地角度
  if (abs(currentKneeAngle - TOUCH_ANGLE) > 1) {
    targetKneeAngle = TOUCH_ANGLE;
  }
  executeJointMotion();
  Serial.println("触地校正完成：关节角度达标");
  t = 0; // 开始下一次轨迹
}

5、多足机器人动态地形自适应触地（多传感器融合）
适用场景：微起伏、缓坡地形（如户外碎石路、缓坡草地），多足机器人（四足）实现自适应行走，通过多传感器感知地形起伏，实时调整贝塞尔曲线的高度与触地角度，避免足端打滑或关节过载。

核心逻辑：多传感器（超声波+IMU+压力传感器）融合感知地形，实时修正贝塞尔控制点的垂直坐标，动态调整触地角度（根据坡面倾斜角度计算），足端接触地面时通过压力传感器判断触地状态，触发闭环力控，实现柔性触地。

/* 多足机器人：动态地形自适应触地 + 贝塞尔轨迹
   核心：超声波+IMU+压力传感器融合感知，动态修正贝塞尔曲线高度，自适应调整触地角度
   硬件：Arduino Mega + 8×BLDC + 4×超声波 + MPU6050 + 4×压力传感器 + 8×编码器
*/
#include <SimpleFOC.h>
#include <MPU6050.h>
#include <Wire.h>

// --- 多足配置 ---
#define LEGS_COUNT 4
// 每足贝塞尔控制点：4组，动态调整
float bezierPoints[LEGS_COUNT][4][2]; // [腿索引][控制点索引][x,y]
float trajectoriesT[LEGS_COUNT] = {0}; // 每足轨迹参数t

// --- 传感器与地形参数 ---
MPU6050 mpu;          // IMU：测机身倾斜角度
float bodyPitch = 0;  // 机身俯仰角（°，用于计算坡面角度）
float terrainHeights[LEGS_COUNT] = {0}; // 每足对应地形高度（超声波测）
float pressureValues[LEGS_COUNT] = {0}; // 每足压力值（压力传感器）
float adaptiveTouchAngle[LEGS_COUNT] = {0}; // 每足自适应触地角度

// --- BLDC电机数组：8个电机，每足2个（髋+膝） ---
BLDCMotor motors[8];
float jointAngles[8] = {0}; // 关节当前角度
float jointTargets[8] = {0}; // 关节目标角度

// --- 地形自适应参数 ---
#define MAX_ADAPTIVE_HEIGHT 100 // 自适应最大抬腿高度
#define PRESSURE_THRESHOLD 50  // 触地压力阈值（ADC值）
#define PITCH_COEFF 1.2       // 俯仰角与坡面角度转换系数

void setup() {
  Serial.begin(115200);
  initMotors();
  initSensors();
  Serial.println("多足动态地形自适应启动");
  // 初始化所有腿的贝塞尔控制点
  for (int leg = 0; leg < LEGS_COUNT; leg++) {
    // 基础控制点：每足相同，后续根据地形动态调整
    bezierPoints[leg][0][0] = 0;   bezierPoints[leg][0][1] = 0;  // 落地点
    bezierPoints[leg][1][0] = 30;  bezierPoints[leg][1][1] = MAX_ADAPTIVE_HEIGHT; // 控制点1
    bezierPoints[leg][2][0] = 60;  bezierPoints[leg][2][1] = MAX_ADAPTIVE_HEIGHT; // 控制点2
    bezierPoints[leg][3][0] = 90;  bezierPoints[leg][3][1] = 0;  // 落地点
  }
}

void loop() {
  // 1. 多传感器数据融合：获取机身倾斜与各足地形高度
  fuseSensorData();
  // 2. 动态调整贝塞尔曲线：根据地形高度修正控制点y坐标
  for (int leg = 0; leg < LEGS_COUNT; leg++) {
    adjustBezierCurve(leg, terrainHeights[leg]);
  }
  // 3. 自适应触地角度计算：根据坡面角度计算每足触地角度
  calculateAdaptiveTouchAngle();
  // 4. 每足轨迹执行：生成贝塞尔轨迹→关节角度→电机执行
  for (int leg = 0; leg < LEGS_COUNT; leg++) {
    executeLegTrajectory(leg);
  }
  // 5. 触地检测与力控：压力传感器触发柔性触地
  detectAndControlTouch(LEGS_COUNT);
}

// 初始化所有BLDC电机与编码器
void initMotors() {
  int motorIdx = 0;
  for (int leg = 0; leg < LEGS_COUNT; leg++) {
    // 髋关节电机（每足第1个）
    motors[motorIdx].linkDriver(new BLDCDriver3PWM(9 + leg * 2, 10 + leg * 2, 11 + leg * 2));
    motors[motorIdx].linkSensor(new Encoder(leg * 2 + 6, leg * 2 + 7));
    motors[motorIdx].controller = MotionControlType::angle;
    motors[motorIdx].init();
    motors[motorIdx].initFOC();
    motorIdx++;
    // 膝关节电机（每足第2个）
    motors[motorIdx].linkDriver(new BLDCDriver3PWM(12 + leg * 2, 13 + leg * 2, 14 + leg * 2));
    motors[motorIdx].linkSensor(new Encoder(leg * 2 + 8, leg * 2 + 9));
    motors[motorIdx].controller = MotionControlType::angle;
    motors[motorIdx].init();
    motors[motorIdx].initFOC();
    motorIdx++;
  }
}

// 初始化传感器（IMU+超声波+压力）
void initSensors() {
  // IMU初始化
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("IMU连接失败，使用模拟俯仰角");
    bodyPitch = 0;
  }
  // 超声波初始化（引脚定义略，每足对应一个超声波）
  // 压力传感器初始化（ADC引脚，pinMode略）
}

// 融合传感器数据：获取机身俯仰角与各足地形高度
void fuseSensorData() {
  // IMU获取机身俯仰角
  mpu.update();
  float ax = mpu.getAccelerationX();
  float az = mpu.getAccelerationZ();
  bodyPitch = atan2(ax, az) * (180.0 / PI); // 计算俯仰角

  // 每足超声波测地形高度
  for (int leg = 0; leg < LEGS_COUNT; leg++) {
    int trigPin = 2 + leg * 2;
    int echoPin = 3 + leg * 2;
    terrainHeights[leg] = getUltrasonicDistance(trigPin, echoPin);
    // 考虑机身俯仰角，修正地形高度（坡面地形，前足与后足高度差与俯仰角相关）
    terrainHeights[leg] += bodyPitch * PITCH_COEFF * (leg < 2 ? -1 : 1); // 前2足为前足，后2足为后足
  }

  // 每足压力传感器读数（ADC，假设接A0-A3）
  for (int leg = 0; leg < LEGS_COUNT; leg++) {
    pressureValues[leg] = analogRead(leg);
  }
}

// 超声波测距函数
float getUltrasonicDistance(int trigPin, int echoPin) {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  float distance = pulseIn(echoPin, HIGH) / 58.0;
  return constrain(distance, 30, 200); // 限制测量范围
}

// 动态调整贝塞尔曲线：根据地形高度修正控制点y坐标
void adjustBezierCurve(int leg, float terrainHeight) {
  // 抬腿高度自适应：地形越高，抬腿高度增加，避免碰撞
  float adaptiveHeight = MAX_ADAPTIVE_HEIGHT + (terrainHeight - 50) * 0.5;
  adaptiveHeight = constrain(adaptiveHeight, 60, 150);
  // 修正控制点1和控制点2的y坐标
  bezierPoints[leg][1][1] = adaptiveHeight;
  bezierPoints[leg][2][1] = adaptiveHeight;
  // 修正落地点y坐标：叠加地形高度
  bezierPoints[leg][0][1] = terrainHeight;
  bezierPoints[leg][3][1] = terrainHeight;
}

// 计算自适应触地角度：根据坡面角度调整，坡面越陡，触地角度越大
void calculateAdaptiveTouchAngle() {
  // 触地角度=基础角度+俯仰角*系数（俯仰角正负对应前后坡）
  for (int leg = 0; leg < LEGS_COUNT; leg++) {
    adaptiveTouchAngle[leg] = TOUCH_ANGLE + abs(bodyPitch) * 0.5;
    adaptiveTouchAngle[leg] = constrain(adaptiveTouchAngle[leg], 5, 30);
  }
}

// 执行单足轨迹：生成贝塞尔轨迹→转换关节角度→电机执行
void executeLegTrajectory(int leg) {
  // 1. 计算当前足端位置
  float footPos[2] = calculateBezierCurve(leg, trajectoriesT[leg]);
  // 2. 足端位置→关节角度（逆运动学，简化）
  int hipIdx = leg * 2;
  int kneeIdx = leg * 2 + 1;
  jointTargets[hipIdx] = asin(footPos[0] / 100.0) * (180.0 / PI); // 髋关节
  jointTargets[kneeIdx] = 90.0 - acos((100.0 - footPos[1]) / 100.0) * (180.0 / PI); // 膝关节
  // 3. 限制角度范围
  jointTargets[hipIdx] = constrain(jointTargets[hipIdx], -45, 45);
  jointTargets[kneeIdx] = constrain(jointTargets[kneeIdx], 0, 120);
  // 4. 电机执行
  motors[hipIdx].target = jointTargets[hipIdx];
  motors[kneeIdx].target = jointTargets[kneeIdx];
  motors[hipIdx].loopFOC();
  motors[kneeIdx].loopFOC();
  // 5. 更新轨迹参数
  if (trajectoriesT[leg] < 1) {
    trajectoriesT[leg] += 0.015; // 调整轨迹速度
  } else {
    trajectoriesT[leg] = 0;
  }
}

// 触地检测与力控：压力传感器触发，柔性调整关节力矩
void detectAndControlTouch(int legCount) {
  for (int leg = 0; leg < legCount; leg++) {
    int hipIdx = leg * 2;
    int kneeIdx = leg * 2 + 1;
    // 压力超过阈值，判断为触地，进入力控模式
    if (pressureValues[leg] > PRESSURE_THRESHOLD) {
      // 调整电机为力矩模式（简化：降低目标角度，实现柔性接触）
      if (jointAngles[hipIdx] > adaptiveTouchAngle[leg]) {
        jointTargets[hipIdx] -= 2; // 缓慢校正到触地角度
      }
      if (jointAngles[kneeIdx] > adaptiveTouchAngle[leg]) {
        jointTargets[kneeIdx] -= 2;
      }
      // 重新执行电机
      motors[hipIdx].target = jointTargets[hipIdx];
      motors[kneeIdx].target = jointTargets[kneeIdx];
      motors[hipIdx].loopFOC();
      motors[kneeIdx].loopFOC();
    }
    // 记录当前关节角度
    jointAngles[hipIdx] = motors[hipIdx].shaftAngle;
    jointAngles[kneeIdx] = motors[kneeIdx].shaftAngle;
  }
}

// 单足贝塞尔计算（融入对应控制点）
float* calculateBezierCurve(int leg, float t) {
  static float footPos[2];
  float* P0 = bezierPoints[leg][0];
  float* P1 = bezierPoints[leg][1];
  float* P2 = bezierPoints[leg][2];
  float* P3 = bezierPoints[leg][3];
  // 三次贝塞尔计算
  footPos[0] = pow(1-t, 3)*P0[0] + 3*pow(1-t,2)*t*P1[0] + 3*(1-t)*pow(t,2)*P2[0] + pow(t,3)*P3[0];
  footPos[1] = pow(1-t, 3)*P0[1] + 3*pow(1-t,2)*t*P1[1] + 3*(1-t)*pow(t,2)*P2[1] + pow(t,3)*P3[1];
  return footPos;
}

6、越障场景下的高阶贝塞尔轨迹+柔性触地控制
适用场景：复杂障碍场景（如台阶、石块、陡坡），机器人需跨越障碍，足端轨迹采用高阶贝塞尔曲线（更高阶数保证越障时足端姿态平稳），结合力控实现柔性触地，避免越障过程中关节过载或足端打滑。

核心逻辑：采用五次贝塞尔曲线（更高阶数，可精确控制足端轨迹的位置、速度、加速度连续，甚至姿态），针对障碍高度动态调整轨迹的最高点与落地点；足端安装六维力传感器，实时感知触地冲击力，通过力控算法调整电机力矩，实现“轻触地-稳支撑-强发力”的柔性越障过程。

/* 越障场景：高阶贝塞尔轨迹 + 柔性触地力控
   核心：五次贝塞尔曲线保证越障姿态平稳，六维力传感器实时力控，动态调整轨迹与触地力
   硬件：Arduino Due + 3×BLDC + 六维力传感器 + 激光雷达 + 3×编码器
*/
#include <SimpleFOC.h>

// --- 高阶贝塞尔参数：五次曲线（6个控制点） ---
#define BEZIER_DEGREE 5
float bezierCtrlPoints[6][2]; // 6个控制点，控制足端轨迹的位置与姿态
float trajectoryT = 0.0;       // 轨迹参数t（0~1）

// --- 障碍与越障参数 ---
float obstacleHeight = 0;     // 障碍高度（激光雷达测）
float obstacleDepth = 0;      // 障碍深度（激光雷达测）
#define MIN_LIFT_HEIGHT 120  // 越障最小抬腿高度
#define MAX_LIFT_HEIGHT 200  // 最大抬腿高度（根据障碍调整）

// --- 力控与柔性触地参数 ---
#define FORCE_THRESHOLD 10   // 触地力阈值（N，超过则判断为触地）
#define FORCE_ADJUST_COEFF 0.5 // 力调整系数，力越大，关节角度修正越小
#define MOTOR_TORQUE_MAX 50  // 电机最大力矩（简化，控制电流实现）
float touchForceX = 0, touchForceY = 0, touchForceZ = 0; // 六维力（简化为Z向力）
float currentTorque = 0;     // 当前电机力矩

// --- BLDC电机与关节配置：3自由度单足 ---
BLDCMotor motorHipYaw, motorHipPitch, motorKnee; // 髋偏航、髋俯仰、膝
float jointAngles[3] = {0};  // 关节当前角度
float jointTargets[3] = {0}; // 关节目标角度

// --- 传感器配置 ---
float lidarData[2] = {0};    // 激光雷达数据：障碍高度、深度
float imuPitchRoll[2] = {0}; // IMU俯仰、横滚角

void setup() {
  Serial.begin(115200);
  initMotors();
  initSensors();
  Serial.println("越障：高阶贝塞尔+柔性触地启动");
  // 初始化五次贝塞尔控制点（基础版，后续根据障碍动态调整）
  initBezierCtrlPoints();
}

void loop() {
  // 1. 障碍检测：激光雷达+IMU获取障碍信息与机身姿态
  detectObstacle();
  // 2. 动态调整高阶贝塞尔曲线：根据障碍高度/深度调整控制点
  adjustHighOrderBezier(obstacleHeight, obstacleDepth);
  // 3. 生成足端轨迹：五次贝塞尔计算，输出位置+姿态
  float footPos[2], footAngle = 0;
  calculateHighOrderBezier(trajectoryT, footPos, &footAngle);
  // 4. 轨迹→关节角度：3自由度逆运动学（完整模型，根据足端位置计算关节角度）
  convertFootToJoints(footPos, footAngle, jointTargets);
  // 5. 力控执行：结合力传感器反馈，调整电机力矩与关节目标
  forceControlExecute();
  // 6. 更新轨迹参数与力信号
  updateTrajectoryAndForce();
}

// 初始化五次贝塞尔控制点（基础姿态）
void initBezierCtrlPoints() {
  // 控制点定义：足端从落地→抬升→越障→落地的完整轨迹，兼顾姿态
  bezierCtrlPoints[0][0] = 0;   bezierCtrlPoints[0][1] = 0;    // P0：落地点（x=0,y=0）
  bezierCtrlPoints[1][0] = 20;  bezierCtrlPoints[1][1] = 50;   // P1：抬升引导点
  bezierCtrlPoints[2][0] = 50;  bezierCtrlPoints[2][1] = 150;  // P2：越障最高点控制点
  bezierCtrlPoints[3][0] = 80;  bezierCtrlPoints[3][1] = 150;  // P3：越障过渡控制点
  bezierCtrlPoints[4][0] = 100; bezierCtrlPoints[4][1] = 50;   // P4：落地过渡控制点
  bezierCtrlPoints[5][0] = 120; bezierCtrlPoints[5][1] = 0;    // P5：下一次落地点
}

// 初始化3自由度BLDC电机
void initMotors() {
  // 髋偏航关节
  motorHipYaw.linkDriver(new BLDCDriver3PWM(9, 10, 11));
  motorHipYaw.linkSensor(new Encoder(2, 3));
  motorHipYaw.controller = MotionControlType::angle;
  motorHipYaw.init();
  motorHipYaw.initFOC();

  // 髋俯仰关节
  motorHipPitch.linkDriver(new BLDCDriver3PWM(12, 13, 14));
  motorHipPitch.linkSensor(new Encoder(4, 5));
  motorHipPitch.controller = MotionControlType::angle;
  motorHipPitch.init();
  motorHipPitch.initFOC();

  // 膝关节
  motorKnee.linkDriver(new BLDCDriver3PWM(15, 16, 17));
  motorKnee.linkSensor(new Encoder(6, 7));
  motorKnee.controller = MotionControlType::angle;
  motorKnee.init();
  motorKnee.initFOC();
}

// 障碍检测：激光雷达+IMU
void detectObstacle() {
  // 模拟激光雷达数据（实际需读取雷达协议）
  obstacleHeight = 80; // 障碍高度80mm（示例，实际从雷达获取）
  obstacleDepth = 50;  // 障碍深度50mm
  // 模拟IMU数据
  imuPitchRoll[0] = 5; // 俯仰角5°（示例）
  imuPitchRoll[1] = 0; // 横滚角0°
}

// 动态调整五次贝塞尔控制点：根据障碍参数修正
void adjustHighOrderBezier(float h, float d) {
  // 抬腿高度随障碍高度增加而提升，且控制点位置随障碍深度调整
  float liftHeight = MIN_LIFT_HEIGHT + h * 1.2;
  liftHeight = constrain(liftHeight, MIN_LIFT_HEIGHT, MAX_LIFT_HEIGHT);
  // 修正最高点控制点（P2、P3）的y坐标
  bezierCtrlPoints[2][1] = liftHeight;
  bezierCtrlPoints[3][1] = liftHeight;
  // 修正落地点的x坐标：障碍深度越大，落地点越远
  bezierCtrlPoints[5][0] = 120 + d * 0.5;
  // 根据IMU俯仰角，微调控制点姿态（保证轨迹与坡面匹配）
  float pitchOffset = imuPitchRoll[0] * 0.3;
  for (int i = 0; i < 6; i++) {
    bezierCtrlPoints[i][1] += pitchOffset;
  }
}

// 计算五次贝塞尔曲线：输出足端位置+姿态（高阶曲线可同时控制位置与姿态）
void calculateHighOrderBezier(float t, float footPos[2], float* footAngle) {
  // 五次贝塞尔公式：B(t) = Σ(C(n,k)*(1-t)^(n-k)*t^k*Pk)，n=5
  float binom[6] = {1, 5, 10, 10, 5, 1}; // 二项式系数C(5,k)
  footPos[0] = 0;
  footPos[1] = 0;
  // 计算x方向坐标
  for (int k = 0; k <= 5; k++) {
    float term = binom[k] * pow(1-t, 5-k) * pow(t, k) * bezierCtrlPoints[k][0];
    footPos[0] += term;
  }
  // 计算y方向坐标
  for (int k = 0; k <= 5; k++) {
    float term = binom[k] * pow(1-t, 5-k) * pow(t, k) * bezierCtrlPoints[k][1];
    footPos[1] += term;
  }
  // 计算足端姿态（高阶贝塞尔可控制姿态导数，简化：姿态与轨迹参数的导数相关）
  // 姿态=轨迹切线的倾斜角度，简化为：footAngle = atan2(dy/dt, dx/dt)
  float dxdt = 0, dydt = 0;
  for (int k = 0; k <= 5; k++) {
    // 导数的二项式系数：k*C(5,k)
    float binomDeriv = k * binom[k];
    dxdt += binomDeriv * pow(1-t, 4-k) * pow(t, k-1) * (k>0 ? bezierCtrlPoints[k][0] : 0);
    dydt += binomDeriv * pow(1-t, 4-k) * pow(t, k-1) * (k>0 ? bezierCtrlPoints[k][1] : 0);
  }
  *footAngle = atan2(dydt, dxdt) * (180.0 / PI);
}

// 足端位置+姿态→3自由度关节角度（完整逆运动学，以足端为终点，关节长度L1=80,L2=70,L3=60）
void convertFootToJoints(float footPos[2], float footAngle, float jointTargets[3]) {
  // 已知：足端位置(x,y)，足端姿态θ，关节长度L1（髋偏航-髋俯仰距离）、L2（髋俯仰-膝距离）、L3（膝-足端距离）
  float L1 = 80, L2 = 70, L3 = 60;
  // 步骤1：计算髋偏航角度（控制足端水平方向）
  jointTargets[0] = atan2(footPos[0], footPos[1]) * (180.0 / PI);
  // 步骤2：计算髋俯仰与膝关节角度（逆运动学经典解，考虑足端姿态）
  // 简化：先计算髋俯仰角，再计算膝关节角，姿态由髋俯仰与膝关节共同控制
  float distFromHip = sqrt(footPos[0]*footPos[0] + footPos[1]*footPos[1]);
  float cosPitch = (distFromHip*distFromHip + L1*L1 - L2*L2 - L3*L3) / (2*L1*sqrt(L2*L2 + L3*L3 + 2*L2*L3*cos(footAngle*PI/180)));
  jointTargets[1] = acos(constrain(cosPitch, -1, 1)) * (180.0 / PI);
  // 膝关节角度：基于髋俯仰与足端姿态
  float cosKnee = (L2*L2 + L3*L3 - distFromHip*distFromHip - L1*L1 + 2*L1*distFromHip*cos(jointTargets[1]*PI/180)) / (2*L2*L3);
  jointTargets[2] = 180 - acos(constrain(cosKnee, -1, 1)) * (180.0 / PI);
  // 限制角度范围
  jointTargets[0] = constrain(jointTargets[0], -60, 60);
  jointTargets[1] = constrain(jointTargets[1], -45, 90);
  jointTargets[2] = constrain(jointTargets[2], 0, 150);
}

// 力控执行：结合力传感器反馈，调整电机力矩
void forceControlExecute() {
  // 1. 读取力传感器数据（简化为Z向触地力）
  touchForceZ = readForceSensor();
  // 2. 根据力信号调整关节目标角度：力越大，角度修正越小（柔性接触）
  if (abs(touchForceZ) > FORCE_THRESHOLD) {
    // 力超过阈值，判断为触地，调整关节目标实现柔性
    for (int i = 0; i < 3; i++) {
      float angleDiff = jointAngles[i] - jointTargets[i];
      // 力越大，修正越小，避免过载
      jointTargets[i] += angleDiff * FORCE_ADJUST_COEFF * (FORCE_THRESHOLD / abs(touchForceZ));
    }
  }
  // 3. 计算电机力矩：力与力矩的简化转换（力×力臂=力矩）
  float torqueZ = touchForceZ * 50; // 假设力臂50mm
  currentTorque = constrain(torqueZ, 0, MOTOR_TORQUE_MAX);
  // 4. 设置电机目标角度，同时通过驱动电路限制电流（实现力矩控制，简化为角度+力矩混合控制）
  motorHipYaw.target = jointTargets[0];
  motorHipPitch.target = jointTargets[1];
  motorKnee.target = jointTargets[2];
  // 简化力矩控制：通过调整PWM占空比模拟力矩限制（实际需驱动电路支持电流闭环）
  adjustMotorPWM(motorHipYaw, currentTorque / MOTOR_TORQUE_MAX);
  adjustMotorPWM(motorHipPitch, currentTorque / MOTOR_TORQUE_MAX);
  adjustMotorPWM(motorKnee, currentTorque / MOTOR_TORQUE_MAX);
  // 执行闭环控制
  motorHipYaw.loopFOC();
  motorHipPitch.loopFOC();
  motorKnee.loopFOC();
  // 记录关节角度
  jointAngles[0] = motorHipYaw.shaftAngle;
  jointAngles[1] = motorHipPitch.shaftAngle;
  jointAngles[2] = motorKnee.shaftAngle;
}

// 读取力传感器（简化为模拟ADC读数，实际需六维力传感器驱动）
float readForceSensor() {
  // 模拟Z向力：ADC值转换为力（假设ADC=200对应10N）
  int adcValue = analogRead(A0);
  return adcValue * 10.0 / 200.0;
}

// 调整电机PWM实现力矩限制（简化）
void adjustMotorPWM(BLDCMotor &motor, float torqueRatio) {
  // 假设电机的PWM占空比与力矩正相关，限制最大占空比实现力矩限制
  // 实际SimpleFOC中需通过电流闭环控制力矩，此处简化为调整目标速度
  motor.target = motor.target * torqueRatio;
}

// 更新轨迹参数与力信号
void updateTrajectoryAndForce() {
  // 更新轨迹参数
  if (trajectoryT < 1) {
    trajectoryT += 0.01; // 越障轨迹更平缓，步长时间更长
  } else {
    trajectoryT = 0; // 完成一次越障，重置
  }
  // 持续读取力信号（已在主循环中完成，此处补充延时）
  delay(20);
}

要点解读

1. 贝塞尔曲线的阶数与场景匹配：兼顾轨迹平滑性与计算复杂度
   贝塞尔曲线的阶数直接决定轨迹的平滑性与计算量，需结合机器人的任务场景与硬件算力（Arduino算力有限）精准匹配，这是平衡性能与落地性的核心：
   阶数选择原则：
   基础行走场景（案例4）：采用三次贝塞尔曲线（3个控制点），仅需保证轨迹的位置与速度连续，计算量小（仅需10余次浮点运算），适配Arduino Mega的算力，避免轨迹生成占用过多控制周期。
   动态地形场景（案例5）：维持三次贝塞尔，通过动态调整控制点实现自适应，无需增加阶数，重点保证实时性，应对地形变化时轨迹能快速修正。
   越障场景（案例6）：采用五次贝塞尔曲线（6个控制点），可同时控制轨迹的位置、速度、加速度、姿态，保证越障时足端姿态平稳（避免姿态突变导致的关节冲击），虽然计算量增大，但通过Arduino Due的高算力支撑，仍能满足实时性要求。
   关键控制点设计：控制点的数量与位置需与足端运动阶段匹配，抬腿阶段控制点引导轨迹上扬，摆动阶段控制点引导轨迹过渡，落地阶段控制点保证姿态平稳，避免控制点过多导致轨迹紊乱或过少导致冲击。

2. 地形自适应感知的多传感器融合：精准与可靠的前提
   地形自适应的核心是精准感知地形信息（高度、坡度、障碍），单一传感器存在局限性（超声波测距范围有限、IMU易受振动干扰），需通过多传感器融合提升感知的精准度与可靠性，为轨迹修正提供依据：
   传感器优势互补：
   超声波+IMU融合（案例5）：超声波负责局部地形高度测量，IMU负责机身全局姿态测量，通过机身俯仰角修正超声波的地形高度数据，解决坡面地形下前后足的高度差问题，避免单一传感器导致的误判。
   激光雷达+IMU+力传感器融合（案例6）：激光雷达实现障碍全局检测（高度、深度），IMU修正机身姿态，力传感器感知触地瞬间的冲击力，三者协同为越障轨迹调整提供全面信息，提升越障的适应性。
   数据校正与容错：对传感器数据进行滤波处理（如均值滤波、卡尔曼滤波，案例中简化为约束与修正），剔除异常值，如超声波测量距离超过合理范围时直接丢弃，同时引入压力传感器作为触地的最终判断依据，避免传感器数据偏差导致的触地误判，提升系统的鲁棒性。

3. 足端轨迹到关节角度的逆运动学建模：精度与效率的平衡
   贝塞尔曲线输出的是足端的空间位置（与姿态），需通过逆运动学转换为关节的目标角度，才能驱动BLDC电机执行，逆运动学的精度与计算效率直接影响轨迹执行的准确性与实时性，需结合机器人的关节结构进行平衡：
   简化逆运动学优先，保证实时性：案例4、5中，机器人关节数较少（2自由度/足），采用简化逆运动学模型，假设腿长固定，通过三角函数快速计算关节角度，计算量极小，满足Arduino的实时控制需求，虽精度略低，但足以应对平坦与微起伏地形，通过编码器闭环控制弥补精度不足。
   完整逆运动学提升精度，适配复杂场景：案例6中，机器人关节数增加（3自由度/足），采用完整逆运动学模型，根据足端位置与姿态，结合关节长度、转动范围，通过经典逆运动学公式计算关节角度，保证越障时足端姿态与位置的精准匹配，同时通过限制关节角度范围避免关节超限，提升越障过程的稳定性。
   闭环校正弥补模型误差：无论采用简化还是完整逆运动学，均需引入编码器反馈构成闭环控制，实时监测关节实际角度与目标角度的偏差，通过PID或SimpleFOC自带的闭环算法修正偏差，消除模型简化、机械间隙带来的误差，保证足端轨迹的执行精度。

4. 柔性触地的力控策略：从“刚性接触”到“柔性缓冲”的核心
   传统触地控制是刚性接触，易导致关节过载、足端打滑、机身震荡，柔性触地通过力控策略实现缓冲，是提升机器人稳定性与关节寿命的关键，需围绕力感知与力调整设计：
   力感知的触发逻辑：以足端压力/力传感器为触发信号，设定合理的触地阈值，案例5、6中，当压力超过阈值时判断为触地，触发柔性调整，避免未触地时的误触发，同时通过力信号的大小判断触地的冲击程度，为后续调整提供依据。
   力-角度的协同调整：案例5中，触地后通过小幅度调整关节目标角度，使足端缓慢贴合地面，实现柔性缓冲；案例6中，引入力矩控制，根据力传感器的反馈动态调整电机力矩，力越大，力矩越小，实现“力越大越柔性”的自适应控制，同时限制最大力矩避免关节过载，平衡缓冲与支撑需求。
   分阶段力控策略：触地过程分为“接触-缓冲-支撑”三个阶段，接触阶段降低关节目标角度，缓冲阶段通过力控调整力矩，支撑阶段逐渐提升力矩以提供支撑力，案例3中通过力传感器实时感知阶段变化，动态切换控制策略，实现从柔性缓冲到稳定支撑的平稳过渡。

5. Arduino嵌入式资源优化：保障实时控制与算法落地
   Arduino的算力、存储、接口资源有限，而贝塞尔轨迹计算、传感器数据处理、闭环控制对实时性要求高，需通过资源优化保证系统稳定运行，这是工程落地的难点与关键：
   内存与算力优化：
   Q表/控制点静态分配：避免动态内存分配（malloc），采用全局静态数组存储贝塞尔控制点、关节角度等数据，如案例1-3中均预先定义固定大小的数组，减少内存碎片化，提升访问速度。
   算法轻量化裁剪：对贝塞尔计算、逆运动学进行简化，如用整数运算近似浮点运算，减少浮点乘除次数；采用查表法替代复杂计算，进一步提升计算速度，保证控制周期内完成所有计算任务。
   控制周期与任务调度优化：
   周期解耦：将控制周期与轨迹计算周期分离，控制周期（50-100ms）保证电机的实时闭环，轨迹计算周期（100-200ms）完成贝塞尔生成与传感器处理，避免计算任务占用控制周期，保证电机控制的稳定。
   优先级调度：核心任务（电机闭环控制、触地判断）优先级最高，优先执行；次要任务（轨迹参数更新、传感器数据读取）在空闲时执行，通过延时控制任务执行节奏，避免任务堆积导致的卡顿。
   接口与通信优化：
   传感器接口精简：采用模拟输入、数字I/O等简单接口连接传感器，减少对串口等低速通信接口的占用，如案例中大量采用模拟ADC读取压力传感器，数字I/O读取编码器，提升数据读取效率。
   通信简化：多机协同时采用精简的串口协议，减少数据传输量，仅传输核心的轨迹参数与传感器数据，避免大数据量传输导致的延迟，保证实时性。

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