Arduino BLDC 自适应阻抗控制外骨骼机器人技术解析

4.2 基于 EMG 信号的肌电协同控制（上肢外骨骼）

#include<SimpleFOC.h>
#include<EMGFilters.h>

// 硬件配置
BLDCMotor motor(7);
BLDCDriver3PWM driver(9, 10, 11, 8);
Encoder encoder(2, 3, 500);
EMGFilters myFilter; // 肌电滤波器

// 肌电信号引脚
const int emgPin = A0;
float muscleActivation = 0;
float adaptiveGain = 1.0;

void setup() {
  Serial.begin(115200);
  myFilter.init(500); // 采样率 500Hz
  motor.linkDriver(&driver);
  motor.linkEncoder(&encoder);
  motor.init();
  motor.initFOC();
}

void loop() {
  // 1. 读取并处理肌电信号
  int rawEMG = analogRead(emgPin);
  float filteredEMG = myFilter.update(rawEMG);
  muscleActivation = constrain(map(filteredEMG, -1000, 1000, 0, 1), 0, 1);

  // 2. 自适应阻抗控制（肌电信号调制）
  float targetAngle = muscleActivation * 90.0; // 映射到 0-90 度范围
  float currentAngle = encoder.getAngle();
  float error = targetAngle - currentAngle;

  // 动态调整阻抗增益（肌肉激活越强，阻抗越低）
  adaptiveGain = 1.0 + 2.0 * muscleActivation; // 范围 1.0-3.0

  // 3. 带自适应增益的 PID 控制
  static float integral = 0;
  float derivative = error - (targetAngle - encoder.getPreviousAngle());
  integral += error * 0.01;
  float output = 1.5 * error + 0.1 * integral + 0.05 * derivative;
  output *= adaptiveGain; // 应用自适应增益
  motor.move(output);

  // 4. 安全限制
  if(abs(error) > 45) motor.setPhaseVoltage(0, 0); // 过大误差时停机

  // 5. 数据记录
  Serial.print("EMG: "); Serial.print(muscleActivation);
  Serial.print(" Angle: "); Serial.print(currentAngle);
  Serial.print(" Gain: "); Serial.println(adaptiveGain);
  delay(10);
}

4.3 基于运动意图预测的自适应控制（步态辅助外骨骼）

#include<SimpleFOC.h>
#include<KalmanFilter.h>

// 系统配置
BLDCMotor motor(7);
BLDCDriver3PWM driver(9, 10, 11, 8);
Encoder encoder(2, 3, 500);
KalmanFilter kf; // 用于预测关节运动意图

// 自适应参数
float stiffness = 0.8; // 初始刚度系数
float damping = 0.3;   // 初始阻尼系数
float lastPrediction = 0;

void setup() {
  Serial.begin(115200);
  // 卡尔曼滤波初始化（状态：位置 + 速度）
  kf.setState(0, 0); // 初始位置和速度
  kf.setProcessNoise(0.01);
  kf.setMeasurementNoise(0.1);
  motor.linkDriver(&driver);
  motor.linkEncoder(&encoder);
  motor.init();
  motor.initFOC();
}

void loop() {
  // 1. 获取传感器数据
  float currentPos = encoder.getAngle();
  float currentVel = encoder.getVelocity();

  // 2. 运动意图预测（卡尔曼滤波）
  kf.update(currentPos, currentVel);
  float predictedPos = kf.getState(0);
  float predictedVel = kf.getState(1);

  // 3. 自适应阻抗参数调整（基于预测）
  float predictionError = abs(predictedVel - lastPrediction);
  if(predictionError > 0.5) { // 检测到运动意图变化
    stiffness = 0.5; // 降低刚度以适应变化
    damping = 0.6;   // 增加阻尼抑制振荡
  } else {
    stiffness = 0.8; // 恢复默认值
    damping = 0.3;
  }
  lastPrediction = predictedVel;

  // 4. 阻抗控制计算（虚拟弹簧 - 阻尼系统）
  float desiredForce = stiffness * (predictedPos - currentPos) + damping * (predictedVel - currentVel);

  // 5. 电机控制（力→电流转换需根据电机参数校准）
  float currentRef = desiredForce * 0.5; // 简化转换系数（需实际标定）
  motor.move(currentRef);

  // 6. 调试输出
  Serial.print("Predicted Vel: "); Serial.print(predictedVel);
  Serial.print(" Stiffness: "); Serial.print(stiffness);
  Serial.print(" Output: "); Serial.println(currentRef);
  delay(10);
}

4.4 下肢步态自适应跟随外骨骼（楼梯/平路切换辅助）

场景说明：面向下肢行动障碍者，外骨骼需跟随人体自然步态，自适应切换平路、楼梯等不同运动模式，根据人体运动意图动态调节阻抗参数，实现平稳跟随不卡滞、跨越障碍不脱钩。

硬件配置：Arduino Mega 2560、TB6612FNG 双路 BLDC 驱动模块、足底压力传感器阵列、关节角度编码器、IMU（MPU6050）、应变式力传感器。

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

// 硬件引脚定义
#define HIP_LEFT_PWM 9
#define HIP_LEFT_DIR 8
#define KNEE_LEFT_PWM 10
#define KNEE_LEFT_DIR 7
#define HIP_RIGHT_PWM 11
#define HIP_RIGHT_DIR 12
#define KNEE_RIGHT_PWM 13
#define KNEE_RIGHT_DIR 14

// 传感器引脚
#define FOOT_PRESSURE_LEFT A0-A3
#define FOOT_PRESSURE_RIGHT A4-A7
#define HIP_ENCODER_LEFT 2, 3
#define KNEE_ENCODER_LEFT 4, 5
#define HIP_ENCODER_RIGHT 6, 7
#define KNEE_ENCODER_RIGHT 8, 9
#define FORCE_SENSOR_LEFT A8
#define FORCE_SENSOR_RIGHT A9

// MPU6050 对象
MPU6050 imu;

// 编码器对象
Encoder hipEncLeft(HIP_ENCODER_LEFT_A, HIP_ENCODER_LEFT_B);
Encoder kneeEncLeft(KNEE_ENCODER_LEFT_A, KNEE_ENCODER_LEFT_B);
Encoder hipEncRight(HIP_ENCODER_RIGHT_A, HIP_ENCODER_RIGHT_B);
Encoder kneeEncRight(KNEE_ENCODER_RIGHT_A, KNEE_ENCODER_RIGHT_B);

// 自适应阻抗控制参数
float adaptiveImpedanceKp = 80; // 基础刚度
float adaptiveImpedanceKd = 25; // 基础阻尼
float targetForce = 0;
float forceError = 0;
float angleTarget = 0;
float impedanceOutput = 0;

// 场景识别参数
int currentScene = 0; // 0:平路，1:楼梯，2:障碍
int footPressureThreshold = 600;
int stairPressureThreshold = 800;

// 状态变量
float hipAngleLeft = 0, kneeAngleLeft = 0;
float hipAngleRight = 0, kneeAngleRight = 0;
float footPressureLeft = 0, footPressureRight = 0;
float humanForceLeft = 0, humanForceRight = 0;
float imuTilt = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  imu.initialize();
  
  pinMode(HIP_LEFT_PWM, OUTPUT);
  pinMode(HIP_LEFT_DIR, OUTPUT);
  pinMode(KNEE_LEFT_PWM, OUTPUT);
  pinMode(KNEE_LEFT_DIR, OUTPUT);
  pinMode(HIP_RIGHT_PWM, OUTPUT);
  pinMode(HIP_RIGHT_DIR, OUTPUT);
  pinMode(KNEE_RIGHT_PWM, OUTPUT);
  pinMode(KNEE_RIGHT_DIR, OUTPUT);
  
  stopAllMotors();
}

void loop() {
  static unsigned long lastLoopTime = 0;
  if(millis() - lastLoopTime < 20) return; // 控制周期 20ms
  lastLoopTime = millis();

  readSensorData();
  identifyMovementScene();
  adjustImpedanceParameters();
  calculateImpedanceControl();
  executeMotorOutput();

  Serial.print("场景:"); Serial.print(currentScene);
  Serial.print(" 左髋角度:"); Serial.print(hipAngleLeft);
  Serial.print(" 左腿力:"); Serial.print(humanForceLeft);
  Serial.print(" 阻抗输出:"); Serial.println(impedanceOutput);
}

void readSensorData() {
  hipAngleLeft = hipEncLeft.read() / 1000.0 * 360;
  kneeAngleLeft = kneeEncLeft.read() / 1000.0 * 360;
  hipAngleRight = hipEncRight.read() / 1000.0 * 360;
  kneeAngleRight = kneeEncRight.read() / 1000.0 * 360;

  footPressureLeft = (analogRead(A0) + analogRead(A1) + analogRead(A2) + analogRead(A3)) / 4.0;
  footPressureRight = (analogRead(A4) + analogRead(A5) + analogRead(A6) + analogRead(A7)) / 4.0;

  humanForceLeft = map(analogRead(FORCE_SENSOR_LEFT), 0, 1023, -50, 50);
  humanForceRight = map(analogRead(FORCE_SENSOR_RIGHT), 0, 1023, -50, 50);

  Vector<float> aa = imu.getArex();
  imuTilt = aa.z;
}

void identifyMovementScene() {
  if(footPressureLeft > footPressureThreshold && footPressureRight < footPressureThreshold) currentScene = 0;
  else if(footPressureRight > footPressureThreshold && footPressureLeft < footPressureThreshold) currentScene = 0;
  else if(footPressureLeft > stairPressureThreshold && imuTilt > 5) currentScene = 1;
  else if(footPressureRight > stairPressureThreshold && imuTilt > 5) currentScene = 1;
}

void adjustImpedanceParameters() {
  if(currentScene == 0) {
    adaptiveImpedanceKp = 80 + map(abs(humanForceLeft), 0, 50, 0, 20);
    adaptiveImpedanceKd = 25 + map(abs(humanForceRight), 0, 50, 5, 15);
  } else if(currentScene == 1) {
    adaptiveImpedanceKp = 120 + map(abs(humanForceLeft), 0, 50, 10, 30);
    adaptiveImpedanceKd = 40 + map(abs(humanForceRight), 0, 50, 10, 20);
  } else {
    adaptiveImpedanceKp = 50;
    adaptiveImpedanceKd = 30;
  }
  adaptiveImpedanceKp = constrain(adaptiveImpedanceKp, 40, 150);
  adaptiveImpedanceKd = constrain(adaptiveImpedanceKd, 20, 50);
}

void calculateImpedanceControl() {
  if(currentScene == 0) angleTarget = hipAngleLeft + 30;
  else if(currentScene == 1) angleTarget = hipAngleLeft + 45;

  static float lastHipAngleLeft = 0;
  float dAngle = (angleTarget - hipAngleLeft) / 0.02;
  float lastDAngle = (lastHipAngleLeft - hipAngleLeft) / 0.02;

  forceError = humanForceLeft - impedanceOutput;
  impedanceOutput = adaptiveImpedanceKp * (angleTarget - hipAngleLeft) + adaptiveImpedanceKd * (dAngle - lastDAngle) + 0.5 * forceError;
  lastHipAngleLeft = hipAngleLeft;
}

void executeMotorOutput() {
  if(impedanceOutput > 0) {
    digitalWrite(HIP_LEFT_DIR, HIGH);
    analogWrite(HIP_LEFT_PWM, constrain(abs(impedanceOutput), 0, 255));
  } else {
    digitalWrite(HIP_LEFT_DIR, LOW);
    analogWrite(HIP_LEFT_PWM, constrain(abs(impedanceOutput), 0, 255));
  }
  // 右侧及膝关节逻辑省略，参考主逻辑
}

void stopAllMotors() {
  analogWrite(HIP_LEFT_PWM, 0);
  analogWrite(KNEE_LEFT_PWM, 0);
  analogWrite(HIP_RIGHT_PWM, 0);
  analogWrite(KNEE_RIGHT_PWM, 0);
}

4.5 上肢康复训练自适应阻抗外骨骼（主动助力模式）

场景说明：面向脑卒中或术后上肢康复患者，外骨骼需根据患者肢体运动能力自适应调节阻抗刚度，实现个性化康复训练。

硬件配置：Arduino Mega 2560、BLDC 伺服驱动器、肌电传感器（EMG）、关节角度编码器、扭矩传感器、OLED 显示屏。

#include<Wire.h>
#include<Adafruit_GFX.h>
#include<Adafruit_SSD1306.h>
#include<EMG_Sensor.h>

#define SHOULDER_PWM 9
#define SHOULDER_DIR 8
#define ELBOW_PWM 10
#define ELBOW_DIR 7
#define EMG_BICEPS A0
#define EMG_TRICEPS A1
#define TORQUE_SENSOR A2
#define SHOULDER_ENCODER 2, 3
#define ELBOW_ENCODER 4, 5
#define OLED_RESET 6

Adafruit_SSD1306 display(128, 64, &Wire, OLED_RESET);

float adaptiveKp = 60;
float adaptiveKd = 20;
float emgThreshold = 30;
float torqueTarget = 0;
int trainingMode = 1;
int muscleStrengthLevel = 0;

float shoulderAngle = 0, elbowAngle = 0;
float emgBiceps = 0, emgTriceps = 0;
float currentTorque = 0;

void setup() {
  Serial.begin(115200);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.display();
  
  pinMode(SHOULDER_PWM, OUTPUT);
  pinMode(SHOULDER_DIR, OUTPUT);
  pinMode(ELBOW_PWM, OUTPUT);
  pinMode(ELBOW_DIR, OUTPUT);
  
  Encoder shoulderEnc(SHOULDER_ENCODER_A, SHOULDER_ENCODER_B);
  Encoder elbowEnc(ELBOW_ENCODER_A, ELBOW_ENCODER_B);
  stopAllMotors();
  displayTrainingMode();
}

void loop() {
  static unsigned long lastLoopTime = 0;
  if(millis() - lastLoopTime < 30) return;
  lastLoopTime = millis();

  readRehabSensorData();
  evaluateMuscleStrength();
  adjustRehabImpedance();
  calculateRehabImpedanceControl();
  executeRehabMotorOutput();
  updateDisplay();
}

void readRehabSensorData() {
  emgBiceps = analogRead(EMG_BICEPS);
  emgTriceps = analogRead(EMG_TRICEPS);
  static float emgBicepsBuffer[5] = {0};
  static float emgTricepsBuffer[5] = {0};
  shiftArray(emgBicepsBuffer, 5, emgBiceps);
  shiftArray(emgTricepsBuffer, 5, emgTriceps);
  emgBiceps = averageArray(emgBicepsBuffer, 5);
  emgTriceps = averageArray(emgTricepsBuffer, 5);

  shoulderAngle = shoulderEnc.read() / 1000.0 * 360;
  elbowAngle = elbowEnc.read() / 1000.0 * 360;
  currentTorque = map(analogRead(TORQUE_SENSOR), 0, 1023, 0, 10);
}

void evaluateMuscleStrength() {
  if(emgBiceps + emgTriceps < emgThreshold) muscleStrengthLevel = 0;
  else if(emgBiceps + emgTriceps < 100) muscleStrengthLevel = 1;
  else if(emgBiceps + emgTriceps < 200) muscleStrengthLevel = 2;
  else if(emgBiceps + emgTriceps < 350) muscleStrengthLevel = 3;
  else if(emgBiceps + emgTriceps < 500) muscleStrengthLevel = 4;
  else muscleStrengthLevel = 5;
}

void adjustRehabImpedance() {
  switch(muscleStrengthLevel) {
    case 0: adaptiveKp = 30; adaptiveKd = 10; torqueTarget = 2; trainingMode = 1; break;
    case 1: adaptiveKp = 45; adaptiveKd = 15; torqueTarget = 3; trainingMode = 1; break;
    case 2: adaptiveKp = 60; adaptiveKd = 20; torqueTarget = 5; trainingMode = 1; break;
    case 3: adaptiveKp = 80; adaptiveKd = 25; torqueTarget = 7; trainingMode = 1; break;
    case 4: adaptiveKp = 100; adaptiveKd = 30; torqueTarget = 8; trainingMode = 1; break;
    case 5: adaptiveKp = 120; adaptiveKd = 35; torqueTarget = 10; trainingMode = 2; break;
  }
  adaptiveKp = constrain(adaptiveKp, 20, 130);
  adaptiveKd = constrain(adaptiveKd, 8, 40);
  torqueTarget = constrain(torqueTarget, 1, 12);
}

void calculateRehabImpedanceControl() {
  float shoulderAngleTarget = 60;
  float elbowAngleTarget = 90;
  float shoulderAngleError = shoulderAngleTarget - shoulderAngle;
  float elbowAngleError = elbowAngleTarget - elbowAngle;
  
  impedanceOutputShoulder = adaptiveKp * shoulderAngleError + adaptiveKd * (shoulderAngleError / 0.03);
  impedanceOutputElbow = adaptiveKp * elbowAngleError + adaptiveKd * (elbowAngleError / 0.03);
}

void executeRehabMotorOutput() {
  if(impedanceOutputShoulder > 0) {
    digitalWrite(SHOULDER_DIR, HIGH);
    analogWrite(SHOULDER_PWM, constrain(abs(impedanceOutputShoulder), 0, 255));
  } else {
    digitalWrite(SHOULDER_DIR, LOW);
    analogWrite(SHOULDER_PWM, constrain(abs(impedanceOutputShoulder), 0, 255));
  }
}

void updateDisplay() {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(10);
  display.setCursor(0, 0);
  display.print("模式:");
  display.print(trainingMode == 1 ? "主动助力" : "阻抗训练");
  display.display();
}

void shiftArray(float arr[], int len, float newVal) {
  for(int i = 0; i < len - 1; i++) arr[i] = arr[i + 1];
  arr[len - 1] = newVal;
}

float averageArray(float arr[], int len) {
  float sum = 0;
  for(int i = 0; i < len; i++) sum += arr[i];
  return sum / len;
}

void stopAllMotors() {
  analogWrite(SHOULDER_PWM, 0);
  analogWrite(ELBOW_PWM, 0);
}

4.6 负重搬运自适应阻抗外骨骼（柔性支撑 + 抗扰动）

场景说明：面向物流搬运、高空作业等负重场景，外骨骼需自适应支撑不同重量的负载，实现'负载越重，支撑刚度越高；负载越轻，支撑柔顺性越好'。

硬件配置：Arduino Mega 2560、高扭矩 BLDC 驱动器、称重传感器、关节扭矩传感器、压力传感器、倾角传感器。

#include<Wire.h>
#include<HX711.h>
#include<Adafruit_LIS3DH.h>

#define HIP_PWM 9
#define HIP_DIR 8
#define KNEE_PWM 10
#define KNEE_DIR 7
#define WEIGHT_SENSOR_DT 2
#define WEIGHT_SENSOR_SCK 3
#define TORQUE_SENSOR_HIP A0
#define TORQUE_SENSOR_KNEE A1
#define FOOT_PRESSURE_LEFT A2
#define FOOT_PRESSURE_RIGHT A3

Adafruit_LIS3DH lis;
HX711 weightSensor;

float loadAdaptiveKp = 50;
float loadAdaptiveKd = 20;
float loadWeight = 0;
float targetSupportTorque = 0;
float disturbanceTilt = 0;

void setup() {
  Serial.begin(115200);
  weightSensor.begin(WEIGHT_SENSOR_DT, WEIGHT_SENSOR_SCK);
  weightSensor.set_scale(1.0);
  lis.begin();
  lis.setRange(LIS3DH_RANGE_8_G);
  
  pinMode(HIP_PWM, OUTPUT);
  pinMode(HIP_DIR, OUTPUT);
  pinMode(KNEE_PWM, OUTPUT);
  pinMode(KNEE_DIR, OUTPUT);
  stopAllMotors();
}

void loop() {
  static unsigned long lastLoopTime = 0;
  if(millis() - lastLoopTime < 25) return;
  lastLoopTime = millis();

  readLoadAndDisturbanceData();
  calculateTargetSupportTorque();
  adjustLoadImpedanceParameters();
  calculateLoadImpedanceControl();
  executeLoadMotorOutput();

  Serial.print("负载:"); Serial.print(loadWeight);
  Serial.print(" 倾角扰动:"); Serial.print(disturbanceTilt);
  Serial.print(" 髋输出:"); Serial.print(impedanceOutputHip);
  Serial.print(" 膝输出:"); Serial.println(impedanceOutputKnee);
}

void readLoadAndDisturbanceData() {
  loadWeight = weightSensor.read_average(10) / 100.0;
  loadWeight = constrain(loadWeight, 0, 50);

  float ax = lis.getAcceleration('x');
  float ay = lis.getAcceleration('y');
  disturbanceTilt = atan2(ay, ax) * 180 / PI;
  disturbanceTilt = constrain(disturbanceTilt, -30, 30);
}

void calculateTargetSupportTorque() {
  targetSupportTorque = loadWeight * 2.5;
  targetSupportTorque = constrain(targetSupportTorque, 0, 125);
}

void adjustLoadImpedanceParameters() {
  loadAdaptiveKp = 50 + loadWeight * 2;
  loadAdaptiveKd = 20 + loadWeight * 0.8;
  if(abs(disturbanceTilt) > 5) loadAdaptiveKd += abs(disturbanceTilt) * 2;
  loadAdaptiveKp = constrain(loadAdaptiveKp, 40, 150);
  loadAdaptiveKd = constrain(loadAdaptiveKd, 15, 50);
}

void calculateLoadImpedanceControl() {
  float hipTorqueError = targetSupportTorque * 0.6;
  float kneeTorqueError = targetSupportTorque * 0.4;
  float tiltCompensation = disturbanceTilt * 0.5;
  
  impedanceOutputHip = loadAdaptiveKp * hipTorqueError + loadAdaptiveKd * (hipTorqueError / 0.025) + tiltCompensation;
  impedanceOutputKnee = loadAdaptiveKp * kneeTorqueError + loadAdaptiveKd * (kneeTorqueError / 0.025) + tiltCompensation;
  
  impedanceOutputHip = constrain(impedanceOutputHip, -255, 255);
  impedanceOutputKnee = constrain(impedanceOutputKnee, -255, 255);
}

void executeLoadMotorOutput() {
  if(impedanceOutputHip > 0) {
    digitalWrite(HIP_DIR, HIGH);
    analogWrite(HIP_PWM, constrain(abs(impedanceOutputHip), 0, 255));
  } else {
    digitalWrite(HIP_DIR, LOW);
    analogWrite(HIP_PWM, constrain(abs(impedanceOutputHip), 0, 255));
  }
}

void stopAllMotors() {
  analogWrite(HIP_PWM, 0);
  analogWrite(KNEE_PWM, 0);
}

5. 要点解读

1. 多源传感器融合：自适应控制的前提基础。需通过多源传感器融合实现全方位感知，包括足底压力、关节角度、人机交互力、IMU 姿态、肌电、扭矩等。数据需滤波与校准，采样周期需匹配控制周期。
2. 自适应阻抗参数调节：动态工况的核心适配逻辑。根据外部条件（场景、肌力、负载）动态调整阻抗参数（刚度 Kp、阻尼 Kd），加入扰动补偿机制，设置参数限幅与安全边界。
3. 闭环反馈控制：动态跟随的核心保障。构建'传感器反馈 - 控制算法 - 执行输出'的闭环，明确闭环目标，实时计算误差，优化 PID 与实时计算。
4. 实时性与控制周期：外骨骼控制的性能约束。MCU 性能需适配（推荐 Arduino Mega 2560），控制周期精准控制（使用 millis 差值替代 delay），中断与主循环分工。
5. 安全性与人体协同：外骨骼设计的底线原则。硬件安全冗余（限位开关、过流保护），力控制与柔顺性设计，异常停机机制。

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