Arduino BLDC融合IMU姿态校正的平滑运动控制机器人，是以Arduino/ESP32为控制核心、BLDC无刷电机为执行器、IMU为姿态感知源，通过传感器融合算法获取高精度姿态数据，并结合FOC驱动与轨迹规划实现低抖动、无冲击、抗扰动平滑运动的智能机器人系统。 该方案具备IMU融合姿态校正、FOC低脉动驱动、轨迹平滑规划与里程计修正四大特点，主要应用于自平衡机器人、云台增稳、智能仓储AGV/AMR、仿生机器人及科研教学等场景；实际部署时需重点关注主控算力与实时性、IMU安装与抗振、电源隔离与EMC、PID调参与安全机制及磁力计干扰处理。
一、 技术架构与主要特点
IMU融合姿态校正：系统通过6轴或9轴IMU（如MPU6050、ICM-20948、BMI088）实时采集陀螺仪与加速度计数据，利用互补滤波或Madgwick等算法融合得到高精度姿态角（Roll/Pitch/Yaw）。陀螺仪短期精度高但存在积分漂移，加速度计长期稳定但易受线性加速度干扰，两者互补融合后姿态估计精度显著提升。 典型离散形式为：angle = α * (angle + gyro_rate * dt) + (1 - α) * accel_angle，其中α通常取0.95~0.99，权衡动态响应与抗扰性。
FOC低脉动驱动：传统六步换相BLDC驱动存在显著转矩纹波（每电周期6次脉动），导致关节或底盘抖动。采用磁场定向控制（FOC）后，通过正弦电流驱动消除换相阶跃，转矩输出与q轴电流呈线性关系，转矩纹波可降低70%以上，显著提升运动平滑性。 开源库SimpleFOC已支持Arduino/ESP32/STM32平台的实时FOC运算。
轨迹平滑规划：平滑运动的核心在于加速度连续。相比梯形速度曲线（加速度突变），S形速度曲线（S-curve Trajectory）通过引入加加速度（jerk）限制，使启停过程无冲击、机械谐振被抑制、跟踪误差更小。Arduino可通过查表法或在线积分生成S曲线位置指令，作为FOC位置环的输入。
里程计修正与抗扰：IMU姿态数据还可用于修正轮式里程计的累积漂移。利用IMU提供的精确偏航角，将机器人坐标系下的位移增量转换到世界坐标系，更新全局位置。当车轮打滑时，IMU可立即感知与里程计不一致的转动，快速修正朝向估计。
二、 典型应用场景
自平衡机器人与两轮车：IMU实时检测车身倾角，控制器根据期望运动速度动态调整平衡目标角度，驱动BLDC轮毂电机产生相应扭矩，维持动态平衡并实现前进/后退。这是最经典的IMU+BLDC融合应用。
手持/车载云台与增稳系统：IMU检测到载体（手、车体）的抖动后，控制器动态计算云台需补偿的角度，驱动BLDC电机反向旋转抵消抖动，确保末端设备始终稳定在目标姿态。姿态更新频率需≥200Hz以捕捉高频手持抖动。
智能仓储AGV/AMR：在仓储物流中，IMU辅助的里程计修正可确保机器人在长距离行驶中保持直线精度（误差<10mm），克服地面摩擦不均、轮胎磨损差异导致的"蛇形"跑偏。结合视觉/激光雷达SLAM，IMU在特征稀疏环境中维持定位精度。
仿生机器人与机械臂：IMU安装在连杆或足端反馈关节实际角度，控制器根据步态规划动态调整目标角度，驱动BLDC关节电机实现仿生的柔顺运动。多关节协同中，IMU姿态数据用于解耦各关节的动力学耦合效应。
科研与教育平台：成本远低于商用伺服系统，适合高校用于"传感器融合"“运动控制”"PID参数整定"等课程的教学实训，也可用于RoboMaster等机器人竞赛中的姿态控制算法验证。
三、 关键注意事项
主控算力与实时性保障：IMU姿态解算、FOC控制与PID闭环对算力要求较高。标准Arduino Uno（16MHz）难以胜任，建议采用ESP32（双核240MHz）或STM32等高算力板卡。 控制回路必须使用硬件定时器中断或非阻塞定时（millis()），严禁使用delay()函数，确保控制频率≥100Hz（平衡类应用建议≥200Hz）。
IMU安装与抗振设计：IMU必须刚性固定在底盘重心附近，并加硅胶减震垫以隔离BLDC电机的高频振动。振动噪声会严重干扰陀螺仪和加速度计数据，导致姿态解算错误。 安装方向需严格校准，避免因杠杆效应引入测量误差。
电源隔离与EMC防护：BLDC电机启停时电流冲击极大，严禁与Arduino及IMU共用电源。必须采用隔离DC-DC模块为控制电路独立供电，并在电机驱动端并联大容量低ESR电容吸收反电动势。IMU信号线需使用屏蔽线，与动力线间距≥50mm，I²C线加100Ω串联电阻+10kΩ上拉。
PID参数整定与安全机制：
参数整定：先调P至临界振荡，再引入D抑制，最后加I消除静差。必须加入积分限幅（Anti-windup）和微分低通滤波，防止参数过大导致振荡或噪声放大。
安全保护：倾角超限（如|Pitch|>30°）自动停机、通讯超时（如500ms无指令）自动刹车、硬件急停按钮直接切断电机驱动电源。
磁力计干扰处理：若使用9轴IMU（含磁力计），BLDC电机产生的强磁场会严重干扰磁力计读数。建议尽量使用6轴算法（如Madgwick六轴）；若必须使用磁力计，需对电机进行磁屏蔽，并在上电时执行严格的"8字形"校准。
打滑检测与容错：在软件中加入打滑检测逻辑（如编码器速度突变而IMU角速度未变），此时应降低编码器在融合算法中的权重，转而更多依赖IMU数据，避免融合算法误判引发失控。

1、自适应地形四轮独立调平机器人
场景：野外巡检或勘探机器人，需要在泥泞、碎石等崎岖地形上保持车体平台水平，以确保搭载设备的稳定性。

核心逻辑：通过IMU实时感知车体的横滚(Roll)和俯仰(Pitch)角度，将姿态误差映射到四个独立驱动的BLDC悬架上，主动伸缩悬架以抵消地形倾斜，实现车体调平。

#include <SimpleFOC.h>
#include <MPU6050.h>

// 定义4个BLDC悬架电机 (BLDCMotor suspension[4])
// 定义MPU6050 IMU对象

float fusedRoll, fusedPitch; // 融合后的姿态角
float targetRoll = 0, targetPitch = 0; // 目标水平姿态

// PID控制器参数 (用于角度环)
float Kp = 1.5f, Ki = 0.02f, Kd = 0.8f; 

void setup() {
  // 初始化IMU和4个BLDC电机
  // ...
}

void loop() {
  // 1. 姿态融合：互补滤波
  // 获取加速度计和陀螺仪原始数据，计算角度
  float accRoll = atan2(imu.getAccelerationX(), 9.8f) * RAD_TO_DEG;
  float gyroRoll = imu.getGyroscopeX();
  // 互补滤波公式: angle = 0.98 * (angle + gyro * dt) + 0.02 * accAngle
  // 对Roll和Pitch分别进行融合
  // ... (融合代码参考[citation:1][citation:5])

  // 2. PID控制计算姿态误差
  float rollError = targetRoll - fusedRoll;
  float pitchError = targetPitch - fusedPitch;
  float rollCorrection = Kp * rollError; // 简化PID，实际可包含I和D项
  float pitchCorrection = Kp * pitchError;

  // 3. 误差分配到4个悬架 (基于杠杆原理)
  float frontLeft =  pitchCorrection + rollCorrection;
  float frontRight = pitchCorrection - rollCorrection;
  float rearLeft =  -pitchCorrection + rollCorrection;
  float rearRight = -pitchCorrection - rollCorrection;

  // 4. 限幅并控制电机 (suspension[i].move(constrain(output, min, max));)
  // ...
  
  delay(5); // 高频控制
}

2、视觉+IMU融合的目标跟随机器人
场景：服务机器人或仓储AGV，需要跟随特定目标（如人、AprilTag码）移动，并保持平滑的跟踪轨迹。

核心逻辑：视觉传感器（如摄像头或树莓派）提供目标相对位置（低频），IMU提供机器人自身的姿态和航向（高频）。融合两者信息，生成平滑的速度和转向指令，避免因视觉数据延迟或抖动导致的运动不平滑。

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

// 定义BLDC差速轮电机 (BLDCMotor leftMotor, rightMotor)
// 定义MPU6050 IMU对象

float yaw = 0; // 融合后的航向角
float targetX, targetY, robotX, robotY; // 从视觉/树莓派获取

void setup() {
  // 初始化电机和IMU
  // 校准IMU零点
  // ...
}

void loop() {
  // 1. 从树莓派(SLAM或视觉)获取目标相对位姿 (低频，例如10Hz)
  // 通过I2C或串口读取 robotPoseX, robotPoseY, targetPoseX, targetPoseY
  // ...

  // 2. IMU航向角实时更新 (高频，例如100Hz)
  // 读取陀螺仪Z轴角速度，积分更新yaw
  // 并与视觉提供的航向进行互补滤波，修正漂移 (参考[citation:2])
  
  // 3. 运动控制计算
  float dx = targetPoseX - robotPoseX;
  float dy = targetPoseY - robotPoseY;
  float targetAngle = atan2(dy, dx);
  float angleError = targetAngle - yaw;
  // 角度归一化到 -PI 到 PI

  float distanceError = sqrt(dx*dx + dy*dy);
  
  // 根据距离和角度误差，计算线速度和角速度指令
  float v_cmd = constrain(distanceError * 0.5, 0, maxSpeed);
  float w_cmd = constrain(angleError * 1.5, -maxTurnRate, maxTurnRate);
  
  // IMU横滚补偿: 转弯时检测到侧倾过大则自动减速 (参考[citation:6])
  if (abs(fusedRoll) > 15.0f) {
      v_cmd *= 0.5;
  }

  // 4. 运动学逆解，计算左右轮速度并驱动BLDC电机
  // ...
  delay(30);
}

3、两轮自平衡机器人
场景：两轮自平衡代步车或机器人平台，利用BLDC的大扭矩和快速响应特性，保持车体直立。

核心逻辑：这是典型的“倒立摆”问题。IMU（通常是MPU6050）检测车体的俯仰角。通过互补滤波或卡尔曼滤波获得高精度、低噪声的姿态角，然后使用级联PID（内环速度环，外环角度环）控制BLDC电机的力矩，实现车体的动态平衡。

#include <SimpleFOC.h>
#include <MPU6050.h>

// 定义BLDC电机和IMU
BLDCMotor motor = BLDCMotor(7);
MPU6050 imu;

float pitchAngle = 0; // 俯仰角
float targetAngle = 0; // 目标直立角度
float gyroOffset = 0;

// PID参数 (角度环)
float Kp = 350.0f, Ki = 2.0f, Kd = 15.0f; 
float integral = 0, lastError = 0;

void setup() {
  // 初始化电机FOC, 编码器, IMU
  // 校准陀螺仪零点
}

void loop() {
  // 1. 姿态解算 (互补滤波)
  // 读取加速度计(ax, ay, az)和陀螺仪(gyroX)
  float accAngle = atan2(ax, ay); // 计算俯仰角
  float gyroRate = gyroX - gyroOffset; // 角速度
  pitchAngle = 0.98f * (pitchAngle + gyroRate * dt) + 0.02f * accAngle; // 互补滤波 [citation:11]

  // 2. 直立环PID计算
  float error = targetAngle - pitchAngle;
  integral += error * dt;
  integral = constrain(integral, -300, 300); // 积分限幅
  float derivative = (error - lastError) / dt;
  float output = Kp * error + Ki * integral + Kd * derivative;
  lastError = error;

  // 3. 输出到电机 (力矩模式)
  motor.move(output); // SimpleFOC的move指令可接受电压/力矩值 [citation:10]

  // 4. 速度环辅助 (可通过编码器反馈调整直立环输出，防止跑飞)
  // ... (参考[citation:11]中的速度环PID)
  
  delay(5);
}

要点解读
互补滤波是Arduino上的“黄金选择”：在资源受限的Arduino上，实现完整的卡尔曼滤波计算量较大。互补滤波因其极低的算力消耗和直观的物理意义（角度 = 权重 * (陀螺仪积分) + (1-权重) * 加速度计角度），成为融合加速度计（长期稳定但噪声大）和陀螺仪（短期精确但会漂移）的最实用方案。只需调节一个权重参数（如0.98），就能在动态响应和静态精度间取得良好平衡。

PID参数是控制效果的“灵魂”：代码框架中的Kp、Ki、Kd需要根据具体机器人的物理特性（质量、重心、电机力矩）仔细调试。一个通用的建议是：先从小到大增加Kp，让系统能够“挣扎”着回到平衡；然后加入Kd来抑制过冲和震荡；最后才考虑加入微弱的Ki来消除静态误差，并务必进行积分限幅，以防止积分饱和。

“松耦合”是工程上最稳健的融合策略：不同于复杂的紧耦合，松耦合指分别处理IMU和里程计/视觉数据，再融合它们的输出结果。例如，用IMU提供精确的航向角yaw，用里程计提供位移增量dx, dy，最后将里程计数据旋转到IMU确定的全局坐标系下。这种策略逻辑清晰，容易调试，且能充分发挥各自传感器的优势。

控制频率决定响应“天花板”：对于BLDC电机驱动和姿态控制，1kHz (1ms周期) 是理想的控制频率目标。在代码中应避免使用delay(20)等较长的阻塞延迟。利用micros()函数实现非阻塞的定时任务，确保姿态解算和PID计算能以尽可能高的频率运行，这对于动态平衡（如案例三）至关重要。

充分利用BLDC的“力矩模式”：在SimpleFOC库中，motor.move(voltage)或motor.move(current)可以直接控制电机的力矩输出。对于需要快速动态响应的自平衡或主动悬挂场景，直接控制力矩比控制速度或位置更直接、响应更快。这意味着控制系统输出的PID计算值（案例三中的output）可以看作是“需要施加的修正力矩”，从而省去了一个速度内环的调节过程，简化了控制链。

4、水下BLDC机器人IMU闭环平衡控制（S型曲线推力平滑）
适用场景：水下自主巡检机器人，需在水流扰动下维持机体水平姿态，BLDC推进器需频繁启停以抵抗水流，但传统开环控制会导致推进器启停冲击，引发机体抖动、姿态跳变，甚至导致IMU数据失真。
核心目标：通过IMU检测横滚角偏差，结合S型曲线算法约束BLDC推力变化率，实现推进器启停无冲击，确保水下姿态稳定与路径跟踪精度。

核心控制逻辑
姿态感知：IMU通过互补滤波输出实时横滚角，精准捕捉水流扰动导致的姿态偏差；
误差校正：将横滚角偏差作为PID输入，输出推进器目标推力；
推力平滑：引入S型曲线算法，对目标推力的变化率进行约束，替代传统阶跃式推力输出，消除启停冲击；
闭环验证：通过推力平滑前后的横滚角波动、推进器电流突变，验证平滑效果。

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

// 硬件配置：左右双BLDC推进器（水下横向平衡）
BLDCMotor motorLeft(9), motorRight(10);
BLDCDriver3PWM drvLeft(3,5,6), drvRight(11,12,13);
MPU6050 imu;

// IMU姿态数据
float rollAngle = 0;        // 当前横滚角
float prevRollAngle = 0;    // 上一周期横滚角
float rollDeviation = 0;    // 横滚角偏差（当前角度-目标角度）

// 平衡控制参数
float targetRoll = 0;                // 目标横滚角（水平姿态）
float kp = 2.5, ki = 0.01, kd = 0.8; // PID参数
float integral = 0;                  // PID积分项
float prevDeviation = 0;             // 上一周期偏差

// S型曲线平滑参数
float currentThrust = 0;       // 当前实际推力
float targetThrust = 0;        // PID输出的目标推力
float maxAccel = 0.3;          // 推力最大加速度（单位：推力值/控制周期）
float jerkStart = 0.5;         // 启动时的加加速度（柔顺度，越大越平滑）
float jerkEnd = 0.3;           // 停止时的加加速度（略小于启动，适配水下阻力）
float accel = 0;               // 当前推力加速度

void setup() {
  Serial.begin(115200);
  // 初始化BLDC推进器
  motorLeft.linkDriver(&drvLeft); motorRight.linkDriver(&drvRight);
  motorLeft.linkSensor(&dummyEnc); motorRight.linkSensor(&dummyEnc);
  motorLeft.controller = MotionControlType::velocity;
  motorRight.controller = MotionControlType::velocity;
  motorLeft.init(); motorLeft.initFOC();
  motorRight.init(); motorRight.initFOC();

  // 初始化IMU（水下需调整采样率适应水流波动）
  Wire.begin();
  imu.initialize();
  imu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); // 250°/s量程适配水下动态
  imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);  // 2g量程，精准捕捉姿态变化
  imu.setRate(4); // 采样率400Hz（Mega算力适配）
}

void loop() {
  // 1. IMU姿态采集：互补滤波融合加速度计与陀螺仪
  int16_t accX = imu.getAccelerationX();
  int16_t accZ = imu.getAccelerationZ();
  int16_t gyroY = imu.getRotationY();

  // 加速度计计算静态横滚角（重力方向）
  float accRoll = atan2(accX, accZ) * RAD_TO_DEG;
  // 陀螺仪计算动态角速度
  float gyroRollRate = gyroY * 0.00745; // 250°/s量程，每LSB对应0.00745°/s

  // 互补滤波（静态+动态融合，应对水下水流冲击）
  const float alpha = 0.98; // 陀螺仪权重，高频动态保留，低频用加速度计校准
  rollAngle = alpha * (rollAngle + gyroRollRate * 0.01) + (1 - alpha) * accRoll;

  // 2. 横滚角偏差计算（目标角度为0，偏差为正则右倾）
  rollDeviation = rollAngle - targetRoll;

  // 3. PID姿态校正：输出推进器目标推力
  // 比例项：快速响应偏差
  float proportion = kp * rollDeviation;
  // 积分项：消除稳态误差（水下阻力易导致静态偏差，积分项补偿）
  integral += ki * rollDeviation;
  integral = constrain(integral, -1.5, 1.5); // 积分限幅，防止过饱和
  // 微分项：抑制动态振荡（抑制水流引起的偏差突变）
  float derivative = kd * (rollDeviation - prevDeviation);
  prevDeviation = rollDeviation;

  // PID输出目标推力（左右推进器差速实现平衡校正）
  targetThrust = proportion + integral + derivative;
  targetThrust = constrain(targetThrust, -0.4, 0.4); // 推力限幅，适配水下推进器负载

  // 4. S型曲线推力平滑：约束推力变化率，消除启停冲击
  // 计算推力变化率（加速度）的变化率（加加速度，实现S型平滑）
  float jerk = 0;
  float targetAccel = 0;

  if (targetThrust > currentThrust) {
    // 推力上升：正向S型启动
    if (currentThrust - jerkStart * maxAccel <= targetThrust) {
      accel += jerkStart * maxAccel; // 加加速度恒定，进入匀加速阶段
      accel = constrain(accel, 0, maxAccel);
    } else {
      // 进入匀速或减速阶段，切换为停止加加速度
      jerk = jerkEnd;
      targetAccel = (targetThrust - currentThrust) / 0.01;
      accel = constrain(accel, 0, maxAccel);
    }
  } else if (targetThrust < currentThrust) {
    // 推力下降：反向S型停止
    if (currentThrust + jerkEnd * maxAccel >= targetThrust) {
      accel -= jerkEnd * maxAccel; // 减速度变化率恒定，匀减速
      accel = constrain(accel, -maxAccel, 0);
    } else {
      jerk = jerkStart;
      targetAccel = (targetThrust - currentThrust) / 0.01;
      accel = constrain(accel, -maxAccel, 0);
    }
  } else {
    accel = 0; // 目标推力无变化，加速度为0
  }

  // 更新当前推力
  currentThrust += accel * 0.01; // 控制周期10ms，乘以周期时间
  currentThrust = constrain(currentThrust, -0.4, 0.4); // 防越限

  // 5. BLDC推进器执行：左右差速输出，校正横滚角
  float motorLeftSpeed = -currentThrust * 1.2; // 系数适配水下推力与转速关系
  float motorRightSpeed = currentThrust * 1.2;

  motorLeftSpeed = constrain(motorLeftSpeed, -0.35, 0.35);
  motorRightSpeed = constrain(motorRightSpeed, -0.35, 0.35);

  motorLeft.move(motorLeftSpeed);
  motorRight.move(motorRightSpeed);

  // 6. 串口调试输出（验证平滑效果）
  Serial.print("Roll: "); Serial.print(rollAngle, 2);
  Serial.print(" Deviation: "); Serial.print(rollDeviation, 2);
  Serial.print(" TargetThrust: "); Serial.print(targetThrust, 2);
  Serial.print(" CurrentThrust: "); Serial.print(currentThrust, 2);
  Serial.println();

  // 电机闭环控制
  motorLeft.loopFOC();
  motorRight.loopFOC();

  delay(10); // 控制周期10ms
}

// 简易编码器（推进器若为开环可省略，此处兼容SimpleFOC接口）
Encoder dummyEnc(0, 0, 1);

5、工业多轴BLDC搬运机器人IMU轨迹跟踪（MPC模型预测平滑）
适用场景：工厂流水线的多轴BLDC搬运机器人，需沿预设矩形轨迹搬运精密工件，轨迹包含90°转弯，传统PID控制转弯时推力突变，导致工件晃动、轨迹偏差，且启停阶段机械臂振动大，影响搬运精度。
核心目标：通过IMU检测轨迹跟踪的俯仰角偏差，结合MPC模型预测算法，提前预判轨迹变化趋势，输出平滑的BLDC推力轨迹，实现转弯无冲击、启停无振动，保障精密搬运的稳定性。

核心控制逻辑
轨迹建模：预设矩形轨迹的坐标点序列，插值得到连续轨迹；
姿态感知：IMU输出实时俯仰角，计算轨迹跟踪误差（位置误差+姿态误差）；
模型预测：基于BLDC电机动力学模型，预测未来3个控制周期的推力输出，使预测轨迹尽可能跟踪目标轨迹；
滚动优化：对预测推力进行二次优化，约束推力变化率，最小化轨迹误差与推力波动，实现平滑运动；
执行反馈：将优化后的推力输出给BLDC，同时反馈位置信息，动态调整MPC预测模型。

#include <Wire.h>
#include <MPU6050.h>
#include <SimpleFOC.h>
#include <math.h>

// 硬件配置：XY双轴BLDC驱动（工业搬运轨迹跟踪）
BLDCMotor motorX(9), motorY(10);
BLDCDriver3PWM drvX(3,5,6), drvY(11,12,13);
MPU6050 imu;

// 轨迹参数：预设矩形轨迹（单位：cm）
float trajectoryX[10] = {0,0,10,10,20,20,10,10,0,0};
float trajectoryY[10] = {0,10,10,20,20,10,10,0,0,0};
int trajectoryIdx = 0; // 当前轨迹点索引
float interpX = 0, interpY = 0; // 插值后的连续轨迹坐标

// 位置与姿态数据
float currentX = 0, currentY = 0; // 当前位置（模拟编码器反馈）
float pitchAngle = 0;             // 当前俯仰角（IMU检测）
float positionError = 0;          // 位置跟踪误差
float orientationError = 0;       // 姿态跟踪误差

// MPC模型预测参数
const int PREDICT_STEPS = 3;       // 预测步长（预测未来3个控制周期）
const float CONTROL_PERIOD = 0.01; // 控制周期10ms
float motorDynamics[2][2] = {{1, 0}, {0, 1}}; // 简化的电机动力学模型
float stateMat[PREDICT_STEPS][2]; // 状态矩阵
float controlMat[PREDICT_STEPS][2]; // 控制矩阵
float thrustPredict[PREDICT_STEPS]; // 预测推力序列
float targetThrust = 0;             // MPC优化后的目标推力

// 平滑约束参数
float maxThrustRate = 0.2; // 最大推力变化率（防止推力突变）
float smoothingCoeff = 0.8; // 平滑系数（推力变化越平滑，系数越接近1）

void setup() {
  Serial.begin(115200);
  // 初始化BLDC轴
  motorX.linkDriver(&drvX); motorY.linkDriver(&drvY);
  motorX.linkSensor(&dummyEnc); motorY.linkSensor(&dummyEnc);
  motorX.controller = MotionControlType::velocity;
  motorY.controller = MotionControlType::velocity;
  motorX.init(); motorX.initFOC();
  motorY.init(); motorY.initFOC();

  // 初始化IMU
  Wire.begin();
  imu.initialize();
  imu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // 工业场景机械振动大，量程放宽至500°/s
  imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);  // 4g量程，适配快速启停的姿态变化
}

void loop() {
  // 1. 轨迹插值：将离散轨迹点插值为连续坐标，避免轨迹突变
  if (millis() % 500 == 0) { // 每500ms更新一个轨迹点（适配搬运速度）
    trajectoryIdx = (trajectoryIdx + 1) % 10;
  }
  // 线性插值，实现轨迹平滑过渡
  int nextIdx = (trajectoryIdx + 1) % 10;
  float progress = (millis() % 500) / 500.0;
  interpX = trajectoryX[trajectoryIdx] + (trajectoryX[nextIdx] - trajectoryX[trajectoryIdx]) * progress;
  interpY = trajectoryY[trajectoryIdx] + (trajectoryY[nextIdx] - trajectoryY[trajectoryIdx]) * progress;

  // 2. 位置反馈模拟：编码器反馈当前位置（实际项目中替换为真实编码器数据）
  currentX += 0.1; currentY += 0.05; // 模拟搬运过程中的位置变化
  currentX = constrain(currentX, 0, 20); currentY = constrain(currentY, 0, 20);

  // 3. IMU姿态采集：互补滤波计算俯仰角
  int16_t accY = imu.getAccelerationY();
  int16_t accZ = imu.getAccelerationZ();
  int16_t gyroX = imu.getRotationX();

  float accPitch = atan2(-accY, sqrt(accY*accY + accZ*accZ)) * RAD_TO_DEG;
  float gyroPitchRate = gyroX * 0.00745;
  const float alpha = 0.95;
  pitchAngle = alpha * (pitchAngle + gyroPitchRate * 0.01) + (1 - alpha) * accPitch;

  // 4. 跟踪误差计算：位置误差+姿态误差
  positionError = sqrt(pow(currentX - interpX, 2) + pow(currentY - interpY, 2));
  orientationError = pitchAngle * 0.1; // 姿态误差与俯仰角关联，简化为线性关系

  // 5. MPC模型预测：基于电机动力学模型预测推力
  // 构建状态方程：x(k+1) = A*x(k) + B*u(k)（简化模型，x为位置，u为推力）
  float state[2] = {currentX, currentY};
  for (int step = 0; step < PREDICT_STEPS; step++) {
    // 预测状态
    stateMat[step][0] = state[0] + motorDynamics[0][0] * CONTROL_PERIOD * 0.1;
    stateMat[step][1] = state[1] + motorDynamics[1][1] * CONTROL_PERIOD * 0.1;
    // 预测推力（初始预测为上一周期推力，后续由优化修正）
    if (step == 0) thrustPredict[step] = targetThrust;
    else thrustPredict[step] = thrustPredict[step-1];
    // 更新状态
    state[0] = stateMat[step][0];
    state[1] = stateMat[step][1];
  }

  // 6. 滚动优化：最小化跟踪误差与推力变化率，输出平滑推力
  float minCost = 1e9;
  for (float thrustStep0 = targetThrust - maxThrustRate; thrustStep0 <= targetThrust + maxThrustRate; thrustStep0 += 0.01) {
    for (float thrustStep1 = thrustStep0 - maxThrustRate; thrustStep1 <= thrustStep0 + maxThrustRate; thrustStep1 += 0.01) {
      for (float thrustStep2 = thrustStep1 - maxThrustRate; thrustStep2 <= thrustStep1 + maxThrustRate; thrustStep2 += 0.01) {
        // 计算成本函数：跟踪误差*权重 + 推力变化率*权重（平滑项）
        float cost = positionError * 0.6 + orientationError * 0.4;
        cost += smoothingCoeff * (abs(thrustStep0 - thrustPredict[0]) + abs(thrustStep1 - thrustStep0) + abs(thrustStep2 - thrustStep1));
        if (cost < minCost) {
          minCost = cost;
          thrustPredict[0] = thrustStep0; thrustPredict[1] = thrustStep1; thrustPredict[2] = thrustStep2;
        }
      }
    }
  }
  // 输出第一步预测推力（滚动优化：仅执行当前周期的推力）
  targetThrust = thrustPredict[0];

  // 7. BLDC执行：将优化后的推力映射为轴转速
  float speedX = targetThrust * 0.15 * (interpX - currentX);
  float speedY = targetThrust * 0.15 * (interpY - currentY);
  speedX = constrain(speedX, -0.3, 0.3);
  speedY = constrain(speedY, -0.3, 0.3);

  motorX.move(speedX);
  motorY.move(speedY);

  // 8. 串口调试
  Serial.print("Interp("); Serial.print(interpX); Serial.print(","); Serial.print(interpY);
  Serial.print(") Current("); Serial.print(currentX); Serial.print(","); Serial.print(currentY);
  Serial.print(") Error: "); Serial.print(positionError,2);
  Serial.print(" Thrust: "); Serial.print(targetThrust,2);
  Serial.println();

  motorX.loopFOC();
  motorY.loopFOC();

  delay(CONTROL_PERIOD * 1000); // 严格控制10ms控制周期
}

// 简易编码器
Encoder dummyEnc(0, 0, 1);

6、户外BLDC越野机器人IMU颠簸自适应控制（柔顺阻抗平滑）
适用场景：户外复杂地形（沙地、碎石、坡道）的BLDC越野机器人，需应对路面颠簸导致的姿态剧烈变化，传统刚性控制会在颠簸时产生强烈振动，甚至导致电机过载，影响运动连续性与电池续航。
核心目标：通过IMU检测姿态变化率，结合柔顺阻抗控制算法，使BLDC推进器具备“弹性”，在颠簸时主动缓冲冲击力，将刚性振动转化为柔顺缓冲，实现自适应地形的平滑运动，降低振动与能耗。

核心控制逻辑
冲击力感知：IMU检测颠簸导致的横滚角速度、加速度，计算地面冲击力；
柔顺建模：建立BLDC电机的“弹簧-阻尼”阻抗模型，推力与姿态变化量满足弹性关系；
自适应调节：根据地形颠簸程度动态调节阻抗参数（刚度、阻尼），颠簸越强，刚度越低、阻尼越大，缓冲效果越强；
平滑推力输出：基于阻抗模型输出平滑推力，约束推力峰值，避免电机过载，同时实现冲击缓冲；
地形识别：通过长期姿态变化特征识别地形类型，提前优化阻抗参数，提升响应速度。

#include <Wire.h>
#include <MPU6050.h>
#include <SimpleFOC.h>
#include <math.h>

// 硬件配置：四驱BLDC越野机器人（应对复杂地形）
BLDCMotor motor1(3), motor2(5), motor3(9), motor4(10);
BLDCDriver3PWM drv1(11,12,13), drv2(14,15,16), drv3(17,18,19), drv4(20,21,22);
MPU6050 imu;

// 姿态与冲击数据
float rollAngle = 0, rollRate = 0, rollAccel = 0; // 横滚角、角速度、角加速度
float terrainVibration = 0; // 地形振动强度（基于角加速度计算）

// 柔顺阻抗控制参数
float stiffness = 10.0;   // 刚度系数（k：推力与位移的比例，颠簸时降低）
float damping = 2.0;      // 阻尼系数（c：推力与速度的比例，颠簸时增大）
float impedanceThrust = 0; // 阻抗模型输出的推力
float defaultStiffness = 10.0, defaultDamping = 2.0; // 平坦地形默认参数
float roughStiffness = 3.0, roughDamping = 5.0;      // 颠簸地形参数

// 地形识别参数
float terrainStat[3] = {0}; // 地形特征统计（最近3个周期的振动强度）
int terrainType = 0; // 0：平坦，1：颠簸，2：上坡
float terrainThreshold = 0.5; // 颠簸识别阈值（角加速度超过此值判定为颠簸）

// 速度控制参数
float targetSpeed = 0.3; // 目标前进速度（单位：相对推力）
float currentSpeed = 0; // 当前实际速度

void setup() {
  Serial.begin(115200);
  // 初始化四驱BLDC
  motor1.linkDriver(&drv1); motor2.linkDriver(&drv2); motor3.linkDriver(&drv3); motor4.linkDriver(&drv4);
  for (auto &m : {&motor1, &motor2, &motor3, &motor4}) {
    m->linkSensor(&dummyEnc);
    m->controller = MotionControlType::velocity;
    m->init(); m->initFOC();
  }

  // 初始化IMU（越野场景高频采样，捕捉快速颠簸）
  Wire.begin();
  imu.initialize();
  imu.setFullScaleGyroRange(MPU6050_GYRO_FS_1000); // 1000°/s量程，覆盖颠簸时的剧烈姿态变化
  imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);   // 8g量程，捕捉大冲击力
  imu.setRate(1); // 采样率1000Hz，快速响应颠簸
}

void loop() {
  // 1. IMU高频采样：快速捕捉颠簸产生的姿态突变
  int16_t accX = imu.getAccelerationX();
  int16_t accY = imu.getAccelerationY();
  int16_t accZ = imu.getAccelerationZ();
  int16_t gyroX = imu.getRotationX();
  int16_t gyroY = imu.getRotationY();
  int16_t gyroZ = imu.getRotationZ();

  // 2. 姿态变化率计算：角速度、角加速度（直接用于冲击判断）
  float accRoll = atan2(accX, accZ) * RAD_TO_DEG;
  float gyroRoll = gyroY * 0.00745; // 角速度（°/s）
  float accRollRate = (gyroRoll - rollRate) / 0.01; // 角加速度（°/s²）
  rollAngle = 0.9 * rollAngle + 0.1 * accRoll; // 简化低通滤波，抑制高频噪声
  rollRate = gyroRoll;
  rollAccel = abs(accRollRate); // 取绝对值，表征颠簸强度

  // 3. 地形振动强度计算与地形识别
  terrainVibration = (terrainVibration + rollAccel) * 0.9 + rollAccel * 0.1; // 指数平滑，反映长期振动水平

  // 地形识别逻辑：3个周期内振动强度超过阈值则判定为颠簸地形
  terrainStat[0] = terrainStat[1]; terrainStat[1] = terrainStat[2]; terrainStat[2] = terrainVibration;
  if (terrainVibration > terrainThreshold) {
    terrainType = 1; // 颠簸地形
    stiffness = roughStiffness;
    damping = roughDamping;
  } else {
    // 判断是否为上坡：加速度计Y轴长期为正（上坡时重力分量导致）
    if (accY > 200) { // 200mg以上，判定为上坡
      terrainType = 2;
      stiffness = defaultStiffness * 0.7; // 上坡适当降低刚度，避免打滑
      damping = defaultDamping * 1.2;
    } else {
      terrainType = 0; // 平坦地形
      stiffness = defaultStiffness;
      damping = defaultDamping;
    }
  }

  // 4. 柔顺阻抗控制：推力 = 刚度*姿态偏差 + 阻尼*姿态变化率（弹簧-阻尼模型）
  // 姿态偏差：假设目标姿态为0，偏差即为当前横滚角
  float impedanceBase = stiffness * rollAngle;
  // 阻尼项：抑制姿态变化率，缓冲颠簸冲击
  float impedanceDamp = damping * rollRate;
  // 输出阻抗推力（弹性推力，实现冲击缓冲）
  impedanceThrust = impedanceBase + impedanceDamp;

  // 5. 总推力输出：目标速度推力 + 柔顺阻抗推力，平滑融合
  // 目标速度偏差：当前速度与目标速度的差值
  float speedError = targetSpeed - currentSpeed;
  float speedThrust = speedError * 0.5; // 速度环比例控制，快速跟踪目标速度
  // 总推力融合：速度推力与柔顺推力线性叠加，约束变化率
  float totalThrust = speedThrust + impedanceThrust;
  totalThrust = constrain(totalThrust, -0.4, 0.4); // 推力限幅，防止电机过载

  // 6. 平滑速度过渡：S型速度平滑，避免速度突变
  float speedAccel = (targetSpeed - currentSpeed) * 0.1;
  speedAccel = constrain(speedAccel, -0.05, 0.05); // 速度加速度限幅
  currentSpeed += speedAccel;
  currentSpeed = constrain(currentSpeed, -0.35, 0.35);

  // 7. BLDC四驱执行：四轮同速，推力由柔顺模型约束
  float motorSpeed = totalThrust * 1.0; // 四驱同速，系数适配越野机器人扭矩
  motorSpeed = constrain(motorSpeed, -0.35, 0.35);

  motor1.move(motorSpeed);
  motor2.move(motorSpeed);
  motor3.move(motorSpeed);
  motor4.move(motorSpeed);

  // 8. 串口调试：输出地形、姿态、推力数据
  Serial.print("Terrain: "); Serial.print(terrainType);
  Serial.print(" Roll: "); Serial.print(rollAngle, 2);
  Serial.print(" Accel: "); Serial.print(rollAccel, 2);
  Serial.print(" Stiffness: "); Serial.print(stiffness, 1);
  Serial.print(" Thrust: "); Serial.print(totalThrust, 2);
  Serial.println();

  // 电机闭环控制
  for (auto &m : {&motor1, &motor2, &motor3, &motor4}) {
    m->loopFOC();
  }

  delay(10); // 控制周期10ms
}

// 简易编码器
Encoder dummyEnc(0, 0, 1);

要点解读

1. IMU姿态校正与运动控制的闭环深度融合：解决数据“落地”的核心
   IMU仅输出姿态数据远远不够，关键在于建立“姿态偏差→平滑控制指令→推力执行→姿态反馈”的完整闭环，让IMU数据真正驱动运动平滑，而非仅作为监测工具。
   闭环架构设计：三个案例均以IMU姿态为反馈、BLDC为执行器，形成闭环，而非开环控制。例如水下案例中，横滚角偏差通过PID转化为推力指令，推力经S型平滑后输出，姿态变化再反馈至PID，实现动态校正，避免传统开环控制“推力突变→姿态跳变→控制失灵”的死循环。
   校正与控制的优先级匹配：姿态校正优先保障运动核心需求，例如水下案例优先校正横滚角维持平衡，越野案例优先校正颠簸冲击。这种优先级设计让IMU校正直接服务于运动平滑，而非盲目追求姿态精度，确保控制资源的高效分配。
   反馈数据的实时性保障：控制周期与IMU采样率深度匹配，例如越野案例1000Hz采样率与10ms控制周期，确保每10ms用最新姿态数据更新推力指令，避免因数据滞后导致的控制振荡，这是闭环融合的基础保障。
2. 平滑运动算法的场景适配：平衡精度、效率与效果
   不同场景对平滑的核心需求不同，需针对性选择算法，而非追求通用型最优，确保算法适配场景约束（算力、负载、动态特性）。
   算法与场景匹配逻辑：
   水下静态平衡场景：核心需求是消除启停冲击，S型曲线通过控制推力变化率，实现启停的加速度平滑过渡，算法简单、算力需求低，完美适配Arduino Mega的有限算力，且无模型依赖，快速落地。
   工业精密轨迹场景：核心需求是轨迹跟踪平滑，MPC通过预测未来轨迹优化推力，兼顾跟踪精度与推力平滑，但算力需求较高，仅适合Mega等算力较强的平台，适配工业场景的高精度需求。
   户外动态颠簸场景：核心需求是冲击缓冲，柔顺阻抗通过“弹簧-阻尼”模型模拟弹性推力，让电机具备缓冲能力，直接解决颠簸冲击问题，算法逻辑简单、鲁棒性强，适合户外地形的不确定性。
   算法参数的动态优化：平滑参数需随场景变化，例如水下案例的S型加加速度随水流强度调节，越野案例的刚度随颠簸强度自适应变化，避免固定参数导致场景适配性下降，这是算法落地的关键。
   算力约束下的算法轻量化：所有案例均对算法做了轻量化处理，例如MPC采用简化的电机动力学模型，柔顺控制采用线性弹簧阻尼模型，避免复杂非线性计算，确保在Arduino平台上实时运行，这是嵌入式开发的必然要求。
3. IMU数据的高频抗干扰处理：保障姿态感知的可靠性
   户外、水下等场景存在大量振动、冲击干扰，IMU数据易被污染，若直接用于控制，会导致平滑推力输出错误，因此高频抗干扰处理是姿态校正的前提。
   互补滤波的多维度应用：融合加速度计与陀螺仪的优势，加速度计提供静态姿态基准，陀螺仪提供动态角速度，通过权重alpha平衡静态稳定性与动态响应性。例如水下案例alpha=0.98，保留陀螺仪的动态响应，同时用加速度计校准静态漂移，有效抵抗水流导致的低频干扰。
   高频噪声抑制：对IMU数据做低通滤波与指数平滑，例如越野案例通过指数平滑计算长期振动强度，滤除单次振动的噪声，避免瞬时干扰触发误控制，确保姿态数据的稳定性。
   冲击异常值过滤：设置冲击阈值，对超过阈值的异常姿态数据做剔除或标记，例如越野案例中，若角加速度超过阈值，仅作为地形识别依据，不直接参与推力计算，防止偶发冲击导致推力突变，确保控制的平稳性。
4. BLDC推力的平滑约束机制：消除运动冲击的核心
   无论何种平滑算法，最终都要落地到BLDC推力输出，对推力变化率的约束是消除运动冲击的关键，本质是通过约束推力的加速度、加加速度，实现推力从“阶跃变化”到“连续平滑变化”的转变。
   推力变化率的分级约束：
   一级约束（加速度）：S型曲线与MPC均对推力变化率设定上限，例如水下案例最大加速度为0.3，避免推力瞬间跳变，从源头减少冲击。
   二级约束（加加速度）：S型曲线的加加速度（jerk）控制推力变化的平缓度，加加速度越小，推力变化越平滑，人体感知越柔和，这是实现启停无抖动的核心。
   三级约束（推力限幅）：对推力最大值做限幅，避免因姿态误差过大导致推力超载，同时防止电机过载损坏，兼顾平滑性与硬件安全性。
   推力约束的动态适配：根据场景动态调整约束强度，例如水下水流强时，放宽推力变化率，让推进器快速抵抗水流；工业精密场景收紧推力变化率，保障轨迹跟踪精度，避免约束过度导致响应迟缓。
   约束与执行的协同：推力约束需与BLDC的速度/电流闭环深度绑定，例如SimpleFOC的速度闭环保障推力指令准确执行，避免因电机特性导致的推力滞后，确保约束后的推力能精准落地，这是约束机制生效的关键。
5. 场景驱动的参数自整定与扩展性：保障长期稳定运行
   机器人运行场景复杂多变，固定参数难以长期适配，需通过自整定与扩展性设计，实现参数随场景自适应调整，确保长期稳定运行。
   场景自适应参数调节：越野案例通过地形识别动态调整刚度与阻尼，颠簸地形降低刚度、增大阻尼增强缓冲，平坦地形恢复默认参数，避免参数固定导致的颠簸缓冲不足或平坦地形控制迟缓，提升环境适应能力。
   参数自整定的轻量化实现：无需复杂的智能算法，通过阈值判断、数据统计实现简单自整定，例如工业案例通过轨迹跟踪误差调节MPC权重，误差大时加大跟踪误差权重，误差小时加大平滑权重，平衡跟踪精度与运动平滑，适配场景动态变化。
   算法与硬件的扩展性设计：代码预留扩展接口，例如多轴控制可扩展至6轴BLDC，IMU可替换为ICM42688等高精度型号，平滑算法可升级为模糊控制、神经网络，适配更复杂的场景需求。同时硬件设计兼容，确保代码可迁移至更强大的平台，满足未来功能升级，避免重复开发。

请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。