【花雕学编程】Arduino BLDC 之两轮自平衡机器人

Arduino BLDC之两轮自平衡机器人

1. 概述
   两轮自平衡机器人是一种利用控制算法保持平衡的移动机器人，通常依靠Arduino控制的无刷直流电机（BLDC）实现平稳的运动和自我平衡。通过传感器（如陀螺仪和加速度计）实时监测机器人的倾斜角度，控制系统能够自动调整电机的速度和方向，从而维持机器人的直立状态。
   主要特点：
   • 自我平衡：通过控制算法，机器人能够在各种环境中保持稳定，避免倾倒。
   • 灵活移动：具备良好的机动性和灵活性，可以在狭小空间内自如移动。
   • 实时反馈：利用传感器提供实时数据，系统能够快速响应环境变化，实现平稳运动。
   • 适应性强：能够适应不同的地面条件和环境，具有较强的适应能力。
2. 应用场景
   两轮自平衡机器人在多个领域具有广泛的应用，包括：
   • 教育与科研：作为控制理论和机器人技术的实验平台，帮助学生和研究人员理解自平衡控制和机器人运动。
   • 娱乐与游戏：可以作为智能玩具或竞赛机器人，提供互动和娱乐体验。
   • 物流与搬运：在一些小型仓储或物流环境中，作为自动搬运工具，提高效率。
   • 服务机器人：在家庭或公共场所，能够作为移动服务机器人，提供信息咨询或物品递送。
3. 注意事项
   在实现两轮自平衡机器人时，需要关注以下事项：
   • 传感器选择与校准：选择高精度的传感器（如MPU6050陀螺仪），并进行适当校准，以确保倾斜角度的准确测量。
   • 控制算法：使用合适的控制算法（如PID控制、LQR控制等），以提高自平衡的稳定性和响应速度。
   • 电机匹配：确保BLDC电机的规格与负载需求相匹配，以避免过载或性能不足。
   • 系统延迟：考虑信号传输和处理的延迟，设计合适的补偿机制以提高控制精度。
   • 电源管理：合理设计电源系统，确保Arduino和电机在各种工作状态下均能获得稳定的电力供应。
   • 调试与测试：在不同环境和工作条件下进行充分的调试与测试，以验证系统的稳定性和可靠性，确保自平衡效果。
   • 安全机制设计：设计必要的安全机制，如过载保护和紧急停机功能，以确保机器人在异常情况下能够安全停机。

1、基础姿态读取

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

void setup() {
    Serial.begin(9600);
    Wire.begin();
    mpu.initialize();

    if (!mpu.testConnection()) {
        Serial.println("MPU6050连接失败");
    }
}

void loop() {
    int16_t ax, ay, az;
    int16_t gx, gy, gz;

    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    Serial.print("加速度: ");
    Serial.print("X: "); Serial.print(ax);
    Serial.print(", Y: "); Serial.print(ay);
    Serial.print(", Z: "); Serial.println(az);

    Serial.print("角速度: ");
    Serial.print("X: "); Serial.print(gx);
    Serial.print(", Y: "); Serial.print(gy);
    Serial.print(", Z: "); Serial.println(gz);

    delay(500);
}

要点解读：
基础读取：使用 MPU6050 传感器获取加速度和角速度信息，适合初学者了解传感器数据获取。
串口输出：通过串口输出加速度和角速度，便于实时监测设备的姿态变化。
简单结构：代码结构清晰，易于理解和扩展。

2、基础自平衡控制

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

MPU6050 mpu;
Servo leftMotor;
Servo rightMotor;

void setup() {
    Serial.begin(9600);
    Wire.begin();
    mpu.initialize();
    leftMotor.attach(9);
    rightMotor.attach(10);

    if (!mpu.testConnection()) {
        Serial.println("MPU6050连接失败");
    }
}

void loop() {
    int16_t ax, ay, az;
    int16_t gx, gy, gz;

    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    float angle = atan2(ax, az) * 180 / PI; // 计算倾斜角度

    // 控制电机
    if (angle > 5) {
        leftMotor.write(0); // 向前
        rightMotor.write(180); // 向后
    } else if (angle < -5) {
        leftMotor.write(180); // 向后
        rightMotor.write(0); // 向前
    } else {
        leftMotor.write(90); // 停止
        rightMotor.write(90); // 停止
    }

    Serial.print("倾斜角度: "); Serial.println(angle);
    delay(100);
}

要点解读：
自平衡控制：通过计算倾斜角度（利用加速度计数据），控制电机的转动来保持平衡。
简单判断：设定阈值（如 5°）来决定电机的转动方向，控制逻辑简单易懂。
实时反馈：输出倾斜角度，便于观察平衡状态。

3、PID 控制实现精准自平衡

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

MPU6050 mpu;
Servo leftMotor;
Servo rightMotor;

// PID 参数
double Kp = 2.0, Ki = 0.5, Kd = 1.0;
double integral = 0, lastError = 0;

void setup() {
    Serial.begin(9600);
    Wire.begin();
    mpu.initialize();
    leftMotor.attach(9);
    rightMotor.attach(10);

    if (!mpu.testConnection()) {
        Serial.println("MPU6050连接失败");
    }
}

void loop() {
    int16_t ax, ay, az;
    mpu.getMotion6(&ax, &ay, &az);

    float angle = atan2(ax, az) * 180 / PI; // 计算倾斜角度
    double error = angle; // 目标角度为 0

    // PID 控制
    integral += error;
    double derivative = error - lastError;
    double output = Kp * error + Ki * integral + Kd * derivative;
    lastError = error;

    // 限制输出范围
    output = constrain(output, -180, 180);

    // 控制电机
    if (output > 0) {
        leftMotor.write(90 + output); // 向前
        rightMotor.write(90 - output); // 向后
    } else {
        leftMotor.write(90 + output); // 向后
        rightMotor.write(90 - output); // 向前
    }

    Serial.print("倾斜角度: "); Serial.print(angle);
    Serial.print(", PID 输出: "); Serial.println(output);
    delay(100);
}

要点解读：
PID 控制：使用 PID 控制算法实现精准的自平衡，适合需要高稳定性的应用。
误差处理：通过计算倾斜角度与目标角度（0°）的误差，动态调整电机输出。
输出限制：使用 constrain() 函数限制电机输出范围，确保电机在安全范围内工作。

4、基础PID平衡控制（仅保持直立）

#include <Wire.h>
#include <MPU6050.h>
#include <SimpleFOC.h> // 需安装SimpleFOC库支持BLDC控制

MPU6050 mpu;
BLDCMotor motor = BLDCMotor(7); // 7对极电机
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8); // PWM引脚+使能引脚

// PID参数
float Kp = 1.0, Ki = 0.1, Kd = 0.05;
float targetAngle = 0.0; // 目标角度（直立时为0度）
float currentAngle = 0.0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050连接失败");
    while (1);
  }

  // 电机初始化
  driver.voltage_sensor_align = 3.0; // 对齐电压
  driver.init();
  motor.linkDriver(&driver);
  motor.init();
  motor.initFOC(); // 感应电机相位
}

void loop() {
  // 读取MPU6050数据（需实现互补滤波或卡尔曼滤波）
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  currentAngle = getFilteredAngle(ax, ay, az, gx, gy, gz); // 需自定义滤波函数

  // PID计算
  float error = targetAngle - currentAngle;
  static float integral = 0, prevError = 0;
  integral += error;
  float derivative = error - prevError;
  prevError = error;

  float output = Kp * error + Ki * integral + Kd * derivative;
  motor.move(output); // 控制电机

  Serial.print("角度: "); Serial.print(currentAngle);
  Serial.print(" | 输出: "); Serial.println(output);
  delay(10);
}

// 简易互补滤波示例（实际建议用卡尔曼滤波）
float getFilteredAngle(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz) {
  float accAngle = atan2(ay, az) * RAD_TO_DEG; // 加速度计角度
  static float gyroAngle = 0;
  gyroAngle += gx * 0.01; // 假设10ms采样周期
  return 0.98 * gyroAngle + 0.02 * accAngle; // 互补滤波
}

要点解读：

传感器融合：加速度计测倾角，陀螺仪测角速度，互补滤波（或卡尔曼滤波）融合数据。
PID调参：Kp过大会震荡，Kd需配合Kp调整，Ki用于消除稳态误差。
BLDC驱动：使用SimpleFOC库简化FOC（磁场定向控制）实现，需正确配置电机极对数和驱动引脚。
局限性：仅平衡无移动，需扩展编码器反馈实现速度控制。

5、速度环+姿态环双闭环控制

#include <Encoder.h>
#include <SimpleFOC.h>

// 硬件定义（同案例4）
Encoder encoder = Encoder(2, 3, 500); // 编码器A/B相，每转500脉冲
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);

// PID参数
float angleKp = 1.2, angleKd = 0.1; // 姿态环PID
float speedKp = 0.5, speedKi = 0.01; // 速度环PID
float targetSpeed = 0.0; // 目标速度（rpm）

void setup() {
  Serial.begin(115200);
  encoder.init();
  motor.linkSensor(&encoder);
  driver.init();
  motor.linkDriver(&driver);
  motor.init();
  motor.initFOC();
}

void loop() {
  // 1. 读取编码器速度
  float currentSpeed = encoder.getVelocity(); // rpm

  // 2. 速度环PID（输出作为姿态环的目标角度）
  static float speedIntegral = 0;
  float speedError = targetSpeed - currentSpeed;
  speedIntegral += speedError;
  float angleTarget = speedKp * speedError + speedKi * speedIntegral;

  // 3. 姿态环PID（控制电机输出）
  float currentAngle = getFilteredAngle(); // 同案例4
  float angleError = angleTarget - currentAngle;
  static float prevAngleError = 0;
  float angleDerivative = angleError - prevAngleError;
  prevAngleError = angleError;

  float motorOutput = angleKp * angleError + angleKd * angleDerivative;
  motor.move(motorOutput);

  Serial.print("速度: "); Serial.print(currentSpeed);
  Serial.print(" | 角度: "); Serial.println(currentAngle);
  delay(10);
}

要点解读：

双闭环结构：
速度环：外环，根据目标速度计算所需倾斜角度。
姿态环：内环，通过调整电机扭矩维持计算出的倾斜角度。
编码器反馈：必需实时速度反馈，否则速度控制会发散。
参数耦合：speedKp直接影响系统响应速度，需与angleKp匹配调试。
启动问题：机器人需外力推动启动（或加入启动策略如“倾倒触发”）。

6、遥控平衡车（蓝牙+平衡控制）

#include <SoftwareSerial.h>
#include <SimpleFOC.h>

SoftwareSerial bluetooth(4, 5); // RX, TX
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
Encoder encoder = Encoder(2, 3, 500);

// PID参数（同案例二）
float angleKp = 1.2, angleKd = 0.1;
float speedKp = 0.5, speedKi = 0.01;
float targetSpeed = 0.0;

void setup() {
  Serial.begin(115200);
  bluetooth.begin(9600);
  encoder.init();
  motor.linkSensor(&encoder);
  driver.init();
  motor.linkDriver(&driver);
  motor.init();
  motor.initFOC();
}

void loop() {
  // 1. 蓝牙指令解析
  if (bluetooth.available()) {
    char cmd = bluetooth.read();
    if (cmd == 'F') targetSpeed = 50.0;  // 前进
    else if (cmd == 'B') targetSpeed = -50.0; // 后退
    else targetSpeed = 0.0; // 停止
  }

  // 2. 双闭环控制（同案例二）
  float currentSpeed = encoder.getVelocity();
  static float speedIntegral = 0;
  float speedError = targetSpeed - currentSpeed;
  speedIntegral += speedError;
  float angleTarget = speedKp * speedError + speedKi * speedIntegral;

  float currentAngle = getFilteredAngle();
  float angleError = angleTarget - currentAngle;
  static float prevAngleError = 0;
  float angleDerivative = angleError - prevAngleError;
  prevAngleError = angleError;

  motor.move(angleKp * angleError + angleKd * angleDerivative);

  // 调试输出
  Serial.print("目标速度: "); Serial.print(targetSpeed);
  Serial.print(" | 当前速度: "); Serial.println(currentSpeed);
}

要点解读：

蓝牙通信：使用SoftwareSerial实现非阻塞通信，避免延迟。
指令设计：简单字符指令（‘F’/‘B’）控制方向，可扩展为速度分级。
安全机制：建议增加倾倒检测（如角度超过±30°时停机）。
扩展功能：
加入超声波模块实现防撞。
通过APP发送更复杂的速度曲线。

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