Arduino BLDC电力巡检机器人分层融合架构的核心在于：以编码器为高频内部基准、GPS为低频全局锚点、超声波为近距安全屏障，通过分层递进融合实现“室外大范围精准巡航+近距可靠避障”，在Arduino有限算力下兼顾定位精度、实时安全与系统鲁棒性。

一、系统架构与核心原理
该系统采用典型的"三层分层融合"架构，各层职责明确、更新频率递进，形成从底层执行到顶层决策的完整闭环：
第一层：编码器里程计层（高频内部感知，100Hz级）
编码器安装在BLDC驱动轮上，通过脉冲计数实时计算左右轮的转速和累计转角，基于差速运动学模型（v = (v_r + v_l) / 2，ω = (v_r - v_l) / L）推算机器人的相对位移和航向角变化。
优势：更新频率高（可达100Hz以上），短期精度好，不受外部环境影响，是底层BLDC速度闭环控制的直接反馈信号。
局限：误差随时间累积（车轮打滑、地面不平、轮胎磨损均会引入偏差），长时间运行后定位会严重漂移。
第二层：GPS全局定位层（低频外部校正，1~10Hz级）
GPS模块（如NEO-6M/NEO-8M）提供WGS84坐标系下的绝对经纬度、速度和航向信息，作为全局位置锚点，周期性校正编码器里程计的累积漂移。
优势：提供绝对位置，无累积误差，适合室外大范围巡检场景。
局限：更新频率低（通常1~10Hz），精度受卫星数量和信号遮挡影响（HDOP值越高精度越差），在变电站构架密集区、隧道、林荫下可能失锁。
第三层：超声波近距安全层（实时避障，响应时间ms级）
超声波传感器阵列（通常前后各3~5只，覆盖全向视野）提供近距离障碍物距离信息，作为安全屏障独立于定位系统运行。
优势：不受光照和颜色影响，对透明物体（如玻璃柜门）也能检测，成本低，响应快。
局限：存在测量盲区（通常50mm以内），声锥角导致角度分辨率低，易受强风、高温气流和电磁干扰影响。
融合策略：分层递进，各司其职
三种传感器并非简单叠加，而是按频率和职责分层协同：
编码器在两个GPS更新周期之间提供高频位姿插值，保证BLDC控制回路的实时性；
GPS在每次有效更新时，通过EKF或互补滤波校正编码器的累积漂移，将机器人"拉回"正确的全局位置；
超声波不参与定位计算，而是作为独立的安全通道——当任一传感器检测到障碍物距离低于阈值时，直接触发BLDC减速或急停，优先级高于所有导航指令。

二、主要特点
. 异构传感器的频率互补
这是该架构最核心的设计哲学。三种传感器的数据更新频率相差1~2个数量级：
编码器：100Hz级（每10ms更新一次位姿增量）
GPS：1_{10Hz级（每100}1000ms更新一次绝对位置）
超声波：按需触发（障碍物出现时即时响应）
分层融合的本质是用高频数据保证实时性，用低频数据保证全局准确性，用安全层保证底线可靠性。在Arduino平台上，通常采用简化版EKF或互补滤波实现——编码器数据作为预测步（Prediction），GPS数据作为更新步（Update），超声波数据作为独立的安全中断。
. GPS失锁降级机制
电力巡检场景的特殊性在于：变电站内金属构架密集、高压线产生电磁干扰，GPS信号可能频繁失锁。系统需设计多级降级策略：
GPS有效时（HDOP < 2.0）：编码器+GPS联合定位，精度可达亚米级；
GPS信号弱时（HDOP ≥ 2.0）：降低GPS权重，主要依赖编码器航位推算，辅以航向平滑；
GPS完全失锁时：切换至纯编码器航位推算模式，同时提高超声波避障灵敏度，降低巡航速度以确保安全。
. BLDC闭环驱动与编码器深度耦合
BLDC电机的高效率（>85%）、低噪声和高功率密度使其成为巡检机器人的理想动力源。编码器不仅服务于定位，更是BLDC速度闭环的直接反馈——通过PID控制器实时调节PWM占空比，补偿地面摩擦变化和负载波动，确保左右轮速度严格跟随指令，减少因轮速不一致导致的航向偏差。
. 超声波阵列的全向安全覆盖
工业级巡检机器人通常在前后各安装3~5只超声波传感器，通过大声锥模式覆盖整个前方视野。运行方向上任何一只传感器的测量值低于设定阈值（如1000mm），机器人立即停车防撞。部分方案还支持IO-Link接口动态调整声锥大小，实现更精准的定位与控制。
. 轻量级但鲁棒的算法选型
受限于Arduino的计算资源，系统通常不运行完整的SLAM或粒子滤波，而是采用：
互补滤波：计算量最小，适合入门级Arduino，用加权平均融合编码器高频数据和GPS低频数据；
简化版EKF：在ESP32等高性能MCU上可运行，对非线性运动模型进行一阶线性化近似；
航向平滑：移动平均或低通滤波消除GPS航向跳变。

三、应用场景
. 变电站室外巡检
最典型的应用场景。机器人在变电站内按预设航点（Waypoints）自主巡航，GPS提供全局定位引导机器人到达各设备检测点，编码器保证航段间的平滑行驶，超声波在靠近变压器、开关柜等设备时提供近距防撞保护。适用于日常设备外观检查、仪表读数采集、红外测温等任务。
. 输电线路走廊巡检
在长距离输电线路走廊中，GPS的全局定位能力尤为关键。机器人沿预设路径行驶，编码器提供里程计保证路径跟踪精度，超声波检测走廊内的树木侵入、施工机械等动态障碍物。适用于线路通道巡查、树障检测等场景。
. 光伏电站巡检
光伏电站面积大、设备排列规则，GPS可高效引导机器人在光伏板阵列间穿梭。编码器保证在板间窄道中的精确行驶，超声波检测光伏板支架、线缆桥架等低矮障碍物。适用于光伏板热斑检测、灰尘积聚评估等任务。
. 电厂厂区巡检
电厂厂区环境复杂，包含锅炉房外围、冷却塔区域、输煤栈桥等。GPS在开阔区域提供全局定位，编码器在GPS信号受建筑遮挡时维持航位推算，超声波在狭窄通道和管廊入口提供安全保障。
. 教育科研与竞赛平台
作为移动机器人导航与多传感器融合的教学实验平台，演示异构传感器分层融合、EKF/互补滤波、BLDC闭环控制和GPS航点跟踪等核心技术。适用于高校机器人课程设计和智能车竞赛。

四、需要注意的事项
. GPS信号质量与精度校验
HDOP阈值管理：必须对GPS数据做有效性校验，仅当HDOP < 2.0时才将GPS数据纳入融合计算，否则低质量GPS数据反而会"拉偏"编码器里程计，造成更大误差。
冷启动延迟：GPS模块冷启动后需要30秒~数分钟才能锁定足够卫星，在此期间系统应仅依赖编码器航位推算，避免使用不稳定的初始GPS数据。
多径效应：变电站内金属构架密集，GPS信号经反射后产生多径干扰，导致定位跳变。建议采用抗多径的GPS模块（如支持GLONASS+GPS双星座的NEO-M8N），并在软件中加入位置跳变检测——若GPS报告的位置变化量超过物理可能的最大速度，则丢弃该数据点。
. 编码器累积误差的抑制
车轮打滑：湿滑地面、砂石路面会导致车轮打滑，编码器记录的里程大于实际位移。建议通过BLDC电流监测间接判断打滑状态（电流异常升高但速度未相应增加），此时降低编码器在融合中的权重。
轮径校准：轮胎磨损、气压变化会改变有效轮径，直接影响里程计精度。需定期校准车轮半径和轮距参数，否则位姿计算误差会持续累积。
编码器分辨率：低分辨率编码器在低速行驶时脉冲数过少，速度估算抖动大。建议选择高分辨率编码器（如每转1000线以上），或在软件中加入脉冲插值算法。
. 超声波传感器的干扰与局限
串扰问题：多只超声波传感器同时工作时，相邻传感器的声波可能互相干扰。需采用分时触发（依次发射）或同步触发+编码区分的方式消除串扰。
温度补偿：超声波传播速度受温度影响（v = 331.4 + 0.6T m/s），在室外巡检场景中温差可达40℃以上，距离测量误差可达数厘米。建议在系统中集成温度传感器，实时修正声速参数。
盲区与假阳性：超声波存在50mm左右的测量盲区，且对吸声材料（如泡沫、布料）检测能力弱。在安全关键区域（如悬崖边缘、高压设备附近），需增加红外或激光传感器作为补充。
. 分层融合的时间同步
三种传感器的数据更新频率差异巨大，时间同步是融合精度的关键：
时间戳对齐：每个传感器数据必须打上精确时间戳，融合算法根据时间戳进行插值和对齐，避免将不同时刻的数据错误关联。
延迟补偿：GPS模块从接收信号到输出数据通常有100~300ms的处理延迟，编码器数据几乎无延迟。融合算法需对GPS数据进行延迟补偿，否则在机器人高速运动时，GPS报告的位置实际上是"过去"的位置，会导致校正方向错误。
Arduino定时器管理：严禁使用delay()阻塞主循环，必须采用millis()非阻塞定时或硬件定时器中断，确保各传感器数据按时采集、不丢帧。
. 电磁兼容（EMC）设计
电力巡检场景的电磁环境极为恶劣——高压设备产生的强电场和电磁脉冲可能干扰传感器信号和通信链路：
电源隔离：BLDC电机供电与传感器/主控供电必须通过隔离DC-DC模块分开，避免电机启停时的电压波动影响GPS和编码器信号。
信号屏蔽：编码器信号线、超声波信号线应采用屏蔽双绞线，PCB布局将动力区与信号区物理隔离。
通信抗干扰：Arduino与上位机之间的UART/CAN通信需加入CRC校验和重传机制，防止电磁干扰导致数据错误。
. 安全冗余与故障降级
电力巡检场景对安全性要求极高，系统必须具备多级安全冗余：
硬件急停：超声波传感器直连BLDC驱动器的使能端，当检测到障碍物时绕过软件直接切断电机动力，确保在软件崩溃时也能紧急停车。
看门狗定时器：加入硬件看门狗，当程序跑飞时自动重启，防止机器人失控。
传感器故障检测：实时监测各传感器数据合理性（如编码器脉冲丢失、GPS位置跳变、超声波读数恒为零），检测到故障时自动切换至降级模式并上报告警。
. Arduino平台算力与架构选择
标准Arduino（Uno/Nano）：16MHz、2KB RAM，仅能运行简单的互补滤波和基础PID控制，适合教学演示。
ESP32：双核240MHz、520KB RAM，可运行简化版EKF，且双核架构支持一核处理传感器融合、另一核处理BLDC电机控制，实现真正的并行实时处理。
推荐分层架构：ESP32负责GPS数据解析、EKF融合计算和航点导航决策；Arduino Mega或专用BLDC驱动板负责编码器脉冲采集、电机FOC/PID闭环和超声波触发控制。两者通过UART或CAN总线通信，各司其职。

五、系统协同关系总结
编码器里程计（100Hz，高频相对位姿，短期精确但长期漂移）→ GPS全局校正（1~10Hz，低频绝对位置，无漂移但受遮挡影响）→ EKF/互补滤波融合（中频融合输出，兼顾实时性与全局精度）→ BLDC差速驱动执行（编码器闭环速度控制，保证轨迹跟踪精度）→ 超声波安全层（独立实时通道，ms级响应，优先级最高）
三者形成"内部高频基准 + 外部低频锚点 + 近距安全屏障"的分层互补体系——编码器解决"短时间内的精确运动控制"，GPS解决"长时间运行的全局定位不漂移"，超声波解决"突发障碍物的即时安全保护"。这一架构在Arduino有限算力下，以最小的计算开销实现了电力巡检场景所需的定位精度、导航鲁棒性和运行安全性。

1、GPS + 编码器 + 超声波多源数据融合定位
此案例实现电力巡检机器人的基础定位与感知融合。GPS提供绝对位置基准，编码器里程计提供高频相对位移，超声波负责局部障碍检测。三者通过简单的优先级与互补机制融合，即使在GPS信号短暂丢失时也能维持短时定位能力。

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

// ===== 硬件定义 =====
// BLDC差速底盘电机
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM drvL(9,10,11), drvR(5,6,8);

// GPS
TinyGPSPlus gps;
SoftwareSerial gpsSerial(4, 3);

// 编码器（左右轮）
Encoder encL(2, 3);
Encoder encR(4, 5);

// 超声波（前方避障）
#define TRIG_PIN 12
#define ECHO_PIN 13
NewPing sonar(TRIG_PIN, ECHO_PIN, 400);

// ===== 融合定位参数 =====
// 里程计变量
float odomX = 0.0, odomY = 0.0, odomHeading = 0.0;
float lastLeftTicks = 0, lastRightTicks = 0;
const float TICK_PER_METER = 4000.0 / (3.14159 * 0.1); // 编码器每米脉冲数
const float WHEEL_BASE = 0.35;

// GPS权重（GPS有效时用于校正里程计漂移）
const float GPS_WEIGHT = 0.3;
const float ODOM_WEIGHT = 0.7;

// ===== 传感器数据更新 =====
void updateOdometry() {
    // 1. 读取编码器增量
    long leftTicks = encL.read();
    long rightTicks = encR.read();
    float dLeft = (leftTicks - lastLeftTicks) / TICK_PER_METER;
    float dRight = (rightTicks - lastRightTicks) / TICK_PER_METER;
    lastLeftTicks = leftTicks;
    lastRightTicks = rightTicks;

    // 2. 差速底盘里程计更新
    float dCenter = (dLeft + dRight) / 2.0;
    float dTheta = (dRight - dLeft) / WHEEL_BASE;
    odomX += dCenter * cos(odomHeading);
    odomY += dCenter * sin(odomHeading);
    odomHeading += dTheta;
}

// ===== GPS校正（当GPS有效时融合）=====
void fuseGPS() {
    if (!gps.location.isValid()) return;

    // 将GPS经纬度转换为局部坐标（需UTM转换，此处简化）
    float gpsX = (gps.location.lng() - 116.4) * 111320.0;  // 粗略转换
    float gpsY = (gps.location.lat() - 39.9) * 110540.0;

    // 互补融合：里程计高频、GPS低频校正
    odomX = ODOM_WEIGHT * odomX + GPS_WEIGHT * gpsX;
    odomY = ODOM_WEIGHT * odomY + GPS_WEIGHT * gpsY;
}

void setup() {
    Serial.begin(115200);
    gpsSerial.begin(9600);

    // BLDC电机初始化（速度闭环）
    drvL.init(); drvR.init();
    motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
}

void loop() {
    // 1. GPS数据更新
    while (gpsSerial.available()) {
        gps.encode(gpsSerial.read());
    }

    // 2. 里程计更新（高频）
    updateOdometry();

    // 3. GPS校正（低频）
    if (millis() % 500 < 10) {  // 每500ms融合一次
        fuseGPS();
    }

    // 4. 超声波测距（避障优先）
    int frontDist = sonar.ping_cm();

    // 5. 导航决策：避障或前行
    if (frontDist > 0 && frontDist < 30) {
        // 紧急制动并转向
        motorL.target = -0.3;
        motorR.target = 0.3;
    } else {
        // 正常前进（沿规划路径）
        motorL.target = 0.5;
        motorR.target = 0.5;
    }

    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC();
    motorR.loopFOC();

    delay(50);
}

关键逻辑：里程计更新频率高（50Hz以上），能捕捉快速位移；GPS更新频率低（1-5Hz）但提供绝对基准[ citation:1]。两者通过互补权重融合，既保证了高频响应，又抑制了长时漂移。超声波作为“最高优先级”输入，当检测到近距障碍时直接干预运动指令。

2、分层导航架构（全局A* + 局部DWA）
此案例采用电力巡检领域主流的分层导航架构：全局层使用A*算法在拓扑地图上搜索宏观路径（适用于GPS或栅格地图），局部层使用动态窗口法（DWA）在滚动窗口内进行高频重规划，实时规避突发障碍。该架构同时支持GPS定位与超声波避障的协同。

#include <SimpleFOC.h>
#include <NewPing.h>

// ===== 硬件定义（同案例一）=====
// ...

// ===== 全局规划（A*在拓扑地图上）=====
#define MAP_SIZE 10
struct Point { int x, y; };
Point robotPos = {0, 0}, targetPos = {9, 9};
byte grid[MAP_SIZE][MAP_SIZE]; // 0=空闲, 1=障碍

bool planGlobalPath(Point start, Point goal, Point* path) {
    // 在grid上运行A*算法（简化）
    // ...
    return true;
}

// ===== 局部规划（DWA滚动窗口）=====
float dwa_plan(float currentX, float currentY, float heading) {
    // 生成候选轨迹（前向模拟），评分函数综合考虑：
    // - 距离目标远近
    // - 避开障碍（超声波数据）
    // - 运动平滑性
    // 返回最优转向修正量
    float turn = 0.0;
    // ...
    return turn;
}

// ===== 超声波避障（硬优先级）=====
int frontDist = 0;

void setup() {
    // BLDC电机初始化（同案例一）
    // ...
}

void loop() {
    // 1. 全局规划（仅在目标变更或路径失效时触发）
    static unsigned long lastPlan = 0;
    if (millis() - lastPlan > 3000) {
        Point path[20];
        if (planGlobalPath(robotPos, targetPos, path)) {
            // 更新全局路径点
            // ...
        }
        lastPlan = millis();
    }

    // 2. 超声波测距（高频，10Hz以上）
    frontDist = sonar.ping_cm();

    // 3. 局部规划（DWA）：基于当前位置、航向和全局路径
    float turn = 0.0;
    if (frontDist > 30) {
        // 无近距障碍，执行DWA局部规划
        turn = dwa_plan(odomX, odomY, odomHeading);
    } else {
        // 近距障碍，硬优先级避障
        turn = (frontDist < 20) ? 0.5 : 0.2;
    }

    // 4. 驱动电机
    float baseSpeed = (frontDist < 25) ? 0.3 : 0.6;
    motorL.target = baseSpeed - turn * 0.3;
    motorR.target = baseSpeed + turn * 0.3;

    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC();
    motorR.loopFOC();

    delay(50);
}

关键逻辑：分层架构将“慢思考”（全局规划，3-5Hz）与“快反应”（局部DWA，10-20Hz）分离。全局规划负责从宏观上找“最优路”，局部规划负责在微观上“安全走”，超声波作为硬优先级在近距障碍时直接覆盖DWA输出。这种设计在电力巡检中尤为适用——既保障了跨区域的长距离导航，又确保了变电站内密集设备间的安全穿行。

3、融合定位与避障协同导航（完整执行层）
此案例将GPS+编码器融合定位与超声波避障整合到完整的导航执行层中。系统同时维护融合位置、避障状态和路径目标，通过状态机协调“正常行驶”、“避障绕行”、“重规划”三种状态，实现电力巡检场景下的鲁棒自主导航。

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

// ===== 硬件定义（同案例一、二）=====
// ...

// ===== 导航状态机 =====
enum NavState { TRACKING, AVOIDING, REPLANNING };
NavState navState = TRACKING;
unsigned long avoidStartTime = 0;
float avoidDirection = 0.0;  // +1:左转, -1:右转

// ===== 融合定位更新（同案例一）=====
// ...

// ===== 路径跟踪控制 =====
void trackPath() {
    // 根据融合位置(odomX, odomY)和下一目标点计算转向
    float dx = targetPosX - odomX;
    float dy = targetPosY - odomY;
    float targetAngle = atan2(dy, dx);
    float error = targetAngle - odomHeading;
    // 归一化到[-PI, PI]
    while (error > PI) error -= 2*PI;
    while (error < -PI) error += 2*PI;

    // PID控制
    float turn = error * 1.2;
    motorL.target = 0.5 - turn * 0.3;
    motorR.target = 0.5 + turn * 0.3;
}

void loop() {
    // 1. 传感器更新（GPS + 编码器）
    while (gpsSerial.available()) gps.encode(gpsSerial.read());
    updateOdometry();
    if (millis() % 500 < 10) fuseGPS();

    // 2. 超声波测距
    int dist = sonar.ping_cm();

    // 3. 导航状态机
    switch (navState) {
        case TRACKING:
            // 正常路径跟踪
            trackPath();

            // 检测到障碍（<30cm）且距离目标>3m时进入避障
            if (dist > 0 && dist < 30) {
                navState = AVOIDING;
                avoidStartTime = millis();
                // 优先选择左侧更空旷的方向（需左右超声波补充）
                avoidDirection = 1.0;  // 简化：左转
                Serial.println("AVOIDING: Obstacle detected");
            }
            break;

        case AVOIDING:
            // 执行避障转向
            motorL.target = -avoidDirection * 0.3;
            motorR.target = avoidDirection * 0.3;

            // 避障超时或前方清空则退出
            if (millis() - avoidStartTime > 1500 || dist > 40) {
                navState = TRACKING;
                Serial.println("TRACKING: Resume navigation");
            }
            break;

        case REPLANNING:
            // 重规划（当避障后目标点被阻断时触发）
            // 实际项目中调用D* Lite或局部A*重规划
            // ...
            break;
    }

    // 4. 执行BLDC控制
    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC();
    motorR.loopFOC();

    delay(50);
}

关键逻辑：状态机将导航行为划分为“跟踪”、“避障”、“重规划”三种状态。避障状态下，机器人暂停对当前航点的追踪，执行转向绕行；避障完成后自动恢复对目标航点的跟踪。融合定位为状态切换提供位置上下文，确保避障后机器人仍清楚自己在全局地图中的位置。

要点解读
GPS与里程计的融合解决了“绝对基准”与“高频响应”的矛盾
GPS提供绝对位置（经纬度），但更新率低（1-5Hz）且易受楼宇遮挡影响；里程计提供高频相对位移（50-100Hz），但存在累积漂移。两者通过互补滤波或扩展卡尔曼滤波（EKF）融合，既获得了高频的运动响应，又确保了长时定位不漂移。这在电力巡检中尤为重要——变电站内高架线、金属设备可能造成GPS信号短暂丢失，此时里程计仍能维持短时导航能力。

分层导航架构将“慢决策”与“快反应”分离
完整的路径规划（A*、Dijkstra）对算力和内存要求较高，难以在高频控制中实时执行。分层架构将全局规划（2-5Hz，宏观最优路径）与局部避障（10-20Hz，微调规避突发障碍）分离，使Arduino/ESP32这类平台也能跑起带全局规划的导航系统。全局规划器基于先验地图或GPS航点生成宏观路径，局部规划器（DWA或VFH）在滚动窗口内高频重规划，实现“战略集中、战术分散”。

超声波避障拥有最高优先级，独立于路径规划
在电力巡检中，设备密集、作业人员走动频繁，突发障碍的响应速度至关重要。超声波传感器以高频（10-20Hz）检测近距障碍，当距离小于安全阈值时，直接中断路径跟踪指令，执行紧急制动或转向。这种“硬优先级”机制不依赖上层规划器的重算，确保反应延迟在50ms以内。

BLDC+FOC是长时巡检的执行保障
电力巡检机器人通常需要连续工作数小时，覆盖数公里路线。BLDC电机配合FOC算法效率可达85%以上，发热量低，且无电刷磨损，故障率远低于有刷电机。FOC的速度闭环控制还能在变电站斜坡或碎石路面保持转速稳定，避免因速度波动导致的定位偏差。

算力分配决定系统上限——推荐ESP32双核异构
标准的Arduino Uno（16MHz，2KB SRAM）难以同时运行GPS解算、EKF融合、A*规划、DWA避障和FOC控制。工程建议采用ESP32双核架构：Core 0专职运行传感器数据采集（GPS、超声波、编码器）和路径规划算法，Core 1专职运行BLDC的FOC电机控制环。两核通过共享变量通信，确保高频控制环不被低频规划阻塞。若仍需更高算力，可采用“树莓派做规划 + Arduino做底层控制”的异构架构。

4、输电线路巡检机器人——GPS全局导航+超声波局部避障+编码器里程闭环
适用场景：输电线路走廊（野外开阔环境）的定期巡检，机器人需沿预设线路自主导航，避开树木、杆塔基础等障碍物，同时实时上报位置与里程信息，解决GPS信号遮挡导致的局部定位偏差问题。

核心逻辑：
分层融合架构：上层（GPS）负责全局路径规划与位置校准，中层（超声波）负责局部障碍物检测，下层（编码器）负责里程计闭环与运动补偿；
动态避障策略：当GPS定位正常时，机器人按预设航点导航；当超声波检测到前方障碍物（距离<安全阈值），立即触发局部避障，同时编码器实时反馈车轮位移，修正避障过程中的轨迹偏差；
里程闭环控制：通过编码器脉冲计数计算机器人行驶距离，结合GPS位置数据，采用互补滤波算法融合，消除车轮打滑、地面不平导致的里程累积误差，确保导航精度。

/* ===== 输电线路巡检：GPS全局导航+超声波避障+编码器里程闭环 =====
 * 硬件：ESP32（Arduino兼容）+ BLDC差速底盘 + NEO-6M GPS + HC-SR04超声波 + 编码器
 * 核心：GPS定位→路径规划→超声波避障→编码器闭环修正
 */
#include <SimpleFOC.h>
#include <TinyGPS++.h>
#include <NewPing.h>

// --- 硬件引脚定义 ---
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM driverL(3,4,5), driverR(11,12,13);
#define GPS_RX 16, GPS_TX 17
#define TRIG_PIN 2, ECHO_PIN 3
#define ENCODER_L_A 4, ENCODER_L_B 5
#define ENCODER_R_A 6, ENCODER_R_B 7
TinyGPSPlus gps;
NewPing sonar(TRIG_PIN, ECHO_PIN, 200);

// --- 导航与避障参数 ---
float targetLat = 30.123456, targetLon = 120.789012; // 目标巡检点
float currentLat, currentLon;
float distanceThreshold = 5.0; // 到达目标距离阈值（米）
float obstacleThreshold = 30; // 障碍物安全距离（厘米）
unsigned long encoderL = 0, encoderR = 0;
float encoderDistance = 0; // 编码器累计里程（米）
float fusedDistance = 0;  // 融合后的里程

// --- 编码器中断服务函数 ---
void IRAM_ATTR encoderL_ISR() {
  static int8_t state = 0;
  state = (state + 1) % 4;
  if (digitalRead(ENCODER_L_A) == state & 1) encoderL++;
  else encoderL--;
}
void IRAM_ATTR encoderR_ISR() {
  static int8_t state = 0;
  state = (state + 1) % 4;
  if (digitalRead(ENCODER_R_A) == state & 1) encoderR++;
  else encoderR--;
}

void setup() {
  Serial.begin(115200);
  // BLDC初始化
  motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
  motorL.init(); motorR.init();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  // GPS初始化
  Serial1.begin(9600); // 硬件串口1连接GPS
  // 编码器初始化
  pinMode(ENCODER_L_A, INPUT); pinMode(ENCODER_L_B, INPUT);
  pinMode(ENCODER_R_A, INPUT); pinMode(ENCODER_R_B, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCODER_L_A), encoderL_ISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_R_A), encoderR_ISR, CHANGE);
  // 启动电机
  motorL.move(0.3); motorR.move(0.3);
  Serial.println("输电线路巡检系统启动完成");
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  // 1. GPS数据采集与定位
  updateGPSPosition();
  // 2. 超声波局部避障
  handleObstacleAvoidance();
  // 3. 编码器里程闭环与融合
  updateEncoderAndFusion();
  // 4. 全局路径导航
  navigateToTarget();
  delay(50);
}

// --- GPS定位更新 ---
void updateGPSPosition() {
  while (Serial1.available() > 0) {
    if (gps.encode(Serial1.read())) {
      if (gps.location.isValid()) {
        currentLat = gps.location.lat();
        currentLon = gps.location.lng();
        Serial.print("GPS定位："); Serial.print(currentLat); Serial.print(","); Serial.println(currentLon);
      }
    }
  }
}

// --- 超声波局部避障 ---
void handleObstacleAvoidance() {
  int dist = sonar.ping_cm();
  if (dist > 0 && dist < obstacleThreshold) {
    // 检测到障碍物，停止并转向
    motorL.move(0); motorR.move(0);
    delay(300);
    // 左转避障
    motorL.move(-0.2); motorR.move(0.2);
    delay(800);
    // 恢复前进
    motorL.move(0.3); motorR.move(0.3);
    Serial.println("检测到障碍物，执行避障转向");
  }
}

// --- 编码器里程与融合计算 ---
void updateEncoderAndFusion() {
  // 计算单轮里程（假设编码器每圈1000脉冲，车轮周长0.5米）
  float wheelL = (encoderL / 1000.0) * 0.5;
  float wheelR = (encoderR / 1000.0) * 0.5;
  // 平均里程（修正打滑）
  encoderDistance = (wheelL + wheelR) / 2;
  // 互补滤波融合GPS与编码器里程（GPS更新率低，编码器实时性高）
  fusedDistance = 0.9 * encoderDistance + 0.1 * gps.speed.kmph() * 0.2778; // 速度转米/秒，累计
  // 重置编码器计数
  encoderL = 0; encoderR = 0;
}

// --- 全局路径导航 ---
void navigateToTarget() {
  if (!gps.location.isValid()) return;
  // 计算当前位置与目标点的距离（Haversine公式简化版，小范围适用）
  float dx = targetLon - currentLon;
  float dy = targetLat - currentLat;
  float distance = sqrt(dx*dx + dy*dy) * 111320; // 经纬度转米（近似）
  // 计算方位角偏差
  float targetAngle = atan2(dy, dx) * 180 / PI;
  float currentAngle = gps.course.deg();
  float angleError = targetAngle - currentAngle;
  // 角度归一化
  if (angleError > 180) angleError -= 360;
  if (angleError < -180) angleError += 360;
  // PID控制转向（简化版，直接比例控制）
  float steer = angleError * 0.02;
  // 差速转向
  motorL.move(0.3 - steer);
  motorR.move(0.3 + steer);
  // 到达目标点判断
  if (distance < distanceThreshold) {
    motorL.move(0); motorR.move(0);
    Serial.println("到达目标巡检点，等待下一步指令");
  }
}

5、变电站设备巡检机器人——GPS区域围栏+超声波设备避障+编码器精准停靠
适用场景：变电站内设备（变压器、断路器、避雷器等）的近距离巡检，机器人需在划定的安全区域内自主移动，避开设备基础、电缆沟等障碍物，精准停靠在设备前方指定位置，配合摄像头完成图像采集。

核心逻辑：
区域围栏约束：通过GPS划定变电站安全作业区域，机器人超出围栏时自动触发返航，防止误入高压危险区域；
设备精准避障：采用多超声波传感器阵列，检测设备轮廓与机器人的距离，结合编码器实现毫米级停靠精度，确保摄像头与设备的距离符合成像要求；
停靠闭环控制：编码器实时反馈机器人位移，当距离设备目标停靠点小于阈值时，启动PID闭环控制，调整电机转速，实现平稳精准停靠。

/* ===== 变电站巡检：GPS区域围栏+超声波设备避障+编码器精准停靠 =====
 * 硬件：ESP32 + BLDC底盘 + 多超声波阵列 + 编码器 + GPS
 * 核心：区域约束→设备避障→编码器停靠闭环
 */
#include <SimpleFOC.h>
#include <TinyGPS++.h>
#include <NewPing.h>

// --- 硬件定义 ---
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM driverL(3,4,5), driverR(11,12,13);
#define GPS_RX 16, GPS_TX 17
#define TRIG_FRONT 2, ECHO_FRONT 3
#define TRIG_LEFT 4, ECHO_LEFT 5
#define TRIG_RIGHT 6, ECHO_RIGHT 7
#define ENCODER_L_A 8, ENCODER_L_B 9
#define ENCODER_R_A 10, ENCODER_R_B 11
TinyGPSPlus gps;
NewPing sonarFront(TRIG_FRONT, ECHO_FRONT, 200);
NewPing sonarLeft(TRIG_LEFT, ECHO_LEFT, 200);
NewPing sonarRight(TRIG_RIGHT, ECHO_RIGHT, 200);

// --- 区域与停靠参数 ---
float fenceCenterLat = 30.123456, fenceCenterLon = 120.789012; // 围栏中心
float fenceRadius = 50.0; // 围栏半径（米）
float deviceThreshold = 20; // 设备避障阈值（厘米）
float stopThreshold = 5;    // 停靠精度阈值（厘米）
unsigned long encoderL = 0, encoderR = 0;
float targetDistance = 100; // 目标停靠距离（厘米，距设备）
float currentDistance = 0;  // 当前距设备距离

// --- 编码器中断 ---
void IRAM_ATTR encoderL_ISR() { encoderL += digitalRead(ENCODER_L_A) ? 1 : -1; }
void IRAM_ATTR encoderR_ISR() { encoderR += digitalRead(ENCODER_R_A) ? 1 : -1; }

void setup() {
  Serial.begin(115200);
  // BLDC初始化
  motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
  motorL.init(); motorR.init();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  // GPS初始化
  Serial1.begin(9600);
  // 编码器中断
  attachInterrupt(digitalPinToInterrupt(ENCODER_L_A), encoderL_ISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_R_A), encoderR_ISR, CHANGE);
  // 启动电机
  motorL.move(0.2); motorR.move(0.2);
  Serial.println("变电站巡检系统启动完成");
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  // 1. GPS区域围栏检测
  checkGeoFence();
  // 2. 多超声波设备避障
  handleDeviceAvoidance();
  // 3. 编码器精准停靠
  preciseStop();
  delay(50);
}

// --- GPS区域围栏检测 ---
void checkGeoFence() {
  if (!gps.location.isValid()) return;
  // 计算当前位置与围栏中心的距离
  float dx = gps.location.lng() - fenceCenterLon;
  float dy = gps.location.lat() - fenceCenterLat;
  float distance = sqrt(dx*dx + dy*dy) * 111320;
  // 超出围栏，触发返航
  if (distance > fenceRadius) {
    motorL.move(-0.2); motorR.move(-0.2); // 原地返航
    Serial.println("超出安全围栏，执行返航");
    // 返航到围栏中心（简化逻辑，实际需路径规划）
    while (distance > 10) {
      updateGPSPosition();
      distance = sqrt(dx*dx + dy*dy) * 111320;
    }
    motorL.move(0); motorR.move(0);
  }
}

// --- 多超声波设备避障 ---
void handleDeviceAvoidance() {
  int distFront = sonarFront.ping_cm();
  int distLeft = sonarLeft.ping_cm();
  int distRight = sonarRight.ping_cm();
  // 前方设备距离更新
  currentDistance = distFront;
  // 避障逻辑
  if (distFront < deviceThreshold) {
    motorL.move(0); motorR.move(0);
    delay(200);
    // 优先向空旷侧转向
    if (distLeft > distRight) {
      motorL.move(-0.15); motorR.move(0.15); // 左转
    } else {
      motorL.move(0.15); motorR.move(-0.15); // 右转
    }
    delay(600);
    motorL.move(0.2); motorR.move(0.2);
  } else if (distLeft < 15) {
    motorL.move(0.25); motorR.move(0.15); // 右微调
  } else if (distRight < 15) {
    motorL.move(0.15); motorR.move(0.25); // 左微调
  }
}

// --- 编码器精准停靠 ---
void preciseStop() {
  if (currentDistance <= 0) return;
  // 计算剩余距离（目标距离 - 当前距离）
  float remaining = targetDistance - currentDistance;
  // 编码器反馈的行驶距离（车轮周长0.5米，编码器每圈1000脉冲）
  float encoderMove = ((encoderL + encoderR) / 2.0) / 1000.0 * 50; // 转换为厘米
  // 剩余距离修正
  remaining -= encoderMove;
  // PID控制停靠（简化比例控制）
  if (abs(remaining) > stopThreshold) {
    float speed = map(abs(remaining), 0, 50, 0.05, 0.2);
    if (remaining > 0) {
      motorL.move(speed); motorR.move(speed); // 前进
    } else {
      motorL.move(-speed); motorR.move(-speed); // 后退
    }
  } else {
    motorL.move(0); motorR.move(0); // 精准停靠
    Serial.println("精准停靠完成，准备设备巡检");
  }
  // 重置编码器
  encoderL = 0; encoderR = 0;
}

6、地下综合管廊巡检机器人——GPS+编码器融合定位+超声波狭窄空间避障
适用场景：地下综合管廊（狭窄、封闭、GPS信号弱）的巡检，机器人需在管廊内自主导航，避开管廊壁、管道支架、检修门等障碍物，同时解决GPS信号丢失时的定位问题，确保巡检无盲区。

核心逻辑：
GPS+编码器融合定位：当GPS信号良好时，通过GPS校准编码器累积误差；当GPS信号丢失（管廊内遮挡），切换为编码器+IMU（简化版用编码器+超声波辅助）的航位推算，确保定位不中断；
狭窄空间避障：采用多超声波传感器检测管廊壁距离，结合编码器反馈的位移，实现沿管廊壁的贴壁导航，同时避开突出障碍物；
信号丢失应急处理：当GPS信号丢失超过阈值，自动切换为航位推算模式，并通过超声波检测前方是否有死胡同，若有则立即转向，避免被困。

/* ===== 地下管廊巡检：GPS+编码器融合定位+超声波狭窄避障 =====
 * 硬件：ESP32 + BLDC履带底盘 + 多超声波 + 编码器 + GPS
 * 核心：GPS校准→编码器推算→超声波贴壁导航
 */
#include <SimpleFOC.h>
#include <TinyGPS++.h>
#include <NewPing.h>

// --- 硬件定义 ---
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM driverL(3,4,5), driverR(11,12,13);
#define GPS_RX 16, GPS_TX 17
#define TRIG_LEFT 2, ECHO_LEFT 3
#define TRIG_RIGHT 4, ECHO_RIGHT 5
#define TRIG_FRONT 6, ECHO_FRONT 7
#define ENCODER_L_A 8, ENCODER_L_B 9
#define ENCODER_R_A 10, ENCODER_R_B 11
TinyGPSPlus gps;
NewPing sonarLeft(TRIG_LEFT, ECHO_LEFT, 200);
NewPing sonarRight(TRIG_RIGHT, ECHO_RIGHT, 200);
NewPing sonarFront(TRIG_FRONT, ECHO_FRONT, 200);

// --- 融合与避障参数 ---
float gpsLat, gpsLon;
unsigned long encoderL = 0, encoderR = 0;
float encoderPosX = 0, encoderPosY = 0; // 航位推算坐标（米）
float lastAngle = 0; // 上一次航向角
bool gpsValid = false;
int gpsLostCount = 0;
const int GPS_LOST_THRESHOLD = 10; // GPS丢失阈值（连续10次无数据）
const float WALL_DISTANCE = 30;    // 贴壁距离（厘米）

// --- 编码器中断 ---
void IRAM_ATTR encoderL_ISR() { encoderL += digitalRead(ENCODER_L_A) ? 1 : -1; }
void IRAM_ATTR encoderR_ISR() { encoderR += digitalRead(ENCODER_R_A) ? 1 : -1; }

void setup() {
  Serial.begin(115200);
  // BLDC初始化
  motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
  motorL.init(); motorR.init();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  // GPS初始化
  Serial1.begin(9600);
  // 编码器中断
  attachInterrupt(digitalPinToInterrupt(ENCODER_L_A), encoderL_ISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_R_A), encoderR_ISR, CHANGE);
  // 启动电机
  motorL.move(0.25); motorR.move(0.25);
  Serial.println("地下管廊巡检系统启动完成");
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  // 1. GPS信号检测与融合
  updateGPSAndFusion();
  // 2. 狭窄空间超声波避障
  narrowSpaceAvoidance();
  // 3. 航位推算（GPS丢失时）
  if (!gpsValid) {
    deadReckoning();
  }
  delay(50);
}

// --- GPS信号检测与融合 ---
void updateGPSAndFusion() {
  bool currentGpsValid = false;
  while (Serial1.available() > 0) {
    if (gps.encode(Serial1.read())) {
      if (gps.location.isValid()) {
        gpsLat = gps.location.lat();
        gpsLon = gps.location.lng();
        currentGpsValid = true;
        gpsLostCount = 0;
        // GPS校准航位推算坐标（简化：将GPS坐标转为相对坐标）
        encoderPosX = (gpsLon - 120.789012) * 111320; // 以起点为原点
        encoderPosY = (gpsLat - 30.123456) * 111320;
        lastAngle = gps.course.deg();
      }
    }
  }
  if (currentGpsValid) {
    gpsValid = true;
  } else {
    gpsLostCount++;
    if (gpsLostCount >= GPS_LOST_THRESHOLD) {
      gpsValid = false;
      Serial.println("GPS信号丢失，切换航位推算模式");
    }
  }
}

// --- 狭窄空间超声波避障 ---
void narrowSpaceAvoidance() {
  int distLeft = sonarLeft.ping_cm();
  int distRight = sonarRight.ping_cm();
  int distFront = sonarFront.ping_cm();
  // 贴壁导航：保持左右距离均衡
  if (distLeft < WALL_DISTANCE - 5) {
    // 左侧过近，右转向
    motorL.move(0.3); motorR.move(0.15);
  } else if (distLeft > WALL_DISTANCE + 5) {
    // 左侧过远，左转向
    motorL.move(0.15); motorR.move(0.3);
  } else if (distRight < WALL_DISTANCE - 5) {
    // 右侧过近，左转向
    motorL.move(0.15); motorR.move(0.3);
  } else if (distRight > WALL_DISTANCE + 5) {
    // 右侧过远，右转向
    motorL.move(0.3); motorR.move(0.15);
  } else {
    // 距离均衡，直行
    motorL.move(0.25); motorR.move(0.25);
  }
  // 前方障碍物检测（死胡同）
  if (distFront < 40) {
    motorL.move(0); motorR.move(0);
    delay(300);
    // 原地转向180度
    motorL.move(-0.25); motorR.move(0.25);
    delay(2000); // 根据转速调整时间
    motorL.move(0.25); motorR.move(0.25);
    Serial.println("前方死胡同，执行掉头");
  }
}

// --- 航位推算（GPS丢失时） ---
void deadReckoning() {
  // 计算左右轮位移
  float wheelL = (encoderL / 1000.0) * 0.5; // 车轮周长0.5米
  float wheelR = (encoderR / 1000.0) * 0.5;
  // 计算航向角变化（简化：左右轮差速导致转向）
  float deltaAngle = ((wheelR - wheelL) / 0.4) * (180 / PI); // 轴距0.4米
  float currentAngle = lastAngle + deltaAngle;
  // 计算位移
  float moveDistance = (wheelL + wheelR) / 2;
  // 更新坐标
  encoderPosX += moveDistance * cos(currentAngle * PI / 180);
  encoderPosY += moveDistance * sin(currentAngle * PI / 180);
  // 更新航向角
  lastAngle = currentAngle;
  // 重置编码器
  encoderL = 0; encoderR = 0;
  // 串口输出推算坐标
  Serial.print("航位推算坐标：X="); Serial.print(encoderPosX);
  Serial.print(", Y="); Serial.println(encoderPosY);
}

要点解读

1. 分层融合架构：解决多传感器优势互补与算力分配的核心
   电力巡检场景中，单一传感器无法应对复杂环境（GPS信号遮挡、超声波盲区、编码器累积误差），分层融合是必然选择：
   层级划分清晰：上层（GPS）负责全局定位与路径规划，提供绝对坐标基准；中层（超声波）负责局部避障，弥补GPS无法检测近距离障碍物的缺陷；下层（编码器）负责实时运动反馈，实现闭环控制，三层数据单向传递（上层决策指导下层执行，下层反馈修正上层偏差）；

算力合理分配：GPS解析、路径规划等算力密集型任务由上位机（ESP32）承担，超声波避障、编码器计数、电机控制等实时性任务由下位机（Arduino核心）承担，避免算力瓶颈，确保控制周期≤50ms，满足实时性要求；

数据互补修正：GPS提供绝对位置校准编码器累积误差，编码器提供高频位移数据弥补GPS更新率低（通常1Hz）的缺陷，超声波提供近距离障碍物信息，三者形成“全局-局部-实时”的互补闭环，提升系统鲁棒性。

2. 传感器特性适配：电力巡检场景下的精准感知保障
   不同传感器的物理特性决定其适用场景，需结合电力巡检的具体需求进行针对性适配：
   GPS：室外开阔场景的核心定位源：优先选用NEO-6M/NEO-8M等支持NMEA协议的GPS模块，通过解析GGA、RMC语句获取经纬度、速度、航向；电力巡检中需设置地理围栏，防止机器人误入高压区域，同时结合HDOP值判断定位精度，避免低精度数据导致误决策；

超声波：近距离避障的低成本方案：电力设备密集区域（如变电站），超声波可快速检测设备、支架等障碍物，需采用多传感器阵列（前、左、右）覆盖盲区，同时设置不同阈值（设备避障阈值<管廊避障阈值），适配不同场景的避障需求；

编码器：闭环控制的精度基石：BLDC电机需搭配高精度编码器（每圈脉冲数≥1000），通过中断计数实现高频位移反馈，用于里程计算、速度闭环和精准停靠；电力巡检中需对编码器进行温度补偿和抗干扰设计，避免电机电磁干扰导致计数错误。

3. BLDC驱动与闭环控制：电力巡检的动力执行核心
   电力巡检机器人常面临复杂地形（管廊不平、设备爬坡）和精准控制需求（停靠、避障），BLDC的闭环控制是关键：
   FOC算法保障平滑驱动：采用磁场定向控制（FOC）替代传统梯形驱动，消除转矩纹波，实现低速平稳运行和快速动态响应，避免急停急启导致传感器数据抖动，适配频繁启停的巡检场景；

双闭环控制提升精度：构建速度环+电流环双闭环，速度环确保电机转速稳定，适应地形变化；电流环监测电机负载，当检测到堵转（如避障卡住）时，立即限制电流，保护电机和机械结构，同时触发报警；

差速控制实现灵活转向：电力巡检中需频繁转向（避开设备、管廊转弯），通过左右轮差速控制实现原地转向或平滑转弯，结合编码器反馈修正转向偏差，确保转向轨迹与规划一致。

4. 电力场景适配：安全与效率的双重保障
   电力巡检场景具有高压、封闭、设备密集等特殊性，代码设计需兼顾安全与效率：
   安全优先的多层防护：硬件层面设置物理急停按钮，直连电机驱动器，确保紧急情况（如碰撞、高压区闯入）下立即切断动力；软件层面设置地理围栏、障碍物安全阈值、电机电流上限，形成“硬件急停+软件限速+阈值报警”的三级安全防护；

环境适应性优化：针对管廊潮湿、变电站电磁干扰等环境，采用IP65以上防护等级的传感器和电机，电源采用隔离DC-DC模块，信号线使用屏蔽双绞线，避免电磁干扰导致传感器失效；编码器线远离动力线，减少PWM噪声干扰；

巡检效率优化：通过GPS预设巡检航点，结合编码器里程闭环，减少重复路径；超声波避障采用优先级策略（优先转向空旷侧），缩短避障时间；编码器精准停靠减少调整次数，提升巡检效率。

5. 异常处理与鲁棒性：复杂环境的可靠运行关键
   电力巡检环境复杂多变，异常处理能力决定机器人能否长期稳定运行：
   GPS信号丢失的应急机制：当GPS信号连续丢失超过阈值，自动切换为编码器+超声波的航位推算模式，通过编码器位移和超声波辅助修正航向，避免机器人“迷路”；同时检测前方是否为死胡同，及时掉头，防止被困；

传感器失效的降级策略：当某一超声波传感器失效，自动降低该方向的避障权重，依赖其他传感器实现避障；当编码器失效，切换为开环控制，同时降低速度，依赖超声波避障保障安全；

看门狗与超时保护：启用Arduino硬件看门狗，防止程序跑飞；设置通信超时机制，若未接收到GPS数据或传感器数据超过阈值，触发安全模式（停止电机、报警），确保机器人处于可控状态。

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