该方案的核心特点是采用拓扑图结构管理多分支管网，并通过动态权重机制实时调整各分支的巡检优先级，结合BLDC电机的高动态响应实现自适应路径执行；主要适用于城市燃气/供水管网、石化厂区、市政综合管廊及教育科研等场景；实际部署需重点解决Arduino算力瓶颈、室外定位退化、通信可靠性及恶劣环境防护等工程难题。

一、主要特点
多分支管网拓扑管理——从"单线巡检"到"网络化覆盖"
室外埋地管道系统本质上是一个复杂的树状或网状拓扑结构，存在大量的T型三通、Y型分支和环形回路。传统巡检机器人只能沿单一管道直线往返，遇到分支就需要人工干预或重新部署。该方案的核心突破在于：
拓扑图数据结构：将管网抽象为有向图（Directed Graph），每个节点（Node）代表一个分支交汇点或关键设备位置（如阀门、弯头），每条边（Edge）代表一段管道。Arduino通过struct或邻接表存储拓扑关系，实现管网结构的数字化建模。
分支决策引擎：当机器人到达分支节点时，决策引擎根据当前任务队列、各分支的巡检状态和历史数据，自动选择最优分支进入，无需人工干预。
回环检测与去重：在环形管网中，机器人需避免重复巡检同一段管道。通过记录每条边的"最后巡检时间戳"，结合拓扑图的连通性分析，确保全覆盖且无冗余。
动态权重压缩——资源受限下的智能优先级调度
室外巡检任务中，不同管段的巡检紧迫性差异巨大。动态权重压缩机制通过实时调整各分支的"巡检权重"，实现资源的最优分配：
多因子权重计算：每条管道分支的巡检权重由多个因子动态决定，例如：
管道年龄因子：老旧管道（服役>20年）权重更高
历史异常因子：曾检测到泄漏或腐蚀的管段权重提升
环境风险因子：靠近道路、建筑密集区的管段权重增加
巡检间隔因子：长时间未巡检的管段权重随时间递增
权重压缩算法：在Arduino有限的内存中，采用整数量化（如0-255范围）替代浮点运算，将多因子加权求和的结果压缩为单个uint8_t值，极大节省SRAM。
贪心选择与回溯：基于权重排序，机器人优先巡检权重最高的分支。当高权重分支因物理障碍（如塌方、积水）无法进入时，自动回溯至最近的可通行节点，选择次优分支，确保任务不因局部阻塞而中断。
BLDC差速驱动——复杂地形下的精准机动
室外埋地管道巡检的地面条件复杂多变（草地、碎石、泥泞、坡道），BLDC电机配合FOC控制提供了关键的运动性能保障：
高扭矩密度：在松软地面或上坡时，BLDC可提供瞬时大扭矩，防止机器人陷入或打滑。
差速转向：通过左右轮差速实现平滑转向，在狭窄的管道检修通道中灵活调整姿态。
再生制动：下坡时BLDC工作于发电模式，将势能转化为电能回馈电池，显著延长续航。
分层控制架构——“上位机规划+Arduino执行”
由于Arduino算力有限，系统采用主从架构：
上位机（ESP32/树莓派）：负责拓扑图管理、权重计算、全局路径规划（如Dijkstra或A*算法）、传感器数据融合。
下位机（Arduino/STM32）：专注BLDC的FOC闭环控制（电流环≥10kHz）、编码器读取、紧急避障等实时性要求极高的任务。

二、典型应用场景
城市燃气/供水管网巡检
这是最核心的应用场景。城市地下管网呈树状分布，分支众多，且管道老化、第三方施工破坏等风险持续存在。
动态权重应用：将管道服役年限、历史泄漏记录、周边施工信息等作为权重因子，机器人自动优先巡检高风险管段。例如，某段管道近期附近有道路施工，权重自动提升，机器人调整巡检计划优先前往。
多分支覆盖：机器人从主管道出发，自动识别并进入各支管分支，完成全覆盖巡检后返回，无需人工搬运或重新部署。
石化厂区管廊巡检
石化厂区管道密集、介质多样（可燃、有毒、高温、高压），且管廊结构呈多层多分支的立体网络。
权重动态调整：结合气体传感器实时数据，当某区域检测到微量可燃气体浓度升高时，该区域所有分支的巡检权重立即提升，机器人自动增加巡检频次。
BLDC防爆适配：在防爆要求下，BLDC驱动系统需配合隔爆外壳和本安电路设计，确保在爆炸性环境中安全运行。
市政综合管廊巡检
综合管廊内集成了电力、通信、给排水、热力等多种管线，空间相对宽敞但分支复杂。
拓扑图优势：管廊的分支结构高度规则化，适合用拓扑图精确建模。机器人在每个交叉口根据权重选择最优路径，实现高效巡检。
通信中继：管廊内金属结构密集，无线信号衰减严重。机器人可搭载通信中继模块，在巡检过程中自动建立Mesh网络，确保数据回传。
农业灌溉管网监测
大型农田灌溉系统中，地下管道呈树状分布，分支众多，且管道堵塞、渗漏等问题难以及时发现。
低成本方案：Arduino+BLDC的组合成本远低于工业级巡检设备，适合大面积农田的分布式部署。多台机器人协同，每台负责一个灌溉片区。
土壤湿度联动：将土壤湿度传感器数据纳入权重计算，干旱区域的管道巡检优先级自动提升。
教育与科研验证平台
作为高校机器人学、嵌入式系统课程的实验平台，用于验证图论算法（Dijkstra、A*）、动态权重调度、多机协同等高级算法。

三、需要注意的事项
Arduino算力瓶颈是首要限制
多分支拓扑管理和动态权重计算涉及图遍历、排序和浮点运算，标准Arduino Uno（ATmega328P，2KB SRAM，16MHz）几乎无法胜任。
平台选型：必须选用ESP32（双核240MHz，520KB SRAM）、STM32F4/H7或Arduino Portenta H7等高性能平台。
算法轻量化：使用整数运算替代浮点运算，采用查表法（LUT）加速三角函数计算，将权重计算结果量化为uint8_t。
主从架构：将全局路径规划和权重计算放在上位机，Arduino仅负责底层电机控制和传感器数据采集。
室外定位是最大技术挑战
室外埋地管道巡检面临GPS信号遮挡（树冠、建筑）、多径效应等问题，定位精度直接影响分支决策的准确性。
多源融合定位：采用RTK-GPS（厘米级）+ IMU（惯性导航）+ 轮式里程计的融合方案，通过扩展卡尔曼滤波（EKF）消除单一传感器的缺陷。
拓扑约束校正：利用已知的管网拓扑结构作为约束条件，当GPS漂移导致机器人偏离管道中心线时，通过拓扑匹配将位置"吸附"回正确的管道上。
分支节点信标：在每个分支交汇点安装RFID或磁信标，机器人经过时进行绝对位置校准，消除累积误差。
通信可靠性与断网自主能力
室外环境地形复杂，无线信号覆盖不稳定，通信中断风险高。
多链路冗余：主链路（4G/5G）+ 备用链路（LoRa），当主链路中断时自动切换。
断网安全策略：通信完全中断时，机器人不能"瘫痪"。应预设"断线返航"或"原地待命+本地存储"的安全策略，待通信恢复后上传缓存数据。
心跳包机制：Arduino端必须编写心跳包检测和超时重连机制，一旦通信中断超过阈值，立即进入安全停机模式。
恶劣环境防护
室外埋地管道巡检机器人面临雨水、泥沙、极端温度等恶劣环境。
IP67防护等级：整机需达到IP67防护等级，确保在暴雨或临时积水中正常运行。
宽温设计：电池和电子元件需适应-20℃~60℃的温度范围。低温下锂电池性能衰减严重，需配备加热膜或选用低温锂电池。
传感器防护：气体传感器探头需加装防尘防水透气膜，防止泥沙堵塞影响检测灵敏度。
电源管理与续航优化
室外巡检距离长、分支多，续航是关键瓶颈。
BLDC高效率：BLDC电机效率>85%，配合FOC控制可最大化能量利用率。
再生制动：下坡时利用BLDC发电回馈电池，可延长续航15%-20%。
低电量策略：当电量低于20%时，机器人自动规划最短路径返回充电桩，确保不会因电量耗尽而困在管网中。
电磁兼容（EMC）与信号完整性
室外环境中，BLDC电机产生的电磁干扰可能影响GPS接收机和气体传感器的精度。
强弱电隔离：BLDC驱动电源与逻辑电源必须物理隔离，仅单点共地。
屏蔽设计：GPS天线和传感器信号线使用屏蔽线，屏蔽层单端接地。
时序隔离：在电机静止或低负载时段进行GPS定位和传感器采样，避开PWM驱动的峰值干扰期。

1、基础循迹与多分支路口识别（梯形驱动 + 霍尔传感器）
这个案例适合入门，使用最经典的梯形（六步）换向法驱动BLDC电机，通过霍尔传感器反馈转子位置实现换相，并利用有限状态机识别简单的T型或Y型分支路口。

// 案例一：基础循迹与多分支路口识别 (基于Arduino UNO + BLDC霍尔驱动)
// 功能：直行循迹，通过管道侧壁反射信号识别分支路口

// 定义BLDC驱动引脚 (基于IR2101或类似三相H桥)
#define PHASE_A_H 3
#define PHASE_A_L 4
#define PHASE_B_H 5
#define PHASE_B_L 6
#define PHASE_C_H 7
#define PHASE_C_L 8

// 定义霍尔传感器输入引脚 (用于六步换相)
#define HALL_A 9
#define HALL_B 10
#define HALL_C 11

// 定义分支路口传感器 (左侧、前方、右侧 管道壁检测)
#define SENSOR_LEFT A0
#define SENSOR_FRONT A1
#define SENSOR_RIGHT A2

// 六步换相表 (霍尔组合 -> 导通MOSFET)
const int commutationTable[7][2] = {
  // {HighSide, LowSide} 对应A, B, C相
  {0, 0}, // 0: 非法状态
  {PHASE_A_H, PHASE_B_L}, // 1: AB导通
  {PHASE_A_H, PHASE_C_L}, // 2: AC导通
  {PHASE_B_H, PHASE_C_L}, // 3: BC导通
  {PHASE_B_H, PHASE_A_L}, // 4: BA导通
  {PHASE_C_H, PHASE_A_L}, // 5: CA导通
  {PHASE_C_H, PHASE_B_L}  // 6: CB导通
};

unsigned int targetSpeed = 150; // 目标PWM占空比

void setup() {
  // 初始化引脚模式
  for (int i = 3; i <= 8; i++) pinMode(i, OUTPUT);
  pinMode(HALL_A, INPUT);
  pinMode(HALL_B, INPUT);
  pinMode(HALL_C, INPUT);
  // 初始化传感器引脚
  pinMode(SENSOR_LEFT, INPUT);
  pinMode(SENSOR_FRONT, INPUT);
  pinMode(SENSOR_RIGHT, INPUT);
  
  Serial.begin(115200);
}

void loop() {
  // 1. 读取霍尔状态进行换相
  int hallState = (digitalRead(HALL_A) << 2) | (digitalRead(HALL_B) << 1) | digitalRead(HALL_C);
  
  if (hallState > 0 && hallState < 7) {
    // 执行六步换相
    digitalWrite(commutationTable[hallState][0], HIGH); // 高端导通
    digitalWrite(commutationTable[hallState][1], LOW);  // 低端导通 (PWM调速可加在此)
    // 实际应用中需加入PWM调速逻辑，此处简化为全速
    analogWrite(commutationTable[hallState][0], targetSpeed);
  }

  // 2. 分支路口识别 (阈值法)
  int leftVal = analogRead(SENSOR_LEFT);
  int frontVal = analogRead(SENSOR_FRONT);
  int rightVal = analogRead(SENSOR_RIGHT);
  
  // 假设管道为直线时，前方有壁，侧方无壁；分支处前方无壁，侧方有壁
  if (frontVal < 200 && leftVal > 200) {
    // 检测到左侧分支 (T型或Y型)
    Serial.println("Left Branch Detected!");
    // 此处可添加转弯逻辑：通过调整PWM占空比实现差速转向
    // 左转：右轮速度 > 左轮速度 (本案例单电机简化，需配合双电机底盘)
  } else if (frontVal < 200 && rightVal > 200) {
    Serial.println("Right Branch Detected!");
  } else if (frontVal < 200 && leftVal < 200 && rightVal < 200) {
    Serial.println("Cross or Dead End Detected!");
  }
  
  delay(50); // 简单去抖
}

2、动态权重压缩——根据环境切换寻迹策略（自适应控制）
此案例引入“动态权重压缩”思想，即在资源有限的Arduino/ESP32上，根据路面颠簸（IMU数据）和管道曲率（雷达数据）动态调整Stanley控制器的权重参数，将复杂的多目标优化简化为权重系数的实时调整，从而在保证循迹精度的同时节省算力。

// 案例二：动态权重压缩——自适应Stanley控制器 (基于ESP32 + 双BLDC + IMU)
// 功能：根据路面颠簸和路径曲率动态调整控制权重，压缩不必要的计算开销

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

MPU6050 mpu;

// 电机控制引脚
#define LEFT_MOTOR_PWM 12
#define RIGHT_MOTOR_PWM 14
#define LEFT_ENCODER_A 2
#define LEFT_ENCODER_B 4
#define RIGHT_ENCODER_A 5
#define RIGHT_ENCODER_B 18

// Stanley控制器基础参数 (权重可调)
float base_Kp = 1.8f;   // 横向位置误差增益
float base_K_heading = 1.2f; // 航向角误差增益

// 动态因子 (由环境感知决定)
float terrainRoughness = 0.0f; // 0(平坦)-1(颠簸)
float pathCurvature = 0.0f;    // 0(直线)-1(急弯)

// 权重压缩结果 (经自适应函数计算后的最终增益)
float adaptiveKp;
float adaptiveKHeading;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  // 引脚初始化略...
  
  // 初始化编码器中断
  attachInterrupt(digitalPinToInterrupt(LEFT_ENCODER_A), countLeftEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(RIGHT_ENCODER_A), countRightEncoder, CHANGE);
}

void loop() {
  // 1. 感知层：获取环境信息 (IMU计算颠簸，雷达/编码器计算曲率)
  updateTerrainRoughness(); // 通过MPU6050加速度变化率计算
  updatePathCurvature();    // 通过左右编码器差值估算转弯半径
  
  // 2. 决策层：动态权重压缩 (核心算法)
  // 颠簸时，减小位置增益Kp，避免震荡；急弯时，增大航向增益K_heading，提升转向灵敏度
  adaptiveKp = base_Kp * (1.0f - terrainRoughness * 0.5f); // 压缩位置环权重
  adaptiveKHeading = base_K_heading * (1.0f + pathCurvature * 0.8f); // 增强航向环权重
  
  // 限制权重范围，防止失控
  adaptiveKp = constrain(adaptiveKp, 0.5f, 3.0f);
  adaptiveKHeading = constrain(adaptiveKHeading, 0.5f, 3.0f);
  
  // 3. 控制层：使用压缩后的权重计算Stanley输出
  float cte = calculateCrossTrackError(); // 横向误差 (来自视觉/激光雷达)
  float headingError = calculateHeadingError(); // 航向误差 (来自IMU)
  
  float steeringAngle = adaptiveKp * cte + adaptiveKHeading * headingError;
  
  // 4. 执行层：将转向角度映射为左右轮速度差
  applyMotorControl(steeringAngle);
  
  // 打印调试数据
  Serial.printf("Rough:%.2f, Curv:%.2f, Kp:%.2f, Kh:%.2f\r\n", terrainRoughness, pathCurvature, adaptiveKp, adaptiveKHeading);
  delay(50);
}

// 占位函数，需自行实现
void updateTerrainRoughness() { /* 读取IMU加速度，计算方差作为颠簸指标 */ }
void updatePathCurvature() { /* 通过编码器差值估算 */ }
float calculateCrossTrackError() { /* 与规划路径的横向偏差 */ }
float calculateHeadingError() { /* 当前航向与目标航向差 */ }
void applyMotorControl(float angle) { /* 差速转向控制 */ }

3、多传感器融合与多分支路径规划（EKF + A* / DWA）
这是较完整的实现方案，适用于复杂的室外埋地管网。它在ESP32的双核上分离任务（Core 0进行A*全局规划与多分支决策，Core 1进行FOC电机控制与DWA局部规划），并通过扩展卡尔曼滤波（EKF）融合IMU、里程计和雷达数据，即便在GPS失效的埋地场景也能稳定定位。

// 案例三：多传感器融合与多分支规划 (基于ESP32双核 + FOC + 激光雷达)
// 功能：构建地图，在分支路口进行全局重规划

TaskHandle_t TaskCore0;
TaskHandle_t TaskCore1;

// 全局变量用于核间通信 (需加volatile及互斥)
volatile float targetLinearSpeed = 0;
volatile float targetAngularSpeed = 0;
volatile bool newPathAvailable = false;

void setup() {
  Serial.begin(115200);
  
  // 创建双核任务
  // Core 0: 负责传感器融合、路径规划、分支决策 (高延迟任务)
  xTaskCreatePinnedToCore(
    core0Task,   /* 任务函数 */
    "Core0Task", /* 任务名称 */
    10000,       /* 栈大小 */
    NULL,        /* 参数 */
    1,           /* 优先级 */
    &TaskCore0,  /* 任务句柄 */
    0);          /* 核心0 */
  
  // Core 1: 负责FOC控制、电机驱动、避障 (高实时性任务)
  xTaskCreatePinnedToCore(
    core1Task,
    "Core1Task",
    8000,
    NULL,
    2,           /* 更高优先级 */
    &TaskCore1,
    1);          /* 核心1 */
}

// Core 0 任务：路径规划与分支决策
void core0Task(void *pvParameters) {
  while (1) {
    // 1. 传感器数据预处理 (模拟)
    float imuYaw = readIMU();
    float leftDist = readLeftUltrasonic();
    float rightDist = readRightUltrasonic();
    
    // 2. EKF融合定位 (扩展卡尔曼滤波，融合里程计/IMU/激光雷达)
    updateEKF(imuYaw, leftDist, rightDist);
    
    // 3. 分支路口检测 (基于侧向距离突变)
    if (isBranchDetected(leftDist, rightDist)) {
      // 触发A*全局重规划，将当前节点标记为分支点
      Serial.println("Branch Point Detected, Replanning Global Path...");
      // A*算法计算新路径 (这里简化为输出指令)
      // 权重压缩体现在A*的启发函数中：根据管道类型动态调整代价权重
      float heuristicWeight = calculateDynamicHeuristic(); // 例如：优先走气体浓度低的分支
      targetLinearSpeed = 0.3f; // 减速转弯
      targetAngularSpeed = 0.5f; // 转向指令
      newPathAvailable = true;
    } else {
      // 正常沿路径前进
      targetLinearSpeed = 0.5f;
      targetAngularSpeed = 0.0f;
    }
    vTaskDelay(100 / portTICK_PERIOD_MS); // 100ms循环一次
  }
}

// Core 1 任务：FOC电机控制与避障 (高频)
void core1Task(void *pvParameters) {
  while (1) {
    // 1. 读取障碍物距离 (超声波/ToF)
    float obstacleDist = readFrontUltrasonic();
    
    // 2. 局部避障 (DWA动态窗口)
    if (obstacleDist < 0.5f) {
      // 紧急避障：覆盖Core0的速度指令，进行急停或转向
      targetLinearSpeed = 0.0f;
      targetAngularSpeed = 0.8f; // 急转
    }
    
    // 3. 将速度指令下发至BLDC驱动器 (FOC算法)
    // 这里调用FOC库，如SimpleFOC，设置目标速度
    motor1.target = targetLinearSpeed;
    motor2.target = targetLinearSpeed - targetAngularSpeed * 0.2f; // 差速
    // 实际FOC更新在loop中高频运行，此处仅更新目标值
    
    vTaskDelay(20 / portTICK_PERIOD_MS); // 50Hz控制周期
  }
}

// 动态启发函数 (体现权重压缩思想)
float calculateDynamicHeuristic() {
  // 例如：当前检测到甲烷浓度高，则压缩“最短路径”权重，增大“远离危险”权重
  float gasConcentration = readGasSensor(); // MQ-2等
  if (gasConcentration > 200) {
    // 压缩距离权重，增大安全权重 -> 路径可能会绕行
    return 0.5f; // 调整A*的启发式系数
  }
  return 1.0f; // 默认最短路径
}

void loop() {
  // 空闲，实际工作由RTOS任务完成
  delay(1000);
}

要点解读

1. “动态权重压缩”的本质是算力与精度的博弈
   在Arduino/ESP32这类嵌入式平台上，无法像工控机那样执行完整的非线性优化。“压缩”指的是根据实时环境（如路面颠簸度、管道曲率）动态调整控制算法中的权重系数，将多目标（如稳定性、快速性、准确性）的复杂求解简化为几个权重因子的在线调整。这既节省了计算资源，又能保证在关键场景下（如急弯、颠簸）侧重提升特定性能指标，是一种非常务实的工程策略。

2. 核心驱动：BLDC为何适合管道巡检
   BLDC电机没有电刷，消除了机械换向产生的火花和磨损，这对于可能存在易燃易爆气体（如甲烷）的埋地管道环境至关重要，是实现本质安全的基础之一。同时，BLDC配合FOC（磁场定向控制）可实现低频平滑运转和低噪音，非常适用于需要低速蠕动精细检查管壁的场景。

3. 多分支识别的硬件基础：传感器阵列
   代码中的分支识别通常依赖于多路超声波/红外/激光雷达传感器。案例一简单采用“前方无壁、侧方有壁”的阈值法识别T/Y型路口。在更复杂的场景（如多通路口），需要依赖激光雷达SLAM构建的拓扑地图进行判断。传感器布置数量和位置直接影响识别算法的可靠性。

4. 双核任务分离是保障实时性的关键架构
   由于路径规划（A*、DWA）和传感器融合（EKF）运算量大，而电机控制（FOC）要求极高的实时性（控制频率通常≥1kHz），将任务分给ESP32的两个核心是解决Arduino算力瓶颈的有效手段。案例三中Core0处理慢速决策，Core1处理高速控制，实现了互不干扰的准并行计算。

5. 电源管理与EMC防护是工程落地的“隐形门槛”
   埋地环境电磁干扰复杂，BLDC驱动产生的PWM噪声和电机启停时的电流冲击是系统稳定性的主要威胁。必须采用隔离电源为Arduino与电机驱动供电（如案例二DigiKey建议的USB隔离），并在传感器信号线（如编码器、IMU的I2C总线）上增加磁珠或光耦隔离，避免电机大电流干扰导致控制器复位或传感器数据跳变。

4、多分支管道基础巡检机器人（分支识别+权重标记压缩）
适用场景：室外埋地长输管道（存在多分支、无复杂动态障碍），核心需求是沿主支管道遍历巡检，标记泄漏点、分支节点等关键位置，通过权重压缩精简路径，提升回溯效率。

核心逻辑：采用“磁导航循迹+分支识别+权重标记”，探索阶段识别分支并记录节点类型与风险权重，压缩阶段按权重阈值保留关键节点，回溯阶段沿压缩路径行驶，速度随节点风险自适应调整。

/* 多分支管道基础巡检：分支识别+动态权重压缩
   硬件：Arduino Mega + BLDC差速驱动 + 双磁导航传感器 + MQ-2气体传感器 + 分支识别传感器
   核心流程：分支探索→权重标记→路径压缩→回溯巡检
   参数：分支权重阈值60、泄漏权重90、管道总长度50000cm
*/
#include <SimpleFOC.h>

// --- 核心参数配置 ---
#define BRANCH_WEIGHT_THRESHOLD 60
#define LEAK_WEIGHT 90
#define PIPE_TOTAL_LENGTH 50000
#define BASE_SPEED 0.3
#define HIGH_SPEED 0.75
#define DANGER_SPEED 0.15

// --- 节点结构：包含权重与分支属性 ---
struct BranchNode {
  long position;
  int type;      // 0=辅助 1=分支 2=泄漏 3=阀门
  int weight;
  bool isDanger;
  int branchId;
};
vector<BranchNode> explorationPath;
vector<BranchNode> weightCompressedPath;
int currentBranch = 0;
long branchStartPos = 0;

// --- 硬件引脚定义 ---
#define MOTOR_L_PWM 5
#define MOTOR_R_PWM 6
#define MAG_L A0
#define MAG_R A1
#define GAS_SENSOR A2
#define BRANCH_SENSOR A3

// --- 全局变量 ---
long currentPosition = 0;
bool isExploring = true;
bool isBranchDetected = false;
int sensorCalibration[2] = {210, 215};

void setup() {
  Serial.begin(115200);
  initBLDC();
  calibrateSensors();
  Serial.println("多分支管道巡检机器人启动");
}

void loop() {
  if (isExploring) {
    branchExploration();
  } else {
    weightedReturn();
  }
  delay(20);
}

// 初始化BLDC电机
void initBLDC() {
  pinMode(MOTOR_L_PWM, OUTPUT);
  pinMode(MOTOR_R_PWM, OUTPUT);
}

// 校准磁导航传感器
void calibrateSensors() {
  for (int i = 0; i < 100; i++) {
    sensorCalibration[0] += analogRead(MAG_L);
    sensorCalibration[1] += analogRead(MAG_R);
  }
  sensorCalibration[0] /= 100;
  sensorCalibration[1] /= 100;
}

// 多分支探索：识别分支并标记权重
void branchExploration() {
  int magL = analogRead(MAG_L) - sensorCalibration[0];
  int magR = analogRead(MAG_R) - sensorCalibration[1];
  int gasValue = analogRead(GAS_SENSOR);
  int branchVal = analogRead(BRANCH_SENSOR);

  // 分支检测：磁信号突变+分支传感器触发
  if (!isBranchDetected && branchVal > 800) {
    isBranchDetected = true;
    branchStartPos = currentPosition;
    currentBranch++;
    Serial.println("检测到管道分支，标记起点");
  }

  // 磁导航纠偏
  if (magL > 150 && magR < 150) {
    turnLeft();
  } else if (magL < 150 && magR > 150) {
    turnRight();
  } else {
    moveForward();
    currentPosition += 10;
  }

  // 动态权重计算
  int nodeType = 0;
  int weight = 0;
  bool isDanger = (gasValue > LEAK_THRESHOLD);
  if (gasValue > LEAK_THRESHOLD) {
    nodeType = 2;
    weight = LEAK_WEIGHT;
  } else if (abs(currentPosition - branchStartPos) < 50 && isBranchDetected) {
    nodeType = 1;
    weight = BRANCH_WEIGHT_THRESHOLD;
    isBranchDetected = false;
  } else if (currentPosition % 1000 == 0) {
    nodeType = 3;
    weight = 50;
  }

  // 权重修正：环境干扰降低权重
  if (gasValue > 500) weight -= 10;

  // 节点记录
  if (currentPosition % 150 == 0) {
    explorationPath.push_back({currentPosition, nodeType, weight, isDanger, currentBranch});
    Serial.print("节点记录：位置");
    Serial.print(currentPosition);
    Serial.print("，类型");
    Serial.print(nodeType);
    Serial.print("，权重");
    Serial.println(weight);
  }

  // 探索完成判定
  if (currentPosition >= PIPE_TOTAL_LENGTH) {
    compressWithWeight();
    isExploring = false;
    Serial.println("多分支探索完成，进入权重压缩");
  }
}

// 基于权重的路径压缩：保留权重≥阈值的节点
void compressWithWeight() {
  for (auto node : explorationPath) {
    if (node.weight >= BRANCH_WEIGHT_THRESHOLD) {
      weightCompressedPath.push_back(node);
    }
  }
  Serial.print("权重压缩完成：原始节点");
  Serial.print(explorationPath.size());
  Serial.print("，核心节点");
  Serial.println(weightCompressedPath.size());
}

// 加权回溯：优先规避高风险节点，按权重规划路径
void weightedReturn() {
  if (weightCompressedPath.empty()) return;

  BranchNode target = weightCompressedPath[0];
  int magL = analogRead(MAG_L) - sensorCalibration[0];
  int magR = analogRead(MAG_R) - sensorCalibration[1];

  // 磁导航纠偏
  if (magL > 150 && magR < 150) {
    turnLeft();
  } else if (magL < 150 && magR > 150) {
    turnRight();
  } else {
    moveForward();
    currentPosition += 10;
  }

  // 速度自适应：权重越高风险越大，速度越低
  if (target.isDanger) {
    setSpeed(DANGER_SPEED);
  } else if (target.weight > 70) {
    setSpeed(BASE_SPEED);
  } else {
    setSpeed(HIGH_SPEED);
  }

  // 节点切换
  if (abs(currentPosition - target.position) < 50) {
    weightCompressedPath.erase(weightCompressedPath.begin());
    if (weightCompressedPath.empty()) {
      Serial.println("回溯完成，任务结束");
      stopMotors();
      while (1);
    }
  }
}

// 辅助函数：电机控制
void moveForward() { analogWrite(MOTOR_L_PWM, BASE_SPEED * 255); analogWrite(MOTOR_R_PWM, BASE_SPEED * 255); }
void turnLeft() { analogWrite(MOTOR_L_PWM, -BASE_SPEED * 255); analogWrite(MOTOR_R_PWM, BASE_SPEED * 255); delay(200); }
void turnRight() { analogWrite(MOTOR_L_PWM, BASE_SPEED * 255); analogWrite(MOTOR_R_PWM, -BASE_SPEED * 255); delay(200); }
void setSpeed(float speed) { analogWrite(MOTOR_L_PWM, speed * 255); analogWrite(MOTOR_R_PWM, speed * 255); }
void stopMotors() { analogWrite(MOTOR_L_PWM, 0); analogWrite(MOTOR_R_PWM, 0); }

5、高危区域多分支巡检机器人（防爆+避险权重压缩）
适用场景：炼油厂高危埋地管道区域（存在易燃易爆区域、紧急切断阀），核心需求是防爆安全巡检、紧急避险，避险后通过动态权重压缩快速回溯。

核心逻辑：采用“防爆安全策略+紧急避险触发+避险路径权重压缩”，探索阶段动态监测环境风险，触发避险时标记避险节点并赋予最高权重，压缩阶段优先保留避险节点与切断阀，回溯时快速抵达安全区域。

/* 高危区域多分支巡检：防爆+避险权重压缩
   硬件：Arduino Mega + 防爆BLDC模块 + 防爆磁导航 + 防爆气体传感器 + 紧急按钮
   核心流程：安全探索→紧急避险→避险路径压缩→快速回溯
   参数：爆炸阈值1000、避险点权重100、切断阀位置15000cm
*/
#include <SimpleFOC.h>

// --- 核心参数配置 ---
#define EXPLOSION_THRESHOLD 1000
#define EVACUATION_WEIGHT 100
#define VALVE_POSITION 15000
#define BASE_SPEED 0.25
#define HIGH_SPEED 0.6
#define EMERGENCY_SPEED 0.5

// --- 节点结构：新增避险类型 ---
struct DangerNode {
  long position;
  int type;      // 0=辅助 1=切断阀 2=泄漏 3=避险点
  int weight;
  bool isDanger;
};
vector<DangerNode> explorationPath;
vector<DangerNode> emergencyCompressedPath;
bool emergencyTriggered = false;
long emergencyPosition = 0;

// --- 硬件引脚 ---
#define MOTOR_L_PWM 5
#define MOTOR_R_PWM 6
#define MAG_L A0
#define MAG_R A1
#define EXPLOSIVE_SENSOR A2
#define EMERGENCY_BUTTON 2

// --- 全局变量 ---
long currentPosition = 0;
bool isExploring = true;
int sensorCalibration[2] = {208, 212};

void setup() {
  Serial.begin(115200);
  initBLDC();
  calibrateSensors();
  pinMode(EMERGENCY_BUTTON, INPUT_PULLUP);
  Serial.println("高危区域防爆巡检机器人启动");
}

void loop() {
  // 紧急触发检测：自动+手动
  int explosiveValue = analogRead(EXPLOSIVE_SENSOR);
  bool buttonPressed = !digitalRead(EMERGENCY_BUTTON);
  if ((explosiveValue > EXPLOSION_THRESHOLD || buttonPressed) && !emergencyTriggered) {
    emergencyTriggered = true;
    emergencyPosition = currentPosition;
    explorationPath.push_back({emergencyPosition, 3, EVACUATION_WEIGHT, true});
    Serial.println("紧急避险触发，标记避险点");
  }

  if (isExploring) {
    dangerExploration(explosiveValue);
  } else {
    emergencyReturn();
  }
  delay(20);
}

// 防爆探索：危险区域低速，触发避险
void dangerExploration(int explosiveValue) {
  int magL = analogRead(MAG_L) - sensorCalibration[0];
  int magR = analogRead(MAG_R) - sensorCalibration[1];

  // 防爆速度限制：风险越高速度越低
  float speed = explosiveValue > EXPLOSION_THRESHOLD * 0.7 ? BASE_SPEED : (BASE_SPEED + 0.1);
  setSpeed(speed);

  // 磁导航纠偏
  if (magL > 150 && magR < 150) {
    turnLeft();
  } else if (magL < 150 && magR > 150) {
    turnRight();
  } else {
    moveForward();
    currentPosition += 10;
  }

  // 节点记录：切断阀、泄漏点、避险点
  if (currentPosition == VALVE_POSITION) {
    explorationPath.push_back({currentPosition, 1, 80, false});
    Serial.println("记录紧急切断阀位置");
  } else if (explosiveValue > EXPLOSION_THRESHOLD * 0.5) {
    explorationPath.push_back({currentPosition, 2, 90, true});
  }

  // 探索完成判定：抵达避险点或全程覆盖
  if (emergencyTriggered || currentPosition >= 30000) {
    compressEmergencyPath();
    isExploring = false;
    Serial.println("防爆探索完成，进入紧急压缩");
  }
}

// 紧急路径压缩：优先保留避险点、切断阀
void compressEmergencyPath() {
  for (auto node : explorationPath) {
    if (node.type == 1 || node.type == 3 || node.weight >= 70) {
      emergencyCompressedPath.push_back(node);
    }
  }
  Serial.print("紧急压缩完成：原始节点");
  Serial.print(explorationPath.size());
  Serial.print("，核心节点");
  Serial.println(emergencyCompressedPath.size());
}

// 紧急回溯：快速抵达避险点/出口
void emergencyReturn() {
  if (emergencyCompressedPath.empty()) return;

  DangerNode target = emergencyCompressedPath[0];
  int magL = analogRead(MAG_L) - sensorCalibration[0];
  int magR = analogRead(MAG_R) - sensorCalibration[1];

  // 避险时速度提升，优先抵达避险点
  setSpeed(target.type == 3 ? EMERGENCY_SPEED : HIGH_SPEED);

  // 磁导航纠偏
  if (magL > 150 && magR < 150) {
    turnLeft();
  } else if (magL < 150 && magR > 150) {
    turnRight();
  } else {
    moveForward();
    currentPosition += 10;
  }

  // 节点切换
  if (abs(currentPosition - target.position) < 50) {
    emergencyCompressedPath.erase(emergencyCompressedPath.begin());
    if (emergencyCompressedPath.empty()) {
      Serial.println("紧急回溯完成，已抵达安全区域");
      stopMotors();
      while (1);
    }
  }
}

// 辅助函数（同前，略）
void initBLDC() { pinMode(MOTOR_L_PWM, OUTPUT); pinMode(MOTOR_R_PWM, OUTPUT); }
void calibrateSensors() { /* 校准逻辑同前 */ }
void moveForward() { /* 同前 */ }
void turnLeft() { /* 同前 */ }
void turnRight() { /* 同前 */ }
void setSpeed(float speed) { analogWrite(MOTOR_L_PWM, speed * 255); analogWrite(MOTOR_R_PWM, speed * 255); }
void stopMotors() { analogWrite(MOTOR_L_PWM, 0); analogWrite(MOTOR_R_PWM, 0); }

6、长距离埋地管道巡检机器人（两阶段探索+动态权重压缩）
适用场景：长距离埋地管道（分支多、路径长，需兼顾探索效率与巡检速度），核心需求是通过两阶段探索覆盖全管道，动态标记节点权重，压缩后实现高速回溯，提升长距离巡检效率。

核心逻辑：采用“两阶段探索（低速探索+高速回溯）+动态权重压缩”，第一阶段低速遍历所有分支，记录节点并动态计算权重；第二阶段对路径进行权重压缩，剔除低权重冗余节点，以高速执行回溯巡检。

/* 长距离管道巡检：两阶段探索+动态权重压缩
   硬件：Arduino Mega + BLDC差速驱动 + 多磁导航传感器 + 气体/温度传感器 + 扩展SPI Flash
   核心流程：第一阶段低速探索→动态权重标记→第二阶段路径压缩→高速回溯
   参数：探索速度0.3m/s、回溯速度0.7m/s、权重阈值50
*/
#include <SimpleFOC.h>
#include <SPI.h>
#include <FlashStorage.h> // 扩展存储，避免内存溢出

// --- 核心参数配置 ---
#define EXPLORE_SPEED 0.3
#define RETRACE_SPEED 0.7
#define WEIGHT_THRESHOLD 50
#define PIPE_MAX_LENGTH 100000

// --- 节点结构：支持长距离存储 ---
struct LongPipeNode {
  long position;
  int type;
  int weight;
  bool isDanger;
};
FlashStorage(path_storage, LongPipeNode, 1000); // 扩展Flash存储路径
vector<LongPipeNode> explorationPath;
vector<LongPipeNode> compressedPath;

// --- 硬件引脚 ---
#define MOTOR_L_PWM 9
#define MOTOR_R_PWM 10
#define MAG_L A0
#define MAG_R A1
#define GAS_SENSOR A2
#define TEMP_SENSOR A3

// --- 全局变量 ---
long currentPosition = 0;
int phase = 0; // 0=探索阶段 1=回溯阶段
int sensorCalibration[2] = {205, 210};

void setup() {
  Serial.begin(115200);
  initBLDC();
  calibrateSensors();
  Serial.println("长距离管道巡检机器人启动");
}

void loop() {
  if (phase == 0) {
    longDistanceExplore();
  } else {
    longDistanceRetrace();
  }
  delay(20);
}

// 初始化BLDC与扩展存储
void initBLDC() {
  pinMode(MOTOR_L_PWM, OUTPUT);
  pinMode(MOTOR_R_PWM, OUTPUT);
  SPI.begin();
}

// 校准传感器
void calibrateSensors() {
  for (int i = 0; i < 100; i++) {
    sensorCalibration[0] += analogRead(MAG_L);
    sensorCalibration[1] += analogRead(MAG_R);
  }
  sensorCalibration[0] /= 100;
  sensorCalibration[1] /= 100;
}

// 长距离探索：低速遍历，动态标记权重
void longDistanceExplore() {
  int magL = analogRead(MAG_L) - sensorCalibration[0];
  int magR = analogRead(MAG_R) - sensorCalibration[1];
  int gasValue = analogRead(GAS_SENSOR);
  int tempValue = analogRead(TEMP_SENSOR);

  // 磁导航纠偏
  if (magL > 150 && magR < 150) {
    turnLeft(EXPLORE_SPEED);
  } else if (magL < 150 && magR > 150) {
    turnRight(EXPLORE_SPEED);
  } else {
    moveForward(EXPLORE_SPEED);
    currentPosition += 10;
  }

  // 动态权重计算：综合气体、温度风险
  int nodeType = 0;
  int weight = 0;
  bool isDanger = (gasValue > 600 || tempValue > 800);
  if (gasValue > 600) {
    nodeType = 2;
    weight = 80;
  } else if (tempValue > 800) {
    nodeType = 4;
    weight = 70;
  } else if (currentPosition % 2000 == 0) {
    nodeType = 3;
    weight = 50;
  } else {
    nodeType = 0;
    weight = 30;
  }

  // 节点记录到扩展存储
  if (currentPosition % 200 == 0) {
    explorationPath.push_back({currentPosition, nodeType, weight, isDanger});
    if (explorationPath.size() % 100 == 0) {
      path_storage.write(explorationPath.data(), explorationPath.size()); // 定期写入Flash
    }
    Serial.print("探索节点：位置");
    Serial.print(currentPosition);
    Serial.print("，权重");
    Serial.println(weight);
  }

  // 探索完成判定
  if (currentPosition >= PIPE_MAX_LENGTH) {
    compressLongPath();
    phase = 1;
    Serial.println("长距离探索完成，进入路径压缩与回溯");
  }
}

// 长距离路径压缩：保留权重≥阈值的节点
void compressLongPath() {
  for (auto node : explorationPath) {
    if (node.weight >= WEIGHT_THRESHOLD) {
      compressedPath.push_back(node);
    }
  }
  // 从Flash读取备份路径验证
  LongPipeNode backup[1000];
  path_storage.read(backup, explorationPath.size());
  Serial.print("长距离压缩完成：原始节点");
  Serial.print(explorationPath.size());
  Serial.print("，核心节点");
  Serial.println(compressedPath.size());
}

// 高速回溯：沿压缩路径行驶
void longDistanceRetrace() {
  if (compressedPath.empty()) {
    Serial.println("回溯完成，任务结束");
    stopMotors();
    while (1);
    return;
  }

  LongPipeNode target = compressedPath[0];
  int magL = analogRead(MAG_L) - sensorCalibration[0];
  int magR = analogRead(MAG_R) - sensorCalibration[1];

  // 磁导航纠偏
  if (magL > 150 && magR < 150) {
    turnLeft(RETRACE_SPEED);
  } else if (magL < 150 && magR > 150) {
    turnRight(RETRACE_SPEED);
  } else {
    moveForward(RETRACE_SPEED);
    currentPosition += 10;
  }

  // 节点切换
  if (abs(currentPosition - target.position) < 50) {
    compressedPath.erase(compressedPath.begin());
  }
}

// 辅助函数：电机控制
void moveForward(float speed) { analogWrite(MOTOR_L_PWM, speed * 255); analogWrite(MOTOR_R_PWM, speed * 255); }
void turnLeft(float speed) { analogWrite(MOTOR_L_PWM, -speed * 255); analogWrite(MOTOR_R_PWM, speed * 255); delay(200); }
void turnRight(float speed) { analogWrite(MOTOR_L_PWM, speed * 255); analogWrite(MOTOR_R_PWM, -speed * 255); delay(200); }
void stopMotors() { analogWrite(MOTOR_L_PWM, 0); analogWrite(MOTOR_R_PWM, 0); }

要点解读

1. 多分支路径决策：精准识别与动态决策的协同
   室外埋地管道多分支是核心场景，路径决策需兼顾分支识别准确性与决策实时性，是多分支巡检的基础：
   分支识别双重校验：案例采用“磁信号突变+辅助传感器触发”双重校验，避免单一传感器误判，确保分支识别可靠，适配埋地管道分支的物理特征；
   决策策略场景适配：基础场景采用“优先遍历”策略，高危场景结合避险优先级，长距离场景兼顾效率与覆盖，确保决策逻辑匹配场景核心需求；
   状态机驱动流程：通过有限状态机区分探索与回溯阶段，避免逻辑混乱，保障多分支场景下流程清晰可控，降低调试难度。
2. 动态权重压缩：风险导向与资源约束的平衡
   动态权重压缩是提升巡检效率的关键，需在资源受限（Arduino内存）与巡检完整性之间找到平衡，核心要点如下：
   权重设计贴合风险：权重计算综合泄漏浓度、温度、分支重要性等风险因子，泄漏点、避险点赋予高权重，辅助节点赋予低权重，确保压缩后保留核心巡检目标；
   压缩策略适配场景：基础场景按固定阈值压缩，高危场景优先保留避险节点，长距离场景结合存储能力动态压缩，避免盲目压缩导致关键信息丢失；
   资源优化保障落地：采用固定长度数组替代动态容器，结合扩展Flash存储，解决Arduino内存不足问题，确保长距离、多分支场景下路径数据不溢出，方案可落地。
3. BLDC差速驱动：精准控制与环境适配的保障
   BLDC差速驱动是机器人运动的核心，需兼顾控制精度与室外复杂环境适应性，保障路径执行的可靠性：
   闭环控制提升精度：采用FOC闭环控制，搭配编码器反馈，实现速度与位置的精准控制，避免开环控制导致的转速漂移，确保沿压缩路径精准行驶；
   差速模型适配转向：基于差速底盘运动学模型，将路径规划的线速度、角速度转换为左右轮转速，实现精准转向，适配管道转弯、分支切换等场景；
   环境适配增强鲁棒性：室外环境存在坡道、松软土壤，BLDC高扭矩特性保障爬坡能力，FOC低转矩脉动减少振动，避免振动影响传感器读数，提升环境适应性。
4. 两阶段流程闭环：探索与回溯的高效衔接
   两阶段流程是效率提升的核心，需确保探索阶段数据完整、回溯阶段高效执行，形成闭环：
   阶段分工明确：第一阶段低速探索，核心目标是全覆盖巡检区域、记录关键节点；第二阶段高速回溯，核心目标是沿优化路径高效巡检，分工清晰避免功能冲突；
   数据衔接可靠：探索阶段记录的节点数据通过压缩算法处理后，直接作为回溯阶段的路径依据，压缩算法确保数据精简且完整，避免数据丢失或衔接断层；
   状态切换可控：通过明确的阶段标志位控制流程切换，探索完成后自动触发压缩与回溯，无需人工干预，保障流程自动化，适配无人值守巡检需求。
5. 安全与可靠性设计：高危环境巡检的底线
   室外埋地管道常涉及高危区域，安全与可靠性是巡检的前提，需从硬件、软件、环境适配多维度筑牢底线：
   防爆与硬件隔离：高危场景采用防爆BLDC模块，电机驱动与主控电气隔离，避免电火花引燃可燃气体，同时采用屏蔽线、滤波电容抑制电磁干扰，保障硬件安全；
   应急避险机制：建立自动+手动双重避险触发机制，触发后标记避险节点并赋予最高权重，回溯阶段优先抵达避险点，确保突发风险时能快速撤离，保障设备与环境安全；
   数据与流程容错：通过Flash存储备份路径数据，避免断电丢失；设置通信超时、看门狗机制，通信中断时自动停机，防止机器人失控，保障流程可靠。

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