基于Arduino与BLDC（无刷直流电机）构建的“融合IMU的姿态稳定与地形补偿系统”，是一套将多源惯性感知、传感器融合滤波与BLDC高动态力矩控制深度耦合的嵌入式稳定控制架构。该方案的核心价值在于：通过IMU实时感知机身姿态与地形坡度，动态补偿BLDC电机的差速输出，使机器人在加减速、路面颠簸或坡面行驶时仍能保持平稳姿态，彻底解决传统轮式机器人“急停点头、加速抬头、过坎打滑”的固有缺陷。

主要特点

1. 多源传感器融合的鲁棒姿态解算
   系统通常采用MPU6050、ICM-20602或BHI260AP等六轴/九轴IMU传感器，集成三轴加速度计与陀螺仪。由于纯加速度计易受线性加速度污染（运动时无法区分重力与惯性力），而陀螺仪存在零偏漂移（积分后角度误差随时间累积），系统需通过互补滤波、Mahony算法、Madgwick算法或扩展卡尔曼滤波（EKF）进行数据融合。融合策略的核心思想是“快慢互补”：陀螺仪高频响应好，用于跟踪姿态的快速变化；加速度计低频稳定性好，用于校正陀螺仪的长期漂移。在资源受限的Arduino平台上，互补滤波或Mahony算法因计算量小而更实用；若需融合磁力计修正Yaw角或融合轮式里程计，则可选用EKF。
2. 基于姿态反馈的串级PID控制架构
   系统采用“外环（姿态角环）+内环（角速度环/速度环）”的串级PID控制结构。外环以IMU解算的俯仰角（Pitch）和横滚角（Roll）为反馈，与期望姿态（通常为零）比较得到角度误差，经PID运算输出目标角速度指令；内环以陀螺仪角速度或BLDC编码器反馈的速度为输入，快速抑制高频扰动，输出PWM或FOC电流指令驱动电机。这种分层架构将“保持平衡”这一高阶目标转化为“维持特定前倾角以产生前进加速度”的运动策略，完美诠释了倒立摆控制的本质逻辑。
3. 地形坡度感知与自适应补偿
   IMU不仅用于姿态稳定，还可作为“电子水平仪”感知地面坡度。当机器人行驶至斜坡时，加速度计的重力分量发生变化，系统可实时估算坡度角，并据此调整BLDC电机的输出扭矩：上坡时增大扭矩防止溜坡，下坡时施加再生制动或负扭矩防止超速。对于四足或轮腿复合构型的机器人，IMU数据还可驱动CPG（中枢模式发生器）步态引擎，自适应调整抬腿高度和步幅——陡坡增加步高防绊倒、减小步幅防打滑，实现全地形自适应穿越。
4. BLDC的FOC高动态柔顺执行
   姿态稳定对电机的动态响应要求极高。BLDC电机配合FOC（磁场定向控制）驱动器，具备毫秒级的转矩响应能力（<5ms）和低速大扭矩特性，能在姿态失稳的瞬间快速输出补偿力矩。当腿部触地或发生碰撞时，FOC的电流闭环可像“弹簧”一样吸收冲击，避免刚性结构损坏；在制动或下坡时，BLDC还可切换至发电模式，通过再生制动将动能回馈给电池，延长续航。
5. IMU辅助的里程计漂移修正
   轮式里程计在平坦地面上短期精度高，但会因车轮打滑、轮胎磨损、地面不平等因素产生累积误差。IMU提供的精确偏航角（Yaw）可作为航向基准，将机器人坐标系下的位移增量准确转换到世界坐标系，显著抑制朝向漂移。这种松耦合融合策略在Arduino上易于实现，是SLAM系统在视觉/激光暂时失效时的可靠“惯性备份”。

应用场景

1. 自平衡机器人与倒立摆平台
   两轮自平衡车是IMU姿态控制的经典应用。系统通过IMU实时检测车身倾角，BLDC差速驱动产生反向力矩维持直立，是学习串级PID、传感器融合和倒立摆动力学的绝佳实践平台。
2. 智能仓储与人机协同AGV
   在仓库环境中，AGV频繁启停、转弯或经过减速带时，IMU辅助可确保底盘保持平稳，防止货物倾倒。IMU修正的里程计还能在特征稀疏的走廊中维持定位精度，提升拣选效率与安全性。
3. 全地形巡检与特种作业机器人
   在光伏电站、农业大棚、变电站或城市下水道等场景中，环境包含草地、沙地、碎石及台阶。轮腿复合或四足机器人通过IMU实时感知机身姿态，动态调整各腿支撑力和步态参数，实现全天候、全地形的自主巡检与数据采集。
4. 服务与接待机器人
   在酒店、机场或商场等动态人流环境中，IMU的姿态补偿确保机器人在人群密集、光照剧烈变化或存在坡坎的环境中，提供平稳、不跟丢的交互体验。当视觉目标短暂丢失时，系统可无缝切换至基于IMU和里程计的“惯性航位推算”模式，维持短暂跟随或安全减速。
5. 教育与科研实验平台
   在高校机器人/控制理论课程中，Arduino+BLDC+IMU构成低成本高开放性的姿态控制验证平台。学生可编程实现不同融合算法（互补滤波、EKF、Madgwick）和控制策略（单环PID、串级PID、MPC），直观理解传感器融合与动态平衡原理。

注意事项

1. 硬件选型与算力瓶颈（首要挑战）
   Arduino Uno（ATmega328P，16MHz）难以胜任FOC电流环（>10kHz）与EKF融合算法的并行运算。强烈推荐Teensy 4.0/4.1（Cortex-M7，600MHz）、ESP32-S3（双核，240MHz）或STM32H7/F4系列作为主控。若需运行复杂的状态估计或视觉SLAM，可采用“上位机（树莓派/Jetson）做高层决策+Arduino/STM32做底层BLDC执行”的异构架构，通过CAN或UART高速通信。
2. 传感器校准与标定（“三分算法，七分标定”）
   IMU的固有误差（零偏、比例因子误差、非正交误差）会直接导致姿态解算失败。必须进行系统校准：上电初始静止阶段估计陀螺仪零偏（静止30秒取均值）；通过六面静置法校准加速度计零偏与比例因子；若使用磁力计，需进行硬铁/软铁校准（八字形旋转）。不标定，再好的融合算法也白搭。
3. IMU安装与振动隔离
   IMU必须刚性固定在底盘重心附近，并加硅胶减震垫以隔离电机高频振动。BLDC的大电流PWM驱动信号极易干扰IMU的I2C/SPI通信，导致数据跳变。在硬件设计上，必须将电机动力线与传感器信号线严格分开布线，并做好屏蔽处理；建议采用隔离电源（如DC-DC模块）为控制电路供电。
4. 融合算法的选择与调参
   互补滤波的权重系数α并非固定经验值，需根据IMU噪声特性和动态工况调整：α过大（过度信任陀螺仪）会导致漂移累积，α过小（过度信任加速度计）会在加减速时引入运动加速度干扰。建议从α=0.95~0.98起步，根据实际效果微调。若选用EKF，需仔细设计过程噪声协方差Q和观测噪声协方差R，二者比值决定了滤波器对模型与观测的信任程度。
5. 控制频率与实时性保障
   姿态稳定控制的带宽通常需达50-100Hz以上（对应响应时间10-20ms），若传感器数据更新滞后或控制周期抖动过大，将直接导致PID积分饱和、微分噪声放大乃至相位延迟失稳。控制回路必须使用硬件定时器中断或millis()非阻塞定时，严禁使用delay()函数，以确保IMU数据与电机控制的严格同步。
6. 坐标系对齐与初始姿态
   IMU芯片上的x/y/z轴不一定与机器人机体轴一致。安装时需查阅datasheet，必要时在软件中乘以固定旋转矩阵进行坐标变换。启动时需假设姿态为水平（或用加速度计+磁力计计算初始四元数），初始姿态错误会导致系统启动时有短暂抖动，但融合算法会逐渐收敛。
7. 动态工况下的加速度计权重管理
   急加速、急转弯时，加速度计测量值包含显著的运动加速度分量，此时若保持固定权重，会将运动加速度误判为重力方向，导致姿态跳变。工程上可采用自适应权重策略：当检测到加速度幅值显著偏离1g时，临时降低加速度计权重，待运动平稳后恢复。

1、差速越野机器人滑移感知与自适应扭矩控制
场景：碎石、泥泞地形的轮式机器人，通过融合轮速与 IMU 数据检测滑移，动态调整左右轮扭矩分配。

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

BLDCMotor motorL = BLDCMotor(11);
BLDCMotor motorR = BLDCMotor(11);
BLDCDriver3PWM driverL = BLDCDriver3PWM(5, 6, 10, 9);
BLDCDriver3PWM driverR = BLDCDriver3PWM(3, 4, 8, 7);
MPU6050 mpu;

// 地形参数：硬地/碎石/泥泞/沙地
float terrainParams[4][4] = {
  {0.2, 0.25, 2.5, 0.5},   // 硬地
  {0.3, 0.35, 2.2, 0.6},   // 碎石
  {0.4, 0.45, 2.0, 0.7},   // 泥泞
  {0.5, 0.55, 1.8, 0.8}    // 沙地
};

float wheelRadius = 0.06;
float wheelTrack = 0.45;
float targetLinearSpeed = 0.3;
float slipL = 0, slipR = 0;

PID torquePID_L(nullptr, nullptr, &targetLinearSpeed, 0.6, 0.04, 0.08, DIRECT);
PID torquePID_R(nullptr, nullptr, &targetLinearSpeed, 0.6, 0.04, 0.08, DIRECT);

void setup() {
  Serial.begin(115200);
  driverL.voltage_power_supply = 24;
  driverL.init();
  motorL.linkDriver(&driverL);
  motorL.init();
  motorL.initFOC();
  motorL.controller = MotionControlType::torque;

  driverR.voltage_power_supply = 24;
  driverR.init();
  motorR.linkDriver(&driverR);
  motorR.init();
  motorR.initFOC();
  motorR.controller = MotionControlType::torque;

  Wire.begin();
  mpu.initialize();
}

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();

  // 读取 IMU 横滚角与角速度
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float rollRate = gx / 131.0;
  float rollAngle = atan2(ax, az) * 180 / PI;

  // 轮速估计（简化：从 FOC 速度获取或编码器计算）
  float speedL = motorL.shaftVelocity();
  float speedR = motorR.shaftVelocity();
  float vx = (speedL + speedR) * wheelRadius / 2.0;

  // 滑移率估计：轮速与 IMU 积分速度的偏差
  static float imuVel = 0;
  imuVel += (ax / 16384.0 * 9.81) * 0.01; // 简化积分
  float slipL_est = abs(speedL * wheelRadius - imuVel) / (abs(speedL * wheelRadius) + 0.01);
  float slipR_est = abs(speedR * wheelRadius - imuVel) / (abs(speedR * wheelRadius) + 0.01);

  // 根据滑移率动态调整扭矩分配
  float baseTorque = torquePID_L.GetOutput() + torquePID_R.GetOutput();
  float torqueL = baseTorque * (1.0 - slipL_est * 0.5);
  float torqueR = baseTorque * (1.0 - slipR_est * 0.5);

  // 安全限幅
  torqueL = constrain(torqueL, -2.5, 2.5);
  torqueR = constrain(torqueR, -2.5, 2.5);

  motorL.move(torqueL);
  motorR.move(torqueR);
  delay(10);
}

核心要点：滑移检测不能仅依赖编码器轮速差，必须融合 IMU 估算的绝对速度。当 IMU 积分速度明显低于轮速时，判定为打滑，降低该轮扭矩，让抓地轮获得更多动力。

2、户外颠簸地形柔顺阻抗自适应控制
场景：沙地、碎石路面的四驱越野机器人，IMU 检测颠簸强度后动态调整“刚度-阻尼”参数，将刚性冲击转化为柔顺缓冲。

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

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;
float damping = 2.0;
float impedanceThrust = 0;

// 地形识别
int terrainType = 0; // 0:平坦 1:颠簸
float terrainThreshold = 0.5;
float targetSpeed = 0.3;

// 简易编码器替代（实际需接 AS5600 等）
Encoder dummyEnc(0, 0, 0);

void setup() {
  Serial.begin(115200);

  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();
  }

  Wire.begin();
  imu.initialize();
  imu.setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
}

void loop() {
  // 高频采样捕捉颠簸冲击
  int16_t ax, ay, az, gx, gy, gz;
  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float newRollRate = gx / 131.0;
  rollAccel = (newRollRate - rollRate) / 0.01;
  rollRate = newRollRate;
  terrainVibration = abs(rollAccel) * 0.7 + abs(gy / 131.0) * 0.3;

  // 地形识别：连续颠簸则切换柔顺模式
  static int roughCount = 0;
  if (terrainVibration > terrainThreshold) {
    roughCount = min(roughCount + 1, 50);
  } else {
    roughCount = max(roughCount - 1, 0);
  }
  terrainType = (roughCount > 20) ? 1 : 0;

  // 动态调整阻抗参数
  float targetStiffness = (terrainType == 1) ? 3.0 : 10.0;
  float targetDamping = (terrainType == 1) ? 5.0 : 2.0;
  stiffness += (targetStiffness - stiffness) * 0.05;
  damping += (targetDamping - damping) * 0.05;

  // 阻抗模型输出推力
  float desiredThrust = targetSpeed * 2.0;
  impedanceThrust = desiredThrust - stiffness * rollAngle - damping * rollRate;
  impedanceThrust = constrain(impedanceThrust, -1.0, 1.0);

  // 平滑输出到四轮（简化：统一推力）
  motor1.move(impedanceThrust);
  motor2.move(impedanceThrust);
  motor3.move(impedanceThrust);
  motor4.move(impedanceThrust);

  delay(10);
}

核心要点：颠簸地形的控制核心不是“对抗”，而是“顺应”。刚度系数降低、阻尼系数增大后，电机推力不再与地面冲击硬碰硬，而是以弹性缓冲方式吸收振动，显著降低电流峰值与机械磨损。

3、四足机器人地形类型识别与步态库切换
场景：Arduino Mega + 四足 BLDC，IMU 检测坡度，超声波检测台阶，自动在平地/斜坡/台阶三种步态间切换。

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

MPU6050 imu;
int ultrasonicPins[2] = {A0, A1};

// 四足关节电机（8 个关节，简化为 8 个 BLDCMotor 声明）
BLDCMotor joints[8] = {
  BLDCMotor(2, 3, 4), BLDCMotor(5, 6, 7),
  BLDCMotor(8, 9, 10), BLDCMotor(11, 12, 13),
  BLDCMotor(14, 15, 16), BLDCMotor(17, 18, 19),
  BLDCMotor(20, 21, 22), BLDCMotor(23, 24, 25)
};

struct GaitParams {
  float stepLength;
  float stepHeight;
  float phaseDiff;
  float jointAngleMax;
} gaitLibrary[3] = {
  {15.0f, 8.0f, 180.0f, 0.52f},   // 平地：大步低抬
  {12.0f, 10.0f, 160.0f, 0.58f},  // 斜坡：减步长、增步高
  {10.0f, 15.0f, 150.0f, 0.65f}   // 台阶：小步高抬
};

GaitParams currentGait = gaitLibrary[0];
int currentTerrain = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  imu.initialize();
  imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2G);
  imu.setFullScaleGyroRange(MPU6050_GYRO_FS_250DPS);

  for (int i = 0; i < 8; i++) {
    joints[i].init();
    joints[i].initFOC();
  }
}

void loop() {
  for (int i = 0; i < 8; i++) joints[i].loopFOC();

  // 读取 IMU 获取俯仰角（坡度）
  int16_t ax, ay, az, gx, gy, gz;
  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float pitch = atan2(ay, az) * 180 / PI;

  // 超声波测距（前方障碍/台阶高度，简化）
  int frontDist = analogRead(ultrasonicPins[0]) / 10;
  int obstacleHeight = 0;
  if (frontDist < 30 && frontDist > 5) obstacleHeight = frontDist / 5;

  // 地形识别
  if (obstacleHeight >= 10) {
    currentTerrain = 2; // 台阶
  } else if (abs(pitch) > 15.0) {
    currentTerrain = 1; // 斜坡
  } else {
    currentTerrain = 0; // 平地
  }

  // 切换到对应步态
  currentGait = gaitLibrary[currentTerrain];

  // 斜坡微调：上坡增加后腿步高，下坡反之
  float stepHeightOffset = 0;
  if (pitch > 5.0) {
    stepHeightOffset = pitch * 0.05;
  } else if (pitch < -5.0) {
    stepHeightOffset = pitch * 0.03;
  }

  float targetStepHeight = constrain(
    currentGait.stepHeight + stepHeightOffset, 5.0, 25.0);

  // 将步态参数映射到关节角度（简化示意）
  float hipSwing = currentGait.stepLength * 0.05;
  float kneeSwing = targetStepHeight * 0.1;

  // 四足对角步态相位分配
  static float phase = 0;
  phase += 0.05;
  if (phase > 2 * PI) phase -= 2 * PI;

  joints[0].move(hipSwing * sin(phase));
  joints[1].move(kneeSwing * sin(phase));
  joints[2].move(hipSwing * sin(phase + PI));
  joints[3].move(kneeSwing * sin(phase + PI));
  joints[4].move(hipSwing * sin(phase + PI));
  joints[5].move(kneeSwing * sin(phase + PI));
  joints[6].move(hipSwing * sin(phase));
  joints[7].move(kneeSwing * sin(phase));

  delay(20);
}

核心要点：四足地形补偿的关键是“识别 → 切换 → 微调”三层逻辑。先通过 IMU 坡度与超声波台阶高度判定地形类型，再切换到预设步态库，最后根据实际坡度值对步高步长做连续微调，而非简单二值切换。

要点解读

1. 传感器融合是滑移与地形感知的基础
   差速轮式机器人的滑移检测不能只看编码器轮速差，必须将 IMU 积分的绝对速度与轮速对比。当轮速明显高于 IMU 估算速度时，判定为打滑，触发扭矩重分配。四足场景中则需融合 IMU（坡度/姿态）与超声波（前方台阶高度）共同决策地形类型。

2. 控制策略必须包含安全限幅与平滑过渡
   无论是扭矩分配还是步态参数调整，都需要对输出进行 constrain() 限幅。柔顺控制中，刚度阻尼参数采用渐进插值而非突变，避免控制量阶跃导致的机械冲击。四足步态切换时，步高步长的变化也应通过限幅约束在机械结构可承受范围内。

3. PID 仍是低成本 MCU 上的可靠基石，复杂场景可引入阻抗/MPC
   对于 Arduino 级别的算力，PID 因其低内存占用和调参直观性，仍是 BLDC 速度/扭矩控制的首选。颠簸地形可在此基础上叠加“阻抗模型”，用刚度-阻尼参数将刚性推力转化为弹性缓冲。MPC 适合四足等高动态平台，但算力要求显著更高，Arduino 通常只能做简化实现。

4. 地形自适应的本质是“参数调制”而非“重写控制律”
   四足或轮式机器人的地形补偿，核心思路是保持底层控制框架不变，仅根据地形识别结果调整高层参数：轮式调扭矩分配系数，四足调步高/步长/相位差，柔顺控制调刚度/阻尼。这种参数化架构使 Arduino 有限的算力能够处理复杂地形逻辑。

5. SimpleFOC 为 BLDC 姿态稳定提供了力矩级执行能力
   BLDC 配合 FOC 的优势在于电流环带宽高，能够精确跟踪姿态控制层输出的目标力矩。在支撑相切换到力矩模式、摆动相切换到位置模式的混合控制策略中，FOC 的正弦驱动保证了低速平稳性，避免了有刷电机齿槽效应对姿态稳定的干扰。

4、两轮差速机器人IMU姿态稳定+坡地地形补偿程序（核心：横滚角稳定+坡度补偿）
应用场景：两轮差速机器人在公园坡地、缓坡道路行驶，核心解决“上坡减速防后仰、下坡缓速防前倾、横滚角防侧翻”的姿态稳定问题，适用于巡检、简易导览等场景。

核心逻辑
IMU 数据读取：通过 I2C 读取 MPU6050 的加速度（横滚角、俯仰角）、角速度数据，进行低通滤波
姿态解算：通过互补滤波融合加速度与角速度，得到稳定的姿态角（重点关注横滚角、俯仰角）
坡度计算：基于俯仰角计算路面坡度，生成速度补偿量
电机控制：根据坡度补偿与姿态修正，调整左右电机速度，实现坡地稳定行驶与姿态纠偏

// 两轮差速IMU姿态稳定+坡地补偿程序
#include <Wire.h>
#include <MPU6050.h>

// ===== 硬件参数定义 =====
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_IN1 A2         // 方向控制
#define LEFT_MOTOR_IN2 A3
#define RIGHT_MOTOR_PWM 6
#define RIGHT_MOTOR_IN1 A4
#define RIGHT_MOTOR_IN2 A5
#define IMU_TYPE 1                // 1=MPU6050
MPU6050 imu;

// ===== 控制参数 =====
const float BASE_SPEED = 30.0f;   // 基础行驶速度（cm/s）
const float PITCH_COMP_GAIN = 0.5f; // 坡度补偿增益（坡度越大，速度补偿越明显）
const float ROLL_CONTROL_GAIN = 1.2f; // 横滚角纠偏增益
const float MAX_TILT_ANGLE = 30.0f;   // 最大允许倾斜角（超过则停车保护）
const float LOW_PASS_FILTER = 0.1f;   // 低通滤波系数
const float IMU_CALIB_TIME = 2000;    // IMU 校准时间（ms）

// ===== 状态变量 =====
float pitchAngle = 0;   // 俯仰角（度，坡度核心参考）
float rollAngle = 0;    // 横滚角（度，侧翻监测核心）
float pitchOffset = 0;  // 坡度零点偏移（校准用）
float targetPitch = 0;  // 目标俯仰角（水平路面为0）
float leftSpeed = 0, rightSpeed = 0;

// ===== 函数声明 =====
void setupIMU();
float readIMUData();
void calibrateIMU();
void controlMotors();

void setup() {
  Serial.begin(115200);
  
  // 电机引脚初始化
  pinMode(LEFT_MOTOR_IN1, OUTPUT);
  pinMode(LEFT_MOTOR_IN2, OUTPUT);
  pinMode(RIGHT_MOTOR_IN1, OUTPUT);
  pinMode(RIGHT_MOTOR_IN2, OUTPUT);
  
  // IMU 初始化
  setupIMU();
  Serial.println("两轮机器人IMU姿态稳定初始化完成，开始校准...");
  // IMU 静态校准（记录零点偏移）
  delay(IMU_CALIB_TIME);
  calibrateIMU();
  Serial.println("校准完成，可启动行驶");
}

void loop() {
  // 1. 读取 IMU 数据（通过互补滤波获取姿态角）
  readIMUData();
  
  // 2. 安全保护：倾斜角超过阈值立即停车
  if (abs(rollAngle) > MAX_TILT_ANGLE || abs(pitchAngle) > MAX_TILT_ANGLE) {
    leftSpeed = 0;
    rightSpeed = 0;
    Serial.println("警告：倾斜角过大，已停车保护！");
    delay(1000);
    return;
  }
  
  // 3. 坡度补偿计算（上坡减速，下坡缓速）
  float pitchError = targetPitch - pitchAngle;
  float speedCompensation = pitchError * PITCH_COMP_GAIN;
  float compensatedSpeed = BASE_SPEED + speedCompensation;
  compensatedSpeed = constrain(compensatedSpeed, 5.0f, BASE_SPEED * 1.2f);
  
  // 4. 横滚角纠偏（修正左右电机速度差，保持稳定）
  float rollError = 0 - rollAngle;  // 目标横滚角为0
  float directionCompensation = rollError * ROLL_CONTROL_GAIN;
  
  // 5. 电机速度计算
  leftSpeed = compensatedSpeed + directionCompensation;
  rightSpeed = compensatedSpeed - directionCompensation;
  
  // 6. 电机控制
  controlMotors();
  
  // 串口输出调试信息
  if (millis() % 500 == 0) {
    Serial.print("Pitch: "); Serial.print(pitchAngle, 1);
    Serial.print("° | Roll: "); Serial.print(rollAngle, 1);
    Serial.print("° | L: "); Serial.print(leftSpeed, 1);
    Serial.print(" R: "); Serial.println(rightSpeed, 1);
  }
  
  delay(10);
}

// IMU 初始化与配置
void setupIMU() {
  if (IMU_TYPE == 1) {
    Wire.begin();
    imu.initialize();
    // 配置量程：陀螺仪±250°/s，加速度计±2g（适配缓坡/普通地形）
    imu.setFullScaleGyroRange(MPU6050_FS_GYRO_250);
    imu.setFullScaleAccRange(MPU6050_FS_ACC_2G);
  }
}

// 读取 IMU 并解算姿态角（互补滤波）
float readIMUData() {
  if (IMU_TYPE == 1) {
    if (!imu.testConnection()) {
      Serial.println("IMU 连接失败，请检查接线！");
      return 0;
    }
    
    // 读取原始数据
    int16_t accX, accY, accZ;
    int16_t gyroX, gyroY, gyroZ;
    imu.getAcceleration(&accX, &accY, &accZ);
    imu.getMotion(&gyroX, &gyroY, &gyroZ);
    
    // 计算横滚角与俯仰角（基于加速度，经低通滤波）
    float accRoll = atan2(accY, sqrt(accX * accX + accZ * accZ)) * 180 / M_PI;
    float accPitch = atan2(-accX, sqrt(accY * accY + accZ * accZ)) * 180 / M_PI;
    
    // 低通滤波，减少振动噪声
    rollAngle = rollAngle * (1 - LOW_PASS_FILTER) + accRoll * LOW_PASS_FILTER;
    pitchAngle = pitchAngle * (1 - LOW_PASS_FILTER) + accPitch * LOW_PASS_FILTER;
    
    // 消除静态偏移（校准后的零点校正）
    pitchAngle -= pitchOffset;
  }
}

// IMU 静态校准（记录水平状态下的零点偏移）
void calibrateIMU() {
  unsigned long startTime = millis();
  float sumPitch = 0, sumRoll = 0, count = 0;
  
  // 静态采集 100 组数据，取平均值作为零点
  while (millis() - startTime < IMU_CALIB_TIME) {
    readIMUData();
    if (millis() % 100 == 0) {  // 每 100ms 采集一次
      sumPitch += pitchAngle;
      sumRoll += rollAngle;
      count++;
    }
  }
  
  if (count > 0) {
    pitchOffset = sumPitch / count;
    rollAngle = rollAngle - (sumRoll / count);
  }
}

// 电机控制
void controlMotors() {
  // 转换为 PWM 信号（基于速度映射，速度范围0~255）
  int leftPWM = constrain(leftSpeed * 8.5, 0, 255);
  int rightPWM = constrain(rightSpeed * 8.5, 0, 255);
  
  // PWM 输出
  analogWrite(LEFT_MOTOR_PWM, leftPWM);
  analogWrite(RIGHT_MOTOR_PWM, rightPWM);
  
  // 方向控制（前进为IN1=高，IN2=低）
  digitalWrite(LEFT_MOTOR_IN1, HIGH);
  digitalWrite(LEFT_MOTOR_IN2, LOW);
  digitalWrite(RIGHT_MOTOR_IN1, HIGH);
  digitalWrite(RIGHT_MOTOR_IN2, LOW);
}

案例优化方向
加入角速度闭环（陀螺仪数据参与 PID 控制），减少动态姿态波动
加入坡度分级补偿，不同坡度区间设置不同补偿增益，提升复杂坡地的稳定性
加入急停保护，当检测到电机电流异常时立即切断动力

5、麦克纳姆轮机器人IMU全姿态稳定+崎岖地形补偿程序（核心：三轴姿态补偿+全向运动）
应用场景：麦克纳姆轮全向机器人在粗糙路面、碎石地行驶，支持平移与转向，核心解决“多轴姿态漂移、全向运动补偿、崎岖路面姿态锁定”问题，适用于重载运输、精密巡检等场景。

核心逻辑
IMU 全轴读取：读取 X/Y/Z 轴角速度与加速度，解算横滚、俯仰、偏航三轴姿态
全向运动补偿：根据坡度、姿态偏差调整四个电机的速度比，实现全向移动时的姿态稳定
地形自适应：通过加速度数据识别崎岖路面的起伏，动态调整电机减震效果

// 麦克纳姆轮IMU全姿态稳定+崎岖地形补偿程序
#include <Wire.h>
#include <MPU6050.h>

// ===== 硬件引脚定义（4路麦克纳姆轮电机） =====
#define MOTOR_PWM_PINS {9, 6, 5, 3}   // 4路电机PWM引脚
#define MOTOR_DIR_PINS1 {A2, A3, A4, A5} // 方向控制引脚1
#define MOTOR_DIR_PINS2 {A0, A1, 2, 7}    // 方向控制引脚2
MPU6050 imu;

// ===== 控制参数 =====
const float BASE_SPEED = 25.0f;
const float YAW_CONTROL_GAIN = 1.0f;    // 偏航角控制增益
const float PITCH_COMP_GAIN = 0.6f;    // 俯仰角补偿增益
const float ROLL_CONTROL_GAIN = 1.5f;  // 横滚角控制增益
const float TILT_DEADZONE = 2.0f;      // 姿态死区（度，避免微小波动频繁调整）
const float ACC_PROFILE_WINDOW = 50;   // 加速度滤波窗口
const int MAX_TILT_ANGLE = 25;          // 最大倾斜角阈值

// ===== 姿态与电机状态 =====
float roll = 0, pitch = 0, yaw = 0;      // 姿态角（度）
float pitchOffset = 0, rollOffset = 0, yawOffset = 0;
float accBuffer[ACC_PROFILE_WINDOW];    // 加速度数据缓冲区
int accBufferIndex = 0;
float floorProfile = 0;                  // 地形轮廓值（用于崎岖路面补偿）
float motorSpeeds[4] = {0};              // 4路电机速度

// ===== 函数声明 =====
void setupIMU();
void readIMU();
void calibratePose();
float filterAcc(float newAcc);
void computeFloorProfile();
void controlMotors();

void setup() {
  Serial.begin(115200);
  // 电机引脚初始化
  for (int i = 0; i < 4; i++) {
    pinMode(MOTOR_PWM_PINS[i], OUTPUT);
    pinMode(MOTOR_DIR_PINS1[i], OUTPUT);
    pinMode(MOTOR_DIR_PINS2[i], OUTPUT);
  }
  
  // IMU 初始化
  setupIMU();
  Serial.println("麦克纳姆轮IMU稳定程序初始化完成，开始姿态校准...");
  delay(3000);
  calibratePose();
  Serial.println("校准完成");
}

void loop() {
  // 1. 读取 IMU 姿态数据
  readIMU();
  
  // 2. 计算地形轮廓（崎岖路面起伏检测）
  computeFloorProfile();
  
  // 3. 安全保护：姿态超限停车
  if (abs(roll) > MAX_TILT_ANGLE || abs(pitch) > MAX_TILT_ANGLE) {
    for (int i = 0; i < 4; i++) motorSpeeds[i] = 0;
    Serial.println("警告：姿态超限，停车保护！");
    delay(500);
    return;
  }
  
  // 4. 全向运动速度解算（平移+转向+姿态补偿）
  float speedComp = BASE_SPEED;
  
  // 坡度补偿（基于俯仰角）
  if (abs(pitch) > TILT_DEADZONE) {
    speedComp += (pitch * PITCH_COMP_GAIN);
  }
  
  // 转向补偿（基于偏航角，保持行驶方向）
  float yawCorrection = 0;
  if (abs(yaw) > TILT_DEADZONE) {
    yawCorrection = yaw * YAW_CONTROL_GAIN;
  }
  
  // 横滚角补偿（保持四轮高度平衡）
  float rollCorrection = 0;
  if (abs(roll) > TILT_DEADZONE) {
   rollCorrection = roll * ROLL_CONTROL_GAIN;
  }
  
  // 崎岖地形补偿（根据地形轮廓调整电机协同）
  speedComp += filterAcc(floorProfile) * 0.3f;
  speedComp = constrain(speedComp, 5.0f, BASE_SPEED * 1.3f);
  
  // 求解四轮速度（麦克纳姆轮全向运动学）
  // 前进运动 + 转向修正 + 姿态补偿
  motorSpeeds[0] = speedComp + yawCorrection - rollCorrection;  // 左前
  motorSpeeds[1] = speedComp + yawCorrection + rollCorrection;  // 右前
  motorSpeeds[2] = speedComp - yawCorrection + rollCorrection;  // 左后
  motorSpeeds[3] = speedComp - yawCorrection - rollCorrection;  // 右后
  
  // 5. 电机控制
  controlMotors();
  
  // 调试输出
  if (millis() % 1000 == 0) {
    Serial.print("Roll: "); Serial.print(roll, 1);
    Serial.print("° Pitch: "); Serial.print(pitch, 1);
    Serial.print("° Yaw: "); Serial.print(yaw, 1);
    Serial.print("° Profile: "); Serial.println(floorProfile, 2);
  }
  
  delay(10);
}

// IMU 初始化
void setupIMU() {
  Wire.begin();
  imu.initialize();
  imu.setFullScaleGyroRange(MPU6050_FS_GYRO_500);   // 陀螺仪±500°/s，适配全向运动
  imu.setFullScaleAccRange(MPU6050_FS_ACC_4G);       // 加速度计±4g，适应崎岖路面冲击
}

// 读取 IMU 并解算姿态
void readIMU() {
  float accX, accY, accZ, gyroX, gyroY, gyroZ;
  imu.getAcceleration(&accX, &accY, &accZ);
  imu.getMotion(&gyroX, &gyroY, &gyroZ);
  
  // 计算三轴姿态角（互补滤波，简化版，可替换为姿态解算库）
  float accRoll = atan2(accY, sqrt(accX * accX + accZ * accZ)) * 180 / M_PI;
  float accPitch = atan2(-accX, sqrt(accY * accY + accZ * accZ)) * 180 / M_PI;
  // 偏航角基于陀螺仪积分（短时间内稳定）
  yaw += gyroZ * 0.01;  // 时间步长约10ms
  yaw = constrain(yaw, -180, 180);
  
  // 静态偏移校正（校准后消除零点误差）
  accRoll -= rollOffset;
  accPitch -= pitchOffset;
  
  // 死区过滤，避免微小抖动
  if (abs(accRoll) < TILT_DEADZONE) accRoll = 0;
  if (abs(accPitch) < TILT_DEADZONE) accPitch = 0;
  
  // 低通滤波
  roll = roll * 0.7f + accRoll * 0.3f;
  pitch = pitch * 0.7f + accPitch * 0.3f;
}

// 姿态校准（零点标定）
void calibratePose() {
  float sumRoll = 0, sumPitch = 0, sumYaw = 0;
  int count = 0;
  unsigned long startTime = millis();
  
  while (millis() - startTime < 3000) {
    readIMU();
    sumRoll += roll;
    sumPitch += pitch;
    sumYaw += yaw;
    count++;
    delay(10);
  }
  
  if (count > 0) {
    rollOffset = sumRoll / count;
    pitchOffset = sumPitch / count;
    yawOffset = sumYaw / count;
  }
}

// 加速度滤波（滑动平均）
float filterAcc(float newAcc) {
  accBuffer[accBufferIndex] = newAcc;
  accBufferIndex = (accBufferIndex + 1) % ACC_PROFILE_WINDOW;
  
  float sum = 0;
  for (int i = 0; i < ACC_PROFILE_WINDOW; i++) {
    sum += accBuffer[i];
  }
  return sum / ACC_PROFILE_WINDOW;
}

// 地形轮廓计算（基于加速度的垂直起伏）
void computeFloorProfile() {
  float accZ;
  imu.getAcceleration(NULL, NULL, &accZ);
  // 垂直加速度反映地形起伏，转换为地形轮廓值
  floorProfile = (accZ - 9.81f) * 0.1f;  // 相对于重力加速度的偏差
}

// 电机控制
void controlMotors() {
  for (int i = 0; i < 4; i++) {
    int pwm = constrain(motorSpeeds[i] * 10, 0, 255);
    analogWrite(MOTOR_PWM_PINS[i], pwm);
    // 方向控制：正转IN1=高，IN2=低；反转相反
    if (motorSpeeds[i] > 0) {
      digitalWrite(MOTOR_DIR_PINS1[i], HIGH);
      digitalWrite(MOTOR_DIR_PINS2[i], LOW);
    } else {
      digitalWrite(MOTOR_DIR_PINS1[i], LOW);
      digitalWrite(MOTOR_DIR_PINS2[i], HIGH);
    }
  }}

案例优化方向
加入卡尔曼滤波提升姿态解算精度，减少动态运动时的姿态漂移
基于地形轮廓实现主动悬挂补偿，调整电机高度差适应碎石路面
加入重载适配，根据负载动态调整增益参数，保持重载下的稳定性

6、轮式机器人IMU动态姿态调整+台阶/台阶地形补偿程序（核心：障碍识别+姿态补偿）
应用场景：轮式机器人上下小台阶、跨越缓坡、穿越门槛，核心解决“台阶冲击下的姿态稳定、台阶高度自适应补偿、上下坡姿态锁定”问题，适用于室内外过渡场景、多地形巡检。

核心逻辑
IMU 数据采集：重点监测俯仰角、加速度（识别台阶冲击与坡度）
台阶识别：通过加速度突变判断是否遇到台阶，记录台阶高度
姿态补偿：上下台阶时调整电机速度与方向，保持机身姿态稳定，避免前倾或后翻

// 动态姿态调整+台阶地形补偿程序
#include <Wire.h>
#include <MPU6050.h>

// ===== 硬件配置（两轮差速+前轮辅助） ==
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_DIR_A A2
#define LEFT_MOTOR_DIR_B A3
#define RIGHT_MOTOR_PWM 6
#define RIGHT_MOTOR_DIR_A A4
#define RIGHT_MOTOR_DIR_B A5
#define ULTRASOUND_TRIG A0
#define ULTRASOUND_ECHO A1
MPU6050 imu;

// ===== 控制参数 =====
const float BASE_SPEED = 28.0f;
const float STEP_COMP_GAIN = 0.8f;     // 台阶补偿增益
const float PITCH_CONTROL_GAIN = 1.2f; // 俯仰角控制增益
const float IMPACT_DETECT_THRESHOLD = 1.5f; // 冲击加速度阈值（g）
const int STEP_HEIGHT_MAX = 8;         // 可跨越最大台阶高度（cm）
const float ACC_FILTER = 0.3f;

// ===== 状态变量 =====
float pitch = 0, accZ = 0;
float stepHeight = 0;
bool onStairs = false;
bool impactDetected = false;
float leftSpeed = 0, rightSpeed = 0;

// ===== 函数声明 =====
void setupModules();
void readIMU();
float getDistance();
void detectStep();
void controlMotors();

void setup() {
  Serial.begin(115200);
  // 电机引脚初始化
  pinMode(LEFT_MOTOR_DIR_A, OUTPUT);
  pinMode(LEFT_MOTOR_DIR_B, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR_A, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR_B, OUTPUT);
  
  // 超声波引脚初始化
  pinMode(ULTRASOUND_TRIG, OUTPUT);
  pinMode(ULTRASOUND_ECHO, INPUT);
  
  // IMU 初始化
  setupModules();
  Serial.println("动态地形补偿机器人初始化完成");
  delay(1000);
}

void loop() {
  // 1. 读取 IMU 数据
  readIMU();
  
  // 2. 检测台阶与地形
  detectStep();
  int obstacleDistance = getDistance();
  
  // 3. 迎着台阶行驶时的补偿逻辑
  if (obstacleDistance < 15 && obstacleDistance > 0) {  // 检测到台阶距离
    onStairs = true;
    impactDetected = (abs(accZ - 9.81f) > IMPACT_DETECT_THRESHOLD);
    
    // 上台阶：减速+姿态前倾补偿
    if (pitch > 15 && !impactDetected) {  // 预测上台阶
      leftSpeed = BASE_SPEED * 0.6;
      rightSpeed = BASE_SPEED * 0.6;
      // 俯仰角补偿，防止后仰
      float pitchComp = (pitch - 15) * PITCH_CONTROL_GAIN * 0.5f;
      leftSpeed += pitchComp;
      rightSpeed += pitchComp;
      Serial.println("检测到台阶，上台阶补偿...");
    }
    // 下台阶：缓速+姿态后仰补偿
    else if (pitch < -15 && !impactDetected) {  // 预测下台阶
      leftSpeed = BASE_SPEED * 0.5;
      rightSpeed = BASE_SPEED * 0.5;
      // 俯仰角补偿，防止前倾
      float pitchComp = (-15 - pitch) * PITCH_CONTROL_GAIN * 0.5f;
      leftSpeed -= pitchComp;
      rightSpeed -= pitchComp;
      Serial.println("检测到台阶，下台阶补偿...");
    }
    // 台阶行驶中：保持稳定
    else if (onStairs) {
      leftSpeed = BASE_SPEED * 0.7;
      rightSpeed = BASE_SPEED * 0.7;
      // 动态姿态修正
      float pitchCorrection = -pitch * PITCH_CONTROL_GAIN * 0.3f;
      leftSpeed += pitchCorrection;
      rightSpeed += pitchCorrection;
    }
  } else {
    onStairs = false;
    impactDetected = false;
    // 平地行驶：正常速度+轻微姿态修正
    leftSpeed = BASE_SPEED + (-pitch * 0.2f);
    rightSpeed = BASE_SPEED + (-pitch * 0.2f);
  }
  
  // 4. 电机控制
  controlMotors();
  
  // 调试输出
  if (millis() % 500 == 0) {
    Serial.print("Pitch: "); Serial.print(pitch, 1);
    Serial.print("° | AccZ: "); Serial.print(accZ, 2);
    Serial.print("g | Dist: "); Serial.print(obstacleDistance);
    Serial.print("cm | OnStairs: "); Serial.println(onStairs);
  }
  
  delay(10);
}

// 模块初始化
void setupModules() {
  Wire.begin();
  imu.initialize();
  imu.setFullScaleAccRange(MPU6050_FS_ACC_4G);  // 适配台阶冲击
  imu.setFullScaleGyroRange(MPU6050_FS_GYRO_250);
}

// 读取 IMU 数据
void readIMU() {
  float x, y, z, gx, gy, gz;
  imu.getAcceleration(&acce速度X, &加速度Y, &加速度Z);
  imu.getMotion(&gx, &gy, &gz);
  
  accZ = 加速度Z;
  // 计算俯仰角
  pitch = atan2(-acce速度X, sqrt(加速度Y * 加速度Y + acce速度Z * acce速度Z)) * 180 / M_PI;
  // 低通滤波
  accZ = accZ * (1 - ACC_FILTER) + acce速度Z * ACC_FILTER;
  pitch = pitch * 0.8f + (atan2(-acce速度X, sqrt(加速度Y * 加速度Y + acce速度Z * acce速度Z)) * 180 / M_PI) * 0.2f;
}

// 超声波测距（检测台阶/障碍物）
float getDistance() {
  digitalWrite(ULTRASOUND_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(ULTRASOUND_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(ULTRASOUND_TRIG, LOW);
  
  float duration = pulseIn(ULTRASOUND_ECHO, HIGH);
  float distance = (duration * 0.0343f) / 2;  // 距离计算公式
  return constrain(distance, 0, 100);
}

// 台阶检测与高度估算
void detectStep() {
  // 基于加速度冲击判断是否遇到台阶
  if (abs(accZ - 9.81f) > IMPACT_DETECT_THRESHOLD) {
    // 估算台阶高度（简化算法，通过碰撞时间估算）
    stepHeight = abs(accZ - 9.81f) * 0.5;
    if (stepHeight > STEP_HEIGHT_MAX) {
      // 台阶过高，停止行驶
      leftSpeed = 0;
      rightSpeed = 0;
      Serial.println("台阶过高，禁止跨越！");
    }
  }
  // 行驶平稳后重置台阶状态
  if (abs(accZ - 9.81f) < 0.2f) {
    stepHeight = 0;
  }
}

// 电机控制
void controlMotors() {
  int leftPWM = constrain(leftSpeed * 9, 0, 255);
  int rightPWM = constrain(rightSpeed * 9, 0, 255);
  
  analogWrite(LEFT_MOTOR_PWM, leftPWM);
  analogWrite(RIGHT_MOTOR_PWM, rightPWM);
  
  // 前进方向
  digitalWrite(LEFT_MOTOR_DIR_A, HIGH);
  digitalWrite(LEFT_MOTOR_DIR_B, LOW);
  digitalWrite(RIGHT_MOTOR_DIR_A, HIGH);
  digitalWrite(RIGHT_MOTOR_DIR_B, LOW);
}

案例优化方向
加入视觉/激光传感器辅助台阶识别，提升台阶检测的准确率
加入台阶高度自适应算法，根据台阶高度动态调整补偿增益
加入运动状态识别，区分平地、上坡、下坡、台阶，实现差异化补偿

要点解读
要点1：IMU 传感器的“校准与数据融合”是姿态稳定的核心基础
IMU 是姿态反馈的核心传感器，其数据精度直接决定姿态稳定效果，校准与数据融合是核心前提。
静态校准：必须进行静态零点校准，消除传感器零点漂移。案例中通过 2~3 秒的静态采集，计算姿态角的平均值作为零点偏移，校准后姿态角误差可降低 80% 以上
量程适配：根据运动场景选择合适的传感器量程，如平地巡检用±2g 加速度计，崎岖路面/台阶场景用±4g，避免数据饱和导致姿态解算失真
数据融合：采用互补滤波（简单高效）或卡尔曼滤波（精度更高）融合加速度与角速度数据，弥补单一传感器的局限性（加速度易受振动干扰，陀螺仪易积分漂移）

要点2：姿态解算需聚焦“姿态角优先级”，把握地形补偿的核心维度
姿态稳定需明确不同姿态角的作用，针对性设计补偿策略，避免盲目控制。
横滚角优先：横滚角直接反映机身侧翻风险，是安全防护的第一优先级，必须设置严格的阈值保护，超限立即停车
俯仰角核心：俯仰角直接对应路面坡度与台阶上下姿态，是地形补偿的核心维度，通过俯仰角计算坡度与台阶状态，实现速度与姿态的联动补偿
偏航角辅助：偏航角用于保持行驶方向，在全向运动场景下需参与补偿，普通直线行驶场景可弱化，重点保证机身稳定

要点3：地形补偿遵循“分级适配”原则，实现动态与静态的平衡
不同地形（平地、缓坡、台阶、崎岖路）的补偿需求不同，需设计分级适配的控制逻辑。
平地适配：仅保留微小的姿态修正，避免频繁调整电机导致行驶抖动，采用死区过滤消除微小姿态波动
坡地/台阶适配：根据坡度/台阶高度动态调整速度，上坡减速防后翻、下坡缓速防前倾，确保行驶平稳
崎岖路面适配：通过加速度滑动平均提取地形轮廓，实现电机的协同补偿，减少颠簸对姿态的影响，重载场景需额外调整增益

要点4：控制算法需具备“安全优先+死区过滤”，保障运行可靠性
姿态稳定控制需将安全机制放在首位，同时优化细节提升控制流畅度。
安全阈值保护：必须设置倾斜角最大阈值，当机身倾斜超过安全范围时立即停车，避免翻倒或跌落，同时加入电流过载保护，异常时切断动力
死区过滤：设置姿态角死区，当姿态偏差小于阈值时不进行补偿，避免电机频繁启停或抖动，提升行驶平顺性
低通滤波：对 IMU 数据进行低通滤波，屏蔽路面振动、电机振动带来的高频噪声，保证姿态数据的稳定性

要点5：“硬件协同+参数适配”是实现复杂地形适应的关键支撑
姿态稳定与地形补偿不仅是算法问题，需硬件与参数的协同优化。
电机闭环控制：必须采用带编码器的 BLDC 电机，实现速度闭环，地形变化时电机能快速响应调整速度，避免速度失稳
动态参数适配：根据机器人负载、行驶速度动态调整控制增益，重载时增大补偿增益，高速时降低增益避免震荡
硬件减震适配：合理设计机械减震结构，减少路面冲击对 IMU 数据的影响，同时配合算法滤波，实现硬件与算法的协同稳定