Arduino BLDC 之野外巡检机器人四轴独立调平，本质上是“IMU 姿态感知 + 四角高度解算 + 独立 PID 闭环 + 丝杠/直线执行器升降”的主动调平系统：IMU 实时测量机身俯仰角和横滚角，控制器把姿态偏差换算成四个支撑点的目标高度，再由四个直线执行器分别伸缩，使搭载传感器或作业平台的上层平面在斜坡、碎石、沟坎等不平地面上保持水平。 它的核心价值不是让机器人“站得稳”，而是让巡检设备在复杂地形上仍能保持传感器视场稳定、测量基准一致和作业平面可靠。

主要特点
. IMU 提供姿态基准，但必须做滤波和补偿
调平系统通常以 MEMS IMU 的加速度计和陀螺仪融合得到俯仰角和横滚角。加速度计提供重力方向参考，陀螺仪提供角速度响应，两者通过互补滤波或卡尔曼滤波融合后，可兼顾静态精度和动态抗扰。 需要注意，IMU 在电机启动、加减速或振动较大时容易受干扰，因此不能直接裸用原始数据。若使用含磁力计的九轴 IMU，还需远离电机和动力线，或进行硬铁/软铁补偿。
. 四轴高度解算把姿态误差映射为四个支撑点位移
调平的关键不是分别控制四个腿，而是先根据目标姿态计算四个角的目标高度。常用模型是：俯仰角偏差主要影响前后高度差，横滚角偏差主要影响左右高度差，再结合轴距和轮距换算成各支撑点的升降量。 例如前左、前右、后左、后右四个点会根据 pitch/roll 偏差产生不同的高度补偿，从而让上层平台绕机身中心旋转至水平。
. 独立 PID 控制保证四轴同步收敛
每个直线执行器可视为一个独立的位置/高度控制通道。外环以姿态误差或高度误差为输入，内环以执行器位移或电机速度为反馈，形成闭环。若只调平两个轴，容易出现三点支撑稳定、第四点悬空或受力的问题；四轴独立控制时，则必须处理超定位和受力不均，通常需要加入压力检测、位移反馈或柔性支撑结构。
. 丝杠/直线执行器具备自锁和高承载优势
相比气动或液压方案，电动丝杠和直线执行器结构紧凑、控制简单、具备机械自锁能力，适合野外巡检机器人长时间保持姿态。 丝杠导程决定升降速度和分辨率，减速比和电机编码器决定位置精度。若使用带电位器、霍尔位移或磁栅尺反馈的执行器，可实现更可靠的位置闭环；若只有开环行程，则需依赖限位开关和电流估算，调平精度和安全性会下降。
. 调平可与 BLDC 底盘解耦，但需共享安全策略
BLDC 负责机器人移动、差速驱动和越野通过；调平系统负责上层平台姿态稳定。两者可以分层运行：底盘根据地形调整速度和路径，调平系统根据 IMU 调整支撑高度。但在大坡度、急转弯或单侧悬空时，调平动作会改变重心分布，必须与底盘速度、制动和倾角保护联动，避免翻车。

应用场景
变电站/电力巡检：搭载可见光、红外热像仪或局放传感器时，需要平台保持水平，确保测温、读表和缺陷识别角度稳定。
野外地质与环境监测：在坡地、碎石路或泥泞地面上部署测量仪器时，主动调平可保证传感器基准面一致。
管道与隧道巡检：地面存在沟槽、台阶和倾斜段，调平平台可减少相机和激光传感器抖动。
农业与林业巡检：田间垄沟、斜坡和不平路面较多，调平系统可提升图像采集和作物监测质量。
科研与教学平台：用于验证 IMU 姿态解算、四轴高度分配、PID 调平、丝杠位置闭环和底盘姿态耦合控制。

需要注意的事项
四支点调平存在超定位风险
刚性四支点很难保证四点同时均匀受力，容易出现“三条腿着地、一条腿悬空”或局部过载。可通过柔性支撑、压力传感器、位移反馈或三点主调平 + 第四点随动来缓解。
IMU 安装位置必须靠近调平平台重心
若 IMU 安装在振动较大的底盘边缘，会引入额外角加速度和振动噪声。应尽量安装在调平平台中心，并使用减振垫或软安装结构。
执行器必须有限位和防堵转保护
丝杠或直线执行器到达行程极限后若继续驱动，会造成电机堵转、齿轮损坏或驱动过热。应设置硬件限位开关、电流检测和软件行程保护，并在异常时立即停机。
PID 参数需区分粗调和平稳保持
倾角较大时应允许较快升降，接近目标姿态时应降低增益，避免超调和振荡。可加入输出限幅、斜率限制和积分抗饱和，防止执行器频繁往复。
电源隔离和 EMC 设计不可忽视
四个执行器同时动作时电流冲击较大，可能干扰 IMU、编码器和主控。控制电源与动力电源应隔离，敏感信号线使用屏蔽线，并远离电机动力线。
调平过程中应限制底盘运动
在大幅调平或单侧支撑受力异常时，不应允许机器人高速转向或急加速。可设置“调平中限速”“倾角超限停机”和“重心异常报警”等安全策略。
Arduino 平台需控制实时负载
若同时运行 BLDC 底盘控制、IMU 滤波、四轴 PID 和执行器驱动，AVR 系列 MCU 可能吃力。建议把 IMU 滤波和调平 PID 放在固定周期中断中，复杂任务交给 ESP32、Teensy 或上位机。

1、基于底盘几何的高度解算 + 四轴独立PID
将 IMU 的 pitch/roll 角映射为四个丝杠的目标高度差，每个轴独立 PID 跟踪。这是四轴调平最核心的数学框架。

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

MPU6050 imu;

// 底盘几何参数 (cm)
const float WHEELBASE = 40.0;   // 前后轴距
const float TRACK_WIDTH = 30.0; // 左右轮距

// 四轴丝杠控制引脚 (FL, FR, BL, BR)
const int ACT_DIR[4] = {2, 4, 7, 8};
const int ACT_PWM[4] = {3, 5, 9, 10};

// 四轴PID
double setpoint[4] = {0, 0, 0, 0};
double input[4] = {0, 0, 0, 0};
double output[4] = {0, 0, 0, 0};
PID pid[4] = {
  PID(&input[0], &output[0], &setpoint[0], 8.0, 0.5, 2.0, DIRECT),
  PID(&input[1], &output[1], &setpoint[1], 8.0, 0.5, 2.0, DIRECT),
  PID(&input[2], &output[2], &setpoint[2], 8.0, 0.5, 2.0, DIRECT),
  PID(&input[3], &output[3], &setpoint[3], 8.0, 0.5, 2.0, DIRECT)
};

// 目标姿态
float targetPitch = 0.0;
float targetRoll = 0.0;

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

  for (int i = 0; i < 4; i++) {
    pinMode(ACT_DIR[i], OUTPUT);
    pinMode(ACT_PWM[i], OUTPUT);
    pid[i].SetMode(AUTOMATIC);
    pid[i].SetOutputLimits(-255, 255);
  }
}

// 读取IMU融合姿态（简化互补滤波）
void readAttitude(float &pitch, float &roll) {
  int16_t ax, ay, az, gx, gy, gz;
  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  static float pitchEst = 0, rollEst = 0;
  const float alpha = 0.98;
  float dt = 0.01;

  float accPitch = atan2(-ax, sqrt((long)ay*ay + (long)az*az)) * 180.0 / PI;
  float accRoll  = atan2(ay, az) * 180.0 / PI;

  pitchEst = alpha * (pitchEst + gx / 131.0 * dt) + (1 - alpha) * accPitch;
  rollEst  = alpha * (rollEst  + gy / 131.0 * dt) + (1 - alpha) * accRoll;

  pitch = pitchEst;
  roll  = rollEst;
}

// 几何解算：pitch/roll → 四角目标高度差
// 参考 RM 四轴底座调平算法[citation:1]与丘陵农机最低点跟踪策略[citation:5]
void computeTargetHeights(float pitch, float roll, float heights[4]) {
  // 将角度差转换为高度差 (cm)
  // pitch 影响前后，roll 影响左右
  float pitchRad = (pitch - targetPitch) * PI / 180.0;
  float rollRad  = (roll - targetRoll) * PI / 180.0;

  // 前后高度差 = 轴距 * sin(pitch)
  float dFrontBack = WHEELBASE * sin(pitchRad) / 2.0;
  // 左右高度差 = 轮距 * sin(roll)
  float dLeftRight = TRACK_WIDTH * sin(rollRad) / 2.0;

  // 最低点跟踪：以最低角为基准，其余轴向上延伸
  // FL = 左下前, FR = 右下前, BL = 左后, BR = 右后
  float baseHeight = 0.0;

  heights[0] = baseHeight + dFrontBack + dLeftRight; // FL
  heights[1] = baseHeight + dFrontBack - dLeftRight; // FR
  heights[2] = baseHeight - dFrontBack + dLeftRight; // BL
  heights[3] = baseHeight - dFrontBack - dLeftRight; // BR

  // 归一化：确保最低轴为0（最低点跟踪策略）
  float minH = heights[0];
  for (int i = 1; i < 4; i++) if (heights[i] < minH) minH = heights[i];
  for (int i = 0; i < 4; i++) heights[i] -= minH;
}

void loop() {
  float pitch, roll;
  readAttitude(pitch, roll);

  float targetH[4];
  computeTargetHeights(pitch, roll, targetH);

  // 每轴PID跟踪目标高度
  for (int i = 0; i < 4; i++) {
    setpoint[i] = targetH[i];
    // input[i] 应来自丝杠编码器或位置反馈，此处暂用目标值占位
    input[i] = setpoint[i]; // TODO: 替换为实际位置反馈
    pid[i].Compute();

    // 输出到H桥
    digitalWrite(ACT_DIR[i], output[i] > 0 ? HIGH : LOW);
    analogWrite(ACT_PWM[i], abs(output[i]));
  }

  delay(20);
}

来源：该结构参考了 RM2026 四轴底座调平算法的“几何解算+独立PID”框架，以及丘陵农机底盘“跟踪最低点平面”的策略。

2、双轴PID + 四轴映射（Arduino Forum 实现）
将 pitch 和 roll 分别用两个 PID 控制器处理，输出值映射到四个执行器。这是 Arduino Forum 上已验证可行的简化方案。

#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <PID_v1.h>

Adafruit_MPU6050 mpu;

// 双轴PID目标
double setpointX = 0, setpointY = 0;
double inputX, inputY;
double outputX, outputY;

// 双轴PID参数（Arduino Forum 实测值）
double kp = 4e-4, ki = 2e-6, kd = 7e-3;
PID pidX(&inputX, &outputX, &setpointX, kp, ki, kd, DIRECT);
PID pidY(&inputY, &outputY, &setpointY, kp, ki, kd, DIRECT);

// L298N 驱动四个执行器
const int ENA[4] = {3, 6, 9, 12};
const int IN1[4] = {4, 7, 10, 13};
const int IN2[4] = {5, 8, 11, 14};

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

  if (!mpu.begin()) {
    Serial.println("MPU6050 未连接");
    while (1) delay(10);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  pidX.SetMode(AUTOMATIC);
  pidY.SetMode(AUTOMATIC);

  for (int i = 0; i < 4; i++) {
    pinMode(ENA[i], OUTPUT);
    pinMode(IN1[i], OUTPUT);
    pinMode(IN2[i], OUTPUT);
  }
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // 加速度计计算倾角
  inputX = atan2(a.acceleration.y, a.acceleration.z) * 180 / PI;
  inputY = atan2(a.acceleration.x, a.acceleration.z) * 180 / PI;

  pidX.Compute();
  pidY.Compute();

  // 双轴输出 → 四轴映射
  // X轴影响前后轴，Y轴影响左右轴
  controlActuators(outputX, outputY);

  delay(20);
}

void controlActuators(double outX, double outY) {
  // 将PID输出映射到PWM范围
  int speedFL = map(outX + outY, -100, 100, -255, 255);
  int speedFR = map(outX - outY, -100, 100, -255, 255);
  int speedBL = map(-outX + outY, -100, 100, -255, 255);
  int speedBR = map(-outX - outY, -100, 100, -255, 255);

  int speeds[4] = {speedFL, speedFR, speedBL, speedBR};

  for (int i = 0; i < 4; i++) {
    analogWrite(ENA[i], abs(speeds[i]));
    digitalWrite(IN1[i], speeds[i] > 0 ? HIGH : LOW);
    digitalWrite(IN2[i], speeds[i] > 0 ? LOW : HIGH);
  }
}

来源：该实现来自 Arduino Forum 社区已验证的自调平平台方案，PID 参数为实测值。其核心思路是“双轴PID分别控制俯仰和横滚，输出叠加到四个执行器”。

3、状态机取零限位 + 软限位保护（工业级流程）
调平前必须先建立各轴的“零点”（通过底部限位开关），再执行调平。同时用软限位防止冲顶。这是 RM 方案中明确强调的流程。

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

MPU6050 imu;

// 四轴丝杠
const int LIMIT_SW[4] = {A0, A1, A2, A3}; // 底部限位开关（常闭）
const int ACT_DIR[4] = {2, 4, 7, 8};
const int ACT_PWM[4] = {3, 5, 9, 10};

// 软限位（脉冲数或位置值）
const int SOFT_LIMIT_HIGH = 2000;
int axisPos[4] = {0, 0, 0, 0};      // 各轴当前位置估计
int axisZero[4] = {0, 0, 0, 0};     // 零点位置

// 状态机
enum LevelState {
  STATE_IDLE,
  STATE_ZERO_CAL,      // 取零点
  STATE_MOVE_UP,       // 上行一段距离
  STATE_LEVELING,      // 调平中
  STATE_LEVELED,       // 调平完成
  STATE_LOWERING,      // 下降
  STATE_ERROR
};
LevelState state = STATE_IDLE;

// 调平参数
const float TARGET_PITCH = 0.0;
const float TARGET_ROLL = 0.0;
const float ANGLE_TOLERANCE = 0.5;   // 度
const int HOLD_TIME = 500;            // 保持时间 ms

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

  for (int i = 0; i < 4; i++) {
    pinMode(LIMIT_SW[i], INPUT_PULLUP);
    pinMode(ACT_DIR[i], OUTPUT);
    pinMode(ACT_PWM[i], OUTPUT);
  }

  state = STATE_ZERO_CAL;
}

// 读取IMU姿态（简化）
void readAttitude(float &pitch, float &roll) {
  int16_t ax, ay, az;
  imu.getAcceleration(&ax, &ay, &az);
  pitch = atan2(-ax, sqrt((long)ay*ay + (long)az*az)) * 180.0 / PI;
  roll  = atan2(ay, az) * 180.0 / PI;
}

// 检查限位
bool checkLimit(int axis) {
  return digitalRead(LIMIT_SW[axis]) == LOW; // 常闭，断开=触发
}

// 单轴向下运动直到触发限位（取零点）
void homeAxis(int axis, int speed) {
  digitalWrite(ACT_DIR[axis], LOW); // 假设LOW=向下
  analogWrite(ACT_PWM[axis], speed);

  unsigned long startTime = millis();
  while (!checkLimit(axis)) {
    if (millis() - startTime > 10000) { // 超时保护
      analogWrite(ACT_PWM[axis], 0);
      state = STATE_ERROR;
      Serial.print("[错误] 轴"); Serial.print(axis); Serial.println(" 取零超时");
      return;
    }
    delay(1);
  }
  analogWrite(ACT_PWM[axis], 0);
  axisZero[axis] = axisPos[axis];
  Serial.print("[取零] 轴"); Serial.print(axis); Serial.print(" 零点="); Serial.println(axisZero[axis]);
}

// 软限位检查
bool checkSoftLimit(int axis, int targetPos) {
  if (targetPos > SOFT_LIMIT_HIGH || targetPos < axisZero[axis]) {
    return false;
  }
  return true;
}

void loop() {
  switch (state) {
    case STATE_ZERO_CAL:
      Serial.println("[状态] 开始取零点...");
      for (int i = 0; i < 4; i++) {
        homeAxis(i, 80); // 慢速下行
        if (state == STATE_ERROR) return;
      }
      state = STATE_MOVE_UP;
      break;

    case STATE_MOVE_UP:
      // 上行一段距离，给调平留出空间
      for (int i = 0; i < 4; i++) {
        digitalWrite(ACT_DIR[i], HIGH); // 上行
        analogWrite(ACT_PWM[i], 100);
      }
      delay(500);
      for (int i = 0; i < 4; i++) analogWrite(ACT_PWM[i], 0);
      state = STATE_LEVELING;
      break;

    case STATE_LEVELING:
    {
      float pitch, roll;
      readAttitude(pitch, roll);

      // 误差在允许范围内持续一段时间后判定完成
      static unsigned long inRangeStart = 0;
      if (abs(pitch - TARGET_PITCH) < ANGLE_TOLERANCE &&
          abs(roll - TARGET_ROLL) < ANGLE_TOLERANCE) {
        if (inRangeStart == 0) inRangeStart = millis();
        if (millis() - inRangeStart > HOLD_TIME) {
          state = STATE_LEVELED;
          Serial.println("[状态] 调平完成");
        }
      } else {
        inRangeStart = 0;
      }

      // TODO: 调用案例一中的几何解算 + PID 控制
      // 简化：用比例控制
      int speed = constrain(abs(pitch) * 5, 0, 150);
      if (pitch > TARGET_PITCH + ANGLE_TOLERANCE) {
        // 前倾，前轴上升或后轴下降
      }
      // ... 具体控制逻辑从略 ...
      break;
    }

    case STATE_LEVELED:
      // 保持姿态，等待上层指令
      break;

    case STATE_LOWERING:
      // 四轴同步下降，直到某一轴触发限位，再重新解算
      for (int i = 0; i < 4; i++) {
        digitalWrite(ACT_DIR[i], LOW);
        analogWrite(ACT_PWM[i], 80);
      }
      // 等待第一个限位触发
      for (int i = 0; i < 4; i++) {
        if (checkLimit(i)) {
          for (int j = 0; j < 4; j++) analogWrite(ACT_PWM[j], 0);
          // 剩余三轴根据当前姿态重新解算
          state = STATE_LEVELING;
          break;
        }
      }
      break;

    case STATE_ERROR:
      // 所有电机停止
      for (int i = 0; i < 4; i++) analogWrite(ACT_PWM[i], 0);
      break;
  }

  delay(10);
}

来源：状态机取零流程参考了 RM2026 开源方案中的 ZERO_CALIBRATE -> LEVEL -> LOWERING 任务流程，软限位保护思路来自该方案的“底部限位防冲底、上部软限位防冲顶”设计。

要点解读

1. 四轴调平的数学核心是“几何解算”，不是“姿态控制”
   三个轴就能确定一个平面，但四轴方案的优势在于大负载且负载不均时更稳定（如 yaw 方向偏载）。几何解算的本质是：将 IMU 测得的 pitch/roll 角差，通过 前后高度差 = 轴距 × sin(pitch)、左右高度差 = 轮距 × sin(roll) 转换为四个角的目标高度。案例一中的 computeTargetHeights 就是这个转换的核心。

2. 最低点跟踪策略避免了“同时上升或下降”的冲突
   传统调平可能要求某些轴上升、某些轴下降，但丝杠行程有限。最低点跟踪策略（案例一中的 minH 归一化）确保最低的那个角不动，其余三个角向上调整。丘陵农机四轮自适应底盘也采用了类似策略，这样调平过程中底盘重心不会异常升高。

3. 双轴 PID 映射四轴是“够用就好”的务实方案
   案例二展示了 Arduino Forum 上的已验证方案：用两个 PID（pitch 和 roll）分别计算输出，再通过 outX ± outY 的组合映射到四个执行器。这种方案比“四轴独立 PID”计算量更小，在 Arduino Uno 上也能流畅运行。代价是无法处理四个角的独立位置反馈，但对于野外巡检的“保持水平”需求已经足够。

4. 取零点流程是四轴丝杠调平的“前置条件”
   四轴丝杠通常使用带减速的直流电机或 BLDC，没有绝对编码器。因此每次上电后必须先执行 ZERO_CALIBRATE：让每个轴向下运动直到触发底部限位开关，以此位置作为该轴的“零点”。没有零点，后续的高度解算就失去了参考基准。案例三的状态机中，homeAxis 函数就是这个过程，超时保护不可省略。

5. 软限位和硬限位缺一不可
   底部硬限位（限位开关）防止丝杠冲底损坏机械结构；上部软限位（软件位置限制）防止丝杠冲顶或超出安全行程。案例三中的 SOFT_LIMIT_HIGH 就是软限位阈值，应在每次运动前用 checkSoftLimit 校验目标位置。野外环境下，限位开关的可靠性直接决定系统能否安全运行。

4、IMU倾角采集+四轴独立PID基础调平
适用场景：野外平地、缓坡巡检场景，需保证机身姿态水平，承载巡检设备稳定运行。

#include <SimpleFOC.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
Adafruit_MPU6050 mpu;
SimpleFOCMotor2 actuator[4] = {
  SimpleFOCMotor2(2,3,4,A0,A1), // 前左支撑轴
  SimpleFOCMotor2(5,6,7,A2,A3), // 前右支撑轴
  SimpleFOCMotor2(8,9,10,A4,A5), // 后左支撑轴
  SimpleFOCMotor2(11,12,13,A6,A7)  // 后右支撑轴
};
float target_level_x = 0.0f, target_level_y = 0.0f; // 目标水平倾角，单位度
float kp = 0.8f, ki = 0.02f, kd = 0.1f;

// 读取IMU双轴倾角
void get_imu_angle(float *x, float *y){
  sensors_event_t a;
  mpu.getEvent(&a);
  *x = atan2f(a.acceleration.y, a.acceleration.z) * 180.0f / M_PI;
  *y = atan2f(-a.acceleration.x, a.acceleration.z) * 180.0f / M_PI;
}

void setup(){
  Serial.begin(9600);
  Wire.begin();
    mpu.begin();
  mpu.setFullScaleRange(MPU_FS_2G);
  mpu.setLowPassFilter(MPU_LOW_PASS_BANDWIDTH_20HZ);
  for(int i=0; i<4; i++){
    actuator[i].init();
    actuator[i].controller = FOC_POSITION; // 位置闭环，控制丝杠行程
    actuator[i].voltage_limit = 6;
    actuator[i].target = 0;  }
}

void loop(){
  delay(10);
  float cur_x, cur_y;
  get_imu_angle(&cur_x, &cur_y);
  // 四轴独立补偿：根据倾角分配行程
  float target1 = (target_level_x + 1.0f); // 前左轴行程，单位cm
  float target2 = (target_level_x - 1.0f); // 前右轴行程
  float target3 = (target_level_y + 1.0f); // 后左轴行程
  float target4 = (target_level_y - 1.0f); // 后右轴行程
  // 误差修正，引入PID
  for(int i=0; i<4; i++){
    float target = (i==0?target1:i==1?target2:i==2?target3:target4);
    float error = target - actuator[i].shaft_angle;
    float u = kp * error + ki * id(error) + kd * (error - id(error));
    actuator[i].setVoltage(constrain(u, -6, 6));
  }
}

5、地形自适应+四轴动态调平
适用场景：野外草地、碎石路等不平整地形，需根据地形起伏实时调整支撑高度，保证机身稳定。

#include <SimpleFOC.h>
#include <Adafruit_MPU6050.h>
Adafruit_MPU6050 mpu;
SimpleFOCMotor2 actuator[4] = {/* 定义同案例1 */};
float terrain_height[4] = {0}; // 地形高度，单位cm
float target_height[4] = {5.0f, 5.0f,5.0f,5.0f}; // 目标支撑高度
float adapt_gain = 0.5f;// 自适应增益

// 地形探测：通过足端距离传感器获取地面高度
void detect_terrain(){
  // 假设4个足端配置红外测距模块，输出0-1023对应0-20cm
  terrain_height[0] = analogRead(A0) * 20.0f / 1023.0f;
  terrain_height[1] = analogRead(A1) * 20.0f / 1023.0f;
  terrain_height[2] = analogRead(A2) * 20.0f / 1023.0f;
  terrain_height[3] = analogRead(A3) * 20.0f / 1023.0f;
}

// 结合IMU倾角与地形高度，规划目标支撑高度
void plan_actuator_target(){
  float cur_x, cur_y;
  get_imu_angle(&cur_x, &cur_y);
  // 地形最高的轴支撑高度最大，保证机身水平
  float max_h = max(max(terrain_height[0], terrain_height[1]), max(terrain_height[2], terrain_height[3]));
  for(int i=0; i<4; i++){
    target_height[i] = 10.0f - terrain_height[i] + (cur_x * 0.05f); // 结合倾角补偿
  }
  // 避免行程越限
  for(int i=0; i<4; i++){
    target_height[i] = constrain(target_height[i], 0.5f, 20.0f);
  }
}

void loop(){
  delay(10);
  detect_terrain();
  plan_actuator_target();
  // 动态调平：先快速接近目标位置，再精调角度
  for(int i=0; i<4; i++){
float error = target_height[i] - actuator[i].shaft_angle;
    float u = adapt_gain * (kp * error + ki *id(error));
    if(abs(error) > 1.0f){
      u = 1.2f * error; // 快速响应
    }
    actuator[i].setVoltage(constrain(u, -6, 6));
  }
}

6、故障容错+剩余轴调平
适用场景：野外复杂环境，单轴执行器或传感器故障时，需用剩余3轴重新调平，保证机器人不倾倒。

#include <SimpleFOC.h>
#include <Adafruit_MPU6050.h>
Adafruit_MPU6050 mpu;
SimpleFOCMotor2 actuator[4] = {/* 定义同案例1 */};
bool axis_fault[4] = {false,false,false,false};
float target_height[4] = {5.0f,5.0f,5.0f,5.0f};
float fault_tol_deg = 5.0f; // 角度容错阈值

// 故障检测：电流/温度/行程异常
void detect_fault(){
  for(int i=0; i<4; i++){
    if(abs(actuator[i].current) > 12.0f || actuator[i].shaft_angle > 21.0f){
      axis_fault[i] = true;
      actuator[i].setVoltage(0); // 立即停机
    }
  }
}

// 单轴故障，剩余3轴重新分配支撑目标
void replan_for_fault(){
  float cur_x, cur_y;
  get_imu_angle(&cur_x, &cur_y);
  int valid_count = 4;
  for(int i=0; i<4; i++){ if(axis_fault[i]) valid_count--; }
  // 前方单轴故障，抬高其余前轴补偿角度
  if(axis_fault[0] && valid_count==3){
    target_height[1] += cur_x * 2.0f;
    target_height[2] = target_height[3] = 5.0f;
  }
  // 对角单轴故障，提升另一对角轴平衡
  else if(axis_fault[0] && axis_fault[3] && valid_count==2){
    target_height[1] = 8.0f;
    target_height[2] = 8.0f;
  }
  // 无故障正常规划
  else{
    for(int i=0; i<4; i++) target_height[i] = 5.0f;
  }
  for(int i=0; i<4; i++){
    target_height[i] = constrain(target_height[i], 0.5f, 20.0f);
  }
}

void loop(){
  delay(10);
  detect_fault();
  replan_for_fault();
  for(int i=0; i<4; i++){
    if(axis_fault[i]) continue;
    float error = target_height[i] - actuator[i].shaft_angle;
float u = 1.0f * error + 0.05f * id(error);
    actuator[i].setVoltage(constrain(u, -6, 6));
  }
}

要点解读
IMU滤波与倾角解算精度是调平核心
野外振动大，IMU原始数据需做低通滤波+滑动平均，避免抖动；倾角需通过加速度解算（静态精度达0.1°），高精度场景可融合陀螺仪积分，同时电机需做高频噪声抑制，防止执行器小幅振荡。

调平控制需区分静态与动态场景
静态巡检要求保持水平，采用位置闭环PID，增益适中避免超调；动态越障/行走时需降低增益、加入前馈，平衡调平精度与快速响应，避免因行走冲击导致姿态失稳。

地形自适应需先探知、后规划，避免盲目调整
需先通过足端距离传感器获取各支撑点的地形高度，优先保证最高点支撑稳定，再调整低支撑点；同时结合IMU倾角补偿，确保机身即使遇到斜坡也能维持水平，而非仅追求支撑等高。

故障检测需加入多维度判据，避免误判
需同时监测电流、行程、电机温度，单一异常仅触发预警，双重异常才判定故障；同时设置故障恢复逻辑，手动复位后可重新启用，避免因瞬时干扰导致单轴永久停机。

剩余轴调平需遵循重心约束，符合机械安全极限
单轴故障时，剩余执行器的行程与支撑力需保证机器人重心在支撑面内，避免倾倒；同时调平过程中需限制最大加速度，防止机身晃动过大损坏搭载的巡检设备。

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