【花雕学编程】Arduino BLDC 之仿人机器人膝关节稳定系统

在仿人机器人（Humanoid Robot）的运动控制系统中，膝关节作为下肢关键的承重与运动单元，其稳定性直接决定了整机行走、站立甚至动态平衡的能力。基于 Arduino 平台构建 BLDC（无刷直流电机）驱动的膝关节稳定系统，虽属低成本方案，但在科研验证、教育平台或轻量级原型开发中具有重要价值。以下从主要特点、应用场景及实施注意事项三个方面，以专业工程视角进行详细阐述。

一、主要特点

高动态响应与低转矩脉动

BLDC 电机相比传统有刷电机或步进电机，具备更高的功率密度、更长寿命及更低的机械噪声。配合磁场定向控制（FOC）算法，可实现近乎正弦波的电流驱动，显著降低转矩脉动，这对膝关节在支撑相（stance phase）中维持平稳受力至关重要。

闭环位置/力矩复合控制能力

膝关节不仅需精确控制屈伸角度（位置控制），还需在行走过程中根据地面反作用力动态调节输出力矩（阻抗或力矩控制）。通过集成高分辨率编码器（用于角度反馈）和可选的电流采样电路（用于估算输出力矩），Arduino 系统可构建基础的复合控制环，模拟人体膝关节的柔顺特性。

轻量化与模块化设计兼容性

BLDC 电机本体结构紧凑，配合行星减速器可实现高扭矩输出而保持较小体积，适合嵌入仿人机器人腿部有限空间。Arduino 主控板（如 Teensy 4.0 或 ESP32）体积小、功耗低，便于分布式部署于各关节，形成模块化驱动单元。

开源生态支持快速迭代

借助 SimpleFOC、Arduino-BLDC、ODrive 社区等开源库，开发者可快速实现 FOC 驱动、编码器解码、PID 调节等功能，大幅缩短控制算法验证周期。这对于研究膝关节在不同步态下的控制策略（如零力矩点 ZMP 补偿、虚拟模型控制 VMC）尤为有利。

实时性受限但可通过架构优化缓解

标准 Arduino Uno 等 8 位平台难以满足高带宽控制需求（典型膝关节控制频率需 ≥200 Hz），但采用 32 位 ARM Cortex-M7（如 Teensy 4.0）或双核 ESP32，配合 DMA 和硬件定时器，可实现 1--5 kHz 的控制环更新率，基本满足慢速行走或静态平衡实验要求。

二、应用场景

该系统主要适用于对成本敏感、性能要求适中的仿人机器人研发场景。例如，在高校机器人实验室中，用于验证双足步态生成算法、平衡控制策略或人机交互接口；在创客或竞赛项目（如 RoboCup@Home 教育组）中，构建具备基本行走能力的轻量级人形平台。

此外，该方案也适用于康复外骨骼的膝关节辅助模块原型开发。虽然临床级设备需更高安全等级与认证，但 Arduino + BLDC 架构可用于早期功能验证，测试助力时机、柔顺控制逻辑或用户意图识别算法。

在动态性能要求不高的场合（如缓慢上下楼梯、原地踏步、静态抗扰），该系统能有效维持膝关节角度稳定，防止因电机"失锁"或外部冲击导致的关节塌陷。然而，对于高速奔跑、跳跃或复杂地形适应等高动态任务，当前 Arduino 平台的计算与通信瓶颈将显著限制系统表现。

三、实施注意事项

机械结构刚性与传动间隙控制

膝关节承受周期性冲击载荷，任何传动链中的间隙（如联轴器松动、减速器背隙）都会引发控制振荡或定位误差。建议采用谐波减速器或零背隙行星减速器，并确保电机-编码器-关节轴三者同轴安装。编码器应尽可能安装在关节输出端（即"输出侧编码器"），以补偿传动误差。

安全机制不可或缺

仿人机器人一旦失稳可能造成设备损坏或人身伤害。必须设计多重保护：软件层面设置角度限位、电流过载切断；硬件层面加入机械止挡、急停开关及独立的电流监控电路。建议在主控之外增加看门狗或协处理器，实现故障安全停机。

电源与热管理

BLDC 电机在支撑相持续输出力矩，易导致绕组温升。需合理选择电机连续堵转电流参数，并在结构中预留散热通道。同时，电池电压波动会影响 PWM 有效占空比，建议使用稳压电源模块或在控制算法中引入电压前馈补偿。

传感器融合提升鲁棒性

单靠关节编码器无法感知整机姿态。理想情况下，应融合 IMU（惯性测量单元）数据，通过状态估计（如卡尔曼滤波）获取躯干倾角，进而调整膝关节目标轨迹以维持整体平衡。Arduino 可通过 I²C/SPI 接口集成 MPU6050 等低成本 IMU，实现初级的姿态反馈。

控制算法需匹配动力学特性

膝关节并非简单的位置伺服系统，其负载随步态周期剧烈变化（从负重到摆动）。若仅使用固定增益 PID，易在不同工况下出现超调或响应迟滞。建议引入自适应增益调度、阻抗控制或基于模型的前馈补偿，以提升系统对负载变化的鲁棒性。

通信延迟与同步问题

若多个关节由不同 Arduino 控制，需确保控制指令同步。可采用 CAN 总线、UART 同步触发或共享时钟信号，避免因通信延迟导致步态相位错乱。对于单膝关节系统，此问题较轻，但在双足或多关节协调中尤为关键。

1、基于PID的膝关节位置伺服控制

// 膝关节PID位置控制系统
#include <PID_v2.h>
#include <Encoder.h>
#include <Servo.h>

// 编码器引脚定义
#define ENC_A_KNEE1 2
#define ENC_B_KNEE1 3
#define ENC_A_KNEE2 4
#define ENC_B_KNEE2 5

Encoder kneeEnc1(ENC_A_KNEE1, ENC_B_KNEE1);
Encoder kneeEnc2(ENC_A_KNEE2, ENC_B_KNEE2);

// PID参数
double Kp = 8.0, Ki = 0.5, Kd = 0.2;
PID kneePID(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

// 电机控制引脚
#define IN1_KNEE1 9
#define IN2_KNEE1 10
#define ENA_KNEE1 8
#define IN1_KNEE2 11
#define IN2_KNEE2 12
#define ENA_KNEE2 10

// 全局变量
long currentAngle1 = 0;
long targetAngle1 = 45; // 目标角度（度）
double input = 0, output = 0, setpoint = 0;
int lastUpdateTime = 0;

void setup() {
  pinMode(IN1_KNEE1, OUTPUT);
  pinMode(IN2_KNEE1, OUTPUT);
  pinMode(ENA_KNEE1, OUTPUT);
  pinMode(IN1_KNEE2, OUTPUT);
  pinMode(IN2_KNEE2, OUTPUT);
  pinMode(ENA_KNEE2, OUTPUT);
  
  kneePID.SetMode(AUTOMATIC);
  kneePID.SetSampleTime(20);
  kneePID.SetOutputLimits(-255, 255);
  
  Serial.begin(115200);
}

void loop() {
  // 读取当前角度
  long pulses1 = kneeEnc1.read();
  currentAngle1 = map(pulses1, 0, 4096, 0, 180); // 假设12位编码器，映射到0-180度
  
  // 设置PID输入输出
  input = currentAngle1;
  setpoint = targetAngle1;
  kneePID.Compute();
  
  // 控制膝关节电机
  controlKneeMotor(1, output);
  
  // 显示调试信息
  Serial.print("Current: "); Serial.print(currentAngle1);
  Serial.print("\tTarget: "); Serial.print(targetAngle1);
  Serial.print("\tError: "); Serial.println(setpoint - input);
  
  delay(20);
}

void controlKneeMotor(int kneeNum, double power) {
  int in1, in2, ena;
  
  if (kneeNum == 1) {
    in1 = IN1_KNEE1;
    in2 = IN2_KNEE1;
    ena = ENA_KNEE1;
  } else {
    in1 = IN1_KNEE2;
    in2 = IN2_KNEE2;
    ena = ENA_KNEE2;
  }
  
  // 限制功率范围
  power = constrain(power, -255, 255);
  
  // 根据正负决定方向
  if (power > 0) {
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
  } else {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    power = -power;
  }
  
  analogWrite(ena, power);
}

// 串口命令处理
void serialEvent() {
  if (Serial.available()) {
    char cmd = Serial.read();
    if (cmd >= '0' && cmd <= '9') {
      targetAngle1 = (cmd - '0') * 30; // 每按键增加30度
    } else if (cmd == 'r') {
      // 重置编码器
      kneeEnc1.write(0);
    }
  }
}

2、自适应阻抗膝关节控制系统

// 自适应阻抗膝关节控制系统
#include <Encoder.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;
Encoder kneeEnc(2, 3);

// 全局变量
double currentAngle = 0;
double targetAngle = 30;
double impedanceGain = 0.5; // 初始阻抗增益
double stiffness = 5.0;     // 刚度系数
double damping = 0.8;       // 阻尼系数

// IMU数据
sensors_event_t accelerometer, gyro, temperature;
double angularVelocity = 0;

void setup() {
  Serial.begin(115200);
  
  // 初始化IMU
  mpu.begin();
  mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DPS);
  mpu.setFilterBandwidth(MPU6050_BANDWIDTH_41_HZ);
  
  // 初始化电机控制引脚
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(8, OUTPUT);
}

void loop() {
  // 读取传感器数据
  readSensors();
  
  // 计算阻抗控制力
  calculateImpedanceForce();
  
  // 应用控制信号
  applyControl();
  
  delay(10);
}

void readSensors() {
  // 读取IMU数据
  mpu.getEvent(&accelerometer, &gyro, &temperature);
  angularVelocity = gyro.gyro[1] * 0.0175; // 转换为度/秒
  
  // 读取编码器数据
  long pulses = kneeEnc.read();
  currentAngle = map(pulses, 0, 4096, 0, 180);
}

void calculateImpedanceForce() {
  // 计算角度误差和角速度误差
  double angleError = targetAngle - currentAngle;
  double velocityError = 0 - angularVelocity; // 目标是静止状态
  
  // 自适应调整阻抗参数（基于外部扰动检测）
  detectExternalDisturbance();
  
  // 阻抗控制公式：F = K*(θ_d - θ) + B*(ω_d - ω)
  double force = stiffness * angleError + damping * velocityError;
  
  // 转换为电机控制信号
  int motorPower = constrain((int)(force * impedanceGain), -255, 255);
  
  // 控制电机
  controlMotor(motorPower);
}

void detectExternalDisturbance() {
  // 基于加速度计检测外部扰动
  double accelMagnitude = sqrt(pow(accelerometer.acceleration[0], 2) + 
                             pow(accelerometer.acceleration[1], 2) + 
                             pow(accelerometer.acceleration[2], 2));
  
  // 如果检测到较大扰动，调整阻抗参数
  if (accelMagnitude > 2.0) { // 阈值可根据实际情况调整
    impedanceGain = constrain(impedanceGain * 0.9, 0.1, 2.0); // 降低阻抗以吸收冲击
  } else {
    impedanceGain = constrain(impedanceGain * 1.01, 0.1, 2.0); // 缓慢恢复阻抗
  }
}

void controlMotor(int power) {
  // 根据功率值控制电机方向和大小
  if (power > 0) {
    digitalWrite(9, HIGH);
    digitalWrite(10, LOW);
  } else {
    digitalWrite(9, LOW);
    digitalWrite(10, HIGH);
    power = -power;
  }
  
  analogWrite(8, power);
}

// 接收上位机指令
void serialEvent() {
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    if (command.startsWith("T")) {
      // 设置目标角度
      targetAngle = command.substring(1).toInt();
    } else if (command.startsWith("S")) {
      // 设置刚度系数
      stiffness = command.substring(1).toDouble();
    }
  }
}

3、多传感器融合的智能膝关节系统

// 多传感器融合智能膝关节控制系统
#include <Encoder.h>
#include <Wire.h>
#include <Adafruit_BNO055.h>
#include <Adafruit_Sensor.h>
#include <PID_v2.h>

// BNO055 IMU传感器
Adafruit_BNO055 bno = Adafruit_BNO055(55);

// 编码器
Encoder kneeEnc(2, 3);

// PID控制器
double Kp = 6.0, Ki = 0.3, Kd = 0.1;
PID kneePID(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

// 肌电信号模拟输入
#define EMG_PIN A0

// 电机控制引脚
#define IN1 9
#define IN2 10
#define ENA 8

// 全局变量
double currentAngle = 0;
double targetAngle = 45;
double input = 0, output = 0, setpoint = 0;
int mode = 0; // 0=位置控制, 1=阻抗控制, 2=自动模式

void setup() {
  Serial.begin(115200);
  
  // 初始化IMU
  if (!bno.begin()) {
    Serial.println("无法初始化BNO055传感器");
    while (1);
  }
  bno.setExtCrystalUse(true);
  
  // 初始化PID
  kneePID.SetMode(AUTOMATIC);
  kneePID.SetSampleTime(20);
  kneePID.SetOutputLimits(-255, 255);
  
  // 初始化电机引脚
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENA, OUTPUT);
  
  // 校准传感器
  calibrateSensors();
}

void loop() {
  // 读取所有传感器数据
  readAllSensors();
  
  // 根据模式选择控制策略
  switch(mode) {
    case 0: positionControl(); break;
    case 1: impedanceControl(); break;
    case 2: adaptiveControl(); break;
  }
  
  // 控制电机
  controlMotor(output);
  
  // 显示调试信息
  displayDebugInfo();
  
  delay(20);
}

void readAllSensors() {
  // 读取编码器数据
  long pulses = kneeEnc.read();
  currentAngle = map(pulses, 0, 4096, 0, 180);
  
  // 读取IMU姿态数据
  imu::Vector<3> euler = bno.getEulerData();
  double pitch = euler.x();
  
  // 读取肌电信号
  int emgValue = analogRead(EMG_PIN);
  
  // 融合传感器数据
  fusedAngle = 0.7 * currentAngle + 0.3 * pitch; // 加权融合
}

void positionControl() {
  input = fusedAngle;
  setpoint = targetAngle;
  kneePID.Compute();
}

void impedanceControl() {
  // 基于肌电信号的目标位置调整
  targetAngle = map(emgValue, 0, 1023, 10, 100);
  
  // 基于IMU数据的阻抗调整
  double stiffnessAdjust = map(abs(pitch), 0, 30, 0.8, 1.5);
  
  // 执行阻抗控制算法
  double angleError = targetAngle - fusedAngle;
  double velocityError = 0 - angularVelocity;
  
  output = stiffnessAdjust * angleError + damping * velocityError;
}

void adaptiveControl() {
  // 综合多种传感器信息进行自适应控制
  
  // 1. 基于肌电信号识别运动意图
  int intention = classifyMovementIntent(emgValue);
  
  // 2. 基于IMU数据评估环境交互
  double interactionLevel = assessInteraction(pitch, roll);
  
  // 3. 动态调整控制参数
  adjustControlParameters(intention, interactionLevel);
  
  // 4. 执行自适应控制算法
  performAdaptiveAlgorithm();
}

int classifyMovementIntent(int emgSignal) {
  // 简单的肌电信号分类
  if (emgSignal < 200) return 0; // 放松
  else if (emgSignal < 500) return 1; // 轻度收缩
  else if (emgSignal < 800) return 2; // 中度收缩
  else return 3; // 强力收缩
}

void adjustControlParameters(int intention, double interaction) {
  // 根据运动意图和环境交互水平动态调整PID参数
  switch(intention) {
    case 0: // 放松状态 - 低刚度
      Kp = 2.0; Ki = 0.1; Kd = 0.05;
      break;
    case 1: // 轻度活动 - 中等刚度
      Kp = 4.0; Ki = 0.2; Kd = 0.1;
      break;
    case 2: // 中度活动 - 较高刚度
      Kp = 6.0; Ki = 0.3; Kd = 0.15;
      break;
    case 3: // 强力活动 - 高刚度
      Kp = 8.0; Ki = 0.4; Kd = 0.2;
      break;
  }
  
  // 根据环境交互调整阻尼
  damping = 0.5 + interaction * 1.0;
  
  // 更新PID参数
  kneePID.SetTunings(Kp, Ki, Kd);
}

void controlMotor(double power) {
  // 实现电机控制逻辑
  power = constrain(power, -255, 255);
  
  if (power > 0) {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  } else {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    power = -power;
  }
  
  analogWrite(ENA, power);
}

void displayDebugInfo() {
  Serial.print("Mode: "); Serial.print(mode);
  Serial.print("\tAngle: "); Serial.print(fusedAngle);
  Serial.print("\tTarget: "); Serial.print(targetAngle);
  Serial.print("\tEMG: "); Serial.print(emgValue);
  Serial.print("\tPitch: "); Serial.print(pitch);
  Serial.print("\tOutput: "); Serial.println(output);
}

// 串口命令处理
void serialEvent() {
  if (Serial.available()) {
    char cmd = Serial.read();
    switch(cmd) {
      case '0': case '1': case '2':
        mode = cmd - '0';
        break;
      case 'T':
        // 设置目标角度
        targetAngle = Serial.parseInt();
        break;
      case 'C':
        // 校准传感器
        calibrateSensors();
        break;
    }
  }
}

void calibrateSensors() {
  // 执行传感器校准程序
  Serial.println("开始传感器校准...");
  
  // 重置编码器
  kneeEnc.write(0);
  
  // IMU校准序列
  for (int i = 0; i < 100; i++) {
    bno.getCalibration(&sysCal, &gyroCal, &accelCal, &magCal);
    delay(100);
  }
  
  Serial.println("校准完成!");
}

要点解读

多源传感器融合技术：成功的膝关节稳定系统需要整合多种传感器数据。案例中结合了编码器（精确位置测量）、IMU（姿态和角速度检测）以及肌电信号（用户意图识别），通过数据融合算法获得更全面准确的状态估计，显著提高了系统的感知能力和控制精度。

自适应控制策略实现：针对仿人机器人膝关节在不同任务和环境下的需求变化，案例实现了多层次的自适应机制。包括基于肌电信号的运动意图识别、基于环境交互水平的参数调整，以及根据工作状态动态改变的控制模式切换，确保系统在各种情况下都能保持稳定性和响应性。

阻抗控制与柔顺性设计：为了模仿人类关节的自然特性，案例采用了阻抗控制方法，通过调节刚度和阻尼参数来控制关节对外界力的响应特性。这种设计不仅提高了人机交互的安全性，还使机器人能够更好地适应不确定环境和执行精细操作任务。

实时性能优化策略：考虑到Arduino平台的计算资源限制，案例中采用了多种优化措施确保实时性。包括固定时间步长的PID计算、高效的数学运算替代复杂函数、适当的采样频率选择，以及分层的任务调度结构，确保系统能在有限硬件条件下实现快速响应。

安全保护机制集成：虽然代码中未完全展示，但实际应用需要考虑完善的安全措施。如过流保护防止电机过热损坏、超程限位避免机械结构损伤、跌倒检测触发紧急保护动作等。这些安全机制对于保障设备长期稳定运行和人员安全至关重要。

4、静态站立平衡控制（基于踝关节策略）

#include <PID_v1.h>
#include <Encoder.h>

// 硬件配置
#define ENCODER_PIN_A 2       // 膝关节编码器A相
#define ENCODER_PIN_B 3       // 膝关节编码器B相
#define MOTOR_PWM_PIN 9       // BLDC驱动PWM
#define MOTOR_DIR_PIN 8       // 方向控制
#define IMU_PIN A0            // 模拟输入（简易IMU，如MPU6050的俯仰角）

Encoder kneeEncoder(ENCODER_PIN_A, ENCODER_PIN_B);
double targetAngle = 0.0;     // 目标膝关节角度（度）
double currentAngle = 0;
double outputPWM = 0;
float imuOffset = 0;          // IMU零偏校准

// PID参数（需根据机器人质量分布调整）
double Kp = 1.2, Ki = 0.05, Kd = 0.1;
PID pid(&currentAngle, &outputPWM, &targetAngle, Kp, Ki, Kd, DIRECT);

void setup() {
  pinMode(MOTOR_DIR_PIN, OUTPUT);
  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255, 255);
  Serial.begin(115200);
  
  // IMU校准（假设读取原始值）
  imuOffset = analogRead(IMU_PIN) * 0.1; // 简易校准
}

void loop() {
  // 1. 读取IMU俯仰角（需转换为膝关节目标角度）
  float imuReading = analogRead(IMU_PIN) * 0.1 - imuOffset;
  targetAngle = imuReading * 0.5; // 比例控制（根据机器人几何关系调整）

  // 2. 读取编码器位置并转换为角度
  long encoderPos = kneeEncoder.read();
  currentAngle = encoderPos * 0.05; // 假设每脉冲=0.05度（需标定）

  // 3. PID计算
  pid.Compute();

  // 4. 驱动电机（带死区处理）
  if (abs(outputPWM) > 30) { // 消除静态摩擦死区
    digitalWrite(MOTOR_DIR_PIN, outputPWM > 0 ? HIGH : LOW);
    analogWrite(MOTOR_PWM_PIN, constrain(abs(outputPWM), 0, 255));
  } else {
    analogWrite(MOTOR_PWM_PIN, 0);
  }

  Serial.print("Target: "); Serial.print(targetAngle);
  Serial.print(" | Current: "); Serial.print(currentAngle);
  Serial.print(" | PWM: "); Serial.println(outputPWM);
  delay(10);
}

2、动态步态相位控制（摆动相与支撑相切换）

#include <Encoder.h>
#include <SimpleFOC.h> // 需安装SimpleFOC库支持FOC控制

// 硬件配置
#define ENCODER_PPR 1000      // 编码器每转脉冲数
#define MOTOR_POLE_PAIRS 7    // 电机极对数
BLDCMotor motor = BLDCMotor(MOTOR_POLE_PAIRS);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8); // PWM+使能
Encoder encoder = Encoder(2, 3, 500); // 中断引脚+CPR

// 步态参数
float gaitPhase = 0;         // 0=支撑相, 1=摆动相
float swingAngle = -30.0;    // 摆动相目标角度（负值为弯曲）
float stanceTorque = 0.5;    // 支撑相扭矩系数（模拟阻抗控制）
unsigned long phaseStart = 0;

void setup() {
  // 初始化FOC
  encoder.init();
  motor.linkSensor(&encoder);
  driver.init();
  motor.linkDriver(&driver);
  motor.controller = MotionControlType::torque; // 扭矩控制模式
  motor.begin();
  motor.useMonitoring(Serial);
  
  // 初始相位（支撑相）
  gaitPhase = 0;
  phaseStart = millis();
}

void loop() {
  motor.loopFOC();
  
  // 步态相位切换（2秒周期）
  unsigned long cycleTime = millis() - phaseStart;
  if (cycleTime > 2000) {
    phaseStart = millis();
    gaitPhase = 1 - gaitPhase; // 切换相位
  }

  // 相位控制逻辑
  if (gaitPhase < 0.5) { 
    // 支撑相：阻抗控制（模拟弹簧-阻尼）
    float angleError = encoder.getAngle() - 0; // 保持直立
    motor.setTorque(-stanceTorque * angleError); // 抵抗偏离
  } else {
    // 摆动相：位置控制（快速摆动）
    float targetAngle = swingAngle * (1 - 2*abs(gaitPhase-0.75)); // 三角波轨迹
    motor.target = targetAngle * PI/180; // 转换为弧度
    motor.move();
  }

  // 监控输出
  Serial.print("Phase: "); Serial.print(gaitPhase);
  Serial.print(" | Angle: "); Serial.println(encoder.getAngle() * 180/PI);
}

6、跌倒检测与保护性恢复

#include <Encoder.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h> // 使用MPU6050 IMU

// 硬件配置
Encoder kneeEncoder(2, 3);
Adafruit_MPU6050 mpu;
#define MOTOR_PWM_PIN 9
#define MOTOR_DIR_PIN 8
#define BUZZER_PIN 7         // 跌倒报警蜂鸣器

// 状态机
enum RobotState { NORMAL, FALLING, PROTECTIVE_LOCK };
RobotState state = NORMAL;
unsigned long fallStartTime = 0;
float fallThreshold = -1.2; // 跌倒检测阈值（加速度Z轴，单位g）

void setup() {
  pinMode(MOTOR_DIR_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  Serial.begin(115200);
  
  // 初始化IMU
  if (!mpu.begin()) {
    Serial.println("IMU初始化失败");
    while (1);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_4G);
}

void loop() {
  // 1. 读取IMU数据
  sensors_event_t accel;
  mpu.getEvent(&accel, NULL, NULL);
  float zAccel = accel.acceleration.z / 9.8; // 转换为g

  // 2. 状态机逻辑
  switch (state) {
    case NORMAL:
      // 跌倒检测（突然Z轴负向加速度）
      if (zAccel < fallThreshold) {
        state = FALLING;
        fallStartTime = millis();
        digitalWrite(BUZZER_PIN, HIGH); // 报警
      }
      // 正常膝关节控制（简化版位置控制）
      else {
        long targetPos = 0; // 保持直立（需根据实际角度标定）
        long currentPos = kneeEncoder.read();
        int pwm = constrain(10 * (targetPos - currentPos), -255, 255);
        driveMotor(pwm);
      }
      break;

    case FALLING:
      // 持续检测是否恢复
      if (millis() - fallStartTime > 500 && zAccel > -0.5) {
        state = NORMAL;
        digitalWrite(BUZZER_PIN, LOW);
      }
      // 超时未恢复则进入保护锁定
      else if (millis() - fallStartTime > 2000) {
        state = PROTECTIVE_LOCK;
        Serial.println("进入保护锁定模式");
      }
      break;

    case PROTECTIVE_LOCK:
      // 锁定电机防止二次伤害
      analogWrite(MOTOR_PWM_PIN, 0);
      // 可添加手动恢复逻辑（如按键）
      break;
  }

  Serial.print("State: "); Serial.print(state);
  Serial.print(" | Z-Accel: "); Serial.println(zAccel);
  delay(10);
}

void driveMotor(int pwm) {
  digitalWrite(MOTOR_DIR_PIN, pwm > 0 ? HIGH : LOW);
  analogWrite(MOTOR_PWM_PIN, abs(pwm));
}

要点解读

传感器融合关键性

编码器+IMU组合：编码器提供精确关节角度，IMU（如MPU6050）检测整体姿态，两者融合实现鲁棒控制（案例1需扩展IMU读取）。

卡尔曼滤波：实际应用中建议用滤波算法融合传感器数据，减少噪声影响。

控制策略分层设计

高层规划：案例5的步态相位切换属于运动规划层。

底层控制：案例4的PID或案例2的FOC属于执行层，需根据动态需求切换（如支撑相用扭矩控制，摆动相用位置控制）。

安全机制优先级

硬件保护：电机驱动器需配置过流/过热保护。

软件冗余：案例6的跌倒检测需设置多级阈值（如加速度+角度联合判断）。

电机驱动技术选择

方波驱动：案例4适用简单PWM+方向控制，但扭矩脉动较大。

FOC控制：案例5使用SimpleFOC库实现磁场定向控制，适合高动态性能需求（需配合编码器绝对位置）。

实际应用调试要点

参数标定：编码器脉冲当量（如0.05度/脉冲）需通过实际测量确定。

负载测试：膝关节控制需考虑机器人重心偏移的影响，建议逐步增加负载测试稳定性。

能量回收：高级系统可集成再生制动，在摆动相回收能量（需硬件支持）。

注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。