【花雕学编程】Arduino BLDC 之机器人园区巡逻跟随+路径优化——园区安防场景
Arduino BLDC 之园区巡逻跟随 + 路径优化的本质，是把“固定巡逻路径、动态跟随安保人员、实时避障与路径重规划”整合到一个分层闭环中：上位机负责地图、路径与目标识别，下位机负责 BLDC 差速闭环、跟随控制和局部避障。 它既要能按预设路线巡航，也要能在遇到行人、车辆、临时障碍或安保人员移动时平滑切换行为，而不是简单地在“巡逻”和“跟随”之间硬切换。
主要特点
. 巡逻跟随双模式协同
系统通常具备至少三种行为模式：预设路线巡逻、人员跟随、避障绕行。巡逻模式依赖全局路径和定位，保证覆盖园区重点点位；跟随模式则根据目标相对距离和方位角，输出线速度和角速度指令，使机器人保持安全跟随间距；当两种任务冲突时，由有限状态机决定优先级，例如避障优先、跟随次之、巡逻巡航最低。
. 全局路径优化降低无效绕行
园区环境通常存在长直道、路口、绕行区域和临时封闭路段。全局层可采用 A*、Dijkstra 或 Voronoi 骨架图生成巡逻路径，再通过拐点筛选、路径平滑和航点压缩减少冗余转向。 对于 Arduino 这类资源受限平台，更推荐由上位机计算全局路径，下位机只接收航点序列并执行路径跟踪。
. 局部避障采用 DWA 或动态分区策略
全局路径解决“去哪里”，局部规划解决“现在怎么走”。DWA 在速度空间内采样 (v, ω)，预测短时轨迹并评价安全性、目标趋近性和速度效率，适合处理突然出现的人员、电动车或临时堆放物。 若算力不足，也可采用动态分区避障：近距危险区触发急停，中距预警区触发局部重规划，远距安全区维持原路径。
. 跟随控制需融合目标感知与底盘闭环
跟随目标可通过 UWB/BLE 标签、视觉识别、激光雷达轮廓跟踪或超声波矩阵实现。 目标相对位姿通常包括距离、方位角和相对速度；控制器再将其转化为期望线速度和角速度。BLDC 差速底盘配合编码器或轮毂电机霍尔反馈，可实现左右轮独立速度闭环，使跟随轨迹更平滑。
. 分层架构适配 Arduino 生态
典型实现为：上位机运行 SLAM、全局规划、目标识别和任务调度；下位机运行 IMU 融合、里程计、DWA/跟随控制、BLDC FOC 或 PID 速度闭环。 Arduino Uno/Nano 适合做轻量执行节点；若加入激光雷达、视觉或 SLAM，建议使用 ESP32、STM32、树莓派或 Jetson 系列承担感知与规划。
应用场景
园区安防巡逻：按固定路线巡查围墙、出入口、停车场、设备间，发现异常时自动记录并上报。
安保人员伴随巡逻：机器人跟随安保人员移动，携带照明、对讲、摄像或检测设备，减少人员负重。
物业/厂区巡检：在工业园区、物流园区、校园、医院外围执行夜间巡逻、环境监测和设备点检。
临时活动保障：展会、赛事、大型活动现场人流变化快，机器人可在巡逻与跟随之间切换，兼顾秩序维护和服务支持。
教学与科研验证：适合验证 SLAM、路径优化、DWA 局部避障、跟随控制、BLDC 差速闭环和多传感器融合。
需要注意的事项
不要把全部算法压在 Arduino 上
SLAM、3D 点云处理、视觉识别和完整 DWA 同时运行时，AVR 系列 MCU 很难稳定支撑。建议采用上位机规划、下位机执行的架构，Arduino 只负责接收速度指令并执行 BLDC 闭环。
跟随与巡逻切换必须平滑
从巡逻切到跟随、从跟随切回巡逻时，不能直接替换目标点或速度指令，否则会出现急停、急转或来回振荡。切换前应进行航向对齐、速度渐变和状态确认，例如目标稳定锁定后再进入跟随模式。
目标丢失要有降级策略
跟随目标可能因遮挡、强光、人流干扰或信号丢失而暂时消失。系统应设置超时机制：短期丢失保持低速等待，长期丢失返回最近巡逻点或原地停机，而不是盲目追搜。
局部避障需兼顾安全与效率
DWA 的障碍物距离权重过低会贴障行驶，过高则容易停滞不前。园区人流密集时应提高安全权重并缩短预测窗口；空旷路段可适当提高速度权重。 同时应保留硬件级急停，防止软件异常导致失控。
户外环境要重点考虑定位与防护
园区场景存在 GPS 遮挡、玻璃幕墙反射、夜间低照度、雨雪和电磁干扰等问题。定位宜采用轮式里程计 + IMU + 激光雷达/视觉融合；外壳、接插件和电池舱应满足防水防尘要求；动力电源与控制电源需隔离，避免 BLDC 启动电流造成 MCU 复位。
1、状态机驱动的巡逻-跟随双模式切换（基础跟随逻辑）
适用场景：园区安防巡逻，机器人默认沿预设航点巡逻，检测到巡逻人员后自动切换跟随模式，保持安全距离跟随，目标丢失后恢复巡逻。核心逻辑参考“有限状态机管理双模式切换”。
/**
* 园区巡逻跟随机器人 - 状态机驱动双模式切换
* 硬件假设：ESP32/Arduino Due, 2×BLDC(SimpleFOC), UWB/视觉目标检测, 超声波
* 核心：巡逻模式 <-> 跟随模式，卡尔曼滤波平滑目标位置
*/
#include <SimpleFOC.h>
BLDCMotor motorL(7), motorR(7);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);
// ===== 状态定义 =====
enum RobotState { STATE_PATROL, STATE_FOLLOW, STATE_GUARD };
RobotState currentState = STATE_PATROL;
// ===== 跟随参数 =====
const float FOLLOW_DIST = 1.5; // 目标跟随距离 (m)
const float MIN_SAFE_DIST = 0.4; // 最小安全距离 (m)
const float TARGET_TIMEOUT = 5000; // 目标丢失超时 (ms)
// 目标位置（UWB或视觉输入）
float targetX = 0, targetY = 0;
unsigned long lastTargetSeen = 0;
// 巡逻航点
const int WAYPOINT_COUNT = 3;
float waypoints[WAYPOINT_COUNT][2] = {{2.0, 0}, {2.0, 2.0}, {0, 2.0}};
int currentWaypoint = 0;
// 超声波
const int SONAR_TRIG = 22, SONAR_ECHO = 23;
float measureDistance() {
digitalWrite(SONAR_TRIG, LOW); delayMicroseconds(2);
digitalWrite(SONAR_TRIG, HIGH); delayMicroseconds(10);
digitalWrite(SONAR_TRIG, LOW);
long dur = pulseIn(SONAR_ECHO, HIGH, 20000);
return (dur == 0) ? 999.0 : dur * 0.0343 / 2.0;
}
void setup() {
Serial.begin(115200);
pinMode(SONAR_TRIG, OUTPUT); pinMode(SONAR_ECHO, INPUT);
motorL.linkSensor(&encL); motorR.linkSensor(&encR);
motorL.init(); motorL.initFOC();
motorR.init(); motorR.initFOC();
motorL.controller = MotionControlType::velocity;
motorR.controller = MotionControlType::velocity;
}
// 跟随控制（距离+角度）
void executeFollow(float frontDist) {
// 1. 避障硬优先级
if (frontDist < MIN_SAFE_DIST && frontDist > 0) {
motorL.move(0); motorR.move(0);
Serial.println("跟随避障触发");
return;
}
// 2. 计算相对距离和角度
float dist = sqrt(targetX * targetX + targetY * targetY);
float angle = atan2(targetY, targetX) * 180 / PI;
// 3. 距离控制（P控制）
float distError = dist - FOLLOW_DIST;
float speed = constrain(distError * 0.5, -0.3, 0.8);
// 4. 角度控制（带死区防抖动）
if (abs(angle) < 3.0) angle = 0;
float turn = constrain(angle * 0.02, -0.5, 0.5);
// 5. 差速驱动
motorL.move(speed - turn);
motorR.move(speed + turn);
Serial.print("Follow Dist:"); Serial.print(dist);
Serial.print(" Angle:"); Serial.print(angle);
Serial.print(" Speed:"); Serial.println(speed);
}
// 巡逻控制（沿航点）
void executePatrol() {
float dx = waypoints[currentWaypoint][0] - 0; // 简化：假设当前位置(0,0)
float dy = waypoints[currentWaypoint][1] - 0;
float dist = sqrt(dx*dx + dy*dy);
if (dist < 0.3) {
currentWaypoint = (currentWaypoint + 1) % WAYPOINT_COUNT;
Serial.print("切换到航点:"); Serial.println(currentWaypoint);
return;
}
// 朝向航点
float targetAngle = atan2(dy, dx) * 180 / PI;
float turn = constrain(targetAngle * 0.02, -0.4, 0.4);
motorL.move(0.4 - turn);
motorR.move(0.4 + turn);
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
float frontDist = measureDistance();
// 状态机
switch (currentState) {
case STATE_PATROL:
executePatrol();
// 检测到目标（简化：串口输入）
if (Serial.available()) {
String cmd = Serial.readStringUntil('\n');
if (cmd == "TARGET") {
currentState = STATE_FOLLOW;
lastTargetSeen = millis();
Serial.println("切换到跟随模式");
}
}
break;
case STATE_FOLLOW:
executeFollow(frontDist);
// 目标丢失超时
if (millis() - lastTargetSeen > TARGET_TIMEOUT) {
currentState = STATE_PATROL;
Serial.println("目标丢失，恢复巡逻");
}
break;
case STATE_GUARD:
motorL.move(0); motorR.move(0);
break;
}
delay(20);
}
要点：该代码实现了巡逻-跟随双模式状态机。核心逻辑参考“无目标时自动巡逻，检测到目标时切换跟随，入侵检测时进入守卫模式”。跟随控制采用“距离P控制 + 角度死区”的组合，避免目标在正前方时机器人抖动。避障作为硬优先级，前方距离过近时无条件中断跟随。
2、UWB+视觉多模态抗遮挡跟随（动态环境）
适用场景：园区环境中巡逻人员可能被树木、车辆遮挡，单一传感器（UWB或视觉）会丢失目标。本案例采用多模态融合，当目标被遮挡时切换至惯性航位推算维持跟随，确保不丢失。
/**
* 园区巡逻跟随 - UWB+视觉多模态抗遮挡
* 核心：UWB与视觉位置融合 -> 遮挡检测 -> 航位推算维持跟随
*/
#include <SimpleFOC.h>
BLDCMotor motorL(7), motorR(7);
// ===== 传感器数据 =====
float uwbPos[2] = {0, 0}; // UWB目标位置
float visionPos[2] = {0, 0}; // 视觉目标位置
float fusedPos[2] = {0, 0}; // 融合位置
float odoX = 0, odoY = 0, odoYaw = 0; // 里程计
const float OCCLUSION_THRESH = 1.0; // 遮挡判定阈值 (m)
bool isOccluded = false;
// 目标丢失处理
unsigned long lastValidTarget = 0;
const float TARGET_TIMEOUT = 3000;
// 跟随参数
const float FOLLOW_DIST = 1.5;
void setup() {
Serial.begin(115200);
motorL.init(); motorR.init();
motorL.initFOC(); motorR.initFOC();
motorL.controller = MotionControlType::velocity;
motorR.controller = MotionControlType::velocity;
}
// 多模态抗遮挡融合
void fuseTargetPosition() {
// 1. 计算UWB与视觉的偏差
float distDiff = sqrt(pow(uwbPos[0]-visionPos[0], 2) + pow(uwbPos[1]-visionPos[1], 2));
// 2. 遮挡判定
isOccluded = (distDiff > OCCLUSION_THRESH);
if (!isOccluded) {
// 正常融合：加权平均
fusedPos[0] = 0.6 * uwbPos[0] + 0.4 * visionPos[0];
fusedPos[1] = 0.6 * uwbPos[1] + 0.4 * visionPos[1];
lastValidTarget = millis();
} else {
// 遮挡：切换至航位推算（基于上次有效位置+航向）
// 简化：沿机器人当前航向保持距离
fusedPos[0] = odoX + cos(odoYaw) * FOLLOW_DIST;
fusedPos[1] = odoY + sin(odoYaw) * FOLLOW_DIST;
Serial.println("遮挡：航位推算维持");
}
}
void executeFollow() {
float dx = fusedPos[0] - odoX;
float dy = fusedPos[1] - odoY;
float dist = sqrt(dx*dx + dy*dy);
float angle = atan2(dy, dx) * 180 / PI;
float speed = constrain((dist - FOLLOW_DIST) * 0.5, -0.2, 0.8);
if (abs(angle) < 3.0) angle = 0;
float turn = constrain(angle * 0.02, -0.5, 0.5);
motorL.move(speed - turn);
motorR.move(speed + turn);
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
// 模拟传感器更新（实际从串口/传感器读取）
// uwbPos, visionPos 应在此处更新
fuseTargetPosition();
// 目标超时检查
if (millis() - lastValidTarget > TARGET_TIMEOUT && !isOccluded) {
motorL.move(0); motorR.move(0);
Serial.println("目标丢失，停止");
return;
}
executeFollow();
// 更新里程计（简化）
odoX += 0.01;
odoY += 0.005;
Serial.print("Fused:"); Serial.print(fusedPos[0]); Serial.print(",");
Serial.print(fusedPos[1]); Serial.print(" Occluded:"); Serial.println(isOccluded);
delay(20);
}
要点：该案例实现了多模态抗遮挡跟随。核心逻辑参考“UWB+视觉多模态抗遮挡跟随策略，当目标被遮挡时切换至惯性航位推算维持跟随”。遮挡判定基于UWB与视觉位置的偏差——当两者差异超过阈值时，判定为视觉被遮挡。航位推算利用机器人自身里程计和航向，在目标暂时不可见时维持跟随方向，避免因单帧丢失而“急刹车”。
3、A全局路径规划 + DWA局部避障跟随（路径优化）
适用场景：园区复杂环境中，巡逻跟随需要全局最优路径与局部动态避障相结合。A提供从起点到各巡逻点的全局路径，DWA在跟随过程中实时避开行人、车辆等动态障碍物。
/**
* 园区巡逻跟随 - A*全局规划 + DWA局部避障
* 参考：A*生成全局路径节点 -> DWA以路径节点为临时目标
*/
#include <SimpleFOC.h>
BLDCMotor motorL(7), motorR(7);
// ===== 全局路径（简化：预定义节点） =====
#define PATH_LEN 5
float globalPath[PATH_LEN][2] = {
{0, 0}, {2, 0}, {2, 2}, {0, 2}, {0, 0} // 矩形巡逻路径
};
int pathIdx = 0;
// ===== DWA参数 =====
const float MAX_VEL = 0.5;
const float VEL_STEP = 0.1;
const float W_STEP = 0.2;
const float SIM_TIME = 1.5;
// 目标位置（UWB/视觉）
float targetX = 0, targetY = 0;
// 机器人状态
float robotX = 0, robotY = 0, robotTheta = 0;
// 超声波
const int SONAR_TRIG = 22, SONAR_ECHO = 23;
float frontDist = 999;
float measureDistance() {
digitalWrite(SONAR_TRIG, LOW); delayMicroseconds(2);
digitalWrite(SONAR_TRIG, HIGH); delayMicroseconds(10);
digitalWrite(SONAR_TRIG, LOW);
long dur = pulseIn(SONAR_ECHO, HIGH, 20000);
return (dur == 0) ? 999.0 : dur * 0.0343 / 2.0;
}
// DWA评价
float evaluateTrajectory(float v, float w, float goalX, float goalY) {
float simX = robotX, simY = robotY, simTheta = robotTheta;
for (float t = 0; t < SIM_TIME; t += 0.1) {
simTheta += w * 0.1;
simX += v * cos(simTheta) * 0.1;
simY += v * sin(simTheta) * 0.1;
}
// 碰撞检查
if (frontDist < 0.2 && v > 0.1) return -9999;
// 朝向目标
float dx = goalX - simX;
float dy = goalY - simY;
float heading = atan2(dy, dx);
float headingDiff = fabs(heading - simTheta);
if (headingDiff > PI) headingDiff = 2*PI - headingDiff;
float headingScore = 1.0 - headingDiff / PI;
// 障碍物距离
float distScore = min(frontDist / 1.0, 1.0);
// 速度
float velScore = v / MAX_VEL;
return 0.4 * headingScore + 0.4 * distScore + 0.2 * velScore;
}
// DWA规划
void dwaPlan(float goalX, float goalY, float &bestV, float &bestW) {
bestV = 0; bestW = 0;
float bestScore = -9999;
for (float v = 0.1; v <= MAX_VEL; v += VEL_STEP) {
for (float w = -1.0; w <= 1.0; w += W_STEP) {
float score = evaluateTrajectory(v, w, goalX, goalY);
if (score > bestScore) {
bestScore = score;
bestV = v;
bestW = w;
}
}
}
}
void setup() {
Serial.begin(115200);
pinMode(SONAR_TRIG, OUTPUT); pinMode(SONAR_ECHO, INPUT);
motorL.init(); motorR.init();
motorL.initFOC(); motorR.initFOC();
motorL.controller = MotionControlType::velocity;
motorR.controller = MotionControlType::velocity;
}
void loop() {
motorL.loopFOC(); motorR.loopFOC();
// 1. 感知
frontDist = measureDistance();
// 2. 获取当前全局路径目标点
float goalX = globalPath[pathIdx][0];
float goalY = globalPath[pathIdx][1];
// 3. 到达判断
float distToGoal = sqrt(pow(goalX - robotX, 2) + pow(goalY - robotY, 2));
if (distToGoal < 0.3) {
pathIdx = (pathIdx + 1) % PATH_LEN;
Serial.print("切换到路径点:"); Serial.println(pathIdx);
}
// 4. DWA局部规划（朝向路径点，避障）
float bestV, bestW;
dwaPlan(goalX, goalY, bestV, bestW);
// 5. 差速执行
float vL = bestV - bestW * 0.15;
float vR = bestV + bestW * 0.15;
motorL.move(vL / 0.05);
motorR.move(vR / 0.05);
// 6. 更新位姿
robotTheta += bestW * 0.05;
robotX += bestV * cos(robotTheta) * 0.05;
robotY += bestV * sin(robotTheta) * 0.05;
Serial.print("V:"); Serial.print(bestV);
Serial.print(" W:"); Serial.print(bestW);
Serial.print(" Goal:"); Serial.print(pathIdx);
Serial.print(" Dist:"); Serial.println(frontDist);
delay(50);
}
要点：该案例实现了全局A* + 局部DWA的分层导航架构。参考“A*生成全局路径节点，DWA以路径节点为临时目标，动态重规划”。全局路径提供宏观引导，DWA的heading评分不指向最终目标而是指向路径上的下一个节点。当DWA检测到新障碍物时，通过评价函数的碰撞检查项自动选择绕行轨迹，实现“全局粗规划 + 局部精避障”。
要点解读
-
巡逻跟随的核心是“状态机管理”，而非简单的“检测到就跟随”。 园区安防场景需要三种状态协同：巡逻模式（无目标时沿航点巡航）、跟随模式（检测到巡逻人员后保持距离）、守卫模式（检测到入侵时停止并报警）。状态切换必须有明确的触发条件和超时机制（如目标丢失5秒后恢复巡逻），否则机器人会在“跟随-巡逻”之间频繁抖动。
-
多模态抗遮挡是园区环境的必要设计，单一传感器不可靠。 园区中树木、车辆、建筑遮挡频繁，纯视觉方案在遮挡时丢失目标，纯UWB方案在非视距环境下精度下降。UWB+视觉融合 + 航位推算兜底的三层架构是务实选择：正常时融合两者，遮挡时切换航位推算，完全丢失时停止等待。遮挡判定可基于两种传感器位置的偏差程度。
-
全局路径规划与局部避障必须分层，不能混为一谈。 A*负责“从起点到巡逻点的最优路线”，DWA负责“沿路线行驶时避开动态障碍”。如果只用DWA朝向最终目标，在复杂园区中会陷入局部最优（如被建筑挡住去路，DWA只会反复尝试绕行而无法找到正确方向）。分层架构让DWA朝向“路径上的下一个节点”而非最终目标，避免了局部最优问题。
-
避障是硬优先级，必须在跟随控制之前检查。 跟随逻辑无论多么精密，前方有障碍物时必须让位于安全。代码中的 if (frontDist < MIN_SAFE_DIST) { stop; return; } 体现了这一原则。关键细节：避障触发后不能立即恢复跟随，应等待障碍物离开安全距离并保持短暂延时（如500ms），防止机器人在障碍物边缘反复启停。
-
园区巡逻的路径优化应关注“覆盖效率”而非“最短路径”。 安防巡逻的目标是最大化区域覆盖，而非最小化行驶距离。简单的A*最短路径可能导致某些区域被反复经过而另一些区域被忽略。实用的优化策略包括：使用“巡逻点轮换”机制（每次巡逻顺序不同）、引入“覆盖度评分”（优先前往近期未巡逻的区域）、多机器人协同时的负载均衡。
4、园区基础巡逻跟随+简单路径优化程序（核心：人员跟随+基础巡逻）
适用场景：园区日常巡逻，需跟随安保人员或预设路线，实现基础跟随与路径规划，适配开阔园区环境。
核心逻辑
通过射频识别或红外跟随，实时锁定人员位置，调整行驶方向与速度
简化路径规划，避开固定障碍，实现巡逻路线往返循环
结合巡逻状态切换，没有人员时自动巡逻，有人跟随时切换跟随模式。
#include <Arduino.h>
// ========== 硬件定义 ==========
const byte PWM_LEFT = 9, DIR_LEFT = 8;
const byte PWM_RIGHT = 10, DIR_RIGHT = 11;
const byte FOLLOW_PIN = 2; // 人员跟随传感器（红外/RFID）
const byte OBSTACLE_PIN = 3;// 简易障碍检测
const byte LED_ALARM = 13; // 巡逻状态指示灯
// ========== 参数定义 ==========
const float TARGET_FOLLOW_DIST = 1.2; // 跟随距离，单位米
const int MOVE_BASE_PWM = 120;
const int CORE_CHECK_INTERVAL = 30; // 核心检测周期
// ========== 状态变量 ==========
bool followMode = false;
bool patrolMode = true;
int patrolWaypoints[4][2] = {{0,0},{5,0},{5,5},{0,5}}; // 巡逻点
int currentWaypoint = 0;
unsigned long lastTime = 0;
// ========== 函数声明 ==========
bool detectPerson();
bool detectObstacle();
void adjustFollowSpeed(float dist);
void pathOptimize();
void driveMotor(int leftPWM, int rightPWM);
void setup() {
Serial.begin(115200);
Serial.println("园区巡逻跟随系统启动");
pinMode(PWM_LEFT, OUTPUT); pinMode(DIR_LEFT, OUTPUT);
pinMode(PWM_RIGHT, OUTPUT); pinMode(DIR_RIGHT, OUTPUT);
pinMode(FOLLOW_PIN, INPUT_PULLUP);
pinMode(OBSTACLE_PIN, INPUT_PULLUP);
pinMode(LED_ALARM, OUTPUT);
}
void loop() {
unsigned long dt = millis() - lastTime;
if (dt < CORE_CHECK_INTERVAL) return;
lastTime = millis();
// 检测人员与障碍bool person = detectPerson();
bool obstacle = detectObstacle();
// 模式切换
if (person) {
followMode = true;
patrolMode = false;
} else {
followMode = false;
patrolMode = true;
}
int leftPWM = MOVE_BASE_PWM, rightPWM = MOVE_BASE_PWM;
if (obstacle) {
// 简易避障，反向微转变向
rightPWM = MOVE_BASE_PWM - 40;
leftPWM = MOVE_BASE_PWM - 20;
} else if (followMode) {
// 跟随人员，调整速度
float dist = analogRead(FOLLOW_PIN) * 0.02; // 模拟距离
adjustFollowSpeed(dist);
leftPWM = MOVE_BASE_PWM + 10;
rightPWM = MOVE_BASE_PWM + 10;
} else if (patrolMode) {
// 路径优化后的巡逻行驶
pathOptimize();
leftPWM = MOVE_BASE_PWM;
rightPWM = MOVE_BASE_PWM;
}
driveMotor(leftPWM, rightPWM);
digitalWrite(LED_ALARM, followMode ? HIGH : LOW);
Serial.print("模式：");Serial.print(followMode ? "跟随" : "巡逻");
Serial.print(" 障碍：");Serial.println(obstacle);
}
// 人员检测
bool detectPerson() {
return digitalRead(FOLLOW_PIN) == LOW; // 传感器检测到人员返回true
}// 障碍检测
bool detectObstacle() {
return digitalRead(OBSTACLE_PIN) == LOW;
}// 跟随速度自适应调整
void adjustFollowSpeed(float dist) {
if (dist < TARGET_FOLLOW_DIST*0.8) {
// 距离过近，减速
MOVE_BASE_PWM = 80;
} else if (dist > TARGET_FOLLOW_DIST*1.2) {
// 距离过远，加速
MOVE_BASE_PWM = 150;
} else {
MOVE_BASE_PWM = 120;
}
}
// 简易路径优化，减少巡逻折返
void pathOptimize() {
// 根据当前位置自动切换巡逻点，简化为顺序循环
currentWaypoint = (currentWaypoint + 1) % 4;
Serial.print("前往巡逻点：");Serial.print(currentWaypoint);
}// 电机驱动
void driveMotor(int leftPWM, int rightPWM) {
leftPWM = constrain(leftPWM, 0, 200);
rightPWM = constrain(rightPWM, 0, 200);
digitalWrite(DIR_LEFT, HIGH);
digitalWrite(DIR_RIGHT, HIGH);
analogWrite(PWM_LEFT, leftPWM);
analogWrite(PWM_RIGHT, rightPWM);
}
适用场景优化
加入GPS/北斗定位，实现精确点位巡逻与自主路径规划
优化跟随传感器，采用视觉或激光跟随，提升人员识别准确性
增加巡逻记录功能，自动记录巡逻时间、路线，便于安防管理
5、安防异常联动巡逻跟随程序（核心：异常触发+紧急调整）
适用场景：园区安防重点区域，需具备异常检测能力，发现入侵、火灾等异常时，自动调整路径并报警。
核心逻辑
加入安防异常检测模块，实时识别入侵、烟雾等异常
异常触发时，自动切换紧急模式，优化路径前往异常点
跟随安保人员同时监测异常，实现人机协同的安全防控。
#include <Arduino.h>
// ========== 硬件定义 ==========
const byte PWM[2] = {9,10}, DIR[2] = {8,11};
const byte FOLLOW_PIN = 2;
const byte PIR_PIN = 3; // 人体入侵传感器
const byte SMOKE_PIN = 4; // 烟雾传感器
const byte ALARM_PIN = 12; // 报警器
const byte LED_PIN = 13;
// ========== 参数定义 ==========
const int BASE_SPEED = 130;
const int EMERGENCY_SPEED = 180;
const byte CHECK_INTERVAL = 25;
// ========== 状态变量 ==========
bool alertMode = false;
bool followMode = false;
int alertPos[2] = {0,0}; // 异常位置
unsigned long lastTime = 0;
// ========== 函数声明 ==========
bool detectAlert();
void getAlertPosition();
void emergencyPath();
void driveMotor(uint8_t idx, int pwm);
void setup() {
Serial.begin(115200);
Serial.println("安防异常联动系统启动");
for (uint8_t i = 0; i < 2; i++) {
pinMode(PWM[i], OUTPUT); pinMode(DIR[i], OUTPUT);
}
pinMode(FOLLOW_PIN, INPUT_PULLUP);
pinMode(PIR_PIN, INPUT_PULLUP);
pinMode(SMOKE_PIN, INPUT);
pinMode(ALARM_PIN, OUTPUT);
pinMode(LED_PIN, OUTPUT);
}
void loop() {
unsigned long dt = millis() - lastTime;
if (dt < CHECK_INTERVAL) return;
lastTime = millis();
// 安防异常检测
alertMode = detectAlert();
if (alertMode) {
digitalWrite(ALARM_PIN, HIGH);
digitalWrite(LED_PIN, HIGH);
// 切换紧急路径，前往异常点
int pwm = EMERGENCY_SPEED;
driveMotor(0, pwm);
driveMotor(1, pwm);
// 路径优化，直达异常点
emergencyPath();
} else {
digitalWrite(ALARM_PIN, LOW);
digitalWrite(LED_PIN, LOW);
// 正常跟随或巡逻
bool person = digitalRead(FOLLOW_PIN) == LOW;
if (person) {
followMode = true;
driveMotor(0, BASE_SPEED);
driveMotor(1, BASE_SPEED);
} else {
followMode = false;
// 常规巡逻
driveMotor(0, BASE_SPEED - 10);
driveMotor(1, BASE_SPEED - 10);
}
}
Serial.print("预警：");Serial.println(alertMode);
}
// 异常检测，入侵或烟雾
bool detectAlert() {
bool pirAlert = digitalRead(PIR_PIN) == LOW;
bool smokeAlert = analogRead(SMOKE_PIN) > 600;
return pirAlert || smokeAlert;
}
// 获取异常位置，简化为固定点，实际结合定位
void getAlertPosition() {
alertPos[0] = 4; alertPos[1] = 3;
}
// 紧急路径优化，直达异常点
void emergencyPath() {
getAlertPosition();
// 简化逻辑，根据当前位置调整方向
Serial.print("前往异常点：");Serial.print(alertPos[0]);Serial.print(",");Serial.println(alertPos[1]);
}
// 电机驱动
void driveMotor(uint8_t idx, int pwm) {
pwm = constrain(pwm, 0, 200);
digitalWrite(DIR[idx], HIGH);
analogWrite(PWM[idx], pwm);
}
适用场景优化
加入异常定位功能，结合传感器与定位系统，精准锁定异常点位
添加异常优先级，区分严重与轻微异常，分级调整响应策略
联动后台管理系统，异常发生时自动推送报警信息，实现远程监控
6、多机协同巡逻+跟随路径优化程序（核心：多机联动+高效巡逻）
适用场景：大型园区，需多台机器人协同巡逻，划分区域、配合跟进，提升巡逻覆盖效率。
核心逻辑
主从机通信，实现多机位置同步与任务分配
主机负责路线规划，从机跟随主机，覆盖不同区域
结合区域划分，优化多机巡逻路径，避免重叠，提升效率。
#include <Arduino.h>
// ========== 硬件定义 ==========
const byte PWM[2] = {9,10}, DIR[2] = {8,11};
const byte COM_PIN = 2; // 串口通信引脚
const byte FOLLOW_PIN = 3; // 主机跟随检测
const byte ROLE_PIN = 4; // 角色定义（主机/从机）
// ========== 参数定义 ==========
const int BASE_SPEED = 120;
const byte COM_INTERVAL = 40;
bool isMaster = false;
int slavePos[2] = {0,0};// 从机位置
// ========== 函数声明 ==========
void readRole();
void masterPlan();
void slaveFollow();
void pathOptimizeMulti();
void driveMotor(uint8_t idx, int pwm);
void setup() {
Serial.begin(9600);
Serial.println("多机协同巡逻系统启动");
for (uint8_t i = 0; i < 2; i++) {
pinMode(PWM[i], OUTPUT); pinMode(DIR[i], OUTPUT);
}
pinMode(COM_PIN, INPUT);
pinMode(FOLLOW_PIN, INPUT_PULLUP);
pinMode(ROLE_PIN, INPUT_PULLUP);
readRole();
}
void loop() {
if (millis() % COM_INTERVAL != 0) return;
if (isMaster) {
// 主机：路径规划+通信发送
masterPlan();
pathOptimizeMulti();
// 向从机发送位置与指令
Serial.print("MP:");Serial.print("1,2");Serial.println(";");
} else {
// 从机：接收主机信息+跟随
slaveFollow();
}
// 共同行驶
int pwm = BASE_SPEED;
driveMotor(0, pwm);
driveMotor(1, pwm);
}
// 读取机器人角色
void readRole() {
isMaster = digitalRead(ROLE_PIN) == HIGH;
Serial.print("角色：");Serial.println(isMaster ? "主机" : "从机");
}
// 主机路径规划
void masterPlan() {
Serial.println("主机执行全局路径规划");
}
// 从机跟随主机
void slaveFollow() {
// 简化：接收主机数据并跟随
Serial.println("从机同步主机位置，执行跟随");
}void 多机路径优化，划分区域避免重叠
void pathOptimizeMulti() {
// 简化区域划分，主机负责北区，从机负责南区
Serial.println("路径优化：多机分区巡逻，避免重复覆盖");
}
// 电机驱动
void driveMotor(uint8_t idx, int pwm) {
pwm = constrain(pwm, 0, 200);
digitalWrite(DIR[idx], HIGH);
analogWrite(PWM[idx], pwm);
}
适用场景优化
优化通信协议，实现高速稳定的多机数据交互，降低延迟
加入动态区域分配，根据机器人位置实时调整巡逻区域，提升灵活性
增加多机避障逻辑，避免机器人之间碰撞，保障协同安全
要点解读
要点1：跟随定位精准度决定跟随稳定性
跟随是园区巡逻的核心功能，定位精准才能实现稳定跟随。
多传感器融合定位：结合红外、激光、视觉与定位系统，提升人员位置检测精度，减少跟随偏差
动态速度匹配：根据跟随距离实时调整车速，过近减速、过远加速，保持稳定跟随距离
抗干扰设计：园区环境人流、障碍物多，需对传感器数据滤波，避免误跟随、错跟随
要点2：路径优化需兼顾效率与覆盖
路径优化的核心是提升巡逻效率，同时保证园区全面覆盖。
全局与局部结合：主机负责全局区域划分与宏观路径规划，从机负责局部跟随，实现高效协同
减少无效折返：通过合理规划巡逻点与路线，减少重复路段，提升巡逻覆盖效率
动态调整路径：遇到异常或人员时，临时优化路径，优先处理安防事件，兼顾灵活与高效
要点3：安防异常联动需快速响应
园区安防的核心是快速发现并处理异常，联动机制至关重要。
多异常检测融合：集成入侵、烟雾、火灾等安防传感器，全方位监测异常，避免漏检
分级响应机制：区分异常等级，紧急异常高速前往，轻微异常降速核查，提升响应合理性
人机协同联动：异常时自动调整路径，同时联动安保人员，实现人机协同的快速处置
要点4：多机协同需保障同步与避碰
多机器人协同可大幅提升巡逻效率，需解决同步、避碰与调度问题。
时钟同步：主机与从机统一控制周期，保障数据同步与动作一致性，避免任务冲突
通信稳定可靠：采用稳定的通信协议，确保位置、指令准确传输，避免通信中断导致失控
协同避碰规则：预设多机器人避碰逻辑，避免相互碰撞，保障协同过程安全
要点5：系统稳定与续航保障，适配长时间巡逻
园区巡逻多为长时间作业，需保障系统稳定与续航能力。
低功耗设计：优化电机与传感器功耗，采用节能模式，延长单次充电续航时间
故障自检与容错：动态监测系统状态，发现异常及时降速或返航，避免任务中断
续航智能管理：电量过低时自动规划返航路径，返回充电，保障巡逻任务持续进行
请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
2
0- 0
已为社区贡献421条内容
所有评论(0)