基于 Arduino 平台实现 BLDC 机器人精密双向位置自适应动态控制，代表了开源硬件在运动控制领域的高阶应用。该系统不仅仅是简单的“点到点”移动，而是融合了精密传感、高级控制算法、实时系统辨识以及机械特性匹配的复杂机电一体化工程。

1、主要特点

1. 精密双向位置闭环 (High-Precision Bi-Directional Loop)
   这是系统的基础，确保执行器能在正反两个方向上达到微米级或角秒级的定位精度。
   高分辨率反馈：摒弃低分辨率的电位器或普通霍尔传感器，采用 12-14 位甚至更高分辨率的绝对式磁性编码器（如 AS5600, MA732）或高 PPR 增量式编码器。这能提供平滑且精确的位置反馈信号，消除量化误差导致的抖动。
   双向运动平滑性：在方向切换点（例如从正向运动突然反转），传统系统容易产生冲击或丢步。该系统通过 S 型加减速曲线（S-curve Trajectory） 规划，对“加加速度”进行限制，确保加速度连续变化，从而消除机械冲击，实现“零抖动”换向。
2. 自适应控制算法 (Adaptive Control Algorithm)
   这是系统的“大脑”，使其能够应对负载变化和外部干扰。
   负载惯量辨识：机器人的负载（如抓取不同重量的物体）会改变系统的转动惯量。自适应算法能够在线估算当前的负载特性。例如，当检测到负载变大、响应变慢时，系统自动调整控制参数。
   参数自整定：传统的固定 PID 参数难以在全工况下保持最优。系统采用自适应 PID 或 模糊 PID 策略。当负载变化或高速/低速切换时，控制器能自动调整比例增益、积分增益和微分增益，以维持系统的阻尼特性和响应速度，防止因参数不匹配导致的振荡或迟滞。
3. 动态前馈补偿 (Dynamic Feedforward Compensation)
   为了实现高动态响应（快速启停、高速跟踪），仅靠反馈控制是不够的。
   速度/加速度前馈：系统不仅根据位置误差进行修正（反馈），还会根据目标轨迹的速度和加速度进行提前预测（前馈）。通过引入前馈项，控制器可以在扰动发生之前就施加相应的控制量，大幅减小跟踪误差，提升系统的“刚性”。
   摩擦力补偿：在低速运动时，库仑摩擦力和粘滞摩擦力是导致运动不平滑的主要原因。自适应控制算法会学习并补偿这些非线性摩擦力，特别是在死区区域，确保低速运动的连续性。

2、应用场景
精密装配与半导体制造
在微型零件装配、芯片贴装或光学对准任务中，机器人需要以微米级的精度进行双向插拔或旋转动作。自适应控制能确保在不同力度接触工件时，依然保持位置的精确性，避免损坏精密元件。
医疗手术辅助机器人
例如显微外科手术机器人或康复训练机器人。在康复场景中，机器人的关节需要根据患者的肢体力量（负载）变化，自适应地调整助力大小和位置跟随精度，确保运动过程对人体是安全且平滑的。
高精度 3D 打印与激光加工
在大幅面或高精度打印/切割中，打印头或激光头需要在高速往复运动中保持极高的位置精度。自适应动态控制能消除皮带传动的弹性变形和惯量变化带来的误差，防止打印层纹或切割偏差。
科研级云台与天文望远镜
用于卫星信号追踪或深空观测的云台，需要极高的角度定位精度和极低的角速度波动。自适应控制能抵消风阻或结构变形带来的干扰，确保视场的绝对稳定。

3、 注意事项
硬件平台的性能瓶颈
算力要求：精密的 S 曲线规划、高阶 PID 运算以及自适应算法计算量巨大。Arduino Uno/Nano 等 8 位单片机完全无法胜任。必须选用 32 位高性能 MCU，如 Teensy 4.0/4.1（600MHz Cortex-M7）、ESP32（双核）或 STM32H7/F7 系列，以保证控制回路的实时性（建议位置环频率 ≥ 1kHz）。
传感器安装：必须采用输出轴端编码器（Output-side Encoder）。若仅在电机轴端安装编码器，无法检测到减速器的背隙（Backlash）和弹性变形，会导致实际末端位置出现误差，使闭环控制失效。
控制算法的复杂性与调试
参数耦合：自适应算法的参数（如自适应率、遗忘因子等）与 PID 参数相互耦合，调试难度极大。通常需要借助 MATLAB/Simulink 进行系统辨识和仿真，再将参数移植到 Arduino 上进行实物调试。
噪声抑制：高分辨率编码器的数据可能包含噪声，在计算微分项（D 项）时会被放大，导致电机抖动。必须在硬件（RC 低通滤波）和软件（数字滤波算法）层面进行降噪处理。
机械传动系统的匹配
背隙与刚性：精密控制对机械结构要求极高。应选用谐波减速器或零背隙行星减速器。普通减速器的背隙会直接转化为位置控制的死区，导致系统在换向时出现明显的滞后和冲击。
共振抑制：在高速自适应控制下，机械结构的固有频率容易被激发，引发共振。需要在控制算法中加入陷波滤波器（Notch Filter），或在机械结构上增加阻尼材料。
电源与热管理
动态电流响应：自适应控制在应对突加负载时，会瞬间输出大电流。电源必须具备足够的动态响应能力，防止电压跌落。
持续温升：由于系统频繁进行高加速度运动，电机和驱动器发热量大。必须做好散热设计（散热片、风扇），并考虑温度对电机电阻和传感器精度的影响（热漂移）。

1、基于编码器+PID的机器人关节位置控制（串口指令）

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

// 电机与编码器配置
#define PWM_PIN 9
#define DIR_PIN 8
Encoder enc(2, 3);  // 编码器A/B相

// PID参数
double targetPos = 0, currentPos = 0, output = 0;
double Kp = 5.0, Ki = 0.1, Kd = 0.5;
PID posPID(&currentPos, &output, &targetPos, Kp, Ki, Kd, DIRECT);

void setup() {
  pinMode(PWM_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  Serial.begin(115200);
  
  posPID.SetMode(AUTOMATIC);
  posPID.SetOutputLimits(-255, 255); // 支持双向PWM
  posPID.SetSampleTime(10);          // 10ms控制周期
}

void loop() {
  // 串口接收目标位置（单位：编码器脉冲数）
  if (Serial.available()) {
    targetPos = Serial.parseFloat();
    Serial.print("新目标位置: ");
    Serial.println(targetPos);
  }

  // 读取编码器位置
  currentPos = enc.read();
  
  // 计算PID输出
  posPID.Compute();
  
  // 驱动电机（带方向控制）
  if (output > 0) {
    digitalWrite(DIR_PIN, HIGH);
    analogWrite(PWM_PIN, abs(output));
  } else {
    digitalWrite(DIR_PIN, LOW);
    analogWrite(PWM_PIN, abs(output));
  }

  // 调试输出（可选）
  static unsigned long lastDebug = 0;
  if (millis() - lastDebug > 100) {
    lastDebug = millis();
    Serial.print("当前位置:"); Serial.print(currentPos);
    Serial.print(" 输出PWM:"); Serial.println(output);
  }
}

2、基于加速度前馈的轨迹跟踪控制（多段位置指令）

#include <Encoder.h>
#include <SimpleFOC.h>

// 电机配置
BLDCMotor motor = BLDCMotor(7);  // 7极对数
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
Encoder encoder = Encoder(2, 3, 2000); // 2000 CPR

// 自适应控制参数
float targetPos = 0;
float velocityFF = 0.3;  // 前馈速度增益
float accelFF = 0.1;     // 前馈加速度增益

void setup() {
  Serial.begin(115200);
  
  // 初始化硬件
  encoder.init();
  motor.linkSensor(&encoder);
  driver.init();
  motor.linkDriver(&driver);
  
  // 配置PID
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 5.0;
  motor.PID_position.P = 1.0; // 位置环PID
  
  // 启用位置控制模式
  motor.controller = MotionControlType::position;
  motor.init();
  motor.initFOC();
}

void loop() {
  // 模拟多段轨迹指令（如机械臂路径点）
  static unsigned long lastStep = 0;
  if (millis() - lastStep > 2000) { // 每2秒更新目标
    lastStep = millis();
    targetPos += 1000; // 每次移动1000脉冲
    if (targetPos > 5000) targetPos = 0;
    Serial.print("新目标位置: "); Serial.println(targetPos);
  }

  // 计算前馈量（动态补偿）
  static float lastPos = 0;
  float velocity = (encoder.getCount() - lastPos) * 50; // 粗略速度估算
  float acceleration = (velocity - lastVel) * 50;     // 粗略加速度估算
  lastVel = velocity;
  lastPos = encoder.getCount();

  // 设置目标（位置+前馈补偿）
  motor.target = targetPos;
  motor.voltage_sensor_align = velocity * velocityFF + acceleration * accelFF;

  // 执行FOC控制
  motor.loopFOC();
  motor.move();
}

3、基于力矩扰动补偿的自适应位置控制（电流反馈）

#include <Encoder.h>
#include <SimpleFOC.h>

// 硬件配置
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
Encoder encoder = Encoder(2, 3, 2000);
InlineCurrentSense currentSense = InlineCurrentSense(0.01, 50.0, A0, A1); // 电流采样

// 自适应参数
float targetPos = 0;
float disturbanceGain = 0.8; // 扰动补偿增益

void setup() {
  Serial.begin(115200);
  
  // 初始化硬件
  encoder.init();
  motor.linkSensor(&encoder);
  driver.init();
  motor.linkDriver(&driver);
  currentSense.init();
  motor.linkCurrentSense(&currentSense);
  
  // 配置PID
  motor.PID_velocity.P = 0.1;
  motor.PID_velocity.I = 1.0;
  motor.PID_position.P = 0.5;
  motor.torque_controller = TorqueControlType::voltage; // 启用力矩模式
  
  // 启用位置控制
  motor.controller = MotionControlType::position;
  motor.init();
  motor.initFOC();
}

void loop() {
  // 动态目标生成（正弦轨迹）
  targetPos = 2000 * sin(millis() / 1000.0);
  
  // 读取当前电流（作为扰动估计）
  PhaseCurrent_s current = currentSense.getPhaseCurrents();
  float disturbance = (current.a + current.b + current.c) / 3.0; // 平均电流
  
  // 自适应补偿计算
  motor.target = targetPos;
  motor.voltage_sensor_align = disturbance * disturbanceGain; // 动态补偿力矩扰动
  
  // 执行控制
  motor.loopFOC();
  motor.move();
  
  // 调试输出
  static unsigned long lastPrint = 0;
  if (millis() - lastPrint > 100) {
    lastPrint = millis();
    Serial.print("位置误差:"); Serial.print(targetPos - encoder.getCount());
    Serial.print(" 补偿量:"); Serial.println(motor.voltage_sensor_align);
  }
}

要点解读
双向控制实现机制
方向信号处理：通过PWM符号（案例1）或FOC库内置方向处理（案例2/3）实现双向驱动
死区管理：在低PWM占空比时关闭驱动（如abs(output) < 10时停止），避免电机抖动
关键点：方向切换需配合速度斜坡（如SimpleFOC的output_ramp参数）
自适应动态控制策略
前馈补偿：案例2通过速度/加速度前馈提升轨迹跟踪响应速度
扰动观测：案例3利用电流反馈实时补偿负载变化（如机械臂抓取物体时的扭矩变化）
参数自整定：可扩展加入Ziegler-Nichols法在线调整PID参数
多层级控制架构
串级控制：典型结构为"位置环（外环）→ 速度环（中环）→ 电流环（内环）"（如案例3）
采样率匹配：位置环周期（10-50ms）＞速度环周期（1-5ms）＞电流环周期（0.1-1ms）
案例2/3优势：SimpleFOC库自动处理多层级控制，开发者只需配置各环参数
传感器融合与滤波
编码器处理：建议使用4倍频解码（如encoder.init()时启用）
低通滤波：对速度/加速度计算添加软件滤波（如velocity = 0.8velocity + 0.2new_velocity）
抗干扰设计：在电流采样中加入硬件RC滤波（如100nF+10kΩ）
性能优化技巧
中断优先级：将编码器读取放入硬件中断（如attachInterrupt）确保时序精度
浮点运算优化：在8位MCU上使用定点数运算或F()宏减少内存占用
能耗管理：到位后进入低功耗模式（如motor.disable()）
诊断接口：通过串口实时输出位置误差、补偿量等数据用于调试

4、精密位置伺服控制
场景：高精度定位应用，如3D打印机、CNC、精密装配。
核心逻辑：编码器高分辨率 + 位置环PID + 自适应前馈。

#include <SimpleFOC.h>
#include <PID_v1.h>

// 高精度编码器配置
#define ENCODER_RESOLUTION 8192  // 8192线编码器
#define ENCODER_CPR 32768        // 4倍频后每转计数
Encoder encoder(2, 3, ENCODER_RESOLUTION);
void doA() { encoder.handleA(); }
void doB() { encoder.handleB(); }

// 位置控制参数
struct PositionControl {
  float targetAngle = 0.0;           // 目标位置 rad
  float currentAngle = 0.0;          // 当前位置 rad
  float error = 0.0;                 // 位置误差
  float errorIntegral = 0.0;         // 误差积分
  float lastError = 0.0;             // 上次误差
  float errorDerivative = 0.0;       // 误差微分
  
  float kp = 50.0;                   // 比例增益
  float ki = 0.5;                    // 积分增益
  float kd = 0.2;                    // 微分增益
  
  float tolerance = 0.001;           // 定位容差 0.001rad ≈ 0.057°
  float maxError = 0.1;              // 最大允许误差
  float maxIntegral = 10.0;          // 最大积分
  
  bool targetReached = false;
  unsigned long settleTime = 0;      // 稳定时间
  float settleWindow = 0.002;        // 稳定窗口
};
PositionControl posCtrl;

// 自适应前馈补偿
struct AdaptiveFeedforward {
  float velocityFF = 0.8;            // 速度前馈
  float accelerationFF = 0.1;        // 加速度前馈
  float frictionComp = 0.05;         // 摩擦力补偿
  
  float learnedFF[3] = {0, 0, 0};    // 学习到的前馈
  float learningRate = 0.01;         // 学习率
  float ffErrorHistory[100] = {0};   // 误差历史
  int ffIndex = 0;
};
AdaptiveFeedforward ff;

// 位置轨迹规划
class TrajectoryPlanner {
private:
  // 五次多项式系数: θ(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵
  float coeff[6] = {0, 0, 0, 0, 0, 0};
  float startTime = 0;
  float duration = 0;
  float startPos = 0, endPos = 0;
  float startVel = 0, endVel = 0;
  float startAcc = 0, endAcc = 0;
  
  // 限制条件
  float maxVel = 10.0;      // 最大速度
  float maxAcc = 20.0;      // 最大加速度
  float maxJerk = 100.0;    // 最大加加速度
  
public:
  void plan(float currentPos, float targetPos, 
            float currentVel, float targetVel,
            float currentAcc, float targetAcc) {
            
    startPos = currentPos;
    endPos = targetPos;
    startVel = currentVel;
    endVel = targetVel;
    startAcc = currentAcc;
    endAcc = targetAcc;
    startTime = micros() / 1000000.0;
    
    // 计算运动时间
    float distance = abs(endPos - startPos);
    duration = calculateOptimalTime(distance);
    
    // 计算五次多项式系数
    calculateQuinticCoeff();
  }
  
  void getSetpoint(float t, float& pos, float& vel, float& acc) {
    float tau = t - startTime;
    if (tau < 0) tau = 0;
    if (tau > duration) tau = duration;
    
    // 计算位置
    pos = coeff[0] + coeff[1]*tau + coeff[2]*tau*tau + 
          coeff[3]*tau*tau*tau + coeff[4]*tau*tau*tau*tau + 
          coeff[5]*tau*tau*tau*tau*tau;
    
    // 计算速度
    vel = coeff[1] + 2*coeff[2]*tau + 3*coeff[3]*tau*tau + 
          4*coeff[4]*tau*tau*tau + 5*coeff[5]*tau*tau*tau*tau;
    
    // 计算加速度
    acc = 2*coeff[2] + 6*coeff[3]*tau + 12*coeff[4]*tau*tau + 
          20*coeff[5]*tau*tau*tau;
  }
  
private:
  float calculateOptimalTime(float distance) {
    // 计算最优运动时间
    
    // 基于梯形速度剖面
    float accelTime = maxVel / maxAcc;
    float accelDist = 0.5 * maxAcc * accelTime * accelTime;
    
    if (distance <= 2 * accelDist) {
      // 三角形速度剖面
      return 2 * sqrt(distance / maxAcc);
    } else {
      // 梯形速度剖面
      float cruiseDist = distance - 2 * accelDist;
      float cruiseTime = cruiseDist / maxVel;
      return 2 * accelTime + cruiseTime;
    }
  }
  
  void calculateQuinticCoeff() {
    // 计算五次多项式系数
    
    float T = duration;
    float T2 = T * T;
    float T3 = T2 * T;
    float T4 = T3 * T;
    float T5 = T4 * T;
    
    // 边界条件矩阵求解
    coeff[0] = startPos;
    coeff[1] = startVel;
    coeff[2] = startAcc / 2.0;
    
    float A = endPos - (coeff[0] + coeff[1]*T + coeff[2]*T2);
    float B = endVel - (coeff[1] + 2*coeff[2]*T);
    float C = endAcc - 2*coeff[2];
    
    coeff[3] = (10*A - 4*B*T + 0.5*C*T2) / T3;
    coeff[4] = (-15*A + 7*B*T - C*T2) / T4;
    coeff[5] = (6*A - 3*B*T + 0.5*C*T2) / T5;
  }
};

TrajectoryPlanner trajectory;

void setup() {
  Serial.begin(1152000);  // 高速串口用于调试
  
  Serial.println("===== 精密双向位置自适应控制系统 =====");
  Serial.println("分辨率: 8192线编码器");
  Serial.println("控制频率: 1kHz");
  
  // 1. 初始化高精度编码器
  encoder.init();
  encoder.enableInterrupts(doA, doB);
  
  // 2. 配置BLDC电机
  motor.linkSensor(&encoder);
  motor.init();
  motor.initFOC();
  
  // 3. 校准编码器零位
  calibrateEncoder();
  
  // 4. 初始化自适应参数
  initAdaptiveControl();
  
  Serial.println("系统就绪");
  Serial.println("定位精度: <0.001rad");
  Serial.println("重复精度: <0.0005rad");
}

void loop() {
  static unsigned long lastControlTime = 0;
  unsigned long now = micros();
  float dt = (now - lastControlTime) / 1000000.0;
  
  if (dt >= 0.001) {  // 1kHz控制频率
    // 1. 读取高精度位置
    readHighResolutionPosition();
    
    // 2. 轨迹规划
    float targetPos, targetVel, targetAcc;
    trajectory.getSetpoint(now/1000000.0, targetPos, targetVel, targetAcc);
    
    // 3. 自适应位置控制
    adaptivePositionControl(targetPos, targetVel, targetAcc, dt);
    
    // 4. 执行FOC
    motor.loopFOC();
    
    // 5. 自适应学习
    adaptiveLearning(dt);
    
    // 6. 性能监控
    monitorPerformance();
    
    lastControlTime = now;
  }
  
  // 状态显示
  static unsigned long lastDisplay = 0;
  if (millis() - lastDisplay >= 20) {  // 50Hz显示
    displayPrecisionStatus();
    lastDisplay = millis();
  }
}

void readHighResolutionPosition() {
  // 读取高分辨率位置
  
  posCtrl.currentAngle = motor.shaft_angle;
  
  // 高分辨率处理
  static int32_t rawCount = 0;
  static float lastRawAngle = 0;
  
  // 获取原始计数
  int32_t newCount = encoder.getCount();
  
  // 处理溢出
  int32_t delta = newCount - rawCount;
  if (delta > ENCODER_CPR/2) {
    delta -= ENCODER_CPR;
  } else if (delta < -ENCODER_CPR/2) {
    delta += ENCODER_CPR;
  }
  
  // 高分辨率角度计算
  float angle = (rawCount + delta) * 2 * PI / ENCODER_CPR;
  
  // 卡尔曼滤波
  angle = kalmanFilter(angle, lastRawAngle, 0.001);
  
  posCtrl.currentAngle = angle;
  rawCount += delta;
  lastRawAngle = angle;
}

float kalmanFilter(float measurement, float lastEstimate, float dt) {
  // 简化卡尔曼滤波
  
  static float P = 1.0;        // 估计误差协方差
  static float Q = 0.001;      // 过程噪声
  static float R = 0.01;       // 测量噪声
  
  // 预测
  float x_pred = lastEstimate;
  float P_pred = P + Q;
  
  // 更新
  float K = P_pred / (P_pred + R);
  float x_est = x_pred + K * (measurement - x_pred);
  P = (1 - K) * P_pred;
  
  return x_est;
}

void adaptivePositionControl(float targetPos, float targetVel, 
                            float targetAcc, float dt) {
  // 自适应位置控制
  
  // 1. 计算位置误差
  posCtrl.error = targetPos - posCtrl.currentAngle;
  
  // 误差限幅
  if (abs(posCtrl.error) > posCtrl.maxError) {
    posCtrl.error = (posCtrl.error > 0) ? posCtrl.maxError : -posCtrl.maxError;
  }
  
  // 2. PID控制
  posCtrl.errorIntegral += posCtrl.error * dt;
  posCtrl.errorIntegral = constrain(posCtrl.errorIntegral, 
                                   -posCtrl.maxIntegral, posCtrl.maxIntegral);
  
  posCtrl.errorDerivative = (posCtrl.error - posCtrl.lastError) / dt;
  posCtrl.lastError = posCtrl.error;
  
  // 3. 自适应增益
  float adaptiveKp = adaptGainKp(posCtrl.error, dt);
  float adaptiveKd = adaptGainKd(posCtrl.errorDerivative, dt);
  
  // 4. PID输出
  float pidOutput = adaptiveKp * posCtrl.error + 
                   posCtrl.ki * posCtrl.errorIntegral + 
                   adaptiveKd * posCtrl.errorDerivative;
  
  // 5. 前馈补偿
  float feedforward = ff.velocityFF * targetVel + 
                     ff.accelerationFF * targetAcc;
  
  // 6. 摩擦力补偿
  float friction = ff.frictionComp * sign(targetVel);
  
  // 7. 总输出
  float totalOutput = pidOutput + feedforward + friction;
  
  // 8. 转换为速度指令
  float speedCommand = totalOutput;
  
  // 9. 应用控制
  motor.move(speedCommand);
  
  // 10. 检查定位完成
  checkPositionReached();
}

float adaptGainKp(float error, float dt) {
  // 自适应调整Kp
  
  static float adaptiveKp = posCtrl.kp;
  static float errorHistory[10] = {0};
  static int index = 0;
  
  // 更新误差历史
  errorHistory[index] = error;
  index = (index + 1) % 10;
  
  // 计算误差统计
  float mean = 0, variance = 0;
  for (int i = 0; i < 10; i++) {
    mean += errorHistory[i];
  }
  mean /= 10.0;
  
  for (int i = 0; i < 10; i++) {
    float diff = errorHistory[i] - mean;
    variance += diff * diff;
  }
  variance /= 10.0;
  
  // 基于误差统计调整Kp
  if (variance < 0.0001) {  // 误差稳定
    adaptiveKp = adaptiveKp * 0.99;  // 稍微减小增益
  } else if (variance > 0.001) {  // 误差波动大
    adaptiveKp = adaptiveKp * 1.01;  // 增加增益
  }
  
  // 限幅
  adaptiveKp = constrain(adaptiveKp, posCtrl.kp * 0.5, posCtrl.kp * 2.0);
  
  return adaptiveKp;
}

void adaptiveLearning(float dt) {
  // 自适应学习
  
  // 1. 学习前馈参数
  learnFeedforward(dt);
  
  // 2. 学习摩擦力
  learnFriction(dt);
  
  // 3. 学习系统动态
  learnSystemDynamics(dt);
}

void learnFeedforward(float dt) {
  // 学习前馈参数
  
  static float lastError = 0;
  static float errorIntegral = 0;
  
  // 计算误差趋势
  float errorTrend = posCtrl.error - lastError;
  lastError = posCtrl.error;
  errorIntegral += posCtrl.error * dt;
  
  // 调整速度前馈
  if (abs(motor.shaft_velocity) > 0.1) {
    ff.velocityFF += ff.learningRate * errorTrend * sign(motor.shaft_velocity);
  }
  
  // 调整加速度前馈
  static float lastVel = 0;
  float acceleration = (motor.shaft_velocity - lastVel) / dt;
  if (abs(acceleration) > 0.1) {
    ff.accelerationFF += ff.learningRate * errorIntegral * sign(acceleration);
  }
  lastVel = motor.shaft_velocity;
  
  // 限幅
  ff.velocityFF = constrain(ff.velocityFF, 0.5, 1.5);
  ff.accelerationFF = constrain(ff.accelerationFF, 0.0, 0.5);
}

5、动态刚度自适应控制
场景：变负载、变刚度应用，如力控装配、柔顺控制。
核心逻辑：刚度观测器 + 动态刚度调整 + 阻抗控制。

#include <SimpleFOC.h>
#include <Filters.h>  // 滤波器库

// 力/扭矩传感器
#define FORCE_SENSOR_PIN A0
#define TORQUE_SENSOR_PIN A1
#define STRAIN_GAUGE_PIN A2

// 刚度控制参数
struct StiffnessControl {
  float desiredStiffness = 100.0;   // 期望刚度 N·m/rad
  float actualStiffness = 0.0;      // 实际刚度
  float minStiffness = 10.0;        // 最小刚度
  float maxStiffness = 1000.0;      // 最大刚度
  float dampingRatio = 0.7;         // 阻尼比
  float naturalFreq = 10.0;         // 自然频率 Hz
  
  // 阻抗控制
  float impedanceMass = 1.0;        // 虚拟质量
  float impedanceDamping = 5.0;     // 虚拟阻尼
  float impedanceStiffness = 100.0; // 虚拟刚度
  
  // 柔顺控制
  float compliance = 0.001;         // 柔度 rad/N·m
  float maxDeflection = 0.1;        // 最大变形
};
StiffnessControl stiffness;

// 刚度观测器
class StiffnessObserver {
private:
  float positionHistory[100] = {0};
  float torqueHistory[100] = {0};
  int historyIndex = 0;
  float dt = 0.01;
  
  // 频率响应分析
  float freqResponse[50] = {0};  // 0-50Hz
  float phaseResponse[50] = {0};
  
public:
  void update(float position, float torque, float dt) {
    // 更新历史数据
    positionHistory[historyIndex] = position;
    torqueHistory[historyIndex] = torque;
    historyIndex = (historyIndex + 1) % 100;
    
    // 计算当前刚度
    if (historyIndex >= 10) {
      calculateStiffness();
    }
    
    // 计算频率响应
    if (historyIndex % 10 == 0) {
      calculateFrequencyResponse();
    }
  }
  
  float getStiffness() { 
    return actualStiffness; 
  }
  
  float getDamping() {
    return calculateDamping();
  }
  
  float getNaturalFrequency() {
    return sqrt(actualStiffness / impedanceMass) / (2 * PI);
  }
  
private:
  void calculateStiffness() {
    // 基于最小二乘法估计刚度
    
    float sumX = 0, sumY = 0, sumXY = 0, sumX2 = 0;
    int n = 10;  // 使用最近10个点
    
    for (int i = 0; i < n; i++) {
      int idx = (historyIndex - i - 1 + 100) % 100;
      float x = positionHistory[idx];
      float y = torqueHistory[idx];
      
      sumX += x;
      sumY += y;
      sumXY += x * y;
      sumX2 += x * x;
    }
    
    // 线性拟合: τ = k * θ
    float numerator = n * sumXY - sumX * sumY;
    float denominator = n * sumX2 - sumX * sumX;
    
    if (abs(denominator) > 0.001) {
      actualStiffness = numerator / denominator;
    }
    
    actualStiffness = constrain(actualStiffness, 1.0, 10000.0);
  }
  
  void calculateFrequencyResponse() {
    // 简化的频率响应估计
    
    // 计算位置信号的FFT (简化)
    for (int freq = 0; freq < 50; freq++) {
      float realPos = 0, imagPos = 0;
      float realTorque = 0, imagTorque = 0;
      
      for (int t = 0; t < 100; t++) {
        float angle = 2 * PI * freq * t * dt / 100.0;
        realPos += positionHistory[t] * cos(angle);
        imagPos += positionHistory[t] * sin(angle);
        realTorque += torqueHistory[t] * cos(angle);
        imagTorque += torqueHistory[t] * sin(angle);
      }
      
      // 计算传递函数
      float magPos = sqrt(realPos*realPos + imagPos*imagPos);
      float magTorque = sqrt(realTorque*realTorque + imagTorque*imagTorque);
      
      if (magPos > 0.001) {
        freqResponse[freq] = magTorque / magPos;
        phaseResponse[freq] = atan2(imagTorque, realTorque) - 
                             atan2(imagPos, realPos);
      }
    }
  }
};

StiffnessObserver stiffnessObserver;

// 阻抗控制器
class ImpedanceController {
private:
  float virtualMass;      // 虚拟质量
  float virtualDamping;   // 虚拟阻尼
  float virtualStiffness; // 虚拟刚度
  
  float positionSetpoint = 0;
  float velocitySetpoint = 0;
  float accelerationSetpoint = 0;
  
  float lastPositionError = 0;
  float lastVelocityError = 0;
  
  // 状态
  float position = 0;
  float velocity = 0;
  float acceleration = 0;
  
public:
  ImpedanceController(float m, float b, float k) :
    virtualMass(m), virtualDamping(b), virtualStiffness(k) {}
  
  float calculate(float targetPos, float externalForce, float dt) {
    // 阻抗控制方程: m*ẍ + b*ẋ + k*x = F_ext
    
    // 计算位置误差
    float positionError = targetPos - position;
    float velocityError = velocitySetpoint - velocity;
    
    // 计算期望加速度
    float desiredAccel = (externalForce - 
                         virtualDamping * velocityError - 
                         virtualStiffness * positionError) / virtualMass;
    
    // 积分得到速度和位置
    velocity += desiredAccel * dt;
    position += velocity * dt;
    
    // 限制
    velocity = constrain(velocity, -10.0, 10.0);
    
    return position;
  }
  
  void setImpedance(float m, float b, float k) {
    virtualMass = m;
    virtualDamping = b;
    virtualStiffness = k;
  }
  
  float getPosition() { return position; }
  float getVelocity() { return velocity; }
  float getAcceleration() { return acceleration; }
};

ImpedanceController impedanceCtrl(1.0, 5.0, 100.0);

// 动态刚度调整
class DynamicStiffnessAdjuster {
private:
  float currentStiffness = 100.0;
  float targetStiffness = 100.0;
  float stiffnessRate = 10.0;  // 刚度变化率
  float adaptationGain = 0.1;
  
  // 性能指标
  float trackingError = 0;
  float settlingTime = 0;
  float overshoot = 0;
  float stabilityMargin = 0;
  
public:
  void update(float positionError, float velocity, float externalTorque, 
              float dt, float& stiffness, float& damping) {
    
    // 计算性能指标
    updatePerformanceMetrics(positionError, dt);
    
    // 基于性能调整刚度
    adjustStiffness(positionError, externalTorque, dt);
    
    // 调整阻尼
    adjustDamping(velocity, externalTorque, dt);
    
    // 平滑变化
    stiffness = smoothTransition(currentStiffness, targetStiffness, dt);
    currentStiffness = stiffness;
  }
  
private:
  void updatePerformanceMetrics(float error, float dt) {
    // 更新性能指标
    
    static float maxError = 0;
    static unsigned long errorStartTime = 0;
    
    trackingError = error;
    
    if (abs(error) < 0.01) {  // 进入稳定区
      if (errorStartTime == 0) {
        errorStartTime = millis();
      }
      settlingTime = (millis() - errorStartTime) / 1000.0;
    } else {
      errorStartTime = 0;
      settlingTime = 0;
    }
    
    if (abs(error) > maxError) {
      maxError = abs(error);
      overshoot = maxError;
    }
  }
  
  void adjustStiffness(float error, float externalTorque, float dt) {
    // 调整刚度
    
    if (abs(externalTorque) > 0.1) {
      // 有外力作用，降低刚度
      targetStiffness = max(stiffness.minStiffness, 
                           targetStiffness * 0.9);
    } else if (abs(error) > 0.05) {
      // 误差大，增加刚度
      targetStiffness = min(stiffness.maxStiffness, 
                           targetStiffness * 1.1);
    } else {
      // 误差小，保持当前刚度
    }
    
    // 基于稳定性调整
    adjustForStability();
  }
  
  void adjustDamping(float velocity, float externalTorque, float dt) {
    // 调整阻尼
    
    float criticalDamping = 2 * sqrt(currentStiffness * impedanceCtrl.getMass());
    float targetDamping = stiffness.dampingRatio * criticalDamping;
    
    // 基于速度调整
    if (abs(velocity) > 1.0) {
      targetDamping *= 1.2;  // 高速增加阻尼
    }
    
    impedanceCtrl.setDamping(targetDamping);
  }
  
  float smoothTransition(float current, float target, float dt) {
    // 平滑过渡
    float rate = stiffnessRate * dt;
    if (current < target) {
      return min(current + rate, target);
    } else {
      return max(current - rate, target);
    }
  }
};

DynamicStiffnessAdjuster stiffnessAdjuster;

// 自适应柔顺控制
void adaptiveComplianceControl(float dt) {
  // 自适应柔顺控制
  
  // 1. 读取传感器
  float externalTorque = readTorqueSensor();
  float externalForce = readForceSensor();
  float strain = readStrainGauge();
  
  // 2. 更新刚度观测器
  stiffnessObserver.update(motor.shaft_angle, externalTorque, dt);
  float observedStiffness = stiffnessObserver.getStiffness();
  
  // 3. 动态调整刚度
  float adjustedStiffness, adjustedDamping;
  stiffnessAdjuster.update(posCtrl.error, motor.shaft_velocity, 
                          externalTorque, dt, adjustedStiffness, adjustedDamping);
  
  // 4. 设置阻抗参数
  impedanceCtrl.setImpedance(stiffness.impedanceMass, 
                           adjustedDamping, 
                           adjustedStiffness);
  
  // 5. 计算柔顺位置
  float compliantPosition = impedanceCtrl.calculate(
    posCtrl.targetAngle, externalTorque, dt);
  
  // 6. 位置控制
  posCtrl.targetAngle = compliantPosition;
  adaptivePositionControl(compliantPosition, 0, 0, dt);
  
  // 7. 记录柔顺数据
  logComplianceData(externalTorque, compliantPosition, adjustedStiffness);
}

6、AI驱动的自适应位置控制
场景：智能自适应系统，使用机器学习实现最优控制。
核心逻辑：神经网络模型 + 强化学习 + 在线优化。

#include <SimpleFOC.h>
#include <TensorFlowLite_ESP32.h>
#include <eloquent_tinyml.h>  // 轻量级ML库

// 神经网络模型
Eloquent::TinyML::TfLite<3, 1> nnController;  // 3输入1输出
Eloquent::TinyML::RandomForest<10, 3> randomForest;  // 随机森林

// 强化学习代理
class RLPositionAgent {
private:
  // 状态空间: [位置误差, 速度, 负载扭矩]
  float state[3] = {0, 0, 0};
  float lastState[3] = {0, 0, 0};
  float action = 0;
  float lastAction = 0;
  float reward = 0;
  
  // Q学习参数
  float qTable[27][9];  // 27个离散状态，9个离散动作
  float alpha = 0.1;    // 学习率
  float gamma = 0.9;    // 折扣因子
  float epsilon = 0.3;  // 探索率
  
  // 经验回放
  struct Experience {
    float state[3];
    float action;
    float reward;
    float nextState[3];
    bool done;
  };
  Experience replayBuffer[100];
  int bufferIndex = 0;
  
public:
  void initialize() {
    // 初始化Q表
    for (int s = 0; s < 27; s++) {
      for (int a = 0; a < 9; a++) {
        qTable[s][a] = 0.1;
      }
    }
  }
  
  float getAction(float currentState[3]) {
    // 获取动作
    
    memcpy(state, currentState, 3 * sizeof(float));
    
    if (random(100) / 100.0 < epsilon) {
      // 探索
      action = random(-4, 5);  // -4到4
    } else {
      // 利用
      int stateIndex = discretizeState(state);
      action = getBestAction(stateIndex);
    }
    
    return action;
  }
  
  void learn(float nextState[3], float reward, bool done) {
    // 强化学习
    
    // 存储经验
    replayBuffer[bufferIndex] = {state[0], state[1], state[2], 
                                 action, reward, 
                                 nextState[0], nextState[1], nextState[2], 
                                 done};
    bufferIndex = (bufferIndex + 1) % 100;
    
    // Q学习更新
    int s = discretizeState(state);
    int s_prime = discretizeState(nextState);
    int a_prime = getBestAction(s_prime);
    
    float td_error = reward + gamma * qTable[s_prime][a_prime] - qTable[s][(int)action+4];
    qTable[s][(int)action+4] += alpha * td_error;
    
    // 衰减探索率
    epsilon *= 0.999;
    epsilon = max(epsilon, 0.01);
    
    // 经验回放学习
    if (bufferIndex >= 10) {
      experienceReplay(10);
    }
  }
  
private:
  int discretizeState(float state[3]) {
    // 状态离散化
    int s0 = discretizeValue(state[0], -0.1, 0.1, 3);
    int s1 = discretizeValue(state[1], -5.0, 5.0, 3);
    int s2 = discretizeValue(state[2], -2.0, 2.0, 3);
    
    return s0 + s1 * 3 + s2 * 9;
  }
  
  int discretizeValue(float value, float min, float max, int bins) {
    float range = max - min;
    float normalized = (value - min) / range;
    int bin = (int)(normalized * bins);
    return constrain(bin, 0, bins-1);
  }
  
  int getBestAction(int stateIndex) {
    int bestAction = 0;
    float bestValue = qTable[stateIndex][0];
    
    for (int a = 1; a < 9; a++) {
      if (qTable[stateIndex][a] > bestValue) {
        bestValue = qTable[stateIndex][a];
        bestAction = a;
      }
    }
    
    return bestAction - 4;  // 转换回-4到4
  }
  
  void experienceReplay(int batchSize) {
    // 经验回放
    for (int i = 0; i < batchSize; i++) {
      int idx = random(bufferIndex);
      Experience exp = replayBuffer[idx];
      
      int s = discretizeState(exp.state);
      int s_prime = discretizeState(exp.nextState);
      int a_prime = getBestAction(s_prime);
      
      float td_error = exp.reward + gamma * qTable[s_prime][a_prime] - qTable[s][(int)exp.action+4];
      qTable[s][(int)exp.action+4] += alpha * td_error;
    }
  }
};

RLPositionAgent rlAgent;

// 神经网络自适应控制器
class NeuralAdaptiveController {
private:
  // 神经网络模型
  float nnInput[3] = {0, 0, 0};
  float nnOutput[1] = {0};
  
  // 自适应参数
  float learningRate = 0.01;
  float momentum = 0.9;
  
  // 训练数据
  float trainingInputs[100][3];
  float trainingTargets[100];
  int trainingIndex = 0;
  bool trainingMode = false;
  
public:
  float predict(float positionError, float velocity, float torque) {
    // 神经网络预测
    
    nnInput[0] = positionError;
    nnInput[1] = velocity;
    nnInput[2] = torque;
    
    // 运行神经网络
    nnController.predict(nnInput, nnOutput);
    
    // 输出是控制量调整
    float controlAdjust = nnOutput[0];
    
    return controlAdjust;
  }
  
  void train(float positionError, float velocity, float torque, 
            float optimalControl, float dt) {
    // 在线训练
    
    if (trainingMode) {
      // 收集训练数据
      trainingInputs[trainingIndex][0] = positionError;
      trainingInputs[trainingIndex][1] = velocity;
      trainingInputs[trainingIndex][2] = torque;
      trainingTargets[trainingIndex] = optimalControl;
      trainingIndex = (trainingIndex + 1) % 100;
      
      // 定期训练
      if (trainingIndex == 0) {
        performTraining();
      }
    }
  }
  
  void enableTraining(bool enable) {
    trainingMode = enable;
  }
  
private:
  void performTraining() {
    // 执行神经网络训练
    
    // 这里简化，实际应使用反向传播
    // 在嵌入式系统上通常在线训练困难，使用预训练模型
    
    // 简单的梯度下降
    float gradient[3] = {0, 0, 0};
    int batchSize = min(trainingIndex, 10);
    
    for (int i = 0; i < batchSize; i++) {
      int idx = random(trainingIndex);
      float* input = trainingInputs[idx];
      float target = trainingTargets[idx];
      
      // 前向传播
      float prediction = predict(input[0], input[1], input[2]);
      float error = prediction - target;
      
      // 计算梯度
      gradient[0] += error * input[0];
      gradient[1] += error * input[1];
      gradient[2] += error * input[2];
    }
    
    // 更新权重
    for (int i = 0; i < 3; i++) {
      gradient[i] /= batchSize;
      // 这里简化，实际应更新神经网络权重
    }
  }
};

NeuralAdaptiveController neuralController;

// 性能优化器
class PerformanceOptimizer {
private:
  // 性能指标
  struct PerformanceMetrics {
    float riseTime = 0;      // 上升时间
    float settlingTime = 0;  // 稳定时间
    float overshoot = 0;     // 超调
    float steadyStateError = 0;  // 稳态误差
    float ise = 0;          // 误差平方积分
    float itae = 0;         // 时间绝对误差积分
    float controlEffort = 0; // 控制量
  };
  PerformanceMetrics metrics;
  
  // 优化目标
  struct OptimizationTarget {
    float maxRiseTime = 0.5;     // 最大上升时间
    float maxSettlingTime = 1.0; // 最大稳定时间
    float maxOvershoot = 0.1;    // 最大超调
    float maxError = 0.01;       // 最大误差
    float minStabilityMargin = 0.3;  // 最小稳定裕度
  };
  OptimizationTarget target;
  
  // 优化参数
  float optimizedKp = 50.0;
  float optimizedKi = 0.5;
  float optimizedKd = 0.2;
  float optimizedFF = 0.8;
  
public:
  void update(float error, float controlOutput, float dt) {
    // 更新性能指标
    
    updateRiseTime(error, dt);
    updateSettlingTime(error, dt);
    updateOvershoot(error);
    updateISE(error, dt);
    updateITAE(error, dt);
    updateControlEffort(controlOutput, dt);
  }
  
  void optimize() {
    // 优化控制参数
    
    // 基于性能指标调整参数
    if (metrics.overshoot > target.maxOvershoot) {
      optimizedKp *= 0.9;  // 减小Kp
      optimizedKd *= 1.1;  // 增加Kd
    }
    
    if (metrics.settlingTime > target.maxSettlingTime) {
      optimizedKi *= 1.1;  // 增加Ki
    }
    
    if (metrics.steadyStateError > target.maxError) {
      optimizedKp *= 1.1;  // 增加Kp
      optimizedKi *= 1.1;  // 增加Ki
    }
    
    // 限制参数范围
    optimizedKp = constrain(optimizedKp, 10.0, 200.0);
    optimizedKi = constrain(optimizedKi, 0.1, 5.0);
    optimizedKd = constrain(optimizedKd, 0.0, 1.0);
    optimizedFF = constrain(optimizedFF, 0.5, 1.5);
  }
  
  void getOptimizedParams(float& kp, float& ki, float& kd, float& ff) {
    kp = optimizedKp;
    ki = optimizedKi;
    kd = optimizedKd;
    ff = optimizedFF;
  }
  
private:
  void updateRiseTime(float error, float dt) {
    static float startError = 0;
    static bool rising = false;
    
    if (!rising && abs(error) > 0.1) {
      rising = true;
      startError = error;
    }
    
    if (rising && abs(error) < 0.01) {
      metrics.riseTime += dt;
      rising = false;
    }
  }
  
  void updateSettlingTime(float error, float dt) {
    if (abs(error) < 0.01) {
      metrics.settlingTime += dt;
    } else {
      metrics.settlingTime = 0;
    }
  }
  
  void updateISE(float error, float dt) {
    metrics.ise += error * error * dt;
  }
  
  void updateITAE(float error, float dt) {
    static float totalTime = 0;
    totalTime += dt;
    metrics.itae += abs(error) * totalTime * dt;
  }
};

PerformanceOptimizer perfOptimizer;

// AI驱动自适应控制
void aiDrivenAdaptiveControl(float dt) {
  // AI驱动自适应控制
  
  // 1. 读取状态
  float state[3] = {
    posCtrl.error,              // 位置误差
    motor.shaft_velocity,      // 速度
    readTorqueSensor()         // 负载扭矩
  };
  
  // 2. 神经网络控制
  float nnAdjust = neuralController.predict(state[0], state[1], state[2]);
  
  // 3. 强化学习控制
  float rlAction = rlAgent.getAction(state);
  
  // 4. 性能优化
  perfOptimizer.update(posCtrl.error, motor.current.q, dt);
  perfOptimizer.optimize();
  
  float optKp, optKi, optKd, optFF;
  perfOptimizer.getOptimizedParams(optKp, optKi, optKd, optFF);
  
  // 5. 自适应调整控制参数
  posCtrl.kp = optKp + nnAdjust * 10.0;  // 神经网络调整
  posCtrl.ki = optKi;
  posCtrl.kd = optKd;
  ff.velocityFF = optFF;
  
  // 6. 应用控制
  adaptivePositionControl(posCtrl.targetAngle, 0, 0, dt);
  
  // 7. 计算奖励
  float reward = calculateReward(posCtrl.error, motor.shaft_velocity, dt);
  
  // 8. 强化学习
  float nextState[3] = {
    posCtrl.error,
    motor.shaft_velocity,
    readTorqueSensor()
  };
  rlAgent.learn(nextState, reward, posCtrl.targetReached);
  
  // 9. 神经网络学习
  neuralController.train(state[0], state[1], state[2], 
                        rlAction, dt);
}

float calculateReward(float error, float velocity, float dt) {
  // 计算奖励函数
  
  float reward = 0;
  
  // 误差惩罚
  reward -= 10.0 * error * error;
  
  // 速度惩罚（防止振荡）
  reward -= 0.1 * velocity * velocity;
  
  // 控制量惩罚
  reward -= 0.01 * abs(motor.current.q);
  
  // 稳定奖励
  if (abs(error) < 0.001 && abs(velocity) < 0.01) {
    reward += 1.0;
  }
  
  // 快速响应奖励
  static float lastError = 0;
  float errorReduction = lastError - abs(error);
  if (errorReduction > 0) {
    reward += 5.0 * errorReduction;
  }
  lastError = abs(error);
  
  return reward;
}

要点解读
高精度位置反馈的实现
编码器分辨率：案例4使用8192线编码器，4倍频后32768 CPR。角度分辨率2π/32768 ≈ 0.00019rad ≈ 0.011°。
溢出处理：案例4的readHighResolutionPosition()处理编码器计数溢出，使用(delta > CPR/2)检测。
滤波算法：必须滤波但需保持相位。案例4的kalmanFilter()是位置估计的最优选择。Q=0.001（过程噪声），R=0.01（测量噪声）。
零位校准：calibrateEncoder()必须实现，通过索引信号或限位开关。
自适应控制的核心算法
增益调度：案例4的adaptGainKp()基于误差方差调整增益。方差<0.0001时减小增益，>0.001时增加增益。
前馈学习：案例4的learnFeedforward()在线学习速度前馈velocityFF和加速度前馈accelerationFF。
模型参考自适应：隐含在刚度控制中，实际刚度向期望刚度调整。
迭代学习控制：通过记录误差历史，调整下一次运动的轨迹。
动态刚度与柔顺控制
刚度观测：案例5的StiffnessObserver用最小二乘法τ = kθ + b估计实际刚度。需要至少10个数据点。
阻抗控制：案例5的ImpedanceController实现m·ẍ + b·ẋ + k·x = F_ext。这是力控的基础。
刚度调整策略：
外力大时降低刚度（柔顺）
误差大时增加刚度（刚性）
基于稳定性调整
阻尼调整：基于速度调整阻尼，高速时增加阻尼防止振荡。
AI/ML在精密控制中的应用
神经网络控制：案例6的NeuralAdaptiveController用3输入（误差、速度、扭矩）预测控制调整。在嵌入式系统上需用量化模型。
强化学习：案例6的RLPositionAgent实现Q学习，27状态×9动作。奖励函数设计是关键。
在线学习限制：嵌入式在线训练非常困难。通常：
PC训练，部署模型
在线微调少量参数
经验回放批处理
特征工程：输入必须包含位置误差、速度、负载扭矩等关键状态。
性能优化与评估指标
时域指标：
上升时间riseTime：到达90%目标值的时间
稳定时间settlingTime：进入±1%误差带的时间
超调overshoot：最大超调量百分比
稳态误差steadyStateError
积分指标：
ISE：误差平方积分，惩罚大误差
ITAE：时间乘绝对误差积分，惩罚长时间误差
优化目标：案例6的OptimizationTarget定义性能边界。
多目标优化：需平衡响应速度、超调、稳态误差、控制能量。

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