Arduino BLDC 四足仿生穿越机器人设计与实现

下面我们通过几个关键场景的代码示例，来拆解如何实现上述功能。请注意，实际编程时，要根据硬件配置、使用场景和具体需求进行调整，并多次实际测试。

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

这是最基础的移动模式，通过交替抬起对角腿来实现稳定的前进。我们使用数组管理 ESC 对象，通过循环初始化/控制，关键点在于确保所有 ESC 信号同步，避免运动不同步导致失衡。

#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 传感器）

为了应对不平坦的地面，我们需要引入 IMU 数据来动态调整抬腿高度。这里使用了 BNO055 传感器获取姿态数据，根据俯仰角调整步态高度，倾斜时增加抬腿高度以防绊倒。

#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. 无线遥控与姿态稳定控制

在实际调试阶段，无线遥控是必不可少的。这里展示了如何使用 nRF24L01 接收遥控器指令，并根据 roll/pitch 调整各腿位置，实现站立姿态的稳定调整。

#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;
    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); // 停止程序直到复位
}

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

针对更复杂的场景，我们引入了 PID 控制和互补滤波。这段代码实现了 50Hz 高频步态更新，通过 IMU 加速度计与陀螺仪数据融合获取机身姿态，并结合 PID 控制动态调整各腿支撑力，确保穿越复杂地形时机身稳定。

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

#define SERVO_COUNT 12
Servo hipJoint[4], kneeJoint[4], ankleJoint[4];
int BLDC_MOTOR_PINS[4] = {3, 5, 6, 9};

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();
  if (!imu.initialize()) {
    Serial.println("IMU 初始化失败");
    while (1);
  }
  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;
  if (millis() - lastGaitTime >= 20) {
    updateAttitude();
    float terrainAngle = detectTerrainAngle();
    adjustGaitForTerrain(terrainAngle);
    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() {
  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);
    } else {
      hipJoint[leg].write(90 - stepLength / 2);
      kneeJoint[leg].write(90 - stepHeight / 2);
      ankleJoint[leg].write(90 + stepHeight / 2);
      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() {
  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));
  }
}

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

遇到障碍物时，机器人需要判断是否跨越。这里结合了激光雷达测距和超声波测高，将跨越过程划分为'接近减速→抬腿跨越→落地承重→复位行走'四个阶段，每个阶段明确 BLDC 与舵机的协同动作。

#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};

float obstacleHeight = 0, obstacleDistance = 0;
float jumpThreshold = 15.0;
float jumpPreDistance = 30.0;
float currentSpeed = 150;
bool isJumping = false;
int jumpPhase = 0;

void setup() {
  Serial.begin(9600);
  frontUltra.begin();
  frontLidar.begin();
  liftJoint.attach(8);
  for (int i = 0; i < 4; i++) pinMode(BLDC_DRIVE_PINS[i], OUTPUT);
  liftJoint.write(0);
}

void loop() {
  detectObstacle();
  if (obstacleHeight >= jumpThreshold) {
    executeJumpOver();
  } else {
    maintainNormalWalk();
  }
  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;
        for (int i = 0; i < 4; i++) analogWrite(BLDC_DRIVE_PINS[i], 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);
      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);
      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);
}

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

最后，整合 GPS、IMU 和超声波数据，实现'全局路径规划 + 局部避障'的自主导航架构。通过 PID 控制航向误差，动态调整左右腿 BLDC 速度实现转向，保证始终沿目标航线前进。

#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;
bool pathPlanningMode = true;

typedef struct {
  float lat;
  float lon;
  float heading;
} TargetPoint;
TargetPoint targetPoints[3] = {{31.230416, 121.473701, 45}, {39.904202, 116.407396, 90}, {23.129110, 113.264385, 180}};
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();
  coursePID.SetMode(AUTOMATIC);
  for (int i = 0; i < 4; i++) pinMode(BLDC_MOTOR_PINS[i], OUTPUT);
  targetCourse = targetPoints[currentTargetIndex].heading;
}

void loop() {
  updateNavigationSensors();
  if (isReachedTarget()) {
    currentTargetIndex = (currentTargetIndex + 1) % 3;
    targetCourse = targetPoints[currentTargetIndex].heading;
    Serial.print("切换至目标点"); Serial.println(currentTargetIndex + 1);
  }
  stabilizeHeading();
  avoidObstacles();
  executeAutonomousMovement();
  delay(20);
}

void updateNavigationSensors() {
  gps.update();
  currentCourse = gps.getCourse();
  int16_t ax, ay, az, gx, gy, gz;
  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  currentCourse = 0.95 * currentCourse + 0.05 * (currentCourse + gx * 0.1);
  float leftDistance = leftUltra.readDistance();
  float rightDistance = rightUltra.readDistance();
}

bool isReachedTarget() {
  float currentLat = gps.getLatitude();
  float currentLon = gps.getLongitude();
  float targetLat = targetPoints[currentTargetIndex].lat;
  float targetLon = targetPoints[currentTargetIndex].lon;
  float distance = sqrt(pow(currentLat - targetLat, 2) + pow(currentLon - targetLon, 2));
  return distance < 0.001;
}

void stabilizeHeading() {
  courseError = targetCourse - currentCourse;
  if (courseError > 180) courseError -= 360;
  if (courseError < -180) courseError += 360;
  double turnOutput = coursePID.Compute();
  int leftSpeed = constrain(moveSpeed - turnOutput, 0, 255);
  int rightSpeed = constrain(moveSpeed + turnOutput, 0, 255);
  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;
    Serial.println("左侧障碍，右转规避");
  } else if (rightDist < safeDist) {
    targetCourse = currentCourse - 15;
    Serial.println("右侧障碍，左转规避");
  } else if (frontDist < safeDist) {
    moveSpeed = 80;
    targetCourse = currentCourse + (random(1) > 0 ? 10 : -10);
    Serial.println("前方障碍，减速绕行");
  } else {
    moveSpeed = 150;
    targetCourse = targetPoints[currentTargetIndex].heading;
  }
}

void executeAutonomousMovement() {
  for (int i = 0; i < 4; i++) {
    int baseSpeed = (i < 2) ? moveSpeed : moveSpeed;
    float comp = (i == 0 || i == 3) ? currentRoll * 2 : -currentRoll * 2;
    int finalSpeed = constrain(baseSpeed + comp, 0, 255);
    analogWrite(BLDC_MOTOR_PINS[i], finalSpeed);
  }
}

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