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 驱动模块（驱动左/右髋关节、膝关节电机）
• 足底压力传感器阵列（每足 4 个，监测足底接触力，识别平路/楼梯踩踏意图）
• 关节角度编码器（1000 线增量式，安装在髋/膝关节，监测人体关节角度）
• 惯性测量单元（IMU，MPU6050，监测人体姿态角，判断运动稳定性）
• 应变式力传感器（粘贴在膝关节连杆，监测人机交互力，判断跟随偏差）

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

// 硬件引脚定义
#define HIP_LEFT_PWM 9 // 左髋关节 BLDC PWM
#define HIP_LEFT_DIR 8 // 左髋关节方向
#define KNEE_LEFT_PWM 10 // 左膝关节 BLDC PWM
#define KNEE_LEFT_DIR 7 // 左膝关节方向
#define HIP_RIGHT_PWM 11 // 右髋关节 BLDC PWM
#define HIP_RIGHT_DIR 12 // 右髋关节方向
#define KNEE_RIGHT_PWM 13 // 右膝关节 BLDC PWM
#define KNEE_RIGHT_DIR 14 // 右膝关节方向

// 传感器引脚
#define FOOT_PRESSURE_LEFT A0-A3 // 左足压力传感器（4 路模拟输入）
#define FOOT_PRESSURE_RIGHT A4-A7 // 右足压力传感器
#define HIP_ENCODER_LEFT 2,3 // 左髋关节编码器（A/B 相）
#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; // 阻抗控制输出（电机 PWM 占空比）

// 场景识别参数
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(); // 初始化 IMU
  // 初始化电机引脚
  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();

  // 1. 多源数据采集：人体运动意图与环境感知
  readSensorData();
  // 2. 运动场景自适应识别
  identifyMovementScene();
  // 3. 自适应阻抗参数调整
  adjustImpedanceParameters();
  // 4. 阻抗控制计算（力跟随）
  calculateImpedanceControl();
  // 5. BLDC 电机输出执行
  executeMotorOutput();
  // 6. 串口调试反馈
  Serial.print("场景:"); Serial.print(currentScene);
  Serial.print(" 左髋角度:"); Serial.print(hipAngleLeft);
  Serial.print(" 左腿力:"); Serial.print(humanForceLeft);
  Serial.print(" 阻抗输出:"); Serial.println(impedanceOutput);
}

// 读取所有传感器数据
void readSensorData(){
  // 编码器读取关节角度（转换为角度值，编码器每转脉冲数 1000）
  hipAngleLeft = hipEncLeft.read()/1000.0*360;
  kneeAngleLeft = kneeEncLeft.read()/1000.0*360;
  hipAngleRight = hipEncRight.read()/1000.0*360;
  kneeAngleRight = kneeEncRight.read()/1000.0*360;

  // 足底压力传感器（4 路均值，模拟量 0-1023 对应 0-100N）
  footPressureLeft = (analogRead(FOOT_PRESSURE_LEFT_0)+analogRead(FOOT_PRESSURE_LEFT_1)+analogRead(FOOT_PRESSURE_LEFT_2)+analogRead(FOOT_PRESSURE_LEFT_3))/4.0;
  footPressureRight = (analogRead(FOOT_PRESSURE_RIGHT_0)+analogRead(FOOT_PRESSURE_RIGHT_1)+analogRead(FOOT_PRESSURE_RIGHT_2)+analogRead(FOOT_PRESSURE_RIGHT_3))/4.0;

  // 人机交互力传感器（应变式，模拟量转换为力值）
  humanForceLeft = map(analogRead(FORCE_SENSOR_LEFT),0,1023,-50,50); // 正力为人体主动发力，负力为抵抗
  humanForceRight = map(analogRead(FORCE_SENSOR_RIGHT),0,1023,-50,50);

  // IMU 获取人体姿态角（倾斜角）
  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);
}

// 阻抗控制核心计算（力跟随模型：F = Kp*(θ_target - θ) + Kd*(dθ_target - dθ)/dt）
void calculateImpedanceControl(){
  // 目标角度：基于当前关节角度与人体意图，确定合理的运动目标角度
  // 平路时跟随当前角度（随动），楼梯时增加角度步幅
  if(currentScene == 0){
    angleTarget = hipAngleLeft + 30; // 平路髋关节目标角度比当前大 30°，实现随动
  }else if(currentScene == 1){
    angleTarget = hipAngleLeft + 45; // 楼梯时步幅增大，提高抬腿高度
  }

  // 计算角度误差与角速度误差（编码器采样频率 50Hz，dt=20ms=0.02s）
  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));
  }

  // 右侧髋关节同步跟随（相位滞后 30°，对应步态周期）
  float rightImpedance = impedanceOutput *cos(30* PI /180);
  if(rightImpedance > 0){
    digitalWrite(HIP_RIGHT_DIR, HIGH);
    analogWrite(HIP_RIGHT_PWM,constrain(abs(rightImpedance),0,255));
  }else{
    digitalWrite(HIP_RIGHT_DIR, LOW);
    analogWrite(HIP_RIGHT_PWM,constrain(abs(rightImpedance),0,255));
  }

  // 膝关节控制：根据髋关节运动角度，按比例输出（简化模型，实际需单独阻抗计算）
  float kneeOutput = impedanceOutput *0.6; // 膝关节扭矩为髋关节的 60%
  if(kneeOutput > 0){
    digitalWrite(KNEE_LEFT_DIR, HIGH);
    analogWrite(KNEE_LEFT_PWM,constrain(abs(kneeOutput),0,255));
  }else{
    digitalWrite(KNEE_LEFT_DIR, LOW);
    analogWrite(KNEE_LEFT_PWM,constrain(abs(kneeOutput),0,255));
  }

  // 右侧膝关节同步控制
  float rightKneeOutput = kneeOutput *cos(30* PI /180);
  if(rightKneeOutput > 0){
    digitalWrite(KNEE_RIGHT_DIR, HIGH);
    analogWrite(KNEE_RIGHT_PWM,constrain(abs(rightKneeOutput),0,255));
  }else{
    digitalWrite(KNEE_RIGHT_DIR, LOW);
    analogWrite(KNEE_RIGHT_PWM,constrain(abs(rightKneeOutput),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，2 通道，监测上肢肱二头肌、肱三头肌肌电信号，识别患者发力意图）
• 关节角度编码器（1000 线增量式，安装于肩、肘关节）
• 扭矩传感器（应变式，监测人机交互扭矩，判断患者发力程度）
• 触控显示屏（可选，显示训练模式与实时参数）

#include<Wire.h>
#include<Adafruit_GFX.h>
#include<Adafruit_SSD1306.h>
#include<EMG_Sensor.h>// 假设的肌电传感器驱动库

// 硬件引脚定义
#define SHOULDER_PWM 9 // 肩关节 BLDC PWM
#define SHOULDER_DIR 8 // 肩关节方向
#define ELBOW_PWM 10 // 肘关节 BLDC PWM
#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 // 肘关节编码器

// OLED 显示屏
#define OLED_RESET 6
Adafruit_SSD1306 display(128,64,&Wire, OLED_RESET);

// 自适应阻抗控制参数
float adaptiveKp = 60; // 基础刚度
float adaptiveKd = 20; // 基础阻尼
float emgThreshold = 30; // 肌电发力阈值（区分主动发力与完全无力）
float torqueTarget = 0; // 目标训练扭矩
float torqueError = 0; // 扭矩误差

// 康复训练参数
int trainingMode = 1; // 1:主动助力，2:阻抗训练
int muscleStrengthLevel = 0; // 肌力等级：0-5（0 为完全无力，5 为正常发力）

// 状态变量
float shoulderAngle = 0, elbowAngle = 0;
float emgBiceps = 0, emgTriceps = 0;
float currentTorque = 0;
float impedanceOutputShoulder = 0, impedanceOutputElbow = 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; // 控制周期 30ms，兼顾肌电信号采样
  lastLoopTime = millis();

  // 1. 采集肌电信号、扭矩、关节角度
  readRehabSensorData();
  // 2. 评估患者肌力等级
  evaluateMuscleStrength();
  // 3. 自适应调节阻抗参数（基于肌力等级）
  adjustRehabImpedance();
  // 4. 康复阻抗控制计算（扭矩闭环）
  calculateRehabImpedanceControl();
  // 5. 电机输出执行
  executeRehabMotorOutput();
  // 6. 显示与反馈
  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;

  // 扭矩传感器读取（映射为实际扭矩值，0-1023 对应 0-10N·m）
  currentTorque = map(analogRead(TORQUE_SENSOR),0,1023,0,10);
}

// 评估患者肌力等级（0-5 级）
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(){
  // 肩关节目标角度：设定肩关节前屈目标角度为 60°，作为训练轨迹
  float shoulderAngleTarget = 60;
  // 肘关节目标角度：设定肘关节屈曲目标角度为 90°
  float elbowAngleTarget = 90;

  // 计算角度误差与扭矩误差
  float shoulderAngleError = shoulderAngleTarget - shoulderAngle;
  float elbowAngleError = elbowAngleTarget - elbowAngle;
  torqueError = torqueTarget - currentTorque;

  // 阻抗控制输出：结合角度误差与扭矩误差，实现力 - 位置协同
  impedanceOutputShoulder = adaptiveKp * shoulderAngleError + adaptiveKd *(shoulderAngleError /0.03)+0.3* torqueError;
  impedanceOutputElbow = adaptiveKp * elbowAngleError + adaptiveKd *(elbowAngleError /0.03)+0.3* torqueError;

  // 阻抗训练模式下，反向施加阻抗力（患者发力越大，阻抗越大）
  if(trainingMode == 2){
    impedanceOutputShoulder = -adaptiveKp *(shoulderAngle - shoulderAngleTarget)- adaptiveKd *((shoulderAngle - shoulderAngleTarget)/0.03);
    impedanceOutputElbow = -adaptiveKp *(elbowAngle - elbowAngleTarget)- adaptiveKd *((elbowAngle - elbowAngleTarget)/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));
  }

  // 肘关节控制（同步跟随肩关节，简化训练轨迹）
  if(impedanceOutputElbow > 0){
    digitalWrite(ELBOW_DIR, HIGH);
    analogWrite(ELBOW_PWM,constrain(abs(impedanceOutputElbow),0,255));
  }else{
    digitalWrite(ELBOW_DIR, LOW);
    analogWrite(ELBOW_PWM,constrain(abs(impedanceOutputElbow),0,255));
  }
}

// 更新显示屏信息
void updateDisplay(){
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(10);
  display.setCursor(0,0);
  display.print("模式:");
  display.print(trainingMode == 1?"主动助力":"阻抗训练");
  display.setCursor(0,16);
  display.print("肌力等级:");
  display.print(muscleStrengthLevel);
  display.setCursor(0,32);
  display.print("肩角度:");
  display.print(shoulderAngle);
  display.setCursor(0,48);
  display.print("扭矩:");
  display.print(currentTorque);
  display.display();
}

// 显示训练模式
void displayTrainingMode(){
  display.clearDisplay();
  display.setTextSize(16);
  display.setCursor(0,20);
  display.print("康复训练");
  display.setCursor(0,40);
  display.print("模式就绪");
  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 // 髋关节 BLDC PWM
#define HIP_DIR 8 // 髋关节方向
#define KNEE_PWM 10 // 膝关节 BLDC PWM
#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;

// 自适应阻抗控制参数
float loadAdaptiveKp = 50; // 基础刚度（随负载自适应调整）
float loadAdaptiveKd = 20; // 基础阻尼
float loadWeight = 0; // 实时负载重量
float targetSupportTorque = 0; // 目标支撑扭矩
float supportTorqueError = 0; // 支撑扭矩误差
float disturbanceTilt = 0; // 扰动倾角

// 状态变量
float hipTorque = 0, kneeTorque = 0;
float leftFootPressure = 0, rightFootPressure = 0;
float impedanceOutputHip = 0, impedanceOutputKnee = 0;
HX711 weightSensor;

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); // 设置量程为±8G
  // 初始化电机引脚
  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; // 控制周期 25ms，平衡称重与倾角采样
  lastLoopTime = millis();

  // 1. 负载与扰动感知数据采集
  readLoadAndDisturbanceData();
  // 2. 自适应支撑扭矩计算（基于负载重量）
  calculateTargetSupportTorque();
  // 3. 自适应阻抗参数调整（负载 + 扰动双自适应）
  adjustLoadImpedanceParameters();
  // 4. 柔性支撑阻抗控制计算
  calculateLoadImpedanceControl();
  // 5. BLDC 电机输出执行（柔性支撑）
  executeLoadMotorOutput();
  // 6. 串口监控反馈
  Serial.print("负载:"); Serial.print(loadWeight);
  Serial.print(" 倾角扰动:"); Serial.print(disturbanceTilt);
  Serial.print(" 髋输出:"); Serial.print(impedanceOutputHip);
  Serial.print(" 膝输出:"); Serial.println(impedanceOutputKnee);
}

// 读取负载重量与扰动数据
void readLoadAndDisturbanceData(){
  // 称重传感器读取实时负载（单位：kg，经校准后直接输出重量）
  loadWeight = weightSensor.read_average(10)/100.0; // 取 10 次平均，转换为 kg
  loadWeight = constrain(loadWeight,0,50); // 负载上限 50kg，避免超载

  // 关节扭矩传感器读取（扭矩值，0-1023 对应 0-50N·m）
  hipTorque = map(analogRead(TORQUE_SENSOR_HIP),0,1023,0,50);
  kneeTorque = map(analogRead(TORQUE_SENSOR_KNEE),0,1023,0,50);

  // 足底压力传感器读取（判断人体姿态扰动，如晃动时压力失衡）
  leftFootPressure = analogRead(FOOT_PRESSURE_LEFT);
  rightFootPressure = analogRead(FOOT_PRESSURE_RIGHT);

  // 倾角传感器读取扰动倾角（加速度计转换为倾角，简化模型）
  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; // 扭矩系数，2.5N·m/kg，可根据实际情况校准
  targetSupportTorque = constrain(targetSupportTorque,0,125); // 扭矩上限 125N·m
}

// 根据负载与扰动自适应调整阻抗参数
void adjustLoadImpedanceParameters(){
  // 负载自适应：负载越重，刚度和阻尼越高，支撑越强
  loadAdaptiveKp = 50+ loadWeight * 2;
  loadAdaptiveKd = 20+ loadWeight * 0.8;

  // 扰动自适应：倾角扰动越大，增加阻尼抑制晃动，提高抗扰动能力
  if(abs(disturbanceTilt)>5){
    loadAdaptiveKd += abs(disturbanceTilt)*2;
  }

  // 足底压力失衡自适应：双足压力差超过阈值，增加关节刚度，提高稳定性
  if(abs(leftFootPressure - rightFootPressure)>150){
    loadAdaptiveKp += 20;
  }

  // 参数限幅，确保电机安全与人体舒适
  loadAdaptiveKp = constrain(loadAdaptiveKp,40,150);
  loadAdaptiveKd = constrain(loadAdaptiveKd,15,50);
}

// 柔性支撑阻抗控制计算（扭矩闭环，基于目标支撑扭矩与实际扭矩误差）
void calculateLoadImpedanceControl(){
  // 髋关节支撑扭矩误差
  float hipTorqueError = targetSupportTorque * 0.6- hipTorque; // 髋关节承担 60% 负载
  // 膝关节支撑扭矩误差
  float kneeTorqueError = targetSupportTorque * 0.4- kneeTorque; // 膝关节承担 40% 负载

  // 倾角扰动补偿：倾角扰动带来的附加扭矩误差，加入阻抗计算抵消扰动
  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));
  }

  // 膝关节控制：同步髋关节，保持支撑姿态平衡
  if(impedanceOutputKnee > 0){
    digitalWrite(KNEE_DIR, HIGH);
    analogWrite(KNEE_PWM,constrain(abs(impedanceOutputKnee),0,255));
  }else{
    digitalWrite(KNEE_DIR, LOW);
    analogWrite(KNEE_PWM,constrain(abs(impedanceOutputKnee),0,255));
  }
}

// 全电机停止
void stopAllMotors(){
  analogWrite(HIP_PWM,0);
  analogWrite(KNEE_PWM,0);
}

要点解读

1. 多源传感器融合：自适应控制的前提基础

自适应阻抗控制的核心是'精准感知外部条件（人体意图、环境、负载）'，单传感器无法全面反映动态工况，需通过多源传感器融合实现全方位感知：

• 感知维度互补：案例 1 融合足底压力、关节角度、人机交互力、IMU 姿态，覆盖运动意图、环境状态；案例 2 融合肌电、扭矩、关节角度，区分患者发力程度；案例 3 融合称重、扭矩、足底压力、倾角，识别负载与扰动，多维度数据交叉验证，避免单一传感器误判（如足底压力误判运动场景）。
• 数据滤波与校准：传感器原始数据存在噪声与误差，需通过移动平均、卡尔曼滤波等算法滤波，同时对传感器进行校准，确保数据准确性，为自适应调节提供可靠依据。
• 采样周期匹配：不同传感器采样需求不同，需匹配控制周期，避免高频传感器数据溢出或低频数据滞后，保证实时性与数据有效性，匹配外骨骼的动态响应速度。

2. 自适应阻抗参数调节：动态工况的核心适配逻辑

自适应的本质是根据外部条件（场景、肌力、负载）动态调整阻抗参数（刚度 Kp、阻尼 Kd），使外骨骼在不同工况下兼顾'跟随性、支撑性、柔顺性'，避免刚性或过软的极端输出：

• 参数与工况正相关：案例 4 中，楼梯场景刚度＞平路刚度，确保跨越障碍的稳定性；案例 5 中，肌力等级越高，刚度越高，减少辅助、提升训练强度；案例 6 中，负载越重，刚度和阻尼越高，增强支撑能力，参数与工况形成明确的正相关映射，符合物理规律。
• 加入扰动补偿机制：除了常规工况，需考虑扰动因素，如案例 6 加入倾角扰动补偿、足底压力失衡补偿，通过额外增加阻尼或刚度抵消扰动，提升外骨骼抗干扰能力，适应地面不平、人体晃动等复杂环境。
• 参数限幅与安全边界：所有自适应参数必须设置安全边界，避免刚度过高导致关节碰撞，或刚度过低导致负载失控，确保硬件安全与人体舒适，这是自适应控制的底线约束。

3. 闭环反馈控制：动态跟随的核心保障

自适应阻抗控制需构建'传感器反馈 - 控制算法 - 执行输出'的闭环，确保输出跟随目标变化，弥补开环控制的偏差：

• 闭环目标明确：不同场景闭环目标不同，案例 4 是力 - 位置闭环，跟随人体运动意图；案例 5 是扭矩闭环，匹配训练强度；案例 6 是扭矩 - 扰动闭环，实现柔性支撑，闭环目标与场景需求一一对应，避免目标混淆。
• 误差动态计算：闭环的核心是实时计算误差，误差越小，输出越接近目标，误差越大，输出调节幅度越大，使输出始终逼近目标，形成动态调节。
• PID 优化与实时计算：纯比例控制存在稳态误差，案例中引入积分和微分环节，积分消除稳态误差，微分抑制超调，提升动态响应，同时在中断中实时计算输出，避免 delay 导致的延迟，保证闭环实时性。

4. 实时性与控制周期：外骨骼控制的性能约束

外骨骼需跟随人体快速运动，实时性是核心约束，控制周期过长会导致滞后，无法及时响应人体意图或环境变化：

• MCU 性能适配：标准 Arduino Uno 主频低，难以满足高频控制需求，三个案例均采用 Arduino Mega 2560，具备更多引脚与更快运算速度，支持多传感器并发采样与实时计算，避免 MCU 性能瓶颈。
• 控制周期精准控制：采用 millis 差值替代 delay 控制周期，确保周期稳定，同时在周期内完成数据采集、参数调节、控制计算、电机输出全流程，控制周期的设定要匹配人体运动的响应时间，过短增加 MCU 负担，过长导致响应滞后，平衡实时性与运算负载。
• 中断与主循环分工：关键任务如编码器脉冲计数、外部中断触发，放在中断中执行，避免被主循环阻塞，主循环专注控制逻辑，确保高频任务的实时响应，这是保障实时性的关键架构设计。

5. 安全性与人体协同：外骨骼设计的底线原则

外骨骼直接作用于人体，安全性是首要原则，需从硬件、软件、控制逻辑多层面保障，同时实现人机协同的柔顺性：

• 硬件安全冗余：硬件上加装限位开关、过流保护电路，软件上设置电机输出限幅、负载上限、倾角限幅，案例 3 限制负载上限、倾角范围，避免超载或倾角过大导致倾倒，双重冗余确保极端情况下的安全。
• 力控制与柔顺性设计：避免刚性输出，通过阻抗控制实现力闭环，案例 4、5 通过力/扭矩误差调节输出，使外骨骼输出力跟随人体发力，实现柔顺跟随，案例 6 根据负载调节刚度，避免刚性支撑导致的人体疲劳，提升人机协同舒适性。
• 异常停机机制：当传感器故障、通信中断、负载超载等异常发生时，立即触发紧急停机，切断电机输出，这是最后一道安全防线，在三个案例中均预留紧急停机接口，确保异常时快速保护人体与设备。

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