我们以专业的技术视角，深入解析“Arduino BLDC智能侦查机器人”所集成的三大核心系统：多传感器互补滤波、自主路径规划与避障导航。这三者分别对应了机器人的感知、决策与执行层，是构建自主移动智能体的关键技术。

一、多传感器互补滤波：感知层的“信息融合中枢”
主要特点
互补滤波的核心在于“取长补短”。它利用不同传感器在频率特性上的互补性，进行数据融合。例如，MPU6050中的陀螺仪动态响应快，能敏锐捕捉瞬间角度变化，但长时间工作后会因积分漂移产生累计误差；而加速度计静态时对重力方向敏感，能提供稳定的绝对参考，但极易受机器人自身运动振动的高频噪声干扰。互补滤波通过一个可调参数（通常设为0.95~0.99），构建一阶低通/高通滤波器组合：让高频的陀螺仪信号通过高通滤波器，让低频的加速度信号通过低通滤波器，再将二者相加，从而获得一个在全频带都较为准确的姿态角估计。其计算极其轻量，仅需几次乘加运算，非常适合Arduino这类资源受限的微控制器。

应用场景
在智能侦查机器人中，该技术主要用于提供稳定的航向角（Yaw）和俯仰/横滚角，以支撑后续的路径跟踪和平衡控制。尤其是在GPS信号拒止的室内、地下管廊或隧道环境中，它是机器人进行航位推算（Dead Reckoning）和自主转向的核心依据。同时，它也用于检测机器人是否处于上下坡状态，为运动控制提供地形参考。

注意事项
互补滤波的性能高度依赖参数α的整定。α值过大（如0.99），虽然能更好地保留陀螺仪的动态性，但长期漂移收敛慢，会导致机器人定向缓慢偏转；α值过小（如0.9），则会引入大量加速度计的振动噪声，使姿态抖动，影响控制平稳性。工程实践中，通常需要根据机器人的运动加速度范围进行现场调试。此外，互补滤波仅适用于单轴或双轴姿态解算，对于需要全姿态（四元数）或高动态场景，其精度不如卡尔曼滤波，此时应考虑改用Madgwick或Mahony滤波算法。

二、自主路径规划：决策层的“任务指挥官”
主要特点
在Arduino平台上实现的自主路径规划，通常是轻量级的全局路径规划，常采用A算法或其变体。A算法通过一个评估函数 f(n) = g(n) + h(n) 来指导搜索方向，其中 g(n) 是起点到当前节点的实际代价，h(n) 是当前节点到目标点的估计代价（启发式函数，常用曼哈顿或欧几里得距离）。在资源受限环境中，为了降低内存开销，工程上会对地图进行栅格化压缩，并使用固定长度的数组来管理开放列表和关闭列表。另一种更轻量的做法是采用贪心最佳优先搜索，即完全忽略 g(n)，仅靠启发式函数 h(n) 驱动搜索，这在无障碍的简单地形中能极快找到路径，但无法保证路径最优且可能绕远路。

应用场景
适用于侦查机器人需要自主到达预设航点的场景，例如对化工厂区进行多点巡检、对灾区进行区域覆盖搜索等。规划出的路径通常是一系列栅格坐标或路点序列，随后由底层的运动控制器转化为具体的电机速度指令。

注意事项
在Arduino上实现A*算法最大的挑战是内存溢出。一个简单的解决办法是严格限制地图分辨率，例如只使用8×8或16×16的栅格地图。同时，应避免使用动态内存分配（new/malloc），所有数组应在编译时静态声明。此外，由于Arduino无法处理复杂的动态障碍物，该规划层通常假设环境是静态已知的，或者只作为全局参考路径，实际的动态避障需要交给下层的避障导航模块处理，这也是所谓的“全局静态规划与局部实时避障”分层架构。

三、避障导航：执行层的“贴身护卫”
主要特点
避障导航是机器人的“条件反射”机制，具有最高优先级，其执行不依赖上层复杂的决策运算。它通常依赖超声波、红外或激光雷达等测距传感器，采用势场法、向量场直方图或动态窗口法等局部规划算法。在Arduino平台上，最实用的做法是基于距离阈值的三段式控制：当障碍物距离小于急停阈值时，无条件停车；介于急停与减速阈值之间时，按比例降低速度并施加转向指令；大于减速阈值时，则执行上层的路径跟踪指令。这种硬优先级机制确保了机器人即使在通讯中断或路径规划失效时，也不会发生碰撞。

应用场景
在侦查任务中，环境是未知且非结构化的，随时可能出现临时堆放的杂物、墙体或人员。避障导航模块确保机器人在执行全局路径时，能实时响应这些突发障碍，安全绕行后再重新回归到全局路径上。在狭窄走廊或房间入口等区域，该机制还能让机器人以缓慢速度“蠕动”通过。

注意事项
避障导航的有效性高度依赖传感器的可靠性和布置位置。超声波传感器存在锥形盲区和镜面反射问题，面对斜面或细小障碍物可能漏检；红外传感器受环境光线和物体颜色影响较大。因此，多传感器融合（如前面+左右共3~5个传感器）是必要的。此外，避障策略应避免在通道中陷入“左右摇摆”的震荡，常见的解决方法是引入记忆机制：记录上一次的转向方向，在障碍物未完全通过前维持该转向趋势，防止机器人因左右距离来回跳变而反复横摆。

三者协同：从“感知”到“决策”再到“执行”的闭环
三者在机器人系统中形成一个递进式的闭环：首先，互补滤波在干扰环境下稳健地提供机器人的姿态和航向，作为全局位置推算的依据；然后，路径规划模块根据当前定位和目标点，在地图上生成一条参考路径；接着，避障导航模块实时监控路径前方的物理空间，若发现障碍则临时干预电机指令进行绕行，并将绕行后的新位置反馈回全局规划层，实现路径的动态重规划。这三个模块并行运行、相互配合，最终使机器人能够在复杂、未知且强干扰的侦查环境中，实现自主、安全、高效的运动。

总而言之，这套系统的设计哲学是“合理分配有限算力”：最复杂的逻辑留给路径规划（但通过地图压缩和算法简化以适应Arduino），最快速的响应留给避障导航（硬件中断或简单阈值），而融合算法则在姿态层面提供最基础的保障。三者缺一不可，共同构成了智能侦查机器人的自主移动基石。

1、基于互补滤波的姿态估计与平衡控制
这个案例展示了如何在Arduino上利用MPU6050（六轴姿态传感器）通过互补滤波算法，融合陀螺仪和加速度计数据，获取稳定的姿态角。这对于需要保持平衡或在颠簸路面行驶的侦查机器人至关重要。

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

Adafruit_MPU6050 mpu;
float angle = 0.0f;
const float alpha = 0.98f; // 互补滤波系数，给陀螺仪更高权重
unsigned long lastTime = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  if (!mpu.begin()) {
    Serial.println("MPU6050 not found!");
    while (1);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  lastTime = micros();
}

void loop() {
  // 1. 获取传感器事件
  sensors_event_t accel, gyro, temp;
  mpu.getEvent(&accel, &gyro, &temp);

  // 2. 计算时间间隔dt，确保精度
  unsigned long now = micros();
  float dt = (now - lastTime) / 1000000.0f;
  lastTime = now;

  // 3. 从加速度计计算俯仰角（单位：度）
  float accelPitch = atan2(accel.acceleration.y, accel.acceleration.z) * 180.0 / PI;

  // 4. 获取陀螺仪X轴角速度（度/秒）
  float gyroRate = gyro.gyro.x;

  // 5. 核心互补滤波公式
  // 角度 = α * (上一时刻角度 + 陀螺仪角速度 * dt) + (1-α) * 加速度计角度
  angle = alpha * (angle + gyroRate * dt) + (1.0f - alpha) * accelPitch;

  // 6. 输出结果用于控制或调试
  Serial.print("Filtered Angle: ");
  Serial.println(angle);

  // 此处可以加入将angle作为误差输入PID控制器，驱动BLDC电机执行修正动作的代码
  // 例如，驱动自平衡车的轮子电机

  delay(10); // 约100Hz控制周期
}

关键逻辑：互补滤波的核心是取长补短。陀螺仪动态响应快但会积分漂移，加速度计静态时准确但易受振动干扰。公式 angle = α * (angle + gyroRate * dt) + (1-α) * accelPitch 通过一个加权系数α，让系统在动态时更信任陀螺仪（高频），在静态时更信任加速度计（低频），从而获得一个实时且无漂移的姿态角。这在Arduino这样资源受限的平台上，比卡尔曼滤波更轻量高效。

2、基础网格地图与A*路径规划
这个案例演示了如何在Arduino上实现一个轻量级的A路径规划算法。它将环境建模为网格地图，并用A算法在已知的静态障碍物之间找到一条最短路径。

#include <ArduinoSTL.h> // 若使用标准C++ STL容器，但建议自行实现简易链表以节省内存
#include <vector>

// 1. 定义网格地图 (0: 自由, 1: 障碍物)
// 注意：Arduino Uno的SRAM仅2KB，地图尺寸不宜过大，如20x20
const int MAP_SIZE = 10;
int map[MAP_SIZE][MAP_SIZE] = {
  {0,0,0,0,0,0,0,0,0,0},
  {0,1,1,0,1,0,0,0,0,0},
  {0,0,0,0,1,0,1,1,1,0},
  {0,1,0,0,0,0,1,0,0,0},
  {0,1,0,1,1,0,1,0,1,0},
  {0,0,0,1,0,0,0,0,1,0},
  {0,1,0,0,0,1,0,0,0,0},
  {0,1,0,1,0,0,0,1,0,0},
  {0,0,0,0,0,1,0,0,0,0},
  {0,0,0,0,0,0,0,0,0,0}
};

struct Node {
  int x, y;      // 坐标
  int g;         // 起点到该点的实际代价
  int h;         // 该点到终点的启发式估算代价 (曼哈顿距离)
  int f;         // f = g + h
  Node* parent;  // 父节点指针，用于回溯路径
};

// 2. 启发式函数：计算曼哈顿距离（单位是格子数）
int heuristic(Node* a, Node* b) {
  return abs(a->x - b->x) + abs(a->y - b->y);
}

// 3. A*搜索函数（伪代码框架，需实现Open/Closed列表）
// AStarSearch(start, goal) {
//   1. 将起点加入Open List
//   2. 循环：从Open List中取出f值最小的节点作为current
//   3. 若current == goal，则成功找到路径，回溯parent指针即可
//   4. 否则，将current移入Closed List，并检查其8个邻居
//   5. 对每个可通行的邻居，若不在Closed List中，计算g,h,f，并更新其parent
//   6. 若Open List为空，则搜索失败
// }

// 4. 路径跟踪与电机控制（示意）
void followPath(std::vector<Node*> path) {
  if (path.empty()) return;
  for (Node* waypoint : path) {
    // 调用运动控制函数，驱动机器人从当前位置移动到 (waypoint->x, waypoint->y)
    // 例如：motorControl.moveTo(waypoint->x * GRID_RESOLUTION, waypoint->y * GRID_RESOLUTION);
    // 需结合BLDC差速控制与PID
  }
}

关键逻辑：A*算法的核心是评价函数 f(n) = g(n) + h(n)。它通过始终优先探索总代价最小的节点来高效搜索路径。但Arduino内存有限，地图尺寸和路径点数量必须严格限制，建议使用byte数组存储地图，并将路径点存储在环形缓冲区中。图中未完全展开的AStarSearch函数，其实现是此案例的关键，但受篇幅限制此处提供了详细注释的框架。

3、动态障碍物避让与路径重规划
这个案例在前一个的基础上，加入了超声波传感器，用于检测动态障碍物。当发现规划好的路径上有新的障碍物时，会触发路径重规划，从而实现动态避障。

#include <NewPing.h>

// 假设案例二中的地图变量为 staticMap，此处创建一个动态地图副本
int dynamicMap[MAP_SIZE][MAP_SIZE];
// 定义超声波传感器引脚
#define TRIG_PIN 2
#define ECHO_PIN 3
#define MAX_DISTANCE 200
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

// 机器人当前位置（网格坐标）
int currentX = 0, currentY = 0;

void setup() {
  Serial.begin(115200);
  // 初始化动态地图为静态地图副本
  memcpy(dynamicMap, map, sizeof(map));
  // ... 其他初始化
}

void loop() {
  // 1. 检测前方障碍物
  int distance = sonar.ping_cm();
  if (distance > 0 && distance < 50) { // 假设50cm内有障碍
    // 简单估算障碍物的网格坐标（此处仅为示意，实际需结合机器人航向）
    int obsX = currentX + 1; 
    int obsY = currentY; 
    if (obsX < MAP_SIZE && obsY < MAP_SIZE) {
      dynamicMap[obsX][obsY] = 1; // 标记为障碍
      Serial.println("Dynamic obstacle detected!");
    }
  }

  // 2. 检查当前路径是否被阻断
  bool pathBlocked = false;
  // 假设 path 是存储当前规划路径点的 vector
  for (auto& point : path) { 
    if (dynamicMap[point.x][point.y] == 1) {
      pathBlocked = true;
      break;
    }
  }

  // 3. 若路径被阻断，则以当前位置为起点，原目标为终点，用dynamicMap重新规划路径
  if (pathBlocked) {
    Serial.println("Path blocked! Replanning...");
    Node* start = new Node{currentX, currentY, 0, 0, 0, nullptr};
    Node* goal = new Node{targetX, targetY, 0, 0, 0, nullptr};
    // 调用A*搜索函数，但传入 dynamicMap 和新的起点
    // path = AStarSearch(start, goal, dynamicMap); 
    // 注意：需要释放 start 和 goal 内存，或使用静态分配
    pathBlocked = false; // 重置标志
  }

  // 4. 执行路径跟踪（略）
  // followPath(path);

  delay(50);
}

关键逻辑：动态避障的关键在于地图的实时更新和重规划触发机制。程序首先通过传感器（如超声波）获取环境中的新障碍信息，并将其标记在dynamicMap上。然后，通过isPathBlocked函数检查当前路径是否因此被阻断。一旦确认路径失效，就立即使用更新后的地图重新运行A*算法，生成一条绕过障碍物的新路径。为了节省算力，重规划的频率需要适当控制，不必每帧都进行。

要点解读
互补滤波是姿态感知的“轻骑兵”：互补滤波的核心思想是利用陀螺仪的高频响应优势与加速度计的低频稳定性，通过一个简单的加权公式实现数据融合。相比卡尔曼滤波，它计算量极小，非常适用于Arduino UNO这类资源有限的平台。其关键在于系统上电后必须进行陀螺仪零偏校准和精确控制采样时间，否则漂移会严重影响姿态估算。

A路径规划必须“量体裁衣”：Arduino的SRAM（如Uno仅2KB）是A算法落地的最大瓶颈。因此，地图分辨率必须被严格控制（通常不超过30x30），并且数据结构应尽量精简，例如使用byte数组存储地图，用固定大小的数组模拟Open List和Closed List。一个工程化的策略是，将地图数据存入Flash（PROGMEM）中，运行时按需读取。

动态避障本质是“感知-重规划”的闭环：静态的A*算法无法应对移动障碍物。实际系统中，需要将声呐、红外等局部传感器的感知结果实时融入地图（即构建动态地图），并建立一个高效的路径有效性检查机制。一旦发现当前路径被“污染”，就立即触发重规划。为减轻算力负担，可以限制重规划频率，或只对路径附近的局部地图进行更新和搜索。

BLDC电机控制与规划算法需要深度解耦：在任何成熟的机器人系统中，规划层和执行层都是分离的。规划层（如A*算法）负责计算出宏观的航点序列，而执行层（BLDC电机控制）则负责通过PID等控制算法，驱动底盘精准地跟踪这些航点。这种分层架构使得开发者可以独立调试运动控制和路径规划，降低系统复杂度。

异构计算是突破算力限制的工程标准：鉴于Arduino在复杂SLAM、大规模地图构建上的性能瓶颈，工业级或面向实战的智能机器人普遍采用异构架构。一个典型的方案是：Arduino作为下位机，专职负责实时性要求高的传感器数据读取和BLDC电机的闭环控制；而树莓派或Jetson Nano作为上位机，负责运算密集型的任务，如激光SLAM、基于视觉的深度学习（YOLO目标检测）以及复杂的全局路径规划。两者通过串口或CAN总线等可靠协议进行通信。

4、仓储智能巡检侦查机器人——多传感器互补滤波+DWA动态避障
适用场景：仓储货架密集区域，机器人需自主巡航、侦查货物破损/标识异常，核心需求是解决激光雷达定位漂移、超声波近距盲区，动态规避叉车、移动货物等障碍物，实现稳定巡检与目标捕捉。

核心逻辑：采用激光雷达+IMU+超声波多传感器互补滤波：激光雷达获取全局栅格地图，IMU通过互补滤波抑制陀螺仪漂移，超声波弥补近距障碍物检测盲区；路径规划层采用动态窗口法（DWA） 实时生成局部路径，BLDC电机通过FOC闭环控制执行差速驱动，结合近距超声波强制避障，实现仓储环境下的自主巡航与目标侦查。

/* ===== 仓储智能巡检侦查机器人：多传感器互补滤波 + DWA动态避障 =====
 * 硬件：Arduino ESP32 + BLDC差速底盘 + RPLIDAR A1（激光雷达） + IMU6050 + 超声波阵列
 * 核心：多传感器互补滤波 + DWA局部路径规划 + FOC闭环驱动
 */
#include <SimpleFOC.h>
#include <Wire.h>  // IMU通信
#include <RPLidar.h>

// ==================== 硬件驱动初始化 ====================
BLDCMotor motorL(7), motorR(7);       // 左右驱动轮
BLDCDriver3PWM driverL(9,10,11,8);    // 左电机驱动
BLDCDriver3PWM driverR(3,5,6,7);      // 右电机驱动
Encoder encL(18,19,2048), encR(20,21,2048); // 轮速编码器

// 传感器接口
RPLidar radar;          // 激光雷达（硬件串口）
const int imuAddr = 0x68; // IMU6050地址
const int sonarF = 2;     // 前端超声波
const int sonarL = 3;     // 左侧超声波
const int sonarR = 4;     // 右侧超声波
const int targetPin = 5;  // 目标识别触发口（摄像头/红外）

// ==================== 多传感器互补滤波参数 ====================
const float Kp = 0.5, Ki = 0.3; // IMU互补滤波系数（抑制陀螺仪漂移）
const float gyroBias[3] = {0, 0, 0}; // 陀螺仪零点校正值
float accelAngle = 0, gyroAngle = 0, fusedAngle = 0; // 加速度计/陀螺仪/融合角度

// ==================== DWA路径规划参数 ====================
const float MAX_VX = 0.8, MAX_VW = 1.2;  // 最大线速度/角速度（m/s）
const float VX_STEP = 0.1, VW_STEP = 0.2; // 速度采样步长
const float ROBOT_RADIUS = 0.15;          // 机器人半径（m）
const float OBSTACLE_SAFE = 0.2;          // 避障安全距离（m）

// ==================== 传感器互补滤波核心 ====================
void imuComplementaryFilter() {
    // 读取IMU数据（实际需调用IMU驱动库，此处简化模拟）
    float ax = 0, ay = 0, az = 0;
    float gx = 0, gy = 0, gz = 0;
    // 模拟读取（实际用Wire通信）
    // imu.getAccel(&ax,&ay,&az); imu.getGyro(&gx,&gy,&gz);
    
    // 加速度计角度计算（简化）
    accelAngle = atan2(ay, sqrt(ax*ax + az*az)) * 180 / 3.1416;
    // 陀螺仪角度积分（校正零点漂移）
    gyroAngle += (gx - gyroBias[0]) * 0.01;
    // 互补滤波融合（抑制陀螺仪漂移，保留加速度计静态稳定性）
    fusedAngle = Kp * accelAngle + (1-Kp) * (fusedAngle + (gyroAngle - fusedAngle)*0.1);
}

float ultrasonicComplementary(int pin, float radarDist) {
    // 超声波近距补偿（弥补激光雷达近距盲区）
    float usDist = analogRead(pin) * 0.1; // 模拟距离转换
    // 近距时以超声波为准，远距以雷达为准
    if (usDist < 0.5) return usDist;
    return radarDist;
}

// ==================== DWA动态避障规划 ====================
void dwaPlanning(float &vx, float &wz, float targetX, float targetY) {
    // 1. 速度采样
    float vxList[10], wzList[10];
    int idx = 0;
    for (float v = -MAX_VX; v <= MAX_VX; v += VX_STEP) {
        for (float w = -MAX_VW; w <= MAX_VW; w += VW_STEP) {
            vxList[idx] = v;
            wzList[idx] = w;
            idx++;
            if (idx >= 10) break;
        }
        if (idx >= 10) break;
    }
    
    // 2. 评估函数：朝向目标+避障+速度平滑
    float bestScore = -1e9;
    float bestVx=0, bestWz=0;
    
    for (int i=0; i<10; i++) {
        // 模拟下一步状态（基于当前位置，简化）
        static float curX=0, curY=0, curTheta=0;
        float nextX = curX + vxList[i]*0.1*cos(curTheta);
        float nextY = curY + vxList[i]*0.1*sin(curTheta);
        float nextTheta = curTheta + wzList[i]*0.1;
        
        // 距离目标评分（越近越高）
        float distScore = -hypot(nextX-targetX, nextY-targetY);
        // 避障评分（根据超声波检测）
        float usF = ultrasonicComplementary(sonarF, 10);
        float usL = ultrasonicComplementary(sonarL, 10);
        float usR = ultrasonicComplementary(sonarR, 10);
        float obsScore = (usF<OBSTACLE_SAFE ? -100 : 0) + (usL<OBSTACLE_SAFE ? -50 : 0) + (usR<OBSTACLE_SAFE ? -50 : 0);
        // 速度平滑评分（避免突变）
        static float lastVx=0, lastWz=0;
        float smoothScore = -hypot(vxList[i]-lastVx, wzList[i]-lastWz);
        
        float totalScore = distScore + obsScore + smoothScore;
        if (totalScore > bestScore) {
            bestScore = totalScore;
            bestVx = vxList[i]; bestWz = wzList[i];
        }
    }
    
    // 3. 输出控制量
    vx = constrain(bestVx, -MAX_VX, MAX_VX);
    wz = constrain(bestWz, -MAX_VW, MAX_VW);
    // 更新当前状态
    curX += vx*0.1*cos(curTheta); curY += vx*0.1*sin(curTheta);
    curTheta += wz*0.1;
    // 强制避障（超声波近距触发）
    if (usF < OBSTACLE_SAFE) {
        if (usL > usR) { vx=0.2; wz=1.0; } // 右转避障
        else { vx=0.2; wz=-1.0; } // 左转避障
    }
}

// ==================== 目标侦查与任务执行 ====================
void executeSurveillance() {
    static bool foundTarget = false;
    // 模拟目标识别（实际接摄像头/红外传感器）
    int targetSignal = digitalRead(targetPin);
    if (targetSignal == HIGH && !foundTarget) {
        foundTarget = true;
        Serial.println("[!] Target Detected: Stop and Surveillance");
        // 停止电机，启动侦查传感器
        motorL.move(0); motorR.move(0);
        delay(3000); // 侦查时间
        foundTarget = false;
        Serial.println("[+] Surveillance Complete: Continue Path");
    }
}

void setup() {
    Serial.begin(115200);
    // BLDC电机初始化
    motorL.linkSensor(&encL); motorL.linkDriver(&driverL);
    motorR.linkSensor(&encR); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    // 传感器初始化
    radar.begin(Serial1); // 激光雷达硬件串口
    pinMode(targetPin, INPUT_PULLUP);
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 多传感器互补滤波更新
    imuComplementaryFilter();
    float radarDist = radar.scan(); // 激光雷达扫描（简化）
    
    // 2. DWA路径规划（目标：沿货架巡航，目标点固定）
    float targetX = 5.0, targetY = 0.0; // 货架巡检目标点
    float vx, wz;
    dwaPlanning(vx, wz, targetX, targetY);
    
    // 3. 差速驱动解算
    float wheelBase = 0.25;
    float leftSpd = vx - wz * wheelBase/2;
    float rightSpd = vx + wz * wheelBase/2;
    motorL.move(leftSpd);
    motorR.move(rightSpd);
    
    // 4. 目标侦查执行
    executeSurveillance();
    
    // 调试输出
    Serial.print("Vx:");Serial.print(vx);
    Serial.print(" Wz:");Serial.print(wz);
    Serial.print(" Angle:");Serial.print(fusedAngle);
    Serial.println();
    
    delay(50);
}

5、园区自主巡逻侦查机器人——A*全局规划+模糊避障
适用场景：园区复杂环境（含主路、支路、临时障碍物），机器人需按照预设巡逻路线自主巡航，侦查异常人员/火情，核心需求是全局路径最优+动态障碍物规避，实现高效巡逻与灵活避障。

核心逻辑：采用A全局路径规划+模糊逻辑避障双层架构：A算法在全局栅格地图中规划最优巡逻路线；模糊逻辑融合激光雷达与视觉传感器数据，处理临时障碍物、行人的动态避障；BLDC电机通过FOC闭环控制跟踪全局路径，避障时实时调整，确保巡逻任务不中断。

/* ===== 园区自主巡逻侦查机器人：A*全局规划 + 模糊避障 =====
 * 硬件：Arduino Mega + BLDC差速底盘 + 激光雷达 + 视觉模块 + 远程通信模块
 * 核心：A*全局路径规划 + 模糊动态避障 + 路径跟踪
 */
#include <SimpleFOC.h>
#include <AStar.h> // 简化A*库（实际需自定义）

// ==================== 硬件驱动初始化 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(3,5,6,7);
Encoder encL(18,19,2048), encR(20,21,2048);

// 传感器接口
const int lidarPin = A0;    // 激光雷达（模拟）
const int visionPin = 2;    // 视觉目标识别
const int commPin = 3;      // 远程指令接收

// ==================== A*全局路径规划参数 ====================
const int MAP_W = 10, MAP_H = 10; // 栅格地图尺寸
int gridMap[MAP_W][MAP_H] = {0};   // 0=可通行，1=障碍物
std::vector<std::pair<int,int>> globalPath; // 全局路径点

// ==================== 模糊避障参数 ====================
#define OBSTACLE_NEAR 0.3  // 近距障碍阈值（m）
#define OBSTACLE_FAR 1.5   // 远距障碍阈值（m）
float fuzzyLeftSpd = 0, fuzzyRightSpd = 0;

// ==================== A*全局路径规划 ====================
std::vector<std::pair<int,int>> planAStar(int startX, int startY, int goalX, int goalY) {
    // 简化A*路径规划（实际需实现启发式搜索、障碍物检测）
    std::vector<std::pair<int,int>> path;
    // 示例：沿直线路径规划（实际需规避障碍物）
    for (int x = startX; x <= goalX; x++) path.push_back({x, startY});
    for (int y = startY+1; y <= goalY; y++) path.push_back({goalX, y});
    return path;
}

// ==================== 模糊避障控制 ====================
void fuzzyObstacleAvoidance(float &leftSpd, float &rightSpd, float frontDist, float leftDist, float rightDist) {
    // 模糊输入：前方/左侧/右侧障碍距离（近/中/远）
    int frontState = (frontDist < OBSTACLE_NEAR ? 0 : (frontDist < OBSTACLE_FAR ? 1 : 2));
    int leftState = (leftDist < OBSTACLE_NEAR ? 0 : (leftDist < OBSTACLE_FAR ? 1 : 2));
    int rightState = (rightDist < OBSTACLE_NEAR ? 0 : (rightDist < OBSTACLE_FAR ? 1 : 2));
    
    // 模糊规则库（动态避障）
    if (frontState == 0) { // 前方近障碍
        if (leftState > rightState) { // 左侧更开阔
            leftSpd = 0.6; rightSpd = -0.6; // 左转避障
        } else if (rightState > leftState) { // 右侧更开阔
            leftSpd = -0.6; rightSpd = 0.6; // 右转避障
        } else { // 两侧均开阔
            leftSpd = 0.5; rightSpd = -0.5; // 左转
        }
    } else if (leftState == 0) { // 左侧近障碍
        leftSpd = -0.5; rightSpd = 0.5; // 右转
    } else if (rightState == 0) { // 右侧近障碍
        leftSpd = 0.5; rightSpd = -0.5; // 左转
    } else { // 无障碍
        leftSpd = 1.0; rightSpd = 1.0; // 全速前进
    }
}

// ==================== 全局路径跟踪 ====================
void pathTracking(float &leftSpd, float &rightSpd, std::vector<std::pair<int,int>> &path) {
    static int pathIdx = 0;
    if (path.empty()) return;
    
    // 模拟机器人当前位置（基于轮式里程计）
    static float curX=0, curY=0, curTheta=0;
    float targetX = path[pathIdx].first * 0.5; // 栅格转实际坐标
    float targetY = path[pathIdx].second * 0.5;
    
    // 计算目标方向角
    float targetTheta = atan2(targetY - curY, targetX - curX);
    float thetaError = targetTheta - curTheta;
    // 归一化角度误差
    if (thetaError > 3.14) thetaError -= 2*3.14;
    if (thetaError < -3.14) thetaError += 2*3.14;
    
    // PID路径跟踪（简化）
    float Kp = 1.0, Kd = 0.5;
    float linearSpd = 0.8; // 线速度
    float angularSpd = Kp * thetaError + Kd * (thetaError - 0);
    
    // 差速解算
    float wheelBase = 0.25;
    leftSpd = linearSpd - angularSpd * wheelBase/2;
    rightSpd = linearSpd + angularSpd * wheelBase/2;
    
    // 更新当前位置
    curX += linearSpd * 0.1 * cos(curTheta);
    curY += linearSpd * 0.1 * sin(curTheta);
    curTheta += angularSpd * 0.1;
    
    // 路径点到达判断
    if (hypot(curX-targetX, curY-targetY) < 0.5) {
        pathIdx++;
        if (pathIdx >= path.size()) pathIdx = 0; // 循环巡逻
    }
}

// ==================== 巡逻任务与侦查 ====================
void patrolSurveillance() {
    // 远程指令处理（模拟）
    int cmd = digitalRead(commPin);
    if (cmd == HIGH) {
        // 更新巡逻目标点
        globalPath = planAStar(0, 0, 5, 5);
        Serial.println("[+] Path Updated: A* Planning");
    }
    // 目标侦查（视觉识别）
    if (digitalRead(visionPin) == HIGH) {
        Serial.println("[!] Suspicious Target Detected");
        // 减速靠近侦查
        motorL.move(0.3); motorR.move(0.3);
        delay(2000);
    }
}

void setup() {
    Serial.begin(115200);
    // BLDC初始化
    motorL.linkSensor(&encL); motorL.linkDriver(&driverL);
    motorR.linkSensor(&encR); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    // 初始化栅格地图（示例）
    memset(gridMap, 0, sizeof(gridMap));
    // 预设巡逻路径
    globalPath = planAStar(0, 0, 5, 5);
    // 传感器初始化
    pinMode(visionPin, INPUT_PULLUP);
    pinMode(commPin, INPUT_PULLUP);
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 传感器数据读取
    float frontDist = analogRead(lidarPin) * 0.1;
    float leftDist = analogRead(A1) * 0.1;
    float rightDist = analogRead(A2) * 0.1;
    
    // 2. 全局路径跟踪
    float leftSpd, rightSpd;
    pathTracking(leftSpd, rightSpd, globalPath);
    
    // 3. 模糊避障（动态修正路径）
    fuzzyObstacleAvoidance(leftSpd, rightSpd, frontDist, leftDist, rightDist);
    
    // 4. 执行电机控制
    motorL.move(leftSpd);
    motorR.move(rightSpd);
    
    // 5. 巡逻侦查任务
    patrolSurveillance();
    
    // 调试输出
    Serial.print("PathIdx:");Serial.print(globalPath.size());
    Serial.print(" FrontDist:");Serial.print(frontDist);
    Serial.print(" LSpd:");Serial.print(leftSpd);
    Serial.println();
    
    delay(50);
}

6、应急救援侦查机器人——多传感器融合+应急避障
适用场景：地震废墟、火灾现场等复杂未知环境，机器人需进入危险区域侦查生命迹象、环境参数，核心需求是强干扰下的感知鲁棒性、复杂地形通过性、应急避障能力，实现无预构图的自主侦查。

核心逻辑：采用多传感器深度融合+应急避障+越障控制：红外+超声波+气体传感器融合检测生命迹象与环境风险；基于滚动时域树（RRT*）的在线路径规划，实时生成无预知地图的路径；BLDC电机采用扭矩/速度混合控制，应对废墟爬坡、翻越障碍，结合应急急停与避障，保障侦查安全。

/* ===== 应急救援侦查机器人：多传感器融合 + 应急避障 + 越障控制 =====
 * 硬件：Arduino ESP32 + BLDC差速底盘 + 红外生命传感器 + 超声波阵列 + 气体传感器
 * 核心：多传感器融合感知 + RRT*在线路径规划 + 扭矩控制越障 + 应急避障
 */
#include <SimpleFOC.h>

// ==================== 硬件驱动初始化 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(3,5,6,7);
Encoder encL(18,19,2048), encR(20,21,2048);

// 传感器接口
const int irPin = A0;      // 红外生命探测传感器
const int gasPin = A1;      // 气体浓度传感器（CO/可燃气体）
const int frontSonar = 2;   // 前端超声波
const int sideSonarL = 3;   // 侧向超声波
const int sideSonarR = 4;   // 侧向超声波
const int hazardPin = 5;    // 危险触发（高温/震动）

// ==================== 应急与越障参数 ====================
#define EMERGENCY_STOP 1   // 急停标志
#define CLIMB_TORQUE 0.8   // 爬坡/越障扭矩
#define MAX_TORQUE 1.0     // 最大扭矩
float motorTorqueL = 0, motorTorqueR = 0;

// ==================== 多传感器融合感知 ====================
struct SensorData {
    float lifeSign;    // 生命迹象（0~100，越高越明显）
    float gasConc;     // 气体浓度（ppm）
    float frontDist;   // 前方距离（m）
    float sideDistL;   // 左侧距离（m）
    float sideDistR;   // 右侧距离（m）
    bool hazard;       // 危险状态（急停触发）
};

SensorData fuseSensors() {
    SensorData data;
    // 红外生命探测（模拟：红外信号越强，生命迹象越明显）
    data.lifeSign = analogRead(irPin) * 0.5;
    // 气体浓度
    data.gasConc = analogRead(gasPin);
    // 超声波距离
    data.frontDist = analogRead(frontSonar) * 0.01;
    data.sideDistL = analogRead(sideSonarL) * 0.01;
    data.sideDistR = analogRead(sideSonarR) * 0.01;
    // 危险触发
    data.hazard = digitalRead(hazardPin) == HIGH;
    return data;
}

// ==================== RRT*在线路径规划（简化） ====================
void rrtStarPlanning(float &targetVx, float &targetWz, const SensorData &data) {
    // 无预构图，实时生成路径（核心思想：向安全区域+生命迹象方向探索）
    // 1. 安全优先级：避开近距障碍
    if (data.frontDist < 0.3) { // 前方障碍
        if (data.sideDistL > data.sideDistR) {
            targetVx = 0.2; targetWz = 1.0; // 左转避障
        } else {
            targetVx = 0.2; targetWz = -1.0; // 右转避障
        }
        return;
    }
    
    // 2. 生命迹象优先：向生命迹象方向移动
    if (data.lifeSign > 30) {
        // 左侧生命迹象更强，左转
        if (data.sideDistL > 0.5) {
            targetVx = 0.5; targetWz = 0.5;
        }
        // 右侧生命迹象更强，右转
        else if (data.sideDistR > 0.5) {
            targetVx = 0.5; targetWz = -0.5;
        }
        // 前方生命迹象更强，直行
        else {
            targetVx = 0.6; targetWz = 0;
        }
    }
    // 3. 无生命迹象：随机探索+避障
    else {
        targetVx = 0.4;
        targetWz = random(-0.5, 0.5); // 随机转向探索
        // 侧向避障
        if (data.sideDistL < 0.2) targetWz = 0.5;
        if (data.sideDistR < 0.2) targetWz = -0.5;
    }
}

// ==================== 越障控制（扭矩控制） ====================
void obstacleClimbing(float &torqueL, float &torqueR, const SensorData &data) {
    // 1. 越障判定：前方距离小但未触发急停，且有爬坡需求
    if (data.frontDist < 0.5 && !data.hazard) {
        // 提升扭矩应对爬坡/障碍
        torqueL = constrain(torqueL + 0.1, 0, CLIMB_TORQUE);
        torqueR = constrain(torqueR + 0.1, 0, CLIMB_TORQUE);
    }
    // 2. 危险触发：急停并降低扭矩
    else if (data.hazard) {
        torqueL = 0; torqueR = 0;
        Serial.println("[!] EMERGENCY STOP TRIGGERED");
    }
    // 3. 正常行驶：保持基础扭矩
    else {
        torqueL = constrain(torqueL - 0.05, 0, 0.3);
        torqueR = constrain(torqueR - 0.05, 0, 0.3);
    }
}

// ==================== 应急避障与任务执行 ====================
void emergencyNavigation(const SensorData &data) {
    // 1. 急停判断
    if (data.hazard) {
        motorL.move(0);
        motorR.move(0);
        return;
    }
    
    // 2. 路径规划与越障控制
    float targetVx, targetWz;
    rrtStarPlanning(targetVx, targetWz, data);
    obstacleClimbing(motorTorqueL, motorTorqueR, data);
    
    // 3. 扭矩/速度混合控制（BLDC控制模式切换）
    if (motorTorqueL > 0.5 || motorTorqueR > 0.5) {
        // 越障时切换至扭矩控制（实际需配置扭矩环）
        // 简化：通过调整FOC的目标速度模拟扭矩提升
        motorL.targetVelocity = motorTorqueL * 2;
        motorR.targetVelocity = motorTorqueR * 2;
    } else {
        // 正常行驶切换至速度控制
        float wheelBase = 0.25;
        float leftSpd = targetVx - targetWz * wheelBase/2;
        float rightSpd = targetVx + targetWz * wheelBase/2;
        motorL.targetVelocity = leftSpd;
        motorR.targetVelocity = rightSpd;
    }
    
    // 4. 生命迹象回传（模拟远程上报）
    if (data.lifeSign > 50) {
        Serial.println("[!] LIFE SIGN DETECTED: Sending Location");
        // 此处添加远程上报逻辑
    }
}

void setup() {
    Serial.begin(115200);
    // BLDC初始化（速度/扭矩控制模式）
    motorL.linkSensor(&encL); motorL.linkDriver(&driverL);
    motorR.linkSensor(&encR); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    // 传感器初始化
    pinMode(hazardPin, INPUT_PULLUP);
    randomSeed(analogRead(0)); // 随机种子初始化
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 多传感器融合
    SensorData data = fuseSensors();
    
    // 2. 应急导航与避障
    emergencyNavigation(data);
    
    // 3. 执行电机控制（速度/扭矩切换执行）
    motorL.move(motorL.targetVelocity);
    motorR.move(motorR.targetVelocity);
    
    // 调试输出
    Serial.print("Life:");Serial.print(data.lifeSign);
    Serial.print(" Gas:");Serial.print(data.gasConc);
    Serial.print(" Front:");Serial.print(data.frontDist);
    Serial.print(" TorqueL:");Serial.print(motorTorqueL);
    Serial.println();
    
    delay(40);
}

要点解读

1. 多传感器互补滤波：解决感知局限，提升环境鲁棒性
   智能侦查的核心是精准感知环境，单一传感器存在固有盲区（如激光雷达近距盲区、超声波远距误差大、IMU漂移），多传感器互补滤波是突破感知局限的关键：

传感器分工与互补：案例1中激光雷达负责全局环境建模，IMU通过互补滤波抑制陀螺仪漂移，超声波弥补近距避障盲区；案例3中红外传感器探测生命迹象，超声波检测障碍物，气体传感器监测环境风险，形成“目标检测+避障+环境监测”的全维度感知。

互补滤波算法落地：通过互补滤波（IMU）、均值滤波、归一化处理，融合不同传感器优势——如IMU互补滤波中，用加速度计校正陀螺仪漂移，保留陀螺仪的动态响应；超声波与激光雷达的融合，近距用超声波的高精度，远距用激光雷达的广覆盖，避免单一传感器数据跳变导致控制失稳。

干扰抑制：对传感器数据进行异常值剔除、限幅处理，抑制环境干扰（如灰尘、震动对红外的影响），提升感知数据的可靠性，为后续路径规划提供稳定输入。

2. 自主路径规划：全局最优与在线探索结合，适配不同场景
   自主侦查需兼顾路径效率与场景适应性，根据环境是否预知，采用不同路径规划策略：

预知环境：全局路径规划：案例2针对园区预知环境，采用A算法在全局栅格地图中规划最优巡逻路线，确保巡逻路径最短、时间成本最低；A算法通过启发式搜索，高效规避静态障碍物（如园区建筑、固定设施），为机器人提供全局导航目标。

未知环境：在线路径规划：案例3针对废墟等未知环境，采用RRT*滚动时域树算法，无预构图实时生成路径，核心是向安全区域+目标方向探索，通过随机采样扩展路径树，快速找到可行路径，适配应急救援的无预知场景。

规划与执行的闭环衔接：路径规划结果转化为机器人的期望速度/方向，通过BLDC电机闭环跟踪，同时根据传感器反馈实时修正路径，如案例2中模糊避障修正全局路径的局部偏差，确保规划不脱离实际环境。

3. 动态避障导航：分层逻辑+应急机制，保障安全
   复杂环境中动态障碍物（行人、车辆、移动货物）是侦查机器人的核心挑战，动态避障需兼顾实时性、安全性与任务连续性：

分层避障逻辑：采用“全局路径规划→局部避障→应急响应”三层逻辑：全局规划确保路径大方向正确，案例2的模糊避障、案例4的DWA属于局部避障，实时处理动态障碍物，微调路径；应急层负责极端危险触发（如碰撞、环境危险），案例6的急停机制，保障机器人与数据安全。

避障算法适配场景：仓储环境用DWA动态窗口法，基于速度采样与评分函数，平衡朝向目标与避障需求；园区环境用模糊避障，通过规则库快速响应临时障碍物，无需精确建模；应急环境用RRT*在线规划，结合传感器数据实时生成可行路径，适配未知环境。

避障与任务的平衡：避障优先级高于路径跟踪，但不影响核心任务——如案例2中，避障后自动回归全局路径；案例3中，避障同时优先向生命迹象方向移动，确保侦查任务不中断。

4. BLDC电机的闭环控制：适配不同场景的运动需求
   侦查机器人需应对平坦巡航、爬坡越障、应急急停等不同运动场景，BLDC电机的闭环控制需满足多场景需求：

多环控制模式切换：案例4、5采用速度闭环控制，满足平坦环境的稳定巡航，通过FOC控制实现精准速度跟踪，避免速度波动导致路径偏移；案例6采用扭矩/速度混合控制，越障时切换至扭矩控制，提升电机输出扭矩应对爬坡、翻越障碍，正常行驶切换回速度控制，兼顾效率与动力。

运动稳定性优化：通过差速驱动解算，确保转向平稳；对电机目标速度进行限幅，避免速度突变导致机器人抖动；结合IMU姿态反馈，修正运动方向，如案例3中，侧向障碍时通过差速调整方向，实现平稳避障。

故障应急处理：电机失控、堵转等故障通过编码器反馈检测，触发急停机制，切断电机动力，同时通过远程上报故障信息，保障机器人安全。

5. 算力与实时性的平衡：适配Arduino平台的工程优化
   Arduino平台算力有限，需通过算法简化、硬件选型、任务分配平衡路径规划、避障的算力需求与实时性：

算法简化与适配：路径规划算法采用简化版本——A简化为栅格地图的直线路径规划，RRT简化为基于传感器的探索规则，避免复杂计算占用过多算力；模糊避障规则库简化为少量核心规则，提升计算效率，确保避障响应时间≤50ms。

硬件选型升级：放弃算力不足的Arduino Uno，选用ESP32、Mega等高性能平台——ESP32双核架构可将传感器融合、路径规划放在Core0，电机控制放在Core1，物理隔离保障电机控制的实时性，避免算力阻塞导致控制延迟。

任务分级与卸载：将计算密集型任务（如复杂A*、点云处理）卸载至外部模块（树莓派、Jetson），Arduino仅负责电机控制、基础传感器处理与执行层逻辑，通过串口接收外部规划结果，减轻自身算力负担，确保核心控制任务的实时性。

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