基于 Arduino 的四足仿生穿越机器人，是一个融合了仿生学、自动控制、机械电子和传感器技术的复杂系统。它旨在模仿四足动物（如狗、猫或昆虫）的运动方式，以实现对复杂、非结构化地形的强大适应能力。

主要特点
仿生多关节驱动与步态生成
这类机器人的核心在于其腿部结构和运动控制逻辑。
多自由度腿构型： 每条腿通常由多个连杆和关节（如髋关节、膝关节）组成，形成2至4个自由度。这种串联机构的设计借鉴了哺乳动物的骨骼肌肉系统，使其能够完成抬腿、摆动、支撑和蹬地等复合动作。
BLDC 高性能驱动： 相较于传统舵机，无刷直流电机凭借其高功率密度、高扭矩输出和低发热特性，成为驱动关节的理想选择。配合减速器（如谐波减速器），能提供穿越崎岖地形所需的瞬间爆发力和持续推力。
步态算法： Arduino（或与其协同的高性能处理器）通过运行步态生成算法（如三角步态、对角小跑等），精确协调四个腿部的运动时序，确保在任何时刻机器人都有至少三条腿着地以维持动态平衡。
柔顺控制与环境交互
真正的仿生不仅在于形似，更在于“触感”。
力矩与阻抗控制： 结合 FOC（磁场定向控制）算法，BLDC 电机可以实现精细的力矩控制，模拟生物肌肉的收缩行为。这使得机器人具备柔顺性，当脚部意外触碰到障碍物时，能像生物一样顺应外力产生弹性位移，而不是硬性碰撞，从而保护自身并适应不平整地面。
能量高效利用： 在制动或下坡时，BLDC 电机可切换至发电模式，通过再生制动将动能回馈给电池，延长续航时间，这符合生物体能量循环利用的原则。
多传感器融合与自主导航
要实现“自主穿越”，机器人必须具备感知和决策能力。
全方位感知： 通常搭载激光雷达、双目相机、IMU（惯性测量单元）等多种传感器。激光雷达构建环境的二维/三维地图，双目视觉提供深度信息和纹理识别，IMU 实时反馈机身的姿态（俯仰、横滚、偏航）。
融合算法： 通过融合这些传感器数据（如使用扩展卡尔曼滤波），机器人能实现高精度的自我定位和地图构建，在充满障碍物的环境中规划出安全的穿越路径，并自主避开危险。

应用场景
凭借其卓越的地形适应性和灵活性，四足仿生穿越机器人在众多领域展现出巨大价值：
工业巡检与维护：
在复杂的工业现场，如港口码头、石化厂区或电厂，机器人可以替代人工完成高危、重复的巡检任务。例如，在错综复杂的皮带机廊道、钢结构楼梯或狭窄通道中自主行走，利用搭载的红外热像仪或气体传感器，实时监测设备温度、识别泄漏隐患，极大提升了巡检效率和安全性。
应急救援与公共安全：
在地震、塌方等灾害后的废墟环境中，轮式或履带式机器人往往寸步难行。四足机器人则能灵活穿梭于碎石瓦砾之间，深入人类难以进入的区域进行生命探测、环境侦察，为救援决策提供第一手信息。此外，也可用于防爆处置、排雷等高风险任务。
特种作业与科研探索：
在军事侦察、野外勘探、核电设施内部检测等特殊场景中，四足机器人能承载一定负载，穿越恶劣地形执行任务。同时，它也是研究仿生运动学、智能控制算法的理想平台。

注意事项
设计和开发此类机器人是一项系统工程，需重点关注以下挑战：
机电一体化结构设计
轻量化与强度的平衡： 腿部结构既要足够轻以减小关节驱动力矩，又要足够坚固以承受冲击和负载。材料选择（如航空铝合金、碳纤维）和拓扑优化至关重要。
散热管理： BLDC 电机和驱动器在大扭矩输出时会产生大量热量，必须设计有效的散热方案（如散热片、风扇），防止过热导致性能下降或损坏。
电源与能源管理
续航瓶颈： 四足机器人的能耗较高，续航是主要短板。需选用高能量密度的电池，并优化运动算法，让机器人在保证性能的同时尽可能采用“经济转速”。
电源隔离： 大电流驱动电路会对敏感的传感器（如 IMU、编码器）造成电磁干扰。在电路设计上，必须严格区分动力电源和信号电源，必要时使用隔离模块。
软硬件协同与计算资源
算力分配： Arduino（特别是 Teensy 或 Portenta 系列）擅长处理实时性要求极高的底层电机控制。但对于复杂的 SLAM（同步定位与地图构建）、图像识别等任务，通常需要搭配树莓派、NVIDIA Jetson 等高性能计算单元，形成“上位机负责决策，下位机（Arduino）负责执行”的分布式架构。
通信稳定性： 上下位机之间以及各关节控制器之间的通信必须稳定、低延迟，以确保运动指令的准确同步。

1、基础步态控制（对角小跑）

#include <Servo.h>

// 定义12个ESC控制引脚（对应4足3关节）
const int escPins[12] = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13};
Servo esc[12];

// 初始角度（微秒值）
const int neutralPos = 1500;
const int minAngle = 1000;
const int maxAngle = 2000;

void setup() {
  Serial.begin(9600);
  
  // 初始化所有ESC
  for (int i = 0; i < 12; i++) {
    esc[i].attach(escPins[i]);
    esc[i].writeMicroseconds(neutralPos); // 中立位置
  }
  
  // 安全启动延时
  delay(3000);
  Serial.println("Robot Ready!");
}

void loop() {
  // 对角小跑步态（左前+右后 vs 右前+左后）
  for (int step = 0; step < 10; step++) {
    // 抬起对角腿
    moveLegs(0, 2, 100); // 左前腿髋关节
    moveLegs(3, 5, 100); // 右后腿髋关节
    moveLegs(6, 8, 100); // 右前腿髋关节
    moveLegs(9, 11, 100); // 左后腿髋关节
    delay(300);
    
    // 恢复中立
    resetAllLegs();
    delay(200);
  }
}

void moveLegs(int hip, int knee, int angle) {
  esc[hip].writeMicroseconds(neutralPos + angle);
  esc[knee].writeMicroseconds(neutralPos - angle*0.7); // 膝关节反向运动
}

void resetAllLegs() {
  for (int i = 0; i < 12; i++) {
    esc[i].writeMicroseconds(neutralPos);
  }
}

2、地形自适应步态（基于IMU传感器）

#include <Servo.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>

Adafruit_BNO055 bno = Adafruit_BNO055(55);
Servo esc[12];
const int escPins[12] = {2,3,4,5,6,7,8,9,10,11,12,13};

void setup() {
  Serial.begin(9600);
  if (!bno.begin()) {
    Serial.println("IMU Not Found!");
    while(1);
  }
  
  for (int i = 0; i < 12; i++) {
    esc[i].attach(escPins[i]);
    esc[i].writeMicroseconds(1500);
  }
  delay(3000);
}

void loop() {
  sensors_event_t event;
  bno.getEvent(&event);
  
  // 根据俯仰角调整步态高度
  int pitch = event.orientation.y;
  int liftHeight = map(pitch, -30, 30, 50, 150); // 倾斜时增加抬腿高度
  
  // 爬行步态（适应复杂地形）
  crawlGait(liftHeight);
}

void crawlGait(int height) {
  // 顺序抬起单腿
  for (int leg = 0; leg < 4; leg++) {
    int hipIdx = leg*3;
    int kneeIdx = hipIdx + 1;
    
    esc[hipIdx].writeMicroseconds(1500 + height);
    esc[kneeIdx].writeMicroseconds(1500 - height*0.7);
    delay(200);
    
    esc[hipIdx].writeMicroseconds(1500);
    esc[kneeIdx].writeMicroseconds(1500);
    delay(100);
  }
}

3、无线遥控与姿态稳定控制

#include <Servo.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

RF24 radio(7, 8); // CE, CSN
const byte address[6] = "00001";
Servo esc[12];

struct ControlData {
  int roll;
  int pitch;
  int yaw;
  int throttle;
  bool run;
};
ControlData cmd;

void setup() {
  Serial.begin(9600);
  radio.begin();
  radio.openReadingPipe(0, address);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();
  
  for (int i = 0; i < 12; i++) {
    esc[i].attach(2+i);
    esc[i].writeMicroseconds(1500);
  }
  delay(3000);
}

void loop() {
  if (radio.available()) {
    radio.read(&cmd, sizeof(ControlData));
    
    if (cmd.run) {
      // 根据遥控器指令调整姿态
      adjustGait(cmd.roll, cmd.pitch, cmd.throttle);
    } else {
      // 紧急停止
      emergencyStop();
    }
  }
}

void adjustGait(int roll, int pitch, int throttle) {
  // 四足站立姿态调整
  for (int leg = 0; leg < 4; leg++) {
    int baseHip = leg*3;
    int baseKnee = baseHip + 1;
    
    // 根据roll/pitch调整各腿位置
    int hipOffset = map(roll, -90, 90, -100, 100);
    int kneeOffset = map(pitch, -90, 90, -80, 80);
    
    esc[baseHip].writeMicroseconds(1500 + hipOffset);
    esc[baseKnee].writeMicroseconds(1500 + kneeOffset - throttle*0.5);
  }
}

void emergencyStop() {
  for (int i = 0; i < 12; i++) {
    esc[i].writeMicroseconds(1500);
  }
  while(1); // 停止程序直到复位
}

要点解读
多ESC同步控制：
四足机器人需要同时控制12个BLDC电机（每腿3个关节）
使用数组管理ESC对象，通过循环初始化/控制
关键点：确保所有ESC信号同步，避免运动不同步导致失衡
安全启动机制：
所有案例均包含3秒初始延时，确保ESC完成校准
紧急停止功能（案例3）通过立即发送中立信号实现
建议：增加物理急停开关作为双重保护
运动学实现：
基础步态（案例1）采用对角小跑，减少动态不稳定
地形自适应（案例2）通过IMU反馈调整抬腿高度
关键参数：髋关节和膝关节的运动比例（通常1:0.7）
传感器融合：
案例2使用BNO055获取姿态数据
案例3通过nRF24L01实现无线遥控
建议：增加足端力传感器实现更智能的地形适应
资源优化：
使用PWM库替代delay()实现更平滑运动
案例3的无线控制采用结构体传输，减少数据量
关键优化：避免在loop()中使用阻塞式延时，改用millis()计时

4、崎岖地形自适应步态控制程序

#include <Servo.h>
#include <MPU6050.h>
#include <PID_v1.h>

// 关节与电机配置
#define SERVO_COUNT 12  // 每条腿2个关节，共4腿×3？此处简化为每腿3关节，可根据硬件调整
Servo hipJoint[4], kneeJoint[4], ankleJoint[4];  // 髋、膝、踝关节舵机
int BLDC_MOTOR_PINS[4] = {3, 5, 6, 9};  // 腿部驱动BLDC引脚

// 传感器与控制参数
MPU6050 imu;
float targetPitch = 0, targetRoll = 0;  // 机身期望姿态
float currentPitch = 0, currentRoll = 0; // 机身实际姿态
double Kp = 4.2, Ki = 0.8, Kd = 1.1;
PID attitudePID(&currentPitch, &currentPitch, &targetPitch, Kp, Ki, Kd, DIRECT);
PID rollPID(&currentRoll, &currentRoll, &targetRoll, Kp, Ki, Kd);

// 步态参数
enum GaitPhase {
    SWING = 0,  // 摆动相（腿抬起前进）
    STANCE = 1  // 支撑相（腿落地承重）
};
GaitPhase legPhase[4] = {STANCE, SWING, STANCE, SWING};  // 对角步态初始化
float stepHeight = 8.0;   // 步幅高度（度数）
float stepLength = 30.0;  // 步幅长度（度数）
float gaitCycle = 1200;   // 步态周期（毫秒）

void setup() {
    Serial.begin(9600);
    Wire.begin();
    // 初始化IMU
    if (!imu.initialize()) {
        Serial.println("IMU初始化失败");
        while (1);
    }
    // 初始化关节舵机和BLDC电机
    for (int i = 0; i < 4; i++) {
        hipJoint[i].attach(10 + i * 2);
        kneeJoint[i].attach(11 + i * 2);
        ankleJoint[i].attach(12 + i * 2);
        pinMode(BLDC_MOTOR_PINS[i], OUTPUT);
    }
    attitudePID.SetMode(AUTOMATIC);
    rollPID.SetMode(AUTOMATIC);
}

void loop() {
    static unsigned long lastGaitTime = 0;
    // 50Hz步态更新频率
    if (millis() - lastGaitTime >= 20) {
        // 1. 姿态感知与稳定
        updateAttitude();
        // 2. 地形识别与步态调整
        float terrainAngle = detectTerrainAngle();
        adjustGaitForTerrain(terrainAngle);
        // 3. 执行步态控制
        executeGaitCycle();
        lastGaitTime = millis();
    }
    // 实时姿态闭环控制
    maintainBodyBalance();
}

void updateAttitude() {
    int16_t ax, ay, az, gx, gy, gz;
    imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    // 互补滤波融合加速度计和陀螺仪数据
    currentPitch = 0.98 * (currentPitch + gx * 0.01) + 0.02 * atan2(ay, az) * 180 / PI;
    currentRoll = 0.98 * (currentRoll + gy * 0.01) + 0.02 * atan2(ax, az) * 180 / PI;
}

float detectTerrainAngle() {
    // 通过IMU俯仰/横滚角判断坡度
    return sqrt(currentPitch * currentPitch + currentRoll * currentRoll);
}

void adjustGaitForTerrain(float angle) {
    // 坡度越大，步幅减小、步高增加
    if (angle > 20) {  // 陡坡
        stepHeight = 12.0;
        stepLength = 20.0;
    } else if (angle > 10) {  // 缓坡
        stepHeight = 10.0;
        stepLength = 25.0;
    } else {  // 平地
        stepHeight = 8.0;
        stepLength = 30.0;
    }
}

void executeGaitCycle() {
    // 对角步态控制：左前-右后同步，右前-左后同步
    for (int leg = 0; leg < 4; leg++) {
        if (legPhase[leg] == SWING) {
            // 摆动相：抬腿前进
            hipJoint[leg].write(90 + stepLength / 2);
            kneeJoint[leg].write(90 + stepHeight);
            ankleJoint[leg].write(90 - stepHeight / 2);
            digitalWrite(BLDC_MOTOR_PINS[leg], LOW);  // BLDC轻载抬腿
        } else {
            // 支撑相：落地承重
            hipJoint[leg].write(90 - stepLength / 2);
            kneeJoint[leg].write(90 - stepHeight / 2);
            ankleJoint[leg].write(90 + stepHeight / 2);
            // 根据机身重量调整BLDC扭矩
            float torque = map(currentPitch, -20, 20, 150, 200);
            analogWrite(BLDC_MOTOR_PINS[leg], constrain((int)torque, 0, 255));
        }
        // 切换步态相位
        if (millis() % gaitCycle < gaitCycle / 2) {
            legPhase[leg] = (legPhase[leg] == SWING) ? STANCE : SWING;
        }
    }
}

void maintainBodyBalance() {
    // 姿态PID控制：维持机身水平
    targetPitch = 0;
    targetRoll = 0;
    // 通过调整各腿支撑力实现姿态平衡
    for (int leg = 0; leg < 4; leg++) {
        float compensation = (leg < 2 ? currentPitch : -currentPitch) + 
                            (leg % 2 == 0 ? currentRoll : -currentRoll);
        int currentTorque = analogRead(BLDC_MOTOR_PINS[leg]);
        analogWrite(BLDC_MOTOR_PINS[leg], constrain(currentTorque + compensation * 2, 0, 255));
    }
}

要点解读：

对角步态与相位控制：采用对角步态（左前-右后同步摆动）提高崎岖地形通过性，通过相位切换机制实现步态周期的精准同步，避免机身失衡。
多传感器姿态闭环：融合IMU加速度计与陀螺仪数据，通过互补滤波获取机身姿态，结合PID控制动态调整各腿支撑力，确保穿越复杂地形时机身稳定。
地形自适应步态调整：根据IMU检测的坡度实时调整步高和步幅——陡坡增加步高防绊倒、减小步幅防打滑，提升不同地形的穿越适应性。
关节与BLDC协同控制：摆动相由舵机完成抬腿动作，BLDC轻载驱动；支撑相BLDC根据机身姿态动态调整扭矩，实现承重与姿态补偿的双重功能，兼顾灵活性与承载力。
高实时性控制架构：步态更新采用50Hz高频循环，姿态闭环实时响应，确保机器人在快速穿越过程中及时应对地形变化，避免因延迟导致的失衡或摔倒。

5、动态障碍跨越控制程序

#include <UltrasonicSensor.h>
#include <LidarLite.h>
#include <Servo.h>

// 传感器与硬件配置
UltrasonicSensor frontUltra(A0, A1);  // 前向超声波
LidarLite frontLidar;                  // 前向激光雷达
Servo liftJoint;                      // 腿部抬升关节（跨越专用）
int BLDC_DRIVE_PINS[4] = {3, 5, 6, 9}; // 腿部驱动BLDC

// 障碍检测与跨越参数
float obstacleHeight = 0, obstacleDistance = 0;
float jumpThreshold = 15.0;  // 障碍高度触发跨越阈值（cm）
float jumpPreDistance = 30.0;// 跨越前减速距离（cm）
float currentSpeed = 150;    // 当前行走速度（BLDC PWM）
bool isJumping = false;
int jumpPhase = 0;           // 跨越阶段：0=接近，1=抬腿，2=落地，3=复位

void setup() {
    Serial.begin(9600);
    // 初始化传感器
    frontUltra.begin();
    frontLidar.begin();
    // 初始化抬升关节和BLDC
    liftJoint.attach(8);
    for (int i = 0; i < 4; i++) pinMode(BLDC_DRIVE_PINS[i], OUTPUT);
    // 设置跨越关节初始位置
    liftJoint.write(0);
}

void loop() {
    // 1. 障碍检测与预判
    detectObstacle();
    // 2. 跨越决策与执行
    if (obstacleHeight >= jumpThreshold) {
        executeJumpOver();
    } else {
        // 正常行走
        maintainNormalWalk();
    }
    // 3. 跨越后状态复位
    if (isJumping && jumpPhase == 3) {
        resetJumpState();
    }
    delay(10);
}

void detectObstacle() {
    // 激光雷达测距，超声波测高
    obstacleDistance = frontLidar.readDistance();
    obstacleHeight = frontUltra.readHeight();
    // 数据滤波：去除异常值
    if (obstacleHeight < 0 || obstacleHeight > 100) obstacleHeight = 0;
    if (obstacleDistance < 0 || obstacleDistance > 500) obstacleDistance = 500;
    // 输出调试信息
    Serial.print("障碍：高度="); Serial.print(obstacleHeight);
    Serial.print("cm，距离="); Serial.println(obstacleDistance);
}

void executeJumpOver() {
    switch (jumpPhase) {
        case 0:  // 接近阶段：减速靠近
            if (obstacleDistance > jumpPreDistance) {
                // 距离远，保持速度
                currentSpeed = 150;
                analogWrite(BLDC_DRIVE_PINS[0], currentSpeed);
                analogWrite(BLDC_DRIVE_PINS[1], currentSpeed);
                analogWrite(BLDC_DRIVE_PINS[2], currentSpeed);
                analogWrite(BLDC_DRIVE_PINS[3], currentSpeed);
            } else {
                // 进入预跨越减速
                currentSpeed = map(obstacleDistance, jumpPreDistance, 0, 150, 50);
                for (int i = 0; i < 4; i++) {
                    analogWrite(BLDC_DRIVE_PINS[i], (int)currentSpeed);
                }
                jumpPhase = 1;
            }
            break;
        case 1:  // 抬腿阶段：前腿抬起跨越
            liftJoint.write(60); // 抬升关节打开，前腿抬起
            // 前腿BLDC加速抬起，后腿支撑减速
            analogWrite(BLDC_DRIVE_PINS[0], 200); // 左前腿抬升
            analogWrite(BLDC_DRIVE_PINS[1], 50);  // 右前腿支撑减速
            analogWrite(BLDC_DRIVE_PINS[2], 50);  // 左后腿支撑减速
            analogWrite(BLDC_DRIVE_PINS[3], 200); // 右后腿抬升
            delay(300); // 抬腿保持时间
            jumpPhase = 2;
            break;
        case 2:  // 落地阶段：前腿落地承重
            liftJoint.write(0); // 抬升关节复位
            // 前腿BLDC减速落地，后腿加速跟进
            analogWrite(BLDC_DRIVE_PINS[0], 80);
            analogWrite(BLDC_DRIVE_PINS[1], 200);
            analogWrite(BLDC_DRIVE_PINS[2], 200);
            analogWrite(BLDC_DRIVE_PINS[3], 80);
            delay(200);
            jumpPhase = 3;
            break;
        case 3:  // 复位阶段：恢复正常行走
            // 等待当前步态周期完成后复位
            break;
    }
    isJumping = true;
}

void maintainNormalWalk() {
    // 正常对角步态行走
    static int stepCount = 0;
    for (int i = 0; i < 4; i++) {
        if (stepCount % 2 == i % 2) {
            // 摆动腿：轻载前进
            analogWrite(BLDC_DRIVE_PINS[i], 120);
        } else {
            // 支撑腿：承重前进
            analogWrite(BLDC_DRIVE_PINS[i], 180);
        }
    }
    stepCount++;
    if (stepCount > 100) stepCount = 0;
}

void resetJumpState() {
    isJumping = false;
    jumpPhase = 0;
    // 恢复正常行走速度
    currentSpeed = 150;
    for (int i = 0; i < 4; i++) {
        analogWrite(BLDC_DRIVE_PINS[i], (int)currentSpeed);
    }
}

要点解读：

多传感器障碍融合：结合激光雷达测距和超声波测高，精准识别障碍的尺寸与距离，避免单一传感器的检测盲区，为跨越决策提供可靠数据支撑。
分阶段跨越策略：将跨越过程划分为“接近减速→抬腿跨越→落地承重→复位行走”四个阶段，每个阶段明确BLDC与舵机的协同动作，实现跨越过程的平稳过渡。
动态速度与扭矩调节：跨越过程中动态调整BLDC速度和扭矩——接近时减速、抬腿时前腿加速抬升/后腿支撑、落地时前腿减速承重/后腿跟进，确保跨越动作的连贯性和稳定性。
边界条件安全控制：设置跨越高度阈值和减速距离阈值，避免非必要跨越或因速度过快导致的跨越失败，同时通过数据滤波去除传感器异常值，提升决策可靠性。
跨越后快速复位：跨越完成后立即切换回正常步态，恢复正常行走速度，避免因跨越动作残留导致的运动卡顿，保证穿越的连续性和效率。

6、自主导航穿越控制程序

#include <Wire.h>
#include <I2Cdev.h>
#include <MPU6050.h>
#include <GPS.h>
#include <PID_v1.h>

// 硬件与传感器配置
MPU6050 imu;
GPSModule gps;
UltrasonicSensor leftUltra(A2, A3), rightUltra(A4, A5);
int BLDC_MOTOR_PINS[4] = {3, 5, 6, 9};

// 导航与控制参数
float targetCourse = 0;     // 目标航向角
float currentCourse = 0;    // 当前航向角
float courseError = 0;
PID coursePID(&courseError, &courseError, &targetCourse, 1.5, 0.2, 0.8);
float moveSpeed = 150;      // 移动速度（BLDC PWM）
bool pathPlanningMode = true;

// 目标点与路径规划
typedef struct {
    float lat;
    float lon;
    float heading;
} TargetPoint;
TargetPoint targetPoints[3] = {
    {31.230416, 121.473701, 45},  // 目标点1：上海东方明珠
    {39.904202, 116.407396, 90},  // 目标点2：北京天安门
    {23.129110, 113.264385, 180}  // 目标点3：广州白云山
};
int currentTargetIndex = 0;

void setup() {
    Serial.begin(115200);
    Wire.begin();
    // 初始化传感器
    if (!imu.initialize()) Serial.println("IMU初始化失败");
    if (!gps.begin(9600)) Serial.println("GPS初始化失败");
    leftUltra.begin(); rightUltra.begin();
    // 初始化PID
    coursePID.SetMode(AUTOMATIC);
    // 初始化BLDC电机
    for (int i = 0; i < 4; i++) pinMode(BLDC_MOTOR_PINS[i], OUTPUT);
    // 设置初始目标航向
    targetCourse = targetPoints[currentTargetIndex].heading;
}

void loop() {
    // 1. 传感器数据采集与融合
    updateNavigationSensors();
    // 2. 路径规划与目标更新
    if (isReachedTarget()) {
        currentTargetIndex = (currentTargetIndex + 1) % 3;
        targetCourse = targetPoints[currentTargetIndex].heading;
        Serial.print("切换至目标点"); Serial.println(currentTargetIndex + 1);
    }
    // 3. 航向闭环控制
    stabilizeHeading();
    // 4. 障碍规避与穿越控制
    avoidObstacles();
    // 5. 自主移动执行
    executeAutonomousMovement();
    delay(20);
}

void updateNavigationSensors() {
    // 更新GPS位置和航向
    gps.update();
    currentCourse = gps.getCourse(); // 获取当前航向角
    // 更新IMU姿态用于辅助稳定
    int16_t ax, ay, az, gx, gy, gz;
    imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    // 融合IMU数据修正航向误差（可选，提升航向精度）
    currentCourse = 0.95 * currentCourse + 0.05 * (currentCourse + gx * 0.1);
    // 更新两侧超声波数据
    float leftDistance = leftUltra.readDistance();
    float rightDistance = rightUltra.readDistance();
}

bool isReachedTarget() {
    // 判断是否到达当前目标点：基于GPS坐标距离判断
    float currentLat = gps.getLatitude();
    float currentLon = gps.getLongitude();
    float targetLat = targetPoints[currentTargetIndex].lat;
    float targetLon = targetPoints[currentTargetIndex].lon;
    // 简化距离计算（实际应用可采用Haversine公式）
    float distance = sqrt(pow(currentLat - targetLat, 2) + pow(currentLon - targetLon, 2));
    return distance < 0.001; // 经纬度距离约111米，0.001≈111米，可根据需求调整阈值
}

void stabilizeHeading() {
    // 计算航向误差
    courseError = targetCourse - currentCourse;
    // 处理航向误差跨越±180°的情况
    if (courseError > 180) courseError -= 360;
    if (courseError < -180) courseError += 360;
    // PID控制调整左右腿速度实现转向
    double turnOutput = coursePID.Compute();
    // 左侧电机降速/反转，右侧电机升速实现右转；反之左转
    int leftSpeed = constrain(moveSpeed - turnOutput, 0, 255);
    int rightSpeed = constrain(moveSpeed + turnOutput, 0, 255);
    // 同步控制左右两侧BLDC
    analogWrite(BLDC_MOTOR_PINS[0], leftSpeed);
    analogWrite(BLDC_MOTOR_PINS[1], rightSpeed);
    analogWrite(BLDC_MOTOR_PINS[2], leftSpeed);
    analogWrite(BLDC_MOTOR_PINS[3], rightSpeed);
}

void avoidObstacles() {
    float leftDist = leftUltra.readDistance();
    float rightDist = rightUltra.readDistance();
    float frontDist = gps.getDistanceToTarget(); // 可结合激光雷达补充
    // 障碍规避阈值
    float safeDist = 20.0;
    // 左侧障碍：右转规避
    if (leftDist < safeDist) {
        coursePID.SetTunings(1.5, 0.2, 0.8); // 临时增大转向增益
        targetCourse = currentCourse + 15; // 目标航向右偏15°
        Serial.println("左侧障碍，右转规避");
    }
    // 右侧障碍：左转规避
    else if (rightDist < safeDist) {
        targetCourse = currentCourse - 15; // 目标航向左偏15°
        Serial.println("右侧障碍，左转规避");
    }
    // 前方障碍：减速并绕行
    else if (frontDist < safeDist) {
        moveSpeed = 80; // 减速
        targetCourse = currentCourse + (random(1) > 0.5 ? 10 : -10); // 随机选择绕行方向
        Serial.println("前方障碍，减速绕行");
    } else {
        // 无障碍，恢复正常速度和目标航向
        moveSpeed = 150;
        targetCourse = targetPoints[currentTargetIndex].heading;
    }
}

void executeAutonomousMovement() {
    // 基于BLDC的自主行走控制：维持步态稳定
    // 简化：通过PWM控制电机保持前进速度，结合姿态调整补偿
    // 可扩展为更复杂的步态生成，此处结合案例1的步态逻辑
    // 注：实际需融合步态控制，此处为框架示意
    for (int i = 0; i < 4; i++) {
        // 基础前进速度叠加姿态补偿
        int baseSpeed = (i < 2) ? moveSpeed : moveSpeed;
        // 叠加姿态补偿（基于IMU数据）
        float comp = (i == 0 || i == 3) ? currentRoll * 2 : -currentRoll * 2;
        int finalSpeed = constrain(baseSpeed + comp, 0, 255);
        analogWrite(BLDC_MOTOR_PINS[i], finalSpeed);
    }
}

要点解读：

多传感器融合导航：整合GPS（全局定位）、IMU（姿态辅助）、超声波（局部避障）数据，实现“全局路径规划+局部避障”的自主导航架构，确保长距离穿越的精准性与安全性。
目标点动态切换与航向闭环：基于GPS坐标判断是否到达目标点，自动切换至下一个目标；通过PID控制航向误差，动态调整左右腿BLDC速度实现转向，保证始终沿目标航线前进。
分级障碍规避策略：针对侧方（超声波检测）和前方障碍采用分级规避——侧方障碍优先转向、前方障碍减速绕行，同时通过临时调整PID增益提升转向响应速度，兼顾导航效率与安全。
全局路径与局部步态融合：自主导航框架可扩展融合案例1的步态控制逻辑，实现“长距离路径规划+崎岖地形步态”的一体化控制，既保证目标穿越的宏观方向，又应对地形的微观挑战。
模块化可扩展架构：代码采用模块化设计（传感器采集、航向控制、避障、移动执行分离），便于扩展更多传感器（如激光雷达、视觉模块）和复杂步态（如奔跑、跳跃），适配不同穿越场景需求。

注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。