基于 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; // 阻抗控制输出（电机 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）、关节角度编码器、扭矩传感器、触控显示屏。

#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"

// 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
#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;

// 自适应阻抗控制参数
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);
}

5. 要点解读

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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