这个题目可以拆成两层理解：一是 Arduino BLDC 控制链，负责电机驱动、姿态保持、推进器分配和采样机构稳定；二是 输油管道泄漏水下巡检机器人，负责在强电磁、流场扰动、密闭管道/水下环境中完成定位、巡检、泄漏识别和采样。

一、主要特点

1. 无刷电机更适合水下与管道环境
   BLDC 没有碳刷和换向器，消除了电火花和机械磨损，比有刷电机更适合潮湿、密闭、易燃易爆或水下环境。对巡检机器人来说，这意味着长时间作业更可靠，也更容易做成 IP68 级密封结构。
2. FOC 与多闭环控制提升抗扰能力
   BLDC 如果只做简单 PWM 调速，遇到水流冲击、管道摩擦突变、采样负载变化时容易抖动。更合理的做法是建立电流环、速度环、位置环或姿态环的级联控制，配合 FOC 降低转矩脉动。对水下机器人来说，推进器通常还要做矢量分配，用姿态角、IMU、深度计、DVL 等反馈实时抵消流场扰动。
3. 强电磁干扰下要重点隔离电机侧与采样侧
   BLDC 驱动器的高频开关、长动力线、大电流回路都会产生 EMI；管道内还可能有泵、阀门、阴保系统、金属管壁反射等复杂电磁环境。采样传感器如果和电机共用电源、共地混乱或走线靠近动力线，数据很容易被污染。工程上通常要做动力电源与控制电源隔离、屏蔽双绞线、差分采样、硬件低通滤波、单点接地和软件滤波。
4. 流场抗扰不能只靠“加大推力”
   水下或管内流场不是恒定阻力，可能包含涡流、回流、局部加速和姿态耦合。机器人如果只靠增大推进器功率，会出现振荡、漂移和功耗飙升。更合适的是把流场扰动视为“合成扰动”，用姿态闭环、推力分配、前馈补偿或扰动观测器来抑制。对采样任务来说，机身稳定比瞬时速度更重要，因为采样口、摄像头、传感器都需要相对稳定的位姿。
5. Arduino 更适合做上层控制与原型验证
   Arduino 平台可以快速验证 BLDC 驱动、传感器读取、PID 整定和通信协议；但如果要做高频电流环、多推进器矢量控制、复杂状态估计或高可靠水下作业，通常需要把关键控制放到更高算力的 MCU/DSP 上，Arduino 负责任务调度、日志、通信和调试。

二、典型应用场景
输油/输水管道内巡检
机器人沿管内行进，检测焊缝、腐蚀、沉积、裂纹和泄漏点。BLDC 驱动轮组或推进器，编码器用于计米和防滑修正，摄像头或声学传感器用于缺陷识别。
水下管道外壁巡检
ROV 或管外爬行机器人靠近海底/水下管道，检查防腐层破损、悬空、冲刷坑和泄漏羽流。此时流场抗扰更关键，需要推进器矢量控制保持姿态稳定。
泄漏点采样与取证
在疑似泄漏区域，机器人需要稳定悬停或低速贴管移动，控制采样泵、阀门或吸附机构完成液体/气体采样。采样瞬间对姿态和位置稳定性要求很高，否则采样量、采样位置和传感器读数都会失真。
高危环境替代人工
包括深水、浑浊水域、含油污水、狭窄管廊、强电磁干扰区域。机器人进入后通过有线光纤/网线或抗干扰无线链路回传视频、传感器数据和故障状态。

三、需要注意的事项

1. 电源隔离要优先做
   电机启动、堵转、制动和 PWM 换相都会产生电流尖峰。BLDC 驱动电源必须与 Arduino、传感器、通信模块电源隔离，控制侧用独立 LDO/DC-DC 供电，并在电源入口布置电解电容和陶瓷电容。
2. 编码器、IMU 和采样信号线必须抗干扰
   编码器 A/B 相、IMU SPI/I2C、压力传感器、浊度/气体/油膜传感器都应使用屏蔽线或差分接口，远离电机三相线和电源线。必要时加磁环、RC 滤波、光耦隔离或数字隔离器
3. 控制频率要分层
   电流环通常需要较高频率，速度环/姿态环居中，路径规划和采样任务管理可以较慢。不要把所有逻辑都塞进同一个 loop()，否则高频控制会被串口打印、网络通信或传感器读取拖慢。
4. 采样数据要做“硬件滤波 + 软件滤波”双重保护
   硬件端用屏蔽、滤波、隔离降低噪声；软件端用中值滤波、滑动平均、卡尔曼滤波或扰动观测器剔除异常点。尤其在水流突变、推进器加减速、阀门动作瞬间，采样数据最容易被污染。
5. 防水、耐压和散热不能只靠外壳
   水下电机和推进器需要可靠密封、压力补偿和耐腐蚀材料；密闭管道内散热条件差，长时间大负载运行要监测温度，避免磁钢退磁、驱动器过热或密封件老化。
6. 必须设置安全边界
   包括最大推进器输出、最大电流、最大姿态角、最大深度、采样超时、通信丢失急停、堵转保护和看门狗。水下和管内场景一旦失控，回收成本远高于普通地面机器人。

从控制角度看，这个系统的难点不在“让电机转起来”，而在 让采样传感器在强电磁和流场扰动下仍然可信。 BLDC 负责提供稳定、可预测的动力；FOC 和多闭环负责抑制扰动；隔离、滤波和状态机负责保证数据与安全边界。

1、BLDC 电磁干扰前馈补偿 + 磁力仪采样抗扰（强电磁对抗）
适用场景：输油管道为铁磁性材料，巡检机器人需要依靠磁力仪检测管壁缺陷或泄漏点。但 BLDC 推进器的 PWM 开关和换向尖峰会产生强电磁干扰，严重扭曲磁力仪读数。核心逻辑是：在 BLDC 速度环运行的同时，根据电机转速和电流指令生成“干扰预测值”，从磁力仪原始数据中减去该预测值，恢复管道磁场的真实信号。

#include <SimpleFOC.h>
#include <Wire.h>
#include <QMC5883L.h>  // 假设使用 QMC5883L 三轴磁力仪

// BLDC 推进器（单个示例，实际 ROV 有多个）
BLDCMotor thruster(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
Encoder encoder(2, 3, 2048);
QMC5883L mag;

// 干扰补偿模型参数（需实验标定）
// 干扰幅值与转速、电流的关系：I_noise = k_rpm * rpm + k_i * current_q
float k_rpm = 0.015;      // 转速干扰系数
float k_current = 0.8;    // 电流干扰系数
float noise_offset[3] = {0, 0, 0};  // 各轴偏置

// 补偿后的磁场
float mag_compensated[3];

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mag.init();
  mag.setMode(QMC5883L_MODE_CONTINUOUS);
  mag.setOdr(QMC5883L_ODR_100HZ);

  encoder.init();
  thruster.linkSensor(&encoder);
  driver.voltage_power_supply = 24;
  driver.init();
  thruster.linkDriver(&driver);
  thruster.controller = MotionControlType::velocity;
  thruster.PID_velocity.P = 0.5;
  thruster.PID_velocity.I = 3.0;
  thruster.init();
  thruster.initFOC();

  // 标定：在电机静止时记录磁力仪本底
  delay(500);
  int16_t raw[3];
  mag.readRaw(&raw[0], &raw[1], &raw[2]);
  noise_offset[0] = raw[0] * 0.001;  // 转 uT（假设量程）
  noise_offset[1] = raw[1] * 0.001;
  noise_offset[2] = raw[2] * 0.001;
  Serial.println("磁力仪本底标定完成");
}

void loop() {
  thruster.loopFOC();

  // 给定推进器转速（实际由控制层下发）
  float targetRPM = 1500.0;  // 约 157 rad/s
  thruster.move(targetRPM * 0.1047 / 0.05);  // 简化为轮径映射

  // ===== 电磁干扰前馈补偿 =====
  // 根据当前转速和 q 轴电流估算各轴干扰
  float rpm = abs(thruster.shaftVelocity()) * 9.55;  // rad/s → RPM
  float current_q = thruster.current.q;

  float noise_pred[3];
  noise_pred[0] = k_rpm * rpm + k_current * fabs(current_q) + noise_offset[0];
  noise_pred[1] = k_rpm * rpm * 0.6 + k_current * fabs(current_q) * 0.4 + noise_offset[1];
  noise_pred[2] = k_rpm * rpm * 0.3 + noise_offset[2];

  // 读取磁力仪原始值并补偿
  int16_t raw[3];
  mag.readRaw(&raw[0], &raw[1], &raw[2]);
  for (int i = 0; i < 3; i++) {
    mag_compensated[i] = raw[i] * 0.001 - noise_pred[i];
  }

  // 输出补偿后的磁场供泄漏检测算法使用
  static unsigned long lastPrint = 0;
  if (millis() - lastPrint > 200) {
    lastPrint = millis();
    Serial.print("RPM:"); Serial.print(rpm, 0);
    Serial.print(" Iq:"); Serial.print(current_q, 2);
    Serial.print(" | Mag_comp: ");
    Serial.print(mag_compensated[0], 2); Serial.print(",");
    Serial.print(mag_compensated[1], 2); Serial.print(",");
    Serial.println(mag_compensated[2], 2);
  }

  delay(10);
}

解读：该案例直接回应了搜索结果中 ROV 电磁干扰问题的核心痛点：BLDC 电机产生的宽频噪声会严重扭曲磁力仪数据。noise_pred 的线性模型基于转速和 q 轴电流，这是对搜索文献中“结合转速传感器数据线性化干扰模型”的工程简化实现。实际标定时，需要在不同转速和负载下采集磁力仪读数，拟合 k_rpm 和 k_current。noise_offset 在电机静止时标定，消除了传感器自身的零偏。补偿后的 mag_compensated 才是管道泄漏磁异常检测的可用输入。

2、流场扰动观测 + 滑模抗流悬停（BLDC 推进器执行层）
场景：输油管道附近存在海流或管道泄漏产生的射流，机器人需要悬停在固定位置进行精细采样。传统 PID 在持续流场作用下会产生稳态误差。本案例实现简化的扰动观测器 + 滑模控制：观测器估计外部流场力，前馈补偿到 BLDC 推进器指令中；滑模项提供鲁棒性。核心是让 BLDC 推力输出对抗流场，而非仅仅跟踪速度指令。

#include <SimpleFOC.h>

// 四推进器布局（简化为单轴抗流）
BLDCMotor thruster[2] = {BLDCMotor(7), BLDCMotor(7)};
BLDCDriver3PWM driver[2] = {
  BLDCDriver3PWM(9, 10, 11, 8),
  BLDCDriver3PWM(3, 5, 6, 4)
};
Encoder enc[2] = {Encoder(18, 19, 2048), Encoder(20, 21, 2048)};

// 深度传感器（模拟）
float depth_actual = 0;
float depth_target = -2.0;  // 目标深度 2m

// 扰动观测器
float disturbance_est = 0;
float disturbance_lpf = 0;
const float OBSERVER_GAIN = 2.5;  // 观测器增益

// 滑模控制器
float sliding_surface = 0;
float lambda = 1.5;        // 滑模面斜率
float K_sw = 0.8;          // 切换增益
float K_cmp = 0;           // 扰动补偿项

// 深度微分
float last_depth = 0;
float depth_rate = 0;

void setup() {
  Serial.begin(115200);
  for (int i = 0; i < 2; i++) {
    enc[i].init();
    thruster[i].linkSensor(&enc[i]);
    driver[i].voltage_power_supply = 24;
    driver[i].init();
    thruster[i].linkDriver(&driver[i]);
    thruster[i].controller = MotionControlType::torque;
    thruster[i].init();
    thruster[i].initFOC();
  }
}

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

  // 模拟深度传感器读数（实际从压力传感器获取）
  // depth_actual = readPressure();
  depth_actual += 0.001 * (disturbance_est + 0.1);  // 简化模拟

  // ===== 扰动观测器 =====
  // 基于深度误差和当前控制输入估计外部流场力
  float depth_error = depth_target - depth_actual;
  depth_rate = (depth_actual - last_depth) / 0.01;
  last_depth = depth_actual;

  // 观测器：扰动 ≈ 增益 * (误差 + 速度项)
  float disturbance_raw = OBSERVER_GAIN * (depth_error + 0.5 * depth_rate);
  // 低通滤波平滑
  disturbance_lpf += (disturbance_raw - disturbance_lpf) * 0.1;
  disturbance_est = disturbance_lpf;

  // ===== 滑模控制 =====
  sliding_surface = depth_rate + lambda * depth_error;

  // 扰动补偿前馈
  K_cmp = disturbance_est * 0.8;

  // 控制输出：滑模项 + 补偿项
  float control = -lambda * depth_rate - K_sw * (sliding_surface > 0 ? 1 : -1) + K_cmp;
  control = constrain(control, -2.0, 2.0);

  // 分配给两个推进器（同向抗流）
  thruster[0].move(control);
  thruster[1].move(control);

  static unsigned long lastPrint = 0;
  if (millis() - lastPrint > 200) {
    lastPrint = millis();
    Serial.print("Depth:"); Serial.print(depth_actual, 2);
    Serial.print(" Dist_est:"); Serial.print(disturbance_est, 2);
    Serial.print(" Ctrl:"); Serial.println(control, 2);
  }

  delay(10);
}

解读：该案例实现了搜索结果中验证的抗流悬停策略：扰动观测器实时估计流场力，滑模控制提供鲁棒性，观测值前馈补偿以降低切换增益和抖振。BLDC 在此使用力矩模式而非速度模式，因为抗流悬停需要直接控制推力大小，而非让速度环与流场“对抗”。disturbance_est 的物理含义是流场对机器人施加的等效力，K_cmp 将其转化为前馈推力指令，使 BLDC 在流场中维持位置而非被推走。

3、声呐采样触发 + 推进器静默窗口（泄漏检测专用采样）
场景：输油管道泄漏检测依赖声呐或声学传感器捕捉泄漏点的高频噪声。但 BLDC 推进器自身的噪声会淹没微弱的泄漏信号。核心策略是：在声呐采样窗口内主动降低推进器转速或短暂关闭，制造一个“静默窗口”用于高质量采样，采样完成后恢复推进。这是水下巡检机器人特有的“采样-运动”协同逻辑。

#include <SimpleFOC.h>

BLDCMotor thruster(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
Encoder encoder(2, 3, 2048);

// 声呐采样状态机
enum SampleState { CRUISING, SILENT_SAMPLE, RESUME };
SampleState sampleState = CRUISING;
unsigned long stateTimer = 0;

// 参数
const float CRUISE_SPEED = 1200.0;   // RPM
const float SILENT_SPEED = 200.0;    // RPM，降低但不完全停止（保持姿态）
const unsigned long SAMPLE_DURATION = 500;  // 采样窗口 500ms
const unsigned long CRUISE_DURATION = 3000; // 巡航间隔 3s

// 声呐数据缓冲（模拟）
float sonarBuffer[50];
int sonarIdx = 0;

void setup() {
  Serial.begin(115200);
  encoder.init();
  thruster.linkSensor(&encoder);
  driver.voltage_power_supply = 24;
  driver.init();
  thruster.linkDriver(&driver);
  thruster.controller = MotionControlType::velocity;
  thruster.PID_velocity.P = 0.4;
  thruster.PID_velocity.I = 2.0;
  thruster.init();
  thruster.initFOC();
  stateTimer = millis();
}

void loop() {
  thruster.loopFOC();

  unsigned long elapsed = millis() - stateTimer;

  switch (sampleState) {
    case CRUISING:
      thruster.move(CRUISE_SPEED * 0.1047 / 0.05);
      if (elapsed > CRUISE_DURATION) {
        sampleState = SILENT_SAMPLE;
        stateTimer = millis();
        Serial.println("→ 静默采样窗口");
      }
      break;

    case SILENT_SAMPLE:
      // 大幅降低转速，减少推进器噪声
      thruster.move(SILENT_SPEED * 0.1047 / 0.05);

      // 在静默窗口内高频采集声呐数据
      if (sonarIdx < 50) {
        // 模拟声呐读数（实际从声呐模块读取）
        sonarBuffer[sonarIdx++] = analogRead(A0) * 0.0049;
      }

      if (elapsed > SAMPLE_DURATION) {
        sampleState = RESUME;
        stateTimer = millis();
        Serial.println("→ 恢复巡航");
      }
      break;

    case RESUME:
      // 平滑恢复转速
      thruster.move(CRUISE_SPEED * 0.1047 / 0.05);
      if (elapsed > 500) {
        sampleState = CRUISING;
        stateTimer = millis();
        sonarIdx = 0;  // 清空缓冲
      }
      break;
  }

  delay(10);
}

解读：该案例的工程逻辑来自水下巡检的实际约束：推进器噪声是声学泄漏检测的主要干扰源。SILENT_SAMPLE 状态将推进器转速从 1200 RPM 降至 200 RPM，使 BLDC 的 PWM 噪声和流体噪声大幅降低，声呐可以在相对安静的环境中捕捉泄漏点的微弱信号。RESUME 状态的 500ms 平滑恢复避免了转速阶跃对机器人姿态的冲击。这一逻辑在纯陆地机器人中不存在，是水下巡检特有的“感知-执行”时序协同。

要点解读

1. 强电磁干扰的对抗必须从 BLDC 驱动端和传感器端同时入手，而非只做软件滤波
   搜索结果明确指出，ROV 的 BLDC 电机是磁力仪数据失真的主要来源，干扰具有非线性频率调制特征。案例一的 noise_pred 基于转速和电流的线性模型，是对搜索文献中“结合转速传感器数据线性化干扰模型”的工程简化。如果只在磁力仪端做低通滤波，泄漏信号的高频特征也会被一起滤掉；只有从驱动端建模干扰、在传感器端做前馈补偿，才能在保留泄漏信号的同时抑制噪声。

2. 抗流悬停的本质是“力对抗”，BLDC 必须工作在力矩模式而非速度模式
   案例二使用 MotionControlType::torque，因为抗流悬停需要直接控制推力大小。如果使用速度模式，速度环会试图维持一个恒定转速，但在流场中，同样的转速对应的推力不足以抵抗流场力——机器人仍然会被推走。搜索文献中滑模控制+扰动估计的方案同样以推力（控制输入）为直接输出，而非转速。disturbance_est 前馈补偿使 BLDC 在流场尚未导致明显位置偏差时就提前出力对抗，而非等到误差累积后才响应。

3. 水下巡检的“采样质量”与“运动效率”存在根本性矛盾，必须用时序协同解决
   案例三的静默采样窗口是一个典型的水下工程折中：巡航时推进器全速运转（高效移动），采样时降速（高质量数据），恢复时平滑加速（姿态稳定）。这个矛盾在陆地机器人中不存在——陆地机器人的传感器（摄像头、激光）不惧怕电机噪声。输油管道泄漏检测依赖声学/声呐信号，推进器噪声直接决定检测灵敏度。SAMPLE_DURATION 和 CRUISE_DURATION 的比例需要根据巡检速度要求和声呐信噪比需求权衡。

4. SimpleFOC 的电流环是电磁干扰补偿和扰动观测的物理基础
   案例一中的 thruster.current.q 和案例二中的力矩输出，都依赖 SimpleFOC 的电流感知能力。如果底层是开环 ESC，current.q 不可用，干扰模型的电流项就无从计算。SimpleFOC 的 FOC 电流环不仅提供平滑推力，还提供了可观测的电机状态（电流、转速），这是上层补偿算法能够工作的前提。搜索结果中笙泉的水下推进器方案同样强调单电阻/双电阻电流采样 FOC 是“高精度平滑推力控制”的核心。

5. Arduino 的算力边界决定了“干扰补偿+抗流控制+采样调度”必须分层分频
   案例中的 delay(10) 对应 100Hz 的补偿与控制计算。三个案例的算法复杂度不同：案例一（线性补偿）最轻，案例二（观测器+滑模）中等，案例三（状态机）最轻。在 Arduino Uno 上同时运行双推进器 FOC 加以上任意一个算法已接近极限。合理的架构是：FOC 环（loopFOC()）以最大频率运行，补偿/控制算法降频到 50-100Hz，声呐采样和状态机再降频。搜索结果中 ROV 的推进器控制通常使用专用 MCU 或 ESC 分担 FOC 计算，主控只负责上层逻辑。若必须用单 Arduino，建议使用 ESP32 或 Teensy 4.x，并将磁力仪/声呐的读取放在独立任务中。

4、IMU 姿态稳定 + BLDC 推进抗流场扰动
这个案例用于让机器人在管道内或水下保持姿态稳定，避免被流场、涡流、管壁摩擦推偏。核心思路是：IMU 提供俯仰/横滚/航向姿态，姿态环输出修正量，叠加到 BLDC 推进指令上，形成“推进 + 姿态稳定”的闭环。

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

// BLDC 推进器
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM driverL = BLDCDriver3PWM(9, 10, 11, 8);
BLDCDriver3PWM driverR = BLDCDriver3PWM(3, 5, 6, 4);
Encoder encL = Encoder(18, 19, 2048);
Encoder encR = Encoder(20, 21, 2048);

// IMU
MPU6050 imu;

// 姿态估计参数
float pitch = 0, roll = 0;
float alpha = 0.98; // 互补滤波系数

// 控制参数
float thrustCmd = 0.0;      // 基础推进指令
float pitchTarget = 0.0;    // 目标俯仰角
float kpAtt = 0.6, kdAtt = 0.15;
float lastPitchError = 0;
unsigned long lastCtrl = 0;

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

  encL.init();
  encR.init();
  motorL.linkSensor(&encL);
  motorR.linkSensor(&encR);

  driverL.voltage_power_supply = 12;
  driverR.voltage_power_supply = 12;
  driverL.init();
  driverR.init();

  motorL.linkDriver(&driverL);
  motorR.linkDriver(&driverR);
  motorL.controller = MotionControlType::torque;
  motorR.controller = MotionControlType::torque;
  motorL.init();
  motorR.init();
  motorL.initFOC();
  motorR.initFOC();
}

void readAttitude() {
  int16_t ax, ay, az, gx, gy, gz;
  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float accelPitch = atan2(ay, az) * 180.0 / PI;
  float gyroRate = gx / 131.0; // MPU6050 默认量程下近似转换

  float dt = (millis() - lastCtrl) / 1000.0;
  if (dt > 0.05) dt = 0.05;

  pitch = alpha * (pitch + gyroRate * dt) + (1.0 - alpha) * accelPitch;
  roll  = atan2(ax, az) * 180.0 / PI;
}

float attitudeCorrection() {
  float error = pitchTarget - pitch;
  float de = (error - lastPitchError);
  lastPitchError = error;
  float corr = kpAtt * error + kdAtt * de;
  return constrain(corr, -0.4, 0.4);
}

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

  readAttitude();
  float corr = attitudeCorrection();

  float left  = constrain(thrustCmd - corr, -1.0, 1.0);
  float right = constrain(thrustCmd + corr, -1.0, 1.0);

  motorL.move(left);
  motorR.move(right);

  lastCtrl = millis();
  delay(5);
}

5、强电磁环境下抗干扰采样
这个案例针对强电磁干扰场景。BLDC 驱动器、动力线、PWM 换相都会污染 ADC 和传感器读数，尤其是油膜、浊度、气体、压力、电流等采样信号。工程上通常采用 硬件隔离 + 软件滤波 + 采样窗口避让 三重策略。

#include <SimpleFOC.h>

// 采样通道
const int sensorPin = A0;   // 泄漏/浊度/压力等模拟传感器
const int currentPin = A1;  // 相电流或总线电流采样

// 滤波参数
const int WINDOW = 16;
float adcBuf[WINDOW];
int idx = 0;

// 抗干扰采样窗口
unsigned long lastPwmCycle = 0;
const unsigned long QUIET_US = 200; // PWM 关断后的静默采样窗口

float filteredSensor = 0;
float filteredCurrent = 0;
bool leakFlag = false;

void setup() {
  Serial.begin(115200);
  pinMode(sensorPin, INPUT);
  pinMode(currentPin, INPUT);
}

float readQuietSample(int pin) {
  // 在电机控制间隙采样，降低 PWM 开关噪声耦合
  unsigned long now = micros();
  if (now - lastPwmCycle < QUIET_US) {
    delayMicroseconds(QUIET_US - (now - lastPwmCycle));
  }
  return analogRead(pin);
}

float updateFilter(float raw) {
  adcBuf[idx % WINDOW] = raw;
  idx++;

  float sum = 0;
  for (int i = 0; i < WINDOW; i++) {
    sum += adcBuf[i];
  }
  return sum / WINDOW;
}

void loop() {
  float rawSensor = readQuietSample(sensorPin);
  float rawCurrent = readQuietSample(currentPin);

  filteredSensor = updateFilter(rawSensor);
  filteredCurrent = updateFilter(rawCurrent);

  // 简单泄漏判定：需结合真实传感器标定
  if (filteredSensor > 2800.0 && filteredCurrent < 0.5) {
    leakFlag = true;
  } else {
    leakFlag = false;
  }

  lastPwmCycle = micros();

  Serial.print("Sensor:");
  Serial.print(filteredSensor);
  Serial.print(" Current:");
  Serial.print(filteredCurrent);
  Serial.print(" Leak:");
  Serial.println(leakFlag ? 1 : 0);

  delay(10);
}

6、流场抗扰 + 采样姿态锁定
这个案例把前两个能力合并：当机器人接近疑似泄漏点时，先降低推进速度，进入姿态锁定模式，再触发采样机构。这样可以减少流场扰动对采样口、摄像头和传感器的影响。

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

BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM driverL = BLDCDriver3PWM(9, 10, 11, 8);
BLDCDriver3PWM driverR = BLDCDriver3PWM(3, 5, 6, 4);
Encoder encL = Encoder(18, 19, 2048);
Encoder encR = Encoder(20, 21, 2048);

MPU6050 imu;

const int samplePin = A0;
const int pumpPin = 7;

enum State { CRUISE, LOCK, SAMPLE, BACKOFF };
State state = CRUISE;

float pitch = 0;
float alpha = 0.98;
float thrustCmd = 0.3;
float kpAtt = 0.7, kdAtt = 0.18;
float lastPitchError = 0;

unsigned long lockStart = 0;
unsigned long sampleStart = 0;
const unsigned long LOCK_MS = 2000;
const unsigned long SAMPLE_MS = 3000;

void readAttitude() {
  int16_t ax, ay, az, gx, gy, gz;
  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float accelPitch = atan2(ay, az) * 180.0 / PI;
  float gyroRate = gx / 131.0;

  float dt = 0.01;
  pitch = alpha * (pitch + gyroRate * dt) + (1.0 - alpha) * accelPitch;
}

float attitudeCorrection(float target) {
  float error = target - pitch;
  float de = error - lastPitchError;
  lastPitchError = error;
  return kpAtt * error + kdAtt * de;
}

void setState(State next) {
  if (next == LOCK) {
    thrustCmd = 0.05; // 低速接近，减少流场扰动
    lockStart = millis();
  } else if (next == SAMPLE) {
    thrustCmd = 0.0;  // 采样期间尽量保持静止
    digitalWrite(pumpPin, HIGH);
    sampleStart = millis();
  } else if (next == BACKOFF) {
    digitalWrite(pumpPin, LOW);
    thrustCmd = -0.2;
  } else {
    thrustCmd = 0.3;
  }
  state = next;
}

void setup() {
  Serial.begin(115200);
  Wire.begin();
  imu.initialize();
  pinMode(pumpPin, OUTPUT);
  digitalWrite(pumpPin, LOW);

  encL.init();
  encR.init();
  motorL.linkSensor(&encL);
  motorR.linkSensor(&encR);

  driverL.voltage_power_supply = 12;
  driverR.voltage_power_supply = 12;
  driverL.init();
  driverR.init();

  motorL.linkDriver(&driverL);
  motorR.linkDriver(&driverR);
  motorL.controller = MotionControlType::torque;
  motorR.controller = MotionControlType::torque;
  motorL.init();
  motorR.init();
  motorL.initFOC();
  motorR.initFOC();

  setState(CRUISE);
}

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

  float corr = attitudeCorrection(0.0);

  float left  = constrain(thrustCmd - corr, -0.6, 0.6);
  float right = constrain(thrustCmd + corr, -0.6, 0.6);

  motorL.move(left);
  motorR.move(right);

  // 简化：用模拟传感器触发采样流程
  float sampleVal = analogRead(samplePin);

  if (state == CRUISE && sampleVal > 2600) {
    setState(LOCK);
  } else if (state == LOCK && millis() - lockStart > LOCK_MS) {
    setState(SAMPLE);
  } else if (state == SAMPLE && millis() - sampleStart > SAMPLE_MS) {
    setState(BACKOFF);
  } else if (state == BACKOFF && millis() - sampleStart > SAMPLE_MS + 1500) {
    setState(CRUISE);
  }

  delay(5);
}

要点解读
1、BLDC 必须走闭环，不能只靠开环 PWM。
水下和管道环境里，流场阻力、管壁摩擦、采样负载都会变化。用 SimpleFOC 的力矩/速度闭环，比直接给 PWM 更稳，也更容易做姿态补偿。
2、姿态环要先于复杂路径控制。
如果机身姿态不稳，采样数据、摄像头画面、泄漏定位都会失真。先用 IMU 做互补滤波或 Madgwick，再把姿态误差叠加到推进指令上，是更稳妥的顺序。
3、强电磁采样要分三层处理。
第一层是硬件隔离：动力电源与控制电源分开，传感器用屏蔽线、差分放大或数字隔离；第二层是采样窗口：尽量避开 PWM 换相瞬间；第三层是软件滤波：滑动平均、中值滤波或卡尔曼滤波。
4、流场抗扰不能只靠加大推力。
水下阻力通常是非线性的，速度越大扰动耦合越强。更合理的是低速接近、姿态锁定、短时采样、采样后回退，必要时加入前馈补偿或串级控制。
5、Arduino 适合原型，但工程化要上更高算力平台。
如果要做多推进器矢量分配、高频电流环、复杂状态估计或长时可靠作业，建议把 Arduino 原型迁移到 STM32、ESP32、Teensy 或专用飞控/MCU。Arduino 阶段的价值是快速验证控制结构和传感器链路。

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