基于Arduino BLDC平台的“两阶段探索+路径优化”方案，其核心可以理解为一种“先摸索，后抄近道”的策略。它在资源受限的嵌入式系统中，通过将复杂的任务分解为“探索”与“回溯”两个阶段，来实现智能且高效的路径执行。

主要特点
这套方案的核心思想，是将“找到路”和“跑得快”两个目标解耦，分阶段完成。结合搜索到的相关资料，其主要特点体现在以下几个方面：
两阶段工作流：系统流程清晰分为两大步：
探索阶段：机器人像一个探险家，遵循“左手法则”或类似的状态机逻辑，在未知或半未知环境中移动，其首要目标是遍历所有需巡检的区域，而非走最短路线。在此过程中，机器人会利用传感器感知环境并记录其行进路径。
优化与回溯阶段：探索结束后，系统会对记录的路径进行“复盘”。通过路径简化算法（如去除冗余转向、识别可走直线段等）剔除无效动作，生成一条更优的路径。随后，机器人将按照这条优化后的“捷径”高速返回起点或前往下一个任务点。
对算力要求友好，适合Arduino平台：与需要实时运行A*、DWA等复杂算法的系统不同，该方案的绝大多数计算量都集中在“离线规划”或任务间隙的“路径化简”环节。Arduino主要负责执行阶段性的运动控制指令，如“以X速度直行Y秒”或“原地旋转90度”，这使得它能在资源受限的情况下胜任此项任务。
依赖精准的执行与定位：路径优化的前提是能“重走”老路，这要求机器人的运动执行（BLDC电机+FOC控制）和定位（编码器+IMU）系统必须足够精准。只有这样，简化后的路径指令（如“前进3米，左转90度”）才能被准确复现，否则优化后的“捷径”可能因累积误差而变得不可用。
行为模式易于预测与调试：系统的行为模式由FSM（有限状态机）驱动，状态转移清晰、确定。这使得开发者能容易地预测机器人在各种情况下的反应，极大地提高了调试和排错效率，对工程开发非常友好。

应用场景
这个方案在那些对“完全未知环境下的实时最优规划”要求不高，但强调“长时间、规律性自主作业”的场景中，能找到最合适的舞台。
结构化环境下的定点巡检：这是最典型、最成熟的应用。例如，工厂产线、数据中心机房、大型商场等已知地图场景。机器人在首次部署时进行一次“探索”和学习，记录下最佳巡检路线，之后便可在无人工干预的情况下，沿优化后的路径进行长时间、高效率的循环巡逻。
农业与园艺监测：在温室大棚或农田中，机器人沿固定作物行（垄沟）移动，任务路线相对固定。它可以先“探索”并记录行间路径，优化后自动完成对土壤湿度、作物生长状况的定点数据采集。

需要注意的事项
在Arduino平台上实现该方案，需重点关注以下工程实践问题：
路径“最优”是相对的：需要理解此方案优化出的路径，通常是“在已有探索经验下的最优”，而非理论上真正的全局最优。它受限于探索时的策略和传感器精度。其核心价值在于低开销、高确定性，适合工程应用，而非追求数学上的极致最优。
底层执行精度是成败关键：路径优化成功的前提是能精确“重走”老路。BLDC电机配合FOC（磁场定向控制）虽然能提供低速平稳和精准的位置保持，但仍需通过编码器和IMU等传感器构成闭环，以对抗打滑和累积误差。特别是，FOC的抗干扰纠偏能力在长距离直线行驶中至关重要。
避免复杂在线重规划：由于算力限制，应避免在Arduino上直接进行复杂的在线路径重规划，尤其是三维空间的实时避障，这几乎不可能实现。若在优化路径上遇到突发障碍，建议采用简单的反应式避障（如停止、后退、小幅绕行），待安全后再回到预设路径点继续执行，以简化逻辑、保证核心任务的完成。
电源与信号完整性需特别照顾：多台BLDC电机同时运行，启动和制动时会产生巨大电流尖峰，极易导致单片机掉电复位，同时带来严重的电磁干扰（EMC）。强电（电机驱动）和弱电（Arduino及传感器）必须独立供电，并严格分离走线，关键信号线需使用屏蔽线。
完善的异常与安全机制：除了硬件级别的急停电路，软件上也需增加保护。例如：状态超时保护，防止因卡住或逻辑错误而无限等待；低电量管理，在电量不足时暂停任务并规划最短路径返回充电。

1、基础两阶段——探索记录 + 高速回溯
这是最经典的两阶段入门方案：第一阶段以左手法则低速探索巡检区域，在每个交叉口记录动作（L/R/S/B）；第二阶段加载记录的路径，以更高速度高速回溯。

#include <SimpleFOC.h>

// ===== 引脚定义 =====
#define IR_LEFT   A0
#define IR_FRONT  A1
#define IR_RIGHT  A2
#define IR_LINE   A3   // 地面循迹传感器

// ===== BLDC电机配置 =====
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(5, 6, 7, 4);

// ===== 运动参数 =====
const float exploreSpeed = 1.5;   // 探索阶段速度(rad/s)
const float fastSpeed    = 3.0;   // 回溯阶段速度(rad/s)
const int turnDelay      = 300;   // 90°转向延时(ms)
const int uTurnDelay     = 600;   // 180°掉头延时(ms)

// ===== 路径存储 =====
char path[100];
int pathLength = 0;
int phase = 0;  // 0=探索阶段, 1=回溯阶段
int pathIndex = 0;

void setup() {
  Serial.begin(9600);
  pinMode(IR_LEFT, INPUT);
  pinMode(IR_FRONT, INPUT);
  pinMode(IR_RIGHT, INPUT);
  pinMode(IR_LINE, INPUT);

  // 初始化左电机
  driverL.voltage_power_supply = 12;
  driverL.init();
  motorL.linkDriver(&driverL);
  motorL.controller = MotionControlType::velocity;
  motorL.init();

  // 初始化右电机
  driverR.voltage_power_supply = 12;
  driverR.init();
  motorR.linkDriver(&driverR);
  motorR.controller = MotionControlType::velocity;
  motorR.init();
}

void loop() {
  if (phase == 0) {
    explorePhase();
  } else {
    fastRetracePhase();
  }
  delay(100);
}

// ===== 第一阶段：探索 =====
void explorePhase() {
  int L = digitalRead(IR_LEFT);
  int F = digitalRead(IR_FRONT);
  int R = digitalRead(IR_RIGHT);

  if (!F && !L && !R) {
    // 死胡同 → 掉头
    uTurn();
    recordAction('B');
  }
  else if (!L) {
    // 左侧有路 → 优先左转（左手法则）
    turnLeft();
    recordAction('L');
  }
  else if (!F) {
    // 前方有路 → 直行
    moveForward(exploreSpeed);
    recordAction('S');
  }
  else if (!R) {
    // 右侧有路 → 右转
    turnRight();
    recordAction('R');
  }
}

// ===== 第二阶段：高速回溯 =====
void fastRetracePhase() {
  if (pathIndex >= pathLength) {
    stopMotors();
    Serial.println("回溯完成");
    return;
  }

  char action = path[pathIndex++];
  switch (action) {
    case 'S': moveForward(fastSpeed); break;
    case 'L': turnLeft();  break;
    case 'R': turnRight(); break;
    case 'B': uTurn();     break;
  }
}

// ===== 记录动作 =====
void recordAction(char action) {
  if (pathLength < 100) {
    path[pathLength++] = action;
  }
}

// ===== 运动函数 =====
void moveForward(float speed) {
  motorL.move(speed);
  motorR.move(speed);
}
void turnLeft() {
  motorL.move(-1.5);
  motorR.move(1.5);
  delay(turnDelay);
  stopMotors();
}
void turnRight() {
  motorL.move(1.5);
  motorR.move(-1.5);
  delay(turnDelay);
  stopMotors();
}
void uTurn() {
  motorL.move(-1.5);
  motorR.move(1.5);
  delay(uTurnDelay);
  stopMotors();
}
void stopMotors() {
  motorL.move(0);
  motorR.move(0);
}

适用场景：竞赛入门、教学演示，逻辑最简洁，适合Arduino Uno等低资源平台。

2、带路径简化的两阶段——在线压缩"xBx"模式
在案例一的基础上，引入路径简化算法：每当路径末尾出现"xBx"模式（x为任意方向，B为掉头），立即将其压缩为等效单步。这是从"记录式"到"优化式"的关键升级。

#include <SimpleFOC.h>

// ===== 引脚定义（同案例一） =====
#define IR_LEFT   A0
#define IR_FRONT  A1
#define IR_RIGHT  A2

// ===== BLDC电机（省略初始化，同案例一） =====
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
// ... driver初始化同案例一 ...

// ===== 路径存储 =====
char path[100];
int pathLength = 0;
int phase = 0;  // 0=探索, 1=回溯
int pathIndex = 0;

void setup() {
  Serial.begin(9600);
  pinMode(IR_LEFT, INPUT);
  pinMode(IR_FRONT, INPUT);
  pinMode(IR_RIGHT, INPUT);
  // 电机初始化同案例一
}

void loop() {
  if (phase == 0) {
    exploreWithSimplify();
  } else {
    fastRetrace();
  }
  delay(100);
}

// ===== 第一阶段：探索+在线简化 =====
void exploreWithSimplify() {
  int L = digitalRead(IR_LEFT);
  int F = digitalRead(IR_FRONT);
  int R = digitalRead(IR_RIGHT);

  if (!F && !L && !R) {
    uTurn();
    recordAndSimplify('B');
  }
  else if (!L) {
    turnLeft();
    recordAndSimplify('L');
  }
  else if (!F) {
    moveForward(1.5);
    recordAndSimplify('S');
  }
  else if (!R) {
    turnRight();
    recordAndSimplify('R');
  }
}

// ===== 记录并在线简化 =====
void recordAndSimplify(char action) {
  path[pathLength++] = action;
  simplifyPath();
}

// ===== 路径简化核心算法 =====
// 每当路径末尾出现 "xBx" 模式，将其压缩为等效单步
void simplifyPath() {
  if (pathLength < 3 || path[pathLength - 2] != 'B') return;

  // 计算三段动作的总角度
  int totalAngle = 0;
  for (int i = 1; i <= 3; i++) {
    switch (path[pathLength - i]) {
      case 'R': totalAngle += 90;  break;
      case 'L': totalAngle += 270; break;
      case 'B': totalAngle += 180; break;
      case 'S': totalAngle += 0;   break;
    }
  }
  totalAngle %= 360;

  // 将三段压缩为一段等效动作
  char simplified;
  switch (totalAngle) {
    case 0:   simplified = 'S'; break;  // 如 LBL → S
    case 90:  simplified = 'R'; break;  // 如 LBS → R
    case 180: simplified = 'B'; break;  // 如 SBS → B
    case 270: simplified = 'L'; break;  // 如 RBL → L
  }

  path[pathLength - 3] = simplified;
  pathLength -= 2;  // 删除后两段
}

// ===== 第二阶段：高速回溯 =====
void fastRetrace() {
  if (pathIndex >= pathLength) {
    stopMotors();
    Serial.println("优化路径执行完毕");
    return;
  }

  char action = path[pathIndex++];
  switch (action) {
    case 'S': moveForward(3.0); break;  // 高速巡航
    case 'L': turnLeft();  break;
    case 'R': turnRight(); break;
    case 'B': uTurn();     break;
  }
}

// moveForward / turnLeft / turnRight / uTurn / stopMotors
// 实现同案例一，此处省略

适用场景：需要"学习-优化-快速执行"的智能巡检场景，如仓储AGV、管道巡检。

3、完整两阶段——探索+简化+传感器校正+自适应速度
这是竞赛级方案。第一阶段用左手法则遍历巡检区域并记录路径，每记录一步就在线简化；同时引入传感器校正和自适应速度控制；第二阶段加载优化后的路径，根据路径特征动态调整速度。

#include <SimpleFOC.h>

// ===== 引脚定义 =====
#define IR_LEFT   A0
#define IR_FRONT  A1
#define IR_RIGHT  A2
#define IR_WALL_L A4  // 左侧墙壁距离传感器
#define IR_WALL_R A5  // 右侧墙壁距离传感器

// ===== BLDC电机（省略初始化，同案例一） =====
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
// ... driver初始化同案例一 ...

// ===== 路径存储 =====
char path[150];
int pathLength = 0;
int phase = 0;  // 0=探索, 1=回溯
int pathIndex = 0;

// ===== 速度参数 =====
const float baseSpeed = 1.5;
const float fastSpeed = 3.5;
const float turnSpeed = 2.0;

void setup() {
  Serial.begin(9600);
  pinMode(IR_LEFT, INPUT);
  pinMode(IR_FRONT, INPUT);
  pinMode(IR_RIGHT, INPUT);
  pinMode(IR_WALL_L, INPUT);
  pinMode(IR_WALL_R, INPUT);
  // 电机初始化同案例一
}

void loop() {
  if (phase == 0) {
    exploreWithCorrection();
  } else {
    adaptiveRetrace();
  }
  delay(50);
}

// ===== 第一阶段：探索+传感器校正 =====
void exploreWithCorrection() {
  // 传感器校正：利用墙壁距离修正航向
  int wallL = analogRead(IR_WALL_L);
  int wallR = analogRead(IR_WALL_R);
  float correction = (wallL - wallR) * 0.001;  // 航向修正系数

  int L = digitalRead(IR_LEFT);
  int F = digitalRead(IR_FRONT);
  int R = digitalRead(IR_RIGHT);

  if (!F && !L && !R) {
    uTurn();
    recordAndSimplify('B');
  }
  else if (!L) {
    turnLeft();
    recordAndSimplify('L');
  }
  else if (!F) {
    // 直行时应用航向修正
    motorL.move(baseSpeed + correction);
    motorR.move(baseSpeed - correction);
    recordAndSimplify('S');
  }
  else if (!R) {
    turnRight();
    recordAndSimplify('R');
  }
}

// ===== 记录并在线简化（同案例二） =====
void recordAndSimplify(char action) {
  path[pathLength++] = action;
  simplifyPath();
}

void simplifyPath() {
  if (pathLength < 3 || path[pathLength - 2] != 'B') return;

  int totalAngle = 0;
  for (int i = 1; i <= 3; i++) {
    switch (path[pathLength - i]) {
      case 'R': totalAngle += 90;  break;
      case 'L': totalAngle += 270; break;
      case 'B': totalAngle += 180; break;
      case 'S': totalAngle += 0;   break;
    }
  }
  totalAngle %= 360;

  char simplified;
  switch (totalAngle) {
    case 0:   simplified = 'S'; break;
    case 90:  simplified = 'R'; break;
    case 180: simplified = 'B'; break;
    case 270: simplified = 'L'; break;
  }

  path[pathLength - 3] = simplified;
  pathLength -= 2;
}

// ===== 第二阶段：自适应高速回溯 =====
void adaptiveRetrace() {
  if (pathIndex >= pathLength) {
    stopMotors();
    Serial.println("自适应回溯完成");
    return;
  }

  char action = path[pathIndex++];
  char nextAction = (pathIndex < pathLength) ? path[pathIndex] : 'S';

  // 根据当前动作和下一个动作动态调整速度
  float currentSpeed = baseSpeed;
  if (action == 'S' && nextAction == 'S') {
    currentSpeed = fastSpeed;  // 连续直行 → 高速巡航
  } else if (action == 'S' && (nextAction == 'L' || nextAction == 'R')) {
    currentSpeed = baseSpeed;  // 即将转弯 → 减速准备
  }

  switch (action) {
    case 'S':
      motorL.move(currentSpeed);
      motorR.move(currentSpeed);
      break;
    case 'L': turnLeft();  break;
    case 'R': turnRight(); break;
    case 'B': uTurn();     break;
  }
}

// ===== 运动函数（同案例一） =====
void turnLeft() {
  motorL.move(-turnSpeed);
  motorR.move(turnSpeed);
  delay(300);
  stopMotors();
}
void turnRight() {
  motorL.move(turnSpeed);
  motorR.move(-turnSpeed);
  delay(300);
  stopMotors();
}
void uTurn() {
  motorL.move(-turnSpeed);
  motorR.move(turnSpeed);
  delay(600);
  stopMotors();
}
void stopMotors() {
  motorL.move(0);
  motorR.move(0);
}

适用场景：Micromouse竞赛、需要"学习-优化-自适应执行"的高级智能巡检场景。

要点解读
两阶段架构是效率与智能的平衡术
三个案例的共同核心是"探索→优化→回溯"的两阶段运行模式。第一阶段以低速、高可靠性探索巡检区域，记录完整路径；第二阶段加载优化后的路径，以高速、高效率执行。这种架构的优势在于：探索阶段可以"慢而稳"，确保路径记录的准确性；回溯阶段可以"快而准"，利用优化后的路径避免重复探索。案例一仅记录路径，案例二引入在线简化，案例三进一步加入自适应速度控制——从"记录式"到"优化式"再到"自适应式"，体现了从基础到高级的演进路径。

路径简化算法是竞赛级方案的分水岭
案例二和案例三的simplifyPath()函数是整个系统中最精妙的部分。其核心思想是：每当路径末尾出现"xBx"模式（x为任意方向，B为掉头），说明机器人走入了死胡同并折返，这三步可以压缩为一步等效动作。通过角度累加（L=270°、R=90°、B=180°、S=0°），将三段旋转角度求和后对360°取模，即可得到等效的单步方向。例如LBL（270°+180°+270°=720°≡0°）等效于S（直行），LBS（270°+180°+0°=450°≡90°）等效于R（右转）。该算法在探索过程中在线执行（每记录一步就尝试简化），避免了后期集中处理的内存峰值，非常适合Arduino等SRAM仅2-8KB的平台。

传感器校正是可靠性的保障
案例三引入了传感器校正机制：通过左右两侧的墙壁距离传感器（IR_WALL_L和IR_WALL_R），实时计算机器人与两侧墙壁的距离差，生成航向修正系数，在直行时动态调整左右轮速度，确保机器人始终保持在通道中心。这种校正机制有效克服了编码器里程计的累积误差，是长距离巡检任务中保持定位精度的关键手段。此外，案例三还引入了自适应速度控制：根据当前动作和下一个动作动态调整速度——连续直行时高速巡航，即将转弯时减速准备，既保证了效率又确保了安全性。

BLDC的FOC控制是精准执行的基石
三个案例均基于SimpleFOC库驱动BLDC无刷电机，采用速度模式（MotionControlType::velocity）。FOC矢量控制的优势在于：电流环响应频率达千赫兹级，使机器人在交叉口可实现毫秒级的速度切换；正弦波驱动消除转矩脉动，确保低速转向时的平稳性；配合编码器可实现±2°以内的90°转向精度。但需注意：turnDelay参数必须根据实际轮径、轮距和地面摩擦进行标定，建议预留校准系数，现场调试适配不同地面材质。

从案例一到案例三的演进路径
三个案例代表了从"记录式"到"优化式"再到"自适应式"的三级演进：案例一是纯记录式（仅记录路径，无优化），案例二引入了在线简化（可优化路径，但速度固定），案例三实现了"探索→简化→自适应执行"的完整智能。在实际工程中，建议从案例一起步验证硬件，确认传感器和电机工作正常后，逐步升级到案例二和案例三。案例三的完整项目代码可在GitHub上找到参考实现（如MJRoBot-Maze-Solver项目）。

4、工业仓储AGV巡检系统（二维码定位+栅格建图优化）
适用场景：标准化工业仓储场景（货架间距固定、地面铺设二维码定位点），巡检机器人需覆盖所有货架通道，探索阶段记录完整路径，优化后筛选高效巡检路线，回溯阶段高速行驶以缩短巡检周期。

核心逻辑：
探索阶段：采用“随机+沿墙”混合探索，结合二维码定位构建栅格地图，记录路径关键点（货架节点、转向点）；
路径简化：基于距离-重复度双阈值筛选，删除重复转向和冗余绕行点，保留核心巡检节点；
高速回溯：基于优化后的精简路径，结合二维码定位实现高速循迹，减少无效行驶时间。

/* 工业仓储AGV巡检：两阶段探索+路径优化
   硬件：Arduino Mega + BLDC驱动轮组 + 二维码摄像头 + 距离传感器
   核心逻辑：探索建图→路径简化→高速回溯
   优化阈值：重复距离阈值30cm，非关键节点距离阈值50cm
*/

#include <Wire.h>
#include <Adafruit_QRCode.h>

// --- 核心参数定义 ---
#define THRESHOLD_REPEAT 30 // 重复路径判断距离（cm）
#define THRESHOLD_SIMPLIFY 50 // 非关键节点删除距离（cm）
#define TARGET_SPEED_EXPLORE 0.3 // 探索阶段速度（占空比）
#define TARGET_SPEED_RETURN 0.8 // 回溯阶段速度（占空比）

// --- 路径存储结构 ---
struct PathNode {
  int x;
  int y;
  bool isKeyNode; // 是否为关键节点（货架、主通道交叉点）
  unsigned long timestamp;
};
vector<PathNode> originalPath; // 探索阶段原始路径
vector<PathNode> simplifiedPath; // 简化后优化路径

// --- 硬件驱动引脚 ---
#define MOTOR_L_PWM 5
#define MOTOR_R_PWM 6
#define QR_CAM_PIN 4
#define DIST_SENSOR_PIN A0

// --- 核心变量 ---
int currentX = 0, currentY = 0; // 当前栅格坐标
bool isExploration = true; // 阶段标志：true=探索，false=回溯
int lastTurn = 0; // 上一次转向方向：0=直行，1=左转，2=右转

void setup() {
  Serial.begin(115200);
  initMotors();
  initSensors();
  Serial.println("工业仓储AGV巡检系统启动");
  startExploration(); // 启动探索阶段
}

void loop() {
  if (isExploration) {
    executeExploration();
  } else {
    executeReturn();
  }
  delay(20);
}

// 初始化BLDC电机（速度控制模式）
void initMotors() {
  pinMode(MOTOR_L_PWM, OUTPUT);
  pinMode(MOTOR_R_PWM, OUTPUT);
  // 实际需结合BLDC驱动模块配置，此处简化为PWM控制
}

// 初始化传感器
void initSensors() {
  pinMode(QR_CAM_PIN, INPUT);
  pinMode(DIST_SENSOR_PIN, INPUT);
}

// 启动探索阶段
void startExploration() {
  Serial.println("进入探索阶段");
  isExploration = true;
}

// 探索阶段执行：随机沿墙+路径记录
void executeExploration() {
  int dist = readDistance();
  int qrCode = readQRCode(); // 识别货架二维码ID

  // 沿墙避障逻辑
  if (dist < 20) { // 距离障碍物<20cm，转向
    turnRight();
    lastTurn = 2;
  } else if (qrCode != -1 && !isNodeRecorded(qrCode)) { // 识别到新货架节点
    moveForward();
    updateCoordinate(0); // 直行x不变，y+1
    recordNode(currentX, currentY, true); // 记录关键节点
  } else {
    // 随机转向逻辑，避免循环
    if (random(10) < 2) {
      turnLeft();
      lastTurn = 1;
    } else {
      moveForward();
      updateCoordinate(0);
      recordNode(currentX, currentY, false);
    }
  }

  // 探索完成条件：所有货架节点记录完毕
  if (qrCode == 100) { // 假设100为探索完成标志（实际需统计货架数量）
    isExploration = false;
    simplifyPath(); // 启动路径简化
    Serial.println("探索完成，开始路径简化");
  }
}

// 路径简化算法：删除重复转向与冗余节点
void simplifyPath() {
  if (originalPath.size() < 3) return;

  // 第一步：删除重复转向的冗余节点
  vector<PathNode> tempPath;
  tempPath.push_back(originalPath[0]); // 保留起点
  for (int i = 1; i < originalPath.size() - 1; i++) {
    if (originalPath[i].isKeyNode) {
      tempPath.push_back(originalPath[i]); // 保留所有关键节点
      continue;
    }
    // 判断是否为重复转向节点
    if (getTurnDirection(originalPath[i-1], originalPath[i], originalPath[i+1]) == lastTurn) {
      // 计算距离，超过阈值则删除
      int dist1 = getDistance(tempPath.back(), originalPath[i]);
      int dist2 = getDistance(tempPath.back(), originalPath[i+1]);
      if (dist1 < THRESHOLD_REPEAT && dist2 > THRESHOLD_REPEAT) {
        continue; // 跳过冗余节点
      }
    }
    tempPath.push_back(originalPath[i]);
  }
  tempPath.push_back(originalPath.back()); // 保留终点

  // 第二步：删除非关键节点中距离过近的冗余点
  simplifiedPath.clear();
  simplifiedPath.push_back(tempPath[0]);
  for (int i = 1; i < tempPath.size(); i++) {
    if (tempPath[i].isKeyNode) {
      simplifiedPath.push_back(tempPath[i]);
      continue;
    }
    int dist = getDistance(simplifiedPath.back(), tempPath[i]);
    if (dist > THRESHOLD_SIMPLIFY) {
      simplifiedPath.push_back(tempPath[i]);
    }
  }

  Serial.print("路径简化完成：原始节点");
  Serial.print(originalPath.size());
  Serial.print("，简化后节点");
  Serial.println(simplifiedPath.size());
}

// 回溯阶段执行：高速循迹优化路径
void executeReturn() {
  if (simplifiedPath.empty()) return;

  // 获取当前节点与目标节点的偏差
  PathNode target = simplifiedPath[0];
  int dx = target.x - currentX;
  int dy = target.y - currentY;

  // 纠偏逻辑：基于偏差调整转向
  if (abs(dx) > abs(dy)) {
    if (dx > 0) turnRight();
    else turnLeft();
  } else {
    if (dy > 0) moveForward();
    else turnAround();
  }

  // 到达目标节点，切换下一个节点
  if (getDistance({currentX, currentY}, target) < 5) {
    simplifiedPath.erase(simplifiedPath.begin());
    if (simplifiedPath.empty()) {
      Serial.println("回溯完成，巡检任务结束");
      stopMotors();
      while (1);
    }
  }

  // 高速行驶：提升电机PWM占空比
  setMotorSpeed(TARGET_SPEED_RETURN);
}

// 辅助函数：计算两点距离
int getDistance(PathNode a, PathNode b) {
  return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
}

// 其他辅助函数：电机控制、传感器读取、坐标更新等（实际需补充具体实现）
void moveForward() { analogWrite(MOTOR_L_PWM, TARGET_SPEED_EXPLORE*255); analogWrite(MOTOR_R_PWM, TARGET_SPEED_EXPLORE*255); }
void turnLeft() { analogWrite(MOTOR_L_PWM, -TARGET_SPEED_EXPLORE*255); analogWrite(MOTOR_R_PWM, TARGET_SPEED_EXPLORE*255); delay(300); }
void turnRight() { analogWrite(MOTOR_L_PWM, TARGET_SPEED_EXPLORE*255); analogWrite(MOTOR_R_PWM, -TARGET_SPEED_EXPLORE*255); delay(300); }
void setMotorSpeed(float speed) { analogWrite(MOTOR_L_PWM, speed*255); analogWrite(MOTOR_R_PWM, speed*255); }
int readDistance() { return analogRead(DIST_SENSOR_PIN) * 30 / 4096; } // 模拟转换为cm
void updateCoordinate(int dir) { if (dir == 0) currentY++; else currentX++; }
void recordNode(int x, int y, bool isKey) { originalPath.push_back({x, y, isKey, millis()}); }

5、园区户外巡检机器人（GPS定位+路径平滑优化）
适用场景：园区户外场景（道路不规则、存在岔路、绿化带遮挡），巡检机器人需覆盖园区主干道与支路，探索阶段通过GPS定位记录路径，优化后实现路径平滑，回溯阶段高速行驶覆盖核心巡检点。

核心逻辑：
探索阶段：采用“主干道优先+支路覆盖”策略，结合GPS定位记录经纬度路径点，标记主干道关键节点；
路径简化：基于Douglas-Peucker算法（DP算法）进行路径平滑，删除偏离主线的冗余点，保留核心拐点；
高速回溯：结合GPS偏差修正，采用差速BLDC电机实现高速循迹，针对户外复杂路面自适应调整速度。

/* 园区户外巡检：两阶段探索+路径优化
   硬件：ESP32 + BLDC驱动轮组 + GPS模块 + 距离传感器
   核心逻辑：探索建图→DP算法路径平滑→高速回溯
   优化参数：DP算法阈值10m，户外高速速度0.9（占空比）
*/

#include <TinyGPS++.h>
#include <SimpleFOC.h>

// --- 核心参数 ---
#define DP_THRESHOLD 10 // DP算法简化阈值（m）
#define OUTDOOR_HIGH_SPEED 0.9 // 户外高速占空比
#define EXPLORE_SPEED 0.3 // 探索阶段速度

// --- 路径点结构（经纬度） ---
struct GPSPoint {
  double lat;
  double lng;
  bool isMainRoad; // 是否为主干道节点
};
vector<GPSPoint> originalPath;
vector<GPSPoint> smoothPath;

// --- 硬件引脚 ---
#define MOTOR_L_PWM 12
#define MOTOR_R_PWM 13
#define GPS_RX 16
#define GPS_TX 17
#define DIST_SENSOR A0

// --- 全局变量 ---
TinyGPSPlus gps;
bool isExploring = true;
double targetLat, targetLng; // 回溯目标点

void setup() {
  Serial.begin(115200);
  initGPS();
  initMotors();
  Serial.println("园区户外巡检系统启动");
}

void loop() {
  if (isExploring) {
    executeOutdoorExploration();
  } else {
    executeOutdoorReturn();
  }
  delay(50);
}

// 初始化GPS模块
void initGPS() {
  Serial1.begin(9600, SERIAL_8N1, GPS_RX, GPS_TX);
}

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

// 户外探索执行
void executeOutdoorExploration() {
  TinyGPSPlus::update();
  if (!gps.location.isValid()) return;

  double currentLat = gps.location.lat();
  double currentLng = gps.location.lng();
  int dist = readDistance();

  // 主干道优先策略：识别主干道（假设经纬度范围）
  bool isMainRoad = (currentLat > 30.0 && currentLat < 30.1) && (currentLng > 120.0 && currentLng < 120.2);

  // 避障逻辑：距离<30cm转向
  if (dist < 30) {
    turnRight();
  } else if (!isMainRoad) {
    // 支路探索：随机转向覆盖支路
    if (random(10) < 3) turnLeft();
    else moveForward();
  } else {
    // 主干道直行
    moveForward();
  }

  // 记录路径点
  if (gps.location.isValid()) {
    originalPath.push_back({currentLat, currentLng, isMainRoad});
    delay(1000); // 每秒记录一个点
  }

  // 探索完成条件：覆盖所有主干道+50%支路（简化判断）
  if (originalPath.size() > 50) {
    isExploring = false;
    smoothPath = douglasPeucker(originalPath, DP_THRESHOLD);
    Serial.println("探索完成，路径平滑完成，进入回溯阶段");
  }
}

// Douglas-Peucker算法：路径平滑核心
vector<GPSPoint> douglasPeucker(const vector<GPSPoint>& points, double epsilon) {
  if (points.size() < 3) return points;

  // 找到距离首尾线段最远的点
  double maxDist = 0;
  int maxIndex = 0;
  GPSPoint p1 = points[0];
  GPSPoint p2 = points[points.size()-1];

  for (int i = 1; i < points.size() - 1; i++) {
    double dist = perpendicularDistance(p1, p2, points[i]);
    if (dist > maxDist) {
      maxDist = dist;
      maxIndex = i;
    }
  }

  // 递归分割
  if (maxDist > epsilon) {
    vector<GPSPoint> left = douglasPeucker(vector<GPSPoint>(points.begin(), points.begin() + maxIndex + 1), epsilon);
    vector<GPSPoint> right = douglasPeucker(vector<GPSPoint>(points.begin() + maxIndex, points.end()), epsilon);
    vector<GPSPoint> result;
    result.insert(result.end(), left.begin(), left.end() - 1);
    result.insert(result.end(), right.begin(), right.end());
    return result;
  } else {
    return {p1, p2};
  }
}

// 计算点到线段的垂直距离
double perpendicularDistance(GPSPoint p1, GPSPoint p2, GPSPoint p) {
  double dx = p2.lng - p1.lng;
  double dy = p2.lat - p1.lat;
  double mag = sqrt(dx*dx + dy*dy);
  if (mag == 0) return 0;
  double u = ((p.lng - p1.lng) * dx + (p.lat - p1.lat) * dy) / (mag * mag);
  double px = p1.lng + u * dx;
  double py = p1.lat + u * dy;
  return sqrt(pow(p.lng - px, 2) + pow(p.lat - py, 2));
}

// 户外高速回溯执行
void executeOutdoorReturn() {
  if (smoothPath.empty()) return;

  TinyGPSPlus::update();
  if (!gps.location.isValid()) return;

  double currentLat = gps.location.lat();
  double currentLng = gps.location.lng();
  GPSPoint target = smoothPath[0];

  // 计算偏差：距离与角度
  double dist = sqrt(pow(target.lat - currentLat, 2) + pow(target.lng - currentLng, 2));
  double angle = atan2(target.lng - currentLng, target.lat - currentLat) * 180 / M_PI;
  double currentAngle = gps.course.value() / 100.0;
  double deviation = angle - currentAngle;

  // 纠偏：基于角度偏差调整转向
  if (deviation > 5) turnLeft();
  else if (deviation < -5) turnRight();
  else moveForward();

  // 到达目标点，切换下一个点
  if (dist < 5) { // 偏差<5m判定到达
    smoothPath.erase(smoothPath.begin());
    if (smoothPath.empty()) {
      Serial.println("回溯完成，户外巡检结束");
      stopMotors();
      while (1);
    }
  }

  // 高速行驶：提升占空比，同时根据路面情况调整（简化逻辑）
  setMotorSpeed(OUTDOOR_HIGH_SPEED);
}

// 其他辅助函数（省略具体实现）
int readDistance() { return analogRead(DIST_SENSOR) * 20 / 4096; }
void moveForward() { analogWrite(MOTOR_L_PWM, EXPLORE_SPEED*255); analogWrite(MOTOR_R_PWM, EXPLORE_SPEED*255); }
void turnLeft() { analogWrite(MOTOR_L_PWM, -EXPLORE_SPEED*255); analogWrite(MOTOR_R_PWM, EXPLORE_SPEED*255); delay(400); }
void turnRight() { analogWrite(MOTOR_L_PWM, EXPLORE_SPEED*255); analogWrite(MOTOR_R_PWM, -EXPLORE_SPEED*255); delay(400); }
void setMotorSpeed(float speed) { analogWrite(MOTOR_L_PWM, speed*255); analogWrite(MOTOR_R_PWM, speed*255); }

6、危化品管道巡检机器人（磁导航+路径节点压缩）
适用场景：危化品管道场景（管道沿线铺设磁条，环境存在危险区域、狭窄通道），巡检机器人需沿管道巡检，探索阶段记录管道节点（阀门、接口），优化后压缩非关键节点，回溯阶段高速沿磁条行驶，确保巡检安全高效。

核心逻辑：
探索阶段：采用“磁导航循迹+节点标记”策略，识别管道节点（阀门、泄漏点）并记录位置，确保全覆盖；
路径简化：基于节点关键度排序，删除非关键辅助节点，仅保留阀门、泄漏点等核心巡检点；
高速回溯：结合磁导航偏差修正，实现高速循迹，针对危险区域自动减速，保障安全。

/* 危化品管道巡检：两阶段探索+路径优化
   硬件：Arduino Mega + BLDC驱动轮组 + 磁导航传感器 + 气体传感器
   核心逻辑：探索（磁导航循迹+节点标记）→路径压缩→高速磁导航回溯
   优化参数：关键节点保留，非关键节点删除阈值20cm
*/

#include <SimpleFOC.h>

// --- 核心参数 ---
#define MAG_THRESHOLD 200 // 磁导航传感器阈值
#define PIPE_NODE_DETECT 50 // 管道节点检测距离（cm）
#define HIGH_SPEED_MAG 0.7 // 磁导航高速占空比
#define DANGER_SPEED 0.2 // 危险区域减速

// --- 节点类型枚举 ---
enum NodeType { VALVE, LEAK, JOINT, AUX }; // 阀门、泄漏点、接口、辅助点
struct PipeNode {
  int x; // 沿管道的坐标（cm）
  NodeType type;
  bool isDanger; // 是否为危险区域
};
vector<PipeNode> originalPath;
vector<PipeNode> compressedPath;

// --- 硬件引脚 ---
#define MOTOR_L_PWM 5
#define MOTOR_R_PWM 6
#define MAG_SENSOR_L A0
#define MAG_SENSOR_R A1
#define GAS_SENSOR A2

// --- 全局变量 ---
int currentX = 0; // 管道沿线坐标
bool isExploring = true;
int dangerCount = 0; // 危险区域计数

void setup() {
  Serial.begin(115200);
  initMagSensors();
  initMotors();
  Serial.println("危化品管道巡检系统启动");
}

void loop() {
  if (isExploring) {
    executePipeExploration();
  } else {
    executePipeReturn();
  }
  delay(20);
}

// 初始化磁导航传感器
void initMagSensors() {
  pinMode(MAG_SENSOR_L, INPUT);
  pinMode(MAG_SENSOR_R, INPUT);
}

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

// 管道探索执行：磁导航循迹+节点标记
void executePipeExploration() {
  int magLeft = analogRead(MAG_SENSOR_L);
  int magRight = analogRead(MAG_SENSOR_R);
  int gasValue = analogRead(GAS_SENSOR);

  // 磁导航纠偏：保持沿磁条行驶
  if (magLeft > MAG_THRESHOLD && magRight < MAG_THRESHOLD) {
    turnLeft(); // 偏右，左转
  } else if (magLeft < MAG_THRESHOLD && magRight > MAG_THRESHOLD) {
    turnRight(); // 偏左，右转
  } else {
    moveForward();
    currentX += 10; // 每步前进10cm
  }

  // 节点检测：距离阈值触发节点记录
  if (currentX % PIPE_NODE_DETECT == 0) {
    NodeType nodeType = AUX;
    bool isDanger = (gasValue > 500); // 气体浓度超标判定为危险
    if (currentX % (PIPE_NODE_DETECT * 5) == 0) nodeType = VALVE; // 每5个节点一个阀门
    else if (gasValue > 800) nodeType = LEAK; // 严重泄漏

    originalPath.push_back({currentX, nodeType, isDanger});
    Serial.print("检测到节点：");
    Serial.print(nodeType == VALVE ? "阀门" : nodeType == LEAK ? "泄漏点" : "辅助点");
    Serial.println(isDanger ? "（危险）" : "（安全）");
  }

  // 探索完成条件：覆盖管道全长（假设总长1000cm）
  if (currentX >= 1000) {
    isExploring = false;
    compressPath(); // 压缩路径，保留关键节点
    Serial.println("管道探索完成，开始路径压缩");
  }
}

// 路径压缩：保留关键节点（阀门、泄漏点），删除辅助点
void compressPath() {
  for (auto node : originalPath) {
    if (node.type == VALVE || node.type == LEAK) {
      compressedPath.push_back(node); // 保留核心巡检节点
    } else if (node.isDanger) {
      compressedPath.push_back(node); // 保留危险区域节点
    }
    // 辅助节点直接删除
  }
  Serial.print("路径压缩完成：原始节点");
  Serial.print(originalPath.size());
  Serial.print("，核心节点");
  Serial.println(compressedPath.size());
}

// 管道高速回溯：磁导航循迹+危险区域减速
void executePipeReturn() {
  if (compressedPath.empty()) return;

  int magLeft = analogRead(MAG_SENSOR_L);
  int magRight = analogRead(MAG_SENSOR_R);
  PipeNode target = compressedPath[0];
  bool isCurrentDanger = target.isDanger;

  // 磁导航纠偏（与探索阶段逻辑一致，但提升速度）
  if (magLeft > MAG_THRESHOLD && magRight < MAG_THRESHOLD) {
    turnLeft();
  } else if (magLeft < MAG_THRESHOLD && magRight > MAG_THRESHOLD) {
    turnRight();
  } else {
    moveForward();
  }

  // 危险区域自动减速
  if (isCurrentDanger) {
    setMotorSpeed(DANGER_SPEED);
    dangerCount++;
    Serial.println("进入危险区域，减速巡检");
  } else {
    setMotorSpeed(HIGH_SPEED_MAG);
  }

  // 到达目标节点，切换下一个节点
  if (abs(currentX - target.x) < 20) {
    compressedPath.erase(compressedPath.begin());
    if (compressedPath.empty()) {
      Serial.println("管道回溯完成，巡检任务结束");
      stopMotors();
      while (1);
    }
  }
  currentX += 10;
}

// 其他辅助函数（省略具体实现）
void moveForward() { analogWrite(MOTOR_L_PWM, 0.3*255); analogWrite(MOTOR_R_PWM, 0.3*255); }
void turnLeft() { analogWrite(MOTOR_L_PWM, -0.3*255); analogWrite(MOTOR_R_PWM, 0.3*255); delay(300); }
void turnRight() { analogWrite(MOTOR_L_PWM, 0.3*255); analogWrite(MOTOR_R_PWM, -0.3*255); delay(300); }
void setMotorSpeed(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. 两阶段流程的刚性边界：阶段切换的触发条件必须清晰可控
   两阶段流程的核心是探索与回溯的明确分割，避免状态混乱或流程反复，关键通过“完成条件”强制切换：
   探索阶段触发回溯的条件必须量化：案例4以“货架覆盖完毕”、案例5以“路径点数量达标”、案例6以“管道全长覆盖”作为切换信号，避免探索陷入死循环；
   阶段切换后不可逆，且需完成中间环节（路径优化）：探索结束后必须执行路径优化，否则回溯无有效路径，代码中需通过标志位严格阻断阶段逆转；
   异常场景的容错处理：若探索提前终止（如传感器故障），需设置兜底机制，如直接进入低速回溯或安全停机，避免机器人失控。
2. 路径优化的核心目标：平衡巡检完整性与效率，拒绝无效行驶
   路径优化不是盲目删除节点，而是基于巡检目标的精准筛选，核心解决“冗余路径导致的时间浪费”：
   优化策略需适配场景特性：
   仓储场景：基于“重复转向+距离阈值”删除冗余绕行点，保留货架节点；
   户外场景：采用DP算法平滑路径，删除偏离主线的冗余点，保留核心拐点；
   管道场景：基于节点关键度筛选，删除辅助节点，保留阀门、泄漏点等核心巡检点；
   关键节点不可删除：必须保留核心巡检目标（如货架、泄漏点、危险区域），确保巡检完整性；
   优化阈值需实际标定：阈值过大导致优化效果弱，过小导致关键信息丢失，需结合场景距离、机器人尺寸标定，如户外DP阈值需适配园区道路宽度。
3. 高速回溯的底层支撑：BLDC电机控制与传感器的协同闭环
   高速回溯的核心矛盾是高速行驶与循迹稳定性的平衡，需BLDC电机的快速响应与传感器的精准反馈形成闭环：
   BLDC电机的闭环控制：回溯阶段需从探索时的“开环低速”切换为“闭环高速”，结合编码器实现速度闭环，避免高速行驶时因负载波动导致速度失控；
   传感器与速度的动态适配：
   磁导航场景：高速行驶时需提升磁传感器采样频率，基于偏差实时调整BLDC差速，确保循迹精度；
   GPS场景：高速行驶时需结合GPS偏差动态调整转向角度，避免因GPS延迟导致偏离路径；
   安全约束优先：高速回溯需叠加场景安全规则，如危化品管道场景中，危险区域自动降速，户外场景中避障优先，避免高速行驶引发事故。
4. 阶段切换的数据衔接：路径数据的高效存储与跨阶段复用
   两阶段流程的数据核心是探索阶段记录的路径数据，需确保数据可靠存储，并能高效传递给回溯阶段：
   数据结构的轻量化设计：路径数据需采用精简结构（如坐标、类型、标志位），避免占用过多MCU内存，如Arduino Mega内存有限，需避免存储冗余历史数据；
   数据存储的可靠性：
   短期存储：基于RAM的向量存储，需考虑断电保护，可结合EEPROM实现断电数据保存；
   长期存储：复杂场景可结合SD卡或无线传输，将路径数据上传至上位机，实现多机器人路径共享；
   数据格式的兼容性：探索与回溯阶段需使用统一的路径数据格式，避免格式转换导致的信息丢失，确保回溯阶段可直接调用路径数据。
5. 场景适配的灵活调整：探索策略、优化算法与速度参数的场景化定制
   不同巡检场景的环境特性差异大，需针对场景定制两阶段策略，避免一刀切：
   探索策略适配场景：
   结构化场景（仓储、管道）：采用规则循迹+节点标记，确保路径规整；
   非结构化场景（户外园区）：采用随机+主干道优先，确保覆盖无死角；
   优化算法适配场景：
   栅格类规整路径：采用阈值筛选；
   连续类路径：采用DP算法平滑；
   节点类路径：采用关键度筛选；
   速度参数适配场景：
   安全要求高的场景（危化品管道）：高速速度低，危险区域进一步减速；
   开阔场景（户外园区）：高速速度可提升，结合GPS精度调整；
   狭窄场景（仓储货架）：高速速度需降低，避免碰撞货架。

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