该方案的核心特点是磁导航提供厘米级定位精度与强抗干扰能力，路径节点压缩大幅节省Arduino内存并提升回溯速度，结合BLDC FOC控制实现精准运动；主要适用于危化厂区管廊、地下管廊、工业AGV及教育竞赛等结构化场景；实际部署需严格满足防爆认证，重点防范电磁干扰、航位推算误差累积及通信中断风险。

一、主要特点
磁导航：结构化环境下的厘米级定位精度
磁导航通过在管道沿线铺设磁条或埋设磁钉，由机器人底部的磁传感器阵列实时检测磁场位置偏差，实现沿管道的高精度循迹。其核心优势在于：
抗干扰能力强：相比红外或超声波导航，磁导航不受粉尘、烟雾、光照变化影响，在危化品环境中可靠性更高。
定位精度高：配合BLDC电机的FOC闭环控制，可实现厘米级的横向定位精度，确保机器人在狭窄管廊中不会剐蹭管道。
部署成本低：磁条/磁钉安装简单，无需复杂的激光雷达或视觉系统，适合预算有限的中小型项目。
路径节点压缩：Arduino有限内存下的关键优化
Arduino Uno仅有2KB SRAM，存储完整路径极易溢出。路径节点压缩通过以下策略解决这一瓶颈：
仅存储关键拐点：将连续直行段合并为一个节点，仅记录转弯、掉头等方向变化点，大幅减少存储量。
位编码压缩：使用uint8_t存储方向编码（如F=前进、L=左转、R=右转、B=掉头），甚至可用位数组（Bit Array）标记已访问节点，将256个节点的状态压缩至32字节。
在线简化：探索过程中实时执行"xBx"模式压缩（如LBL→S），避免路径记录无限膨胀。
BLDC+FOC：高动态精准执行
BLDC无刷电机配合SimpleFOC库的磁场定向控制（FOC），为巡检机器人提供：
高扭矩密度：支持机器人在管道坡道、阀门密集区等复杂工况下稳定运行。
低转矩脉动：正弦波驱动消除抖动，确保低速巡检时的平稳性，避免振动影响传感器读数。
快速响应：电流环响应频率达千赫兹级，在交叉口可实现毫秒级的速度切换和精确90°转向。
两阶段运行模式：探索→优化→高速回溯
第一阶段（探索）：以低速、高可靠性沿磁导航路径遍历巡检区域，记录关键节点并在线压缩。
第二阶段（回溯）：加载压缩后的优化路径，以高速执行巡检任务，显著提升巡检效率。

二、应用场景
危化厂区管廊巡检
这是最核心的应用场景。危化厂区管廊管线密集、通道狭窄，人工巡检存在盲区大、响应慢、数据不可追溯等问题。
典型任务：管道跑冒滴漏检测、法兰/阀门密封状态核查、可燃/有毒气体浓度监测、红外测温。
实际案例：广东石化已部署管廊"巡弋"机器人系统，实现7×24小时不间断巡检，响应速度从人工平均30分钟提升至秒级报警，漏检率从约3%降至0.1%。
磁导航优势：管廊结构高度规则化，磁条铺设一次即可长期使用，机器人沿固定路径反复巡检，路径节点压缩使单次巡检时间大幅缩短。
地下综合管廊与受限空间探测
进入人员难以进入的地下管廊、阀门井或储罐内部，对甲烷、硫化氢等有毒有害气体进行检测和定位。
磁导航优势：地下环境无GPS信号、光照不足，磁导航不依赖外部信号，可靠性远优于视觉或激光导航。
路径压缩优势：地下管廊路径长且分支多，压缩后的路径数据可轻松存入Arduino内存，避免使用外部存储设备。
工业AGV原型验证
在结构化仓储环境中，磁导航+路径压缩方案可作为低成本AGV的验证平台，实现"通道巡检→障碍绕行→返回充电"的完整任务闭环。
教育与竞赛
高校自动控制、机器人课程的教学实验平台，以及Micromouse、智能汽车竞赛等赛事中，用于演示路径规划、闭环控制与嵌入式系统集成。

三、需要注意的事项
防爆认证是硬性门槛
危化品环境对电气设备有严格的防爆要求。实际工业部署中，巡检机器人整机需达到Ex db eb ib mb IIC T6 Gb等防爆等级，采用全封闭防爆一体化结构，可在-20℃~60℃宽温区间稳定运行。
Arduino平台局限：标准Arduino开发板不具备防爆认证，仅适用于教学原型验证。实际工业部署需选用专业防爆控制器或对Arduino进行防爆封装（隔爆外壳+本安电路设计）。
电气隔离：BLDC驱动板与主控之间需做电气隔离，防止电机换相产生的电火花引燃可燃气体。
电磁干扰（EMI）是最大技术挑战
BLDC电机运行时产生的强电磁噪声，会严重干扰磁传感器和气体传感器的读数。
时序隔离：在电机刹车或静止的瞬间进行传感器采样，避开PWM驱动的峰值干扰期。
硬件滤波：在传感器信号线和电源线上加装磁珠（Ferrite Bead）和去耦电容。
屏蔽设计：磁传感器线缆使用屏蔽线，屏蔽层单端接地。
航位推算误差累积
编码器打滑、轮径变化、IMU漂移会导致位置估计随时间发散，尤其在长距离管廊巡检中。
磁导航校正：利用磁条/磁钉作为绝对参照物，在每个节点进行横向位置微调（Wall Following Correction）。
周期性校准：在管廊入口和关键拐点设置磁信标，进行周期性绝对坐标校准，重置累积误差。
限制单次探索距离：建议单次探索距离不超过10米，避免误差过大导致回溯失败。
通信可靠性与断网自主能力
危化品厂区金属结构密集，无线信号衰减严重，通信中断风险高。
多链路冗余：主通信链路（WiFi/4G）+ 备用链路（LoRa），当主链路中断时自动切换。
断网安全策略：通信完全中断时，机器人不能"瘫痪"，应预设"断线返航"或"原地待命"的安全策略。
心跳包机制：Arduino端必须编写心跳包检测和超时重连机制，一旦通信中断超过阈值，立即进入安全停机模式。
传感器防护与寿命管理
气体传感器中毒：某些有毒化学物质可能导致传感器"中毒"失效，需选用抗中毒能力强的传感器，并设计可快速更换的模块化传感器仓。
光学窗口保护：摄像头和红外测温模块的光学窗口需配备高压气刀（Air Knife）持续吹扫，防止粉尘附着。
定期校准：气体传感器每3个月使用标准气体进行零点和量程校准，确保检测精度。
内存管理与数据结构优化
Arduino Uno仅2KB SRAM，路径存储和地图数据必须精打细算：
使用PROGMEM将静态地图存入Flash。
路径存储采用固定长度数组+索引指针，避免使用String或动态vector。
必要时启用外部SPI Flash（如W25Q64）扩展存储。
复杂路径规划算法（如A*、Dijkstra）建议移至上位机（如Raspberry Pi），Arduino仅接收路径点序列执行。

1、基础磁导航管道巡检机器人

#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>

// BLDC电机驱动引脚定义
#define MOTOR_L_PWM 9
#define MOTOR_L_DIR 8
#define MOTOR_R_PWM 10
#define MOTOR_R_DIR 11

// 霍尔磁导航传感器阵列
#define HALL_SENSOR_NUM 8
const int hallPins[HALL_SENSOR_NUM] = {A0, A1, A2, A3, A4, A5, 2, 3};

// 危化品气体传感器
#define GAS_SENSOR_PIN A6
#define TEMP_SENSOR_PIN A7

// 编码器接口
#define ENCODER_L_A 4
#define ENCODER_L_B 5
#define ENCODER_R_A 6
#define ENCODER_R_B 7

// PID控制参数
float Kp = 2.5, Ki = 0.1, Kd = 0.5;
float errorSum = 0, lastError = 0;
float targetPosition = 3.5; // 目标位置（传感器阵列中心）

// 里程计数据
volatile long encoderLCount = 0;
volatile long encoderRCount = 0;
float totalDistance = 0;

// 路径节点记录
struct PathNode {
    float distance;
    int gasLevel;
    float temperature;
    int nodeType; // 0-普通 1-弯道 2-阀门 3-异常点
};

PathNode pathNodes[100];
int nodeCount = 0;

void setup() {
    Serial.begin(115200);
    
    // 初始化电机驱动
    pinMode(MOTOR_L_PWM, OUTPUT);
    pinMode(MOTOR_L_DIR, OUTPUT);
    pinMode(MOTOR_R_PWM, OUTPUT);
    pinMode(MOTOR_R_DIR, OUTPUT);
    
    // 初始化霍尔传感器
    for(int i = 0; i < HALL_SENSOR_NUM; i++) {
        pinMode(hallPins[i], INPUT);
    }
    
    // 初始化编码器中断
    attachInterrupt(digitalPinToInterrupt(ENCODER_L_A), encoderLISR, CHANGE);
    attachInterrupt(digitalPinToInterrupt(ENCODER_R_A), encoderRISR, CHANGE);
    
    Serial.println("危化品管道巡检机器人启动");
    Serial.println("磁导航系统初始化完成");
}

void loop() {
    // 读取霍尔传感器阵列
    float position = readHallSensors();
    
    // 计算PID控制
    float error = position - targetPosition;
    errorSum += error;
    float errorDiff = error - lastError;
    float correction = Kp * error + Ki * errorSum + Kd * errorDiff;
    lastError = error;
    
    // 基础速度
    int baseSpeed = 120;
    
    // 计算左右轮速度
    int leftSpeed = baseSpeed - correction;
    int rightSpeed = baseSpeed + correction;
    
    // 限幅
    leftSpeed = constrain(leftSpeed, -255, 255);
    rightSpeed = constrain(rightSpeed, -255, 255);
    
    // 驱动电机
    driveMotors(leftSpeed, rightSpeed);
    
    // 读取环境数据
    int gasLevel = analogRead(GAS_SENSOR_PIN);
    float temperature = analogRead(TEMP_SENSOR_PIN) * 0.488;
    
    // 计算里程
    totalDistance += (encoderLCount + encoderRCount) / 2.0 * 0.01;
    encoderLCount = 0;
    encoderRCount = 0;
    
    // 检测路径节点
    if(abs(position - targetPosition) > 2.0) {
        // 偏离磁轨，可能是弯道或异常
        recordPathNode(totalDistance, gasLevel, temperature, 1);
    }
    
    // 危险气体检测
    if(gasLevel > 800) {
        recordPathNode(totalDistance, gasLevel, temperature, 3);
        Serial.println("警告：检测到高浓度危化品气体！");
        emergencyStop();
    }
    
    // 数据上报
    if(millis() % 1000 < 50) {
        sendTelemetry(position, gasLevel, temperature, totalDistance);
    }
    
    delay(20);
}

// 读取霍尔传感器阵列位置
float readHallSensors() {
    float weightedSum = 0;
    float totalWeight = 0;
    
    for(int i = 0; i < HALL_SENSOR_NUM; i++) {
        int value = digitalRead(hallPins[i]);
        weightedSum += value * i;
        totalWeight += value;
    }
    
    if(totalWeight == 0) return -1; // 失去磁轨
    return weightedSum / totalWeight;
}

// 驱动电机
void driveMotors(int leftSpeed, int rightSpeed) {
    if(leftSpeed >= 0) {
        digitalWrite(MOTOR_L_DIR, HIGH);
        analogWrite(MOTOR_L_PWM, leftSpeed);
    } else {
        digitalWrite(MOTOR_L_DIR, LOW);
        analogWrite(MOTOR_L_PWM, -leftSpeed);
    }
    
    if(rightSpeed >= 0) {
        digitalWrite(MOTOR_R_DIR, HIGH);
        analogWrite(MOTOR_R_PWM, rightSpeed);
    } else {
        digitalWrite(MOTOR_R_DIR, LOW);
        analogWrite(MOTOR_R_PWM, -rightSpeed);
    }
}

// 记录路径节点
void recordPathNode(float dist, int gas, float temp, int type) {
    if(nodeCount < 100) {
        pathNodes[nodeCount].distance = dist;
        pathNodes[nodeCount].gasLevel = gas;
        pathNodes[nodeCount].temperature = temp;
        pathNodes[nodeCount].nodeType = type;
        nodeCount++;
    }
}

// 紧急停止
void emergencyStop() {
    driveMotors(0, 0);
    Serial.println("紧急停止！");
    while(1);
}

// 编码器中断服务程序
void encoderLISR() {
    if(digitalRead(ENCODER_L_B)) encoderLCount++;
    else encoderLCount--;
}

void encoderRISR() {
    if(digitalRead(ENCODER_R_B)) encoderRCount++;
    else encoderRCount--;
}

// 发送遥测数据
void sendTelemetry(float pos, int gas, float temp, float dist) {
    Serial.print("位置:");
    Serial.print(pos);
    Serial.print(" 气体:");
    Serial.print(gas);
    Serial.print(" 温度:");
    Serial.print(temp);
    Serial.print(" 里程:");
    Serial.println(dist);
}

2、路径节点压缩与智能巡检

#include <Arduino.h>
#include <EEPROM.h>

// 电机和传感器定义（同案例一）
#define MOTOR_L_PWM 9
#define MOTOR_R_PWM 10
#define HALL_SENSOR_NUM 8

// EEPROM存储地址
#define EEPROM_NODE_START 100
#define MAX_STORED_NODES 50

// 压缩路径节点结构
struct CompressedNode {
    uint16_t distance;      // 距离（cm）
    uint8_t gasLevel;       // 气体浓度（压缩为0-255）
    int8_t temperature;     // 温度偏移
    uint8_t curvature;      // 曲率信息
    uint8_t nodeFlags;      // 节点标志位
};

// 节点标志位定义
#define FLAG_NORMAL      0x00
#define FLAG_CURVE       0x01
#define FLAG_VALVE       0x02
#define FLAG_ANOMALY     0x04
#define FLAG_CHECKPOINT  0x08
#define FLAG_HIGH_GAS    0x10

class PipelineInspector {
private:
    CompressedNode nodes[MAX_STORED_NODES];
    int nodeCount;
    float currentDistance;
    float lastNodeDistance;
    
    // 磁导航PID
    float Kp, Ki, Kd;
    float errorSum, lastError;
    
    // 巡检状态
    bool inspectionMode;
    bool returnMode;
    int currentNodeIndex;
    
public:
    PipelineInspector() {
        Kp = 2.8;
        Ki = 0.08;
        Kd = 0.4;
        errorSum = 0;
        lastError = 0;
        currentDistance = 0;
        lastNodeDistance = 0;
        nodeCount = 0;
        inspectionMode = false;
        returnMode = false;
        currentNodeIndex = 0;
    }
    
    void begin() {
        Serial.begin(115200);
        initPins();
        loadNodesFromEEPROM();
        Serial.println("管道巡检系统初始化完成");
    }
    
    void initPins() {
        pinMode(MOTOR_L_PWM, OUTPUT);
        pinMode(MOTOR_R_PWM, OUTPUT);
        for(int i = 0; i < HALL_SENSOR_NUM; i++) {
            pinMode(A0 + i, INPUT);
        }
    }
    
    void startInspection() {
        inspectionMode = true;
        currentNodeIndex = 0;
        Serial.println("开始巡检任务");
    }
    
    void update() {
        float position = readPosition();
        float error = position - 3.5;
        
        // 自适应PID（根据速度调整）
        float adaptiveKp = Kp * (1 + abs(error) * 0.1);
        float correction = adaptiveKp * error + Ki * errorSum + Kd * (error - lastError);
        errorSum += error;
        lastError = error;
        
        if(inspectionMode) {
            // 巡检模式
            int speed = 100;
            driveMotors(speed - correction, speed + correction);
            
            // 检测和记录节点
            detectAndRecordNodes();
            
        } else if(returnMode) {
            // 返回模式 - 使用记录的路径
            followRecordedPath();
        }
        
        // 紧急检查
        if(checkEmergency()) {
            emergencyProcedure();
        }
        
        delay(30);
    }
    
    void detectAndRecordNodes() {
        int gas = analogRead(A6);
        float temp = analogRead(A7) * 0.488;
        float pos = readPosition();
        
        // 节点检测条件
        bool isCurve = abs(pos - 3.5) > 2.0;
        bool isValve = detectValveMarker();
        bool isAnomaly = gas > 700 || temp > 50;
        
        if(isCurve || isValve || isAnomaly) {
            CompressedNode newNode;
            newNode.distance = (uint16_t)(currentDistance * 10); // 压缩到厘米
            newNode.gasLevel = (uint8_t)map(gas, 0, 1023, 0, 255);
            newNode.temperature = (int8_t)(temp - 25); // 相对25度的偏移
            newNode.curvature = (uint8_t)(abs(pos - 3.5) * 50);
            newNode.nodeFlags = 0;
            
            if(isCurve) newNode.nodeFlags |= FLAG_CURVE;
            if(isValve) newNode.nodeFlags |= FLAG_VALVE;
            if(isAnomaly) newNode.nodeFlags |= FLAG_ANOMALY;
            if(gas > 700) newNode.nodeFlags |= FLAG_HIGH_GAS;
            
            // 检查点标记（每10个节点）
            if(nodeCount % 10 == 0) newNode.nodeFlags |= FLAG_CHECKPOINT;
            
            // 节点压缩：合并相邻的相似节点
            if(nodeCount > 0 && 
               abs(nodes[nodeCount-1].distance - newNode.distance) < 20 &&
               nodes[nodeCount-1].nodeFlags == newNode.nodeFlags) {
                // 更新现有节点而不是添加新节点
                nodes[nodeCount-1] = newNode;
            } else if(nodeCount < MAX_STORED_NODES) {
                nodes[nodeCount++] = newNode;
                lastNodeDistance = currentDistance;
            }
            
            // 存储到EEPROM
            saveNodeToEEPROM(nodeCount - 1);
        }
    }
    
    void followRecordedPath() {
        if(currentNodeIndex >= nodeCount) {
            // 路径完成
            stopMotors();
            returnMode = false;
            Serial.println("返回完成");
            return;
        }
        
        // 根据记录的路径导航
        CompressedNode targetNode = nodes[currentNodeIndex];
        
        if(currentDistance >= targetNode.distance) {
            // 到达节点，执行节点动作
            executeNodeAction(targetNode);
            currentNodeIndex++;
            return;
        }
        
        // 使用曲率信息调整速度
        int speed = 80;
        if(targetNode.curvature > 100) {
            speed = 50; // 弯道减速
        }
        
        float pos = readPosition();
        float error = pos - 3.5;
        float correction = Kp * error;
        
        driveMotors(speed - correction, speed + correction);
    }
    
    void executeNodeAction(CompressedNode node) {
        if(node.nodeFlags & FLAG_VALVE) {
            Serial.println("到达阀门位置，执行检查");
            // 执行阀门检查程序
            delay(2000);
        }
        
        if(node.nodeFlags & FLAG_HIGH_GAS) {
            Serial.println("高浓度气体区域，加强检测");
            // 增加采样频率
        }
        
        if(node.nodeFlags & FLAG_CHECKPOINT) {
            Serial.print("检查点:");
            Serial.println(node.distance);
        }
    }
    
    void saveNodeToEEPROM(int index) {
        int addr = EEPROM_NODE_START + index * sizeof(CompressedNode);
        EEPROM.put(addr, nodes[index]);
    }
    
    void loadNodesFromEEPROM() {
        for(int i = 0; i < MAX_STORED_NODES; i++) {
            int addr = EEPROM_NODE_START + i * sizeof(CompressedNode);
            EEPROM.get(addr, nodes[i]);
            if(nodes[i].distance > 0 && nodes[i].distance < 10000) {
                nodeCount++;
            }
        }
        Serial.print("从EEPROM加载");
        Serial.print(nodeCount);
        Serial.println("个路径节点");
    }
    
    bool detectValveMarker() {
        // 检测特殊磁标记（阀门位置）
        return digitalRead(2) == HIGH;
    }
    
    bool checkEmergency() {
        int gas = analogRead(A6);
        float temp = analogRead(A7) * 0.488;
        return gas > 900 || temp > 60;
    }
    
    void emergencyProcedure() {
        stopMotors();
        Serial.println("紧急情况，停止运行");
        delay(5000);
    }
    
    void driveMotors(int left, int right) {
        // 电机驱动实现
        left = constrain(left, -255, 255);
        right = constrain(right, -255, 255);
        analogWrite(MOTOR_L_PWM, abs(left));
        analogWrite(MOTOR_R_PWM, abs(right));
        digitalWrite(8, left > 0);
        digitalWrite(11, right > 0);
    }
    
    void stopMotors() {
        analogWrite(MOTOR_L_PWM, 0);
        analogWrite(MOTOR_R_PWM, 0);
    }
    
    float readPosition() {
        int sum = 0;
        int count = 0;
        for(int i = 0; i < HALL_SENSOR_NUM; i++) {
            if(digitalRead(A0 + i)) {
                sum += i;
                count++;
            }
        }
        if(count == 0) return -1;
        return (float)sum / count;
    }
};

PipelineInspector inspector;

void setup() {
    inspector.begin();
    inspector.startInspection();
}

void loop() {
    inspector.update();
}

3、多模式智能巡检与数据压缩

#include <Arduino.h>
#include <SD.h>
#include <Wire.h>

// 系统配置
#define SYSTEM_VERSION "V3.2"
#define MAX_NODES 200
#define SD_CS_PIN 53

// 运行模式
enum RobotMode {
    MODE_STANDBY,
    MODE_EXPLORATION,   // 探索模式
    MODE_INSPECTION,    // 巡检模式
    MODE_RETURN,        // 返回模式
    MODE_EMERGENCY      // 紧急模式
};

// 传感器数据结构
struct SensorData {
    uint16_t gasConcentration;
    int16_t temperature;     // 温度 * 10
    uint16_t humidity;       // 湿度 * 10
    uint8_t pressure;        // 压力偏移
    uint16_t magneticField;  // 磁场强度
    uint8_t vibration;       // 振动等级
    uint32_t timestamp;      // 时间戳
};

// 压缩路径节点
struct PathNodeV2 {
    uint16_t nodeId;
    uint16_t distance;       // 距离（cm）
    uint8_t position;        // 磁轨位置（0-7）
    uint8_t velocity;        // 速度等级
    uint8_t flags;           // 节点标志
    SensorData sensorData;   // 传感器数据
    uint16_t checksum;       // 校验和
};

// 节点标志位
#define NODE_FLAG_NONE          0x00
#define NODE_FLAG_TURN_LEFT     0x01
#define NODE_FLAG_TURN_RIGHT    0x02
#define NODE_FLAG_STOP          0x04
#define NODE_FLAG_SLOW          0x08
#define NODE_FLAG_VALVE         0x10
#define NODE_FLAG_SENSOR_ALERT  0x20
#define NODE_FLAG_CRITICAL      0x40
#define NODE_FLAG_RESERVED      0x80

class SmartPipelineRobot {
private:
    RobotMode currentMode;
    PathNodeV2 pathNodes[MAX_NODES];
    int nodeCount;
    
    // 导航参数
    float pidOutput;
    float navigationError;
    float totalDistance;
    
    // 数据压缩
    uint16_t dataChecksum;
    uint32_t lastCompression;
    
    // SD卡
    File dataFile;
    
    // 传感器
    SensorData currentSensorData;
    
public:
    SmartPipelineRobot() {
        currentMode = MODE_STANDBY;
        nodeCount = 0;
        pidOutput = 0;
        navigationError = 0;
        totalDistance = 0;
        dataChecksum = 0;
        lastCompression = 0;
    }
    
    void initialize() {
        Serial.begin(115200);
        Serial.print("智能管道巡检机器人 ");
        Serial.println(SYSTEM_VERSION);
        
        initHardware();
        initSDCard();
        initSensors();
        
        currentMode = MODE_STANDBY;
        Serial.println("系统就绪");
    }
    
    void initHardware() {
        // 电机PWM初始化
        pinMode(9, OUTPUT);   // 左电机PWM
        pinMode(10, OUTPUT);  // 右电机PWM
        pinMode(8, OUTPUT);   // 左电机方向
        pinMode(11, OUTPUT);  // 右电机方向
        
        // 霍尔传感器
        for(int i = 0; i < 8; i++) {
            pinMode(A0 + i, INPUT_PULLUP);
        }
        
        // I2C传感器
        Wire.begin();
    }
    
    void initSDCard() {
        if(!SD.begin(SD_CS_PIN)) {
            Serial.println("SD卡初始化失败");
        } else {
            Serial.println("SD卡就绪");
            dataFile = SD.open("inspect.dat", FILE_WRITE);
        }
    }
    
    void initSensors() {
        // 初始化气体传感器
        pinMode(A6, INPUT);
        // 初始化温湿度传感器
        pinMode(A7, INPUT);
        // 初始化压力传感器
        pinMode(A8, INPUT);
    }
    
    void run() {
        switch(currentMode) {
            case MODE_STANDBY:
                standbyLoop();
                break;
            case MODE_EXPLORATION:
                explorationLoop();
                break;
            case MODE_INSPECTION:
                inspectionLoop();
                break;
            case MODE_RETURN:
                returnLoop();
                break;
            case MODE_EMERGENCY:
                emergencyLoop();
                break;
        }
    }
    
    void standbyLoop() {
        // 等待指令
        if(Serial.available()) {
            char cmd = Serial.read();
            switch(cmd) {
                case 'E': // 探索模式
                    startExploration();
                    break;
                case 'I': // 巡检模式
                    startInspection();
                    break;
                case 'R': // 返回模式
                    startReturn();
                    break;
                case 'S': // 停止
                    stopRobot();
                    break;
            }
        }
    }
    
    void startExploration() {
        currentMode = MODE_EXPLORATION;
        nodeCount = 0;
        totalDistance = 0;
        Serial.println("开始探索模式");
    }
    
    void explorationLoop() {
        // 读取传感器
        readAllSensors();
        
        // 磁导航
        float position = readMagneticPosition();
        float error = position - 3.5;
        
        // 自适应控制
        float speed = 100;
        float correction = pidController(error);
        
        // 检测特殊节点
        if(abs(error) > 2.0) {
            // 弯道检测
            recordNode(NODE_FLAG_TURN_LEFT, position, speed);
        }
        
        if(currentSensorData.gasConcentration > 800) {
            recordNode(NODE_FLAG_SENSOR_ALERT, position, speed);
            speed = 60; // 减速
        }
        
        // 驱动电机
        driveMotors(speed - correction, speed + correction);
        
        // 数据记录
        if(nodeCount % 20 == 0) {
            compressAndStoreData();
        }
        
        // 检查结束条件
        if(totalDistance > 1000) { // 探索1000cm后结束
            finishExploration();
        }
    }
    
    void inspectionLoop() {
        // 使用已记录的路径进行巡检
        if(nodeCount == 0) {
            Serial.println("无路径数据，请先探索");
            currentMode = MODE_STANDBY;
            return;
        }
        
        static int currentTarget = 0;
        static unsigned long nodeTime = 0;
        
        // 读取当前位置
        float position = readMagneticPosition();
        readAllSensors();
        
        // 导航到下一个节点
        if(currentTarget < nodeCount) {
            PathNodeV2 target = pathNodes[currentTarget];
            
            // 检查是否到达目标节点
            if(abs(totalDistance - target.distance) < 5) {
                // 执行节点动作
                executeNodeAction(target);
                currentTarget++;
                nodeTime = millis();
            }
            
            // 根据节点标志调整速度
            float speed = getSpeedForNode(target);
            float error = position - 3.5;
            float correction = pidController(error);
            
            driveMotors(speed - correction, speed + correction);
        } else {
            // 巡检完成
            Serial.println("巡检完成");
            currentMode = MODE_STANDBY;
        }
    }
    
    void returnLoop() {
        // 沿原路返回
        static int returnIndex = 0;
        
        if(returnIndex >= nodeCount) {
            Serial.println("返回完成");
            currentMode = MODE_STANDBY;
            return;
        }
        
        // 从后向前遍历节点
        int targetIndex = nodeCount - 1 - returnIndex;
        PathNodeV2 target = pathNodes[targetIndex];
        
        // 导航逻辑
        float position = readMagneticPosition();
        float error = position - 3.5;
        float speed = 80;
        float correction = pidController(error);
        
        driveMotors(speed - correction, speed + correction);
        
        if(abs(totalDistance - target.distance) < 5) {
            returnIndex++;
        }
    }
    
    void emergencyLoop() {
        // 紧急停止，等待人工干预
        stopRobot();
        Serial.println("紧急模式 - 等待人工处理");
        
        // 持续监测，如果危险解除则恢复
        delay(5000);
        readAllSensors();
        if(currentSensorData.gasConcentration < 500) {
            currentMode = MODE_STANDBY;
            Serial.println("危险解除，回到待机模式");
        }
    }
    
    float pidController(float error) {
        static float errorSum = 0;
        static float lastError = 0;
        
        float Kp = 2.5;
        float Ki = 0.1;
        float Kd = 0.4;
        
        errorSum += error;
        float errorDiff = error - lastError;
        lastError = error;
        
        return Kp * error + Ki * errorSum + Kd * errorDiff;
    }
    
    void readAllSensors() {
        currentSensorData.gasConcentration = analogRead(A6);
        currentSensorData.temperature = (int16_t)(analogRead(A7) * 48.8); // 温度*10
        currentSensorData.humidity = (uint16_t)(analogRead(A8) * 10);
        currentSensorData.magneticField = readMagneticField();
        currentSensorData.timestamp = millis();
    }
    
    float readMagneticPosition() {
        int sum = 0;
        int count = 0;
        
        for(int i = 0; i < 8; i++) {
            int value = digitalRead(A0 + i);
            if(value) {
                sum += i;
                count++;
            }
        }
        
        if(count == 0) return -1;
        return (float)sum / count;
    }
    
    uint16_t readMagneticField() {
        // 读取磁场强度
        int fieldStrength = 0;
        for(int i = 0; i < 8; i++) {
            fieldStrength += analogRead(A0 + i);
        }
        return fieldStrength / 8;
    }
    
    void recordNode(uint8_t flags, float position, float speed) {
        if(nodeCount >= MAX_NODES) return;
        
        PathNodeV2 newNode;
        newNode.nodeId = nodeCount;
        newNode.distance = (uint16_t)totalDistance;
        newNode.position = (uint8_t)(position * 10);
        newNode.velocity = (uint8_t)(speed / 10);
        newNode.flags = flags;
        newNode.sensorData = currentSensorData;
        newNode.checksum = calculateChecksum(newNode);
        
        pathNodes[nodeCount++] = newNode;
        
        // 存储到SD卡
        if(dataFile) {
            dataFile.write((uint8_t*)&newNode, sizeof(PathNodeV2));
            dataFile.flush();
        }
    }
    
    void compressAndStoreData() {
        // 数据压缩算法
        if(nodeCount < 2) return;
        
        // 检查相邻节点是否相似
        PathNodeV2& last = pathNodes[nodeCount-1];
        PathNodeV2& prev = pathNodes[nodeCount-2];
        
        // 如果距离接近且标志相同，合并节点
        if(abs(last.distance - prev.distance) < 10 && 
           last.flags == prev.flags) {
            // 更新最后一个节点
            prev.sensorData = last.sensorData;
            nodeCount--;
        }
        
        Serial.print("节点数:");
        Serial.print(nodeCount);
        Serial.print(" 压缩率:");
        Serial.print((1 - (float)nodeCount / (totalDistance / 10)) * 100);
        Serial.println("%");
    }
    
    uint16_t calculateChecksum(PathNodeV2 node) {
        uint16_t sum = 0;
        uint8_t* data = (uint8_t*)&node;
        for(int i = 0; i < sizeof(PathNodeV2) - sizeof(uint16_t); i++) {
            sum += data[i];
        }
        return sum;
    }
    
    void executeNodeAction(PathNodeV2 node) {
        if(node.flags & NODE_FLAG_VALVE) {
            Serial.println("执行阀门检查");
        }
        if(node.flags & NODE_FLAG_SENSOR_ALERT) {
            Serial.println("传感器警报区域");
        }
        if(node.flags & NODE_FLAG_CRITICAL) {
            Serial.println("关键节点");
        }
    }
    
    float getSpeedForNode(PathNodeV2 node) {
        if(node.flags & NODE_FLAG_SLOW) return 40;
        if(node.flags & NODE_FLAG_STOP) return 0;
        if(node.flags & NODE_FLAG_TURN_LEFT || node.flags & NODE_FLAG_TURN_RIGHT) 
            return 50;
        return 100;
    }
    
    void driveMotors(int leftSpeed, int rightSpeed) {
        leftSpeed = constrain(leftSpeed, -255, 255);
        rightSpeed = constrain(rightSpeed, -255, 255);
        
        digitalWrite(8, leftSpeed > 0);
        digitalWrite(11, rightSpeed > 0);
        analogWrite(9, abs(leftSpeed));
        analogWrite(10, abs(rightSpeed));
    }
    
    void stopRobot() {
        driveMotors(0, 0);
    }
    
    void finishExploration() {
        stopRobot();
        Serial.print("探索完成，共记录");
        Serial.print(nodeCount);
        Serial.println("个节点");
        
        // 最终数据压缩
        compressAndStoreData();
        
        currentMode = MODE_STANDBY;
    }
};

SmartPipelineRobot robot;

void setup() {
    robot.initialize();
}

void loop() {
    robot.run();
}

要点解读

1. 磁导航系统设计
   多传感器阵列：使用8个霍尔传感器组成阵列，通过加权平均算法精确定位磁轨位置，实现±0.5cm的定位精度
   PID自适应控制：采用变参数PID控制，根据误差大小动态调整Kp值，在弯道和直道都能保持稳定
   失磁检测：当所有霍尔传感器均无信号时，系统能及时检测到脱轨状态并采取紧急措施

2. 路径节点压缩算法
   智能节点筛选：只在关键位置（弯道、阀门、异常点）记录节点，而非连续记录，大幅减少存储需求
   数据压缩策略：将浮点数据量化为整数（距离用cm、温度用0.1度），使用位标志存储节点特征
   自适应压缩：根据环境复杂度动态调整记录密度，直道稀疏记录，复杂区域密集记录

3. 多模式运行机制
   探索-巡检分离：首次运行进行路径探索和记录，后续运行直接使用记录路径进行快速巡检
   安全返回机制：机器人能够沿原路返回，在紧急情况下具备自主撤离能力
   模式切换逻辑：根据传感器数据和外部指令自动切换运行模式，适应不同工作场景

4. 危化品环境适应性
   多传感器融合：集成气体、温度、湿度、压力等多种传感器，全方位监测管道环境
   分级预警机制：设置多级报警阈值，根据危险程度采取减速、停止、紧急撤离等不同响应
   数据溯源能力：每个路径节点都关联环境数据，便于事后分析和风险评估

5. 系统可靠性和维护性
   EEPROM/SD卡双重存储：关键路径数据同时存储在EEPROM和SD卡，防止数据丢失
   校验和机制：每个数据节点包含CRC校验，确保数据完整性
   模块化设计：采用类封装，便于功能扩展和维护，支持固件升级

4、室内管道基础巡检（单管道、关键节点标记）
适用场景：化工厂区室内平行管道（长度≤200m、无复杂分支、管道间距固定），核心需求是沿磁条管道巡检，标记泄漏点、阀门等关键节点，回溯时快速复现巡检轨迹。

核心逻辑：采用「磁导航基础循迹+阈值标记节点」，探索阶段按管道轨迹行驶并记录节点类型（泄漏/阀门/辅助），压缩阶段仅保留关键节点，回溯阶段沿压缩路径行驶，速度根据节点风险自动调整。

/* 室内管道基础巡检：磁导航+节点压缩
   硬件：Arduino Nano + BLDC驱动模块 + 双磁导航传感器 + MQ-2气体传感器
   核心流程：探索循迹→标记节点→压缩路径→安全回溯
   参数：管道长度20000cm、泄漏阈值800、阀门间隔500cm
*/

#include <SimpleFOC.h>

// --- 核心参数配置 ---
#define PIPE_LENGTH 20000     // 管道总长度（cm）
#define MAG_THRESHOLD 200      // 磁导航传感器阈值
#define LEAK_THRESHOLD 800     // 气体泄漏检测阈值
#define VALVE_INTERVAL 500     // 阀门标记间隔（cm）
#define BASE_SPEED 0.3         // 基础行驶速度（PWM占空比）
#define HIGH_SPEED 0.7         // 非风险区回溯速度
#define DANGER_SPEED 0.2       // 风险区回溯速度

// --- 节点类型枚举 ---
enum NodeType { AUXILIARY, VALVE, LEAK };
struct PipeNode {
  long position;   // 沿管道的位置（cm）
  NodeType type;
  bool isDanger;   // 是否为危险节点
};
vector<PipeNode> explorationPath; // 探索原始路径
vector<PipeNode> compressedPath;  // 压缩后核心路径

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

// --- 全局变量 ---
long currentPosition = 0;
bool isExploring = true;
int sensorCalibration[2] = {205, 210}; // 磁导航传感器校准值

void setup() {
  Serial.begin(9600);
  initBLDC();
  calibrateSensors();
  Serial.println("室内管道基础巡检系统启动");
}

void loop() {
  if (isExploring) {
    basicExploration(); // 探索阶段
  } else {
    basicReturn();      // 回溯阶段
  }
  delay(20);
}

// 初始化BLDC电机
void initBLDC() {
  pinMode(MOTOR_L_PWM, OUTPUT);
  pinMode(MOTOR_R_PWM, OUTPUT);
  // 简化PWM控制，实际需搭配BLDC驱动模块实现闭环
}

// 校准磁导航传感器
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;
  Serial.println("磁导航传感器校准完成");
}

// 基础探索：循迹+节点标记
void basicExploration() {
  int magL = analogRead(MAG_L) - sensorCalibration[0];
  int magR = analogRead(MAG_R) - sensorCalibration[1];
  int gasValue = analogRead(GAS_SENSOR);

  // 磁导航纠偏：沿磁条稳定行驶
  if (magL > MAG_THRESHOLD && magR < MAG_THRESHOLD) {
    turnLeft(); // 偏右，左转修正
  } else if (magL < MAG_THRESHOLD && magR > MAG_THRESHOLD) {
    turnRight(); // 偏左，右转修正
  } else {
    moveForward();
    currentPosition += 10; // 每步前进10cm
  }

  // 节点标记：按阈值标记泄漏点、阀门
  NodeType nodeType = AUXILIARY;
  bool isDanger = (gasValue > LEAK_THRESHOLD);
  if (gasValue > LEAK_THRESHOLD) nodeType = LEAK;
  else if (currentPosition % VALVE_INTERVAL == 0) nodeType = VALVE;

  if (currentPosition % 100 == 0) { // 每100cm记录一次节点
    explorationPath.push_back({currentPosition, nodeType, isDanger});
    Serial.print("节点记录：位置");
    Serial.print(currentPosition);
    Serial.print("，类型：");
    Serial.print(nodeType == VALVE ? "阀门" : (nodeType == LEAK ? "泄漏" : "辅助"));
    Serial.println(isDanger ? "（危险）" : "（安全）");
  }

  // 探索完成判定：管道全程覆盖
  if (currentPosition >= PIPE_LENGTH) {
    compressBasicPath();
    isExploring = false;
    Serial.println("探索完成，进入路径压缩");
  }
}

// 基础路径压缩：仅保留阀门、泄漏点（危险节点）
void compressBasicPath() {
  for (auto node : explorationPath) {
    if (node.type == VALVE || node.type == LEAK || node.isDanger) {
      compressedPath.push_back(node);
    }
  }
  Serial.print("路径压缩完成：原始节点");
  Serial.print(explorationPath.size());
  Serial.print("，核心节点");
  Serial.println(compressedPath.size());
}

// 基础回溯：沿压缩路径行驶，风险区减速
void basicReturn() {
  if (compressedPath.empty()) return;

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

  // 磁导航纠偏（同探索阶段逻辑）
  if (magL > MAG_THRESHOLD && magR < MAG_THRESHOLD) {
    turnLeft();
  } else if (magL < MAG_THRESHOLD && magR > MAG_THRESHOLD) {
    turnRight();
  } else {
    moveForward();
    currentPosition += 10;
  }

  // 风险区速度调整
  if (target.isDanger) {
    setSpeed(DANGER_SPEED);
    Serial.println("进入危险区域，减速行驶");
  } else {
    setSpeed(HIGH_SPEED);
  }

  // 到达节点后切换目标
  if (abs(currentPosition - target.position) < 50) {
    compressedPath.erase(compressedPath.begin());
    if (compressedPath.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驱动模块 + 双磁导航 + 土壤湿度传感器 + MQ-2气体传感器
   核心流程：分支探索→权重标记→权重压缩→最优回溯
   参数：分支权重阈值60、泄漏权重90、湿度阈值400
*/

#include <SimpleFOC.h>

// --- 核心参数配置 ---
#define BRANCH_WEIGHT_THRESHOLD 60
#define LEAK_WEIGHT 90
#define SOIL_HUMIDITY_THRESHOLD 400
#define PIPE_TOTAL_LENGTH 50000 // 总管道长度（cm）
#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;    // 节点权重（0-100）
  bool isDanger;
  int branchId;  // 分支ID，主干为0
};
vector<BranchNode> explorationPath;
vector<BranchNode> weightCompressedPath;
int currentBranch = 0; // 当前分支ID
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 HUMIDITY_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 humidity = analogRead(HUMIDITY_SENSOR);

  // 分支检测：磁信号突变+湿度突变
  if (!isBranchDetected && abs(magL - magR) > 150) {
    isBranchDetected = true;
    branchStartPos = currentPosition;
    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 (humidity > SOIL_HUMIDITY_THRESHOLD) {
    weight -= 20;
  }

  // 节点记录
  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);
    }
  }
}

// 辅助函数（同案例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); }

6、高危区域管道巡检（防爆+紧急避险+压缩路径）
适用场景：炼油厂高危管道区域（存在易燃易爆区域、紧急切断阀），核心需求是防爆安全巡检、紧急避险、避险后快速回溯，压缩路径需优先保留紧急切断阀与避险点。

核心逻辑：采用「防爆安全策略+紧急避险触发+避险路径压缩」，探索阶段动态监测环境风险，触发避险时标记避险节点，压缩阶段优先保留避险节点与切断阀，回溯时快速抵达避险点或出口。

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

#include <SimpleFOC.h>

// --- 核心参数配置 ---
#define EXPLOSION_THRESHOLD 1000
#define EVACUATION_WEIGHT 100
#define VALVE_POSITION 15000 // 紧急切断阀位置（cm）
#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); }

要点解读

1. 磁导航的抗干扰与精准循迹：保障路径稳定性的基础
   危化品管道多存在强电磁干扰（如泵机、输电线路）、环境因素干扰（室外湿度、土壤磁性），磁导航的精准度直接决定探索与回溯的可靠性，核心要点如下：
   传感器校准不可跳过：必须通过多次采样平均值完成传感器零点校准，消除硬件偏移；案例中均加入校准函数，避免因初始偏差导致循迹偏离管道；
   差分式纠偏逻辑适配场景：双通道磁传感器采用差分纠偏，核心是判断左右传感器与磁条的偏差，案例中通过阈值判断实现精准转向，避免过度纠偏；
   干扰过滤机制：室外场景需增加湿度、温度对磁信号的干扰过滤，案例2中通过湿度修正节点权重，同时调整循迹灵敏度；高危场景通过防爆传感器直接过滤爆炸性气体干扰，确保信号稳定。
2. 路径节点压缩的核心原则：风险优先级+信息完整性平衡
   路径压缩不是盲目删减，而是在保障巡检完整性的前提下提升效率，核心需围绕“风险优先、场景适配、信息闭环”三大原则：
   风险优先级为首要依据：无论何种场景，泄漏点、紧急切断阀、避险点等高危节点必须优先保留，压缩时不能删除，案例3中避险点权重设为100，强制保留；
   场景差异化压缩策略：
   室内简单管道：仅保留关键节点（阀门、泄漏），删除辅助点，降低成本；
   室外多分支管道：按权重保留节点，既覆盖主干分支，又过滤低价值辅助点；
   高危区域：优先保留避险点和切断阀，确保紧急情况下能快速响应；
   信息完整性校验：压缩前需统计原始节点与压缩节点比例，确保核心巡检目标无遗漏，案例中均通过串口打印比例，便于实时校验。
3. BLDC电机的安全调速策略：防爆与效率的双约束
   危化品管道巡检的核心安全风险来自电机高速运转引发的火花，同时需兼顾巡检效率，BLDC调速需遵循“安全优先、场景适配、闭环控制”：
   防爆速度约束：高危区域需严格限制电机最大速度，采用低速行驶，减少机械摩擦和火花风险，案例3中高危区域基础速度仅为0.25，符合防爆要求；
   风险自适应调速：回溯阶段根据节点风险动态调整速度，危险节点（泄漏、避险点）减速，安全节点提速，平衡安全与效率，案例2、3均采用此策略；
   闭环速度控制：实际工程中需搭配编码器实现BLDC闭环调速，避免因管道阻力变化导致速度波动，案例代码简化了PWM控制，实际落地需补充闭环逻辑，确保速度稳定。
4. 紧急避险与路径重构：高危场景的安全保障核心
   高危区域巡检的核心是应对突发风险，紧急避险与路径重构需具备“快速触发、精准标记、高效回溯”能力：
   双重触发机制：采用自动监测（气体浓度、爆炸阈值）+手动触发（紧急按钮）双重保障，案例3中同时监测传感器与按钮，确保避险信号不遗漏；
   避险节点精准标记：触发避险时立即记录当前位置作为避险点，加入路径节点，确保回溯时有明确目标，案例3中触发后立即标记避险点位置与高权重；
   避险路径优先压缩：压缩阶段将避险点设为最高优先级，同时保留紧急切断阀，确保回溯时快速抵达避险点或切断阀，实现安全撤离，案例3的压缩策略直接服务于避险需求。
5. 探索与回溯的流程闭环：数据衔接与状态管理的可靠性
   两阶段流程的顺畅切换依赖清晰的状态管理与可靠的数据衔接，避免探索与回溯逻辑冲突，核心要点如下：
   状态标志严格管控：通过全局布尔变量区分探索与回溯阶段，避免状态混乱，案例中均采用标志位切换，确保一个阶段结束后再进入下一个阶段；
   路径数据可靠存储：探索阶段记录的路径节点需存储在稳定内存中，避免断电丢失，Arduino Mega内存充足，可存储大量节点；实际工程中可搭配EEPROM，确保数据断电不丢失；
   阶段切换触发条件量化：探索结束、压缩完成的条件必须量化，避免因条件模糊导致流程卡死，案例中通过管道长度、节点数量、避险触发等明确条件触发切换，确保流程可控。

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