使用Arduino的DIY自平衡机器人

原文地址：https://circuitdigest.com/microcontroller-projects/arduino-based-self-balancing-robot

使用Arduino的DIY自平衡机器人

ARDUINO项目

在受到Segway的RYNO马达和其他自平衡踏板车的启发之后，我一直想构建自己的Arduino Segway机器人。考虑了一下，我决定使用Arduino构建自平衡机器人。这样，我将能够掌握所有这些踏板车背后的基本概念，并了解PID算法的工作原理。

开始构建后，我意识到该机器人很难构建。有太多选项可供选择，因此混淆会从选择电动机开始，并一直持续到调整PID值为止。还有很多事情要考虑，例如电池类型，电池位置，车轮抓地力，电动机驱动器类型，维护CoG（重心)等等。

但是，让我将它交给您，一旦您构建它，您将同意它并不像听起来那样难。因此，让我们面对现实吧，在本教程中，我将记录我在构建自平衡机器人方面的经验。您可能是一个刚入门的绝对初学者，或者在长时间没有使您的机器人工作后感到沮丧之后落入了这里。这个地方旨在成为您的最终目的地。因此，让我们开始吧……

选择自平衡机器人的零件

在我告诉您构建机器人的所有选项之前，让我列出一下我在此自平衡机器人项目中使用的项目

• Arduino UNO
• 减速直流电动机（黄色)– 2个
• L298N电机驱动器模块
• MPU6050
• 一双轮子
• 7.4V锂离子电池
• 连接线
• 3D打印体

您可以根据可用性混合选择上述任意一种组件，以制作自己的自平衡机器人套件，只需确保这些组件符合以下条件即可。

控制器： 我在这里使用的控制器是Arduino UNO，这是为什么，因为它简单易用。您也可以使用Arduino Nano或Arduino mini，但我建议您坚持使用UNO，因为我们无需任何外部硬件即可直接对其进行编程。

电动机： 毫无疑问，可用于自平衡机器人的电动机最好是步进电动机。但是为了简单起见，我使用了直流齿轮电动机。是的，不是必须要有步进器。该机器人还可以与这些便宜的常用黄色直流齿轮电动机一起很好地工作。

**电动机驱动器：**如果您选择了像我这样的直流齿轮电动机，则可以像我一样使用L298N驱动器模块，甚至L293D都可以正常工作。了解有关使用L293D和Arduino控制直流电动机的更多信息。

**Wheels：**不要低估这些家伙；我很难弄清楚问题出在我的车轮上。因此，请确保您的轮子在使用的地板上具有良好的抓地力。仔细观察，抓地力绝对不能让车轮在地板上打滑。

加速度计和陀螺仪：机器人的加速度计和陀螺仪的最佳选择是MPU6050。因此，请勿尝试使用像ADXL345之类的普通加速度计来构建传感器，否则将无法正常工作。您将在本文结尾处知道为什么。您也可以查看有关将MPU6050与Arduino结合使用的专门文章。

电池： 我们需要一个尽可能轻的电池，并且工作电压应大于5V，这样我们就可以在没有升压模块的情况下直接为Arduino供电。因此，理想的选择将是7.4V锂聚合物电池。在这里，由于我有一个随时可用的7.4V锂离子电池，因此我使用了它。但是请记住，锂电池比锂离子电池更具优势。

底盘： 另一个您不应该妥协的地方是机器人底盘。您可以使用硬纸板，木材，塑料等任何您喜欢的东西。但是，只要确保底盘坚固，并且在机器人试图进行平衡时就不要晃动。我根据自己在Solidworks上的机箱设计了其他机器人并进行3D打印。如果您有打印机，那么您也可以打印设计，设计文件将附加在接下来的标题中。

3D打印和组装我们的自动平衡机器人

如果您决定用3D打印与我用来构建bot的机箱相同的机箱，则可以从THiverse下载STL文件。我还一起添加了设计文件，因此您还可以根据人员的喜好对其进行修改。

这些零件没有悬垂结构，因此您可以在没有任何支撑的情况下轻松打印它们，并且填充25％即可正常工作。设计非常简单，任何基本打印机都应该能够轻松处理它。我使用Cura软件对模型进行切片，然后使用Tevo Tarantula进行打印，设置如下所示。

您将必须打印车身部件以及四个电机安装部件。组装很简单。使用3毫米螺母和螺栓将电动机和电路板固定到位。组装后，其外观应如下图所示。

如上图所示，实际设计是通过Arduino底部机架中的L298N驱动器模块和其顶部的电池进行的。如果您遵循相同的顺序，则可以直接通过提供的孔将板拧紧，并为Li-po电池使用接线标签。除了我以后必须更换的超级普通轮之外，这种安排也应该起作用。

在我的机器人程序中，我将电池和Arduino UNO板的位置换成了便于编程的位置，还不得不引入一块perf板来完成连接。所以我的机器人看上去并不像我最初计划的那样。完成接线编程测试和所有步骤之后，我的两轮机器人终于看起来像这样

电路图

为基于Arduino的自平衡机器人建立连接非常简单。这是一个使用Arduino和MPU6050的**自平衡机器人，**因此我们必须将MPU6050与Arduino接口并通过电机驱动器模块连接电机。整个设置由7.4V锂离子电池供电。相同的电路图如下所示。

Arduino和L298N电机驱动器模块分别通过Vin引脚和12V端子直接供电。Arduino板上的板载稳压器会将输入7.4V转换为5V，ATmega IC和MPU6050将由其供电。直流电动机的工作电压范围为5V至12V。但是，我们将7.4V正极线从电池连接到电动机驱动器模块的12V输入端子。这将使电动机在7.4V电压下运行。下表将列出MPU6050和L298N电机驱动器模块如何与Arduino连接。

组件引脚	Arduino引脚
MPU6050
Vcc	+5伏
地面	nd
SCL	A5
SDA	A4
INT	D2
L298N
IN1	D6
IN2	D9
IN3	D10
IN4	D11

MPU6050通过I2C接口与Arduino通信，因此我们使用Arduino的SPI引脚A4和A5。直流电动机分别连接到PWM引脚D6，D9，D10和D11。我们需要将它们连接到PWM引脚，因为我们将通过改变PWM信号的占空比来控制直流电动机的速度。如果您不熟悉这两个组件，则建议通读MPU6050接口和L298N电机驱动程序教程。

自平衡机器人代码

现在，我们必须对Arduino UNO板进行编程以平衡机器人。这是所有魔术发生的地方。它背后的概念很简单。我们必须使用MPU6050检查机器人是否朝前或朝后倾斜，然后如果朝前倾斜，则必须向前旋转车轮，如果朝后倾斜，则必须旋转车轮在相反的方向。

同时，我们还必须控制车轮的旋转速度，如果机器人从中心位置稍微偏离方向，车轮将缓慢旋转，并且随着距离中心位置越来越远，速度会增加。为了实现此逻辑，我们使用PID算法，该算法的中心位置为设定点，而定向度为输出。

要知道机器人的当前位置，我们使用MPU6050，它是6轴加速度计和陀螺仪传感器的组合。为了从传感器获得可靠的位置值，我们需要同时使用加速度计和陀螺仪的值，因为来自加速度计的值存在噪声问题，并且陀螺仪的值倾向于随时间漂移。因此，我们必须将两者结合起来，以获取机器人的偏航俯仰和侧倾的值，其中，我们将仅使用偏航的值。

听起来有点head头吧？但是不用担心，感谢Arduino社区，我们拥有易于使用的库，可以执行PID计算，还可以从MPU6050获得偏航的值。该库分别由br3ttb和jrowberg开发。在继续下载它们的库之前，请通过以下链接将它们添加到您的Arduino lib目录中。

https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h

https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

现在，我们已经将库添加到了Arduino IDE中。让我们开始为我们的自平衡机器人编程。就像通常在本页面的结尾处给出MPU6050平衡机器人的完整代码一样，这里我只是在解释代码中最重要的代码片段。前面已经说过，该代码建立在MPU6050示例代码的基础上，我们将为此目的优化代码，并为自平衡机器人添加PID和控制技术。

首先，我们包括该程序正常运行所需的库。它们包括我们刚刚下载的内置I2C库，PID库和MPU6050库。

#include“ I2Cdev.h” 
#include <PID_v1.h> //来自https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h 
#include“ MPU6050_6Axis_MotionApps20.h” // https ：//github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

然后，我们声明从MPU6050传感器获取数据所需的变量。我们读取重力矢量和四元数值，然后计算机器人的偏航俯仰和侧倾值。所述*浮子阵列YPR[3]*将持有的最终结果。

// MPU控制/状态变量
bool dmpReady = false; //如果DMP初始化成功，则设置为true 
uint8_t mpuIntStatus; //保存来自MPU的实际中断状态字节；
uint8_t devStatus; //每次设备操作后返回状态（0 =成功，！0 =错误)uint16_t packetSize; //预期的DMP数据包大小（默认为42个字节)
uint16_t fifoCount; //当前FIFO中所有字节的计数
uint8_t fifoBuffer[64]; // FIFO存储缓冲区

//方向/运动变量
Quaternion q; //[w，x，y，z]四元数容器
VectorFloat gravity；//[x，y，z]引力向量
float ypr[3]; //[偏航，俯仰，滚动]偏航/俯仰/滚动容器和重力矢量

接下来是代码中非常重要的部分，这是您将花费大量时间调整正确的值集的地方。如果您的机器人的重心很好，并且组件对称布置（在大多数情况下不是这样)，则设定点的值为180。否则，将您的机器人连接到Arduino串行监视器并倾斜直到您找到一个良好的平衡位置，读取串行监视器上显示的值，这是您的设定值。必须根据您的机器人调整Kp，Kd和Ki的值。没有两个相同的机器人会具有相同的Kp，Kd和Ki值，因此不会逃避。观看本页结尾的视频，以了解如何调整这些值。

/ *********调整BOT的这4个值********* /
double setpoint= 176; //使用串行监视器在机器人垂直于地面时设置值。
//阅读circuitdigest.com上的项目文档，以了解如何将这些值设置为
double Kp = 21; //设置第一个
double Kd = 0.8; //将此秒数设置为
double Ki = 140; //最后设置此
/ ******值结束设置********* /

在下一行中，我们通过传递输入变量input，output，设定点，Kp，Ki和Kd来初始化PID算法。其中，我们已经在上面的代码片段中设置了设定点Kp，Ki和Kd的值。输入的值将是从MPU6050传感器读取的当前偏航值，而输出的值将是由PID算法计算出的值。因此，基本上，PID算法将为我们提供一个输出值，该输出值应用于将输入值校正为接近设定点。

PID pid(&input，&output，&setpoint，Kp，Ki，Kd，DIRECT);

在*void设置*功能内部，我们通过配置DMP（数字运动处理器)来初始化MPU6050。这将有助于我们将加速度计数据与陀螺仪数据结合起来，并提供可靠的偏航角，俯仰角和横滚角值。我们不会对此进行深入研究，因为它将远远超出本主题。无论如何，您必须在设置功能中查找的一段代码是陀螺偏移值。每个MPU6050传感器都有自己的偏移值，您可以使用此Arduino草图计算传感器的偏移值，并在程序中相应地更新以下几行。

    //在此处提供您自己的陀螺仪偏移量，以最小灵敏度缩放；
    mpu.setXGyroOffset（220)
    mpu.setYGyroOffset（76); 
    mpu.setZGyroOffset（-85); 
    mpu.setZAccelOffset（1688);

我们还必须初始化用于连接电机的数字PWM引脚。在我们的例子中是D6，D9，D10和D11。因此，我们将这些引脚初始化为默认情况下的输出引脚使其变为低电平。

//初始化马达输出引脚
    pinMode（6，OUTPUT); 
    pinMode（9，输出); 
    pinMode（10，输出); 
    pinMode（11，输出);

//默认情况下，关闭两个电机的
    analogWrite（6，LOW); 
    AnalogWrite（9，LOW); 
    AnalogWrite（10，LOW); 
    AnalogWrite（11，LOW);

在主*循环*功能中，我们检查MPU6050的数据是否已准备好读取。如果是，则使用它来计算PID值，然后在串行监视器上显示PID的输入和输出值，只是为了检查PID的响应方式。然后，根据输出值，我们决定机器人是必须前进还是后退或静止不动。

由于我们假设机器人直立时，MPU6050将返回180。当bot朝前落下时，我们将得到正的校正值;如果bot朝后落下，我们将得到负的校正值。因此，我们检查这种情况并调用适当的函数来使机器人前进或后退。

 while (!mpuInterrupt && fifoCount < packetSize)
    {
        //no mpu data - performing PID calculations and output to motors    
        pid.Compute();  
       
        //Print the value of Input and Output on serial monitor to check how it is working.
        Serial.print(input); Serial.print(" =>"); 		           Serial.println(output);
              
        if (input>150 && input<200){//If the Bot is falling
         
        if (output>0) //Falling towards front
        Forward(); //Rotate the wheels forward
        else if (output<0) //Falling towards back
        Reverse(); //Rotate the wheels backward
        }
        else //If Bot not falling
        Stop(); //Hold the wheels still
    }
         

该PID输出变量也决定了马达具有多快被旋转。如果机器人即将掉落，则可以通过缓慢旋转车轮来进行较小的校正。如果这些较小的校正工作正常，并且机器人仍然掉落，我们将提高电动机的速度。车轮旋转多快的值将由PI算法确定。请注意，对于Reverse函数，我们将输出值乘以-1，以便可以将负值转换为正值。

void Forward（)//使车轮向前旋转的代码
{ 
    AnalogWrite（6，output); 
    AnalogWrite（9,0); 
    AnalogWrite（10，输出); 
    AnalogWrite（11,0); 
    Serial.print（“ F”); //调试信息
}

void Reverse（)//使轮子向后旋转的代码
{ 
    AnalogWrite（6,0); 
    AnalogWrite（9，output * -1); 
    AnalogWrite（10,0); 
    AnalogWrite（11，output * -1); 
   Serial.print（“ R”); 
}

void Stop（)//停止两个车轮的代码
{ 
    AnalogWrite（6,0); 
    AnalogWrite（9,0); 
    AnalogWrite（10,0); 
    AnalogWrite（11,0); 
    Serial.print（“ S”); 
}

Arduino自平衡机器人的工作

一旦准备好硬件，就可以将代码上传到Arduino板上。由于我们使用的是锂离子电池，因此请确保连接正确。因此，请仔细检查是否存在短路，并确保即使您的机器人受到一些小的影响，也不会接触到端子。打开模块电源并打开串行监视器，如果您的Arduino可以成功与MPU6050通信，并且一切正常，那么您应该看到以下屏幕。

在这里，我们以输入=>输出的格式查看PID算法的输入和输出值**。如果机器人达到完美平衡，则输出值为0**。输入值为MPU6050传感器的当前值。字母“ F”表示机器人正在向前移动，字母“ R”表示机器人正在反向移动。

在PID的初始阶段，我建议您将Arduino电缆保持与机器人的连接，以便您可以轻松地监视输入和输出的值，并且可以很容易地校正和上传程序的Kp，Ki和Kd值。以下**视频显示了该漫游器的完整工作，**还显示了如何更正您的PID值。

如果您在使用它时遇到任何问题，希望这有助于构建您自己的自平衡机器人，然后将您的问题留在下面的评论部分，或使用论坛中的其他技术性问题。如果您想获得更多的乐趣，您还可以使用相同的逻辑来构建一个球平衡机器人。

/*Arduino Self Balancing Robot
 * Code by: B.Aswinth Raj
 * Build on top of Lib: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
 * Website: circuitdigest.com 
 */

#include "I2Cdev.h"
#include <PID_v1.h> //From https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h
#include "MPU6050_6Axis_MotionApps20.h" //https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           //[w, x, y, z]         quaternion container
VectorFloat gravity;    //[x, y, z]            gravity vector
float ypr[3];           //[yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

 

/*********Tune these 4 values for your BOT*********/
double setpoint= 176; //set the value when the bot is perpendicular to ground using serial monitor. 
//Read the project documentation on circuitdigest.com to learn how to set these values
double Kp = 21; //Set this first
double Kd = 0.8; //Set this secound
double Ki = 140; //Finally set this 
/******End of values setting*********/

double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

 

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
    mpuInterrupt = true;
}

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

  // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

     // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // load and configure the DMP
    devStatus = mpu.dmpInitialize();

    
    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1688); 

      // make sure it worked (returns 0 if so)
    if (devStatus == 0)
    {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
        
        //setup PID
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-255, 255);  
    }
    else
    {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

//Initialise the Motor outpu pins
    pinMode (6, OUTPUT);
    pinMode (9, OUTPUT);
    pinMode (10, OUTPUT);
    pinMode (11, OUTPUT);

//By default turn off both the motors
    analogWrite(6,LOW);
    analogWrite(9,LOW);
    analogWrite(10,LOW);
    analogWrite(11,LOW);
}

 

void loop() {
 
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize)
    {
        //no mpu data - performing PID calculations and output to motors     
        pid.Compute();   
        
        //Print the value of Input and Output on serial monitor to check how it is working.
        Serial.print(input); Serial.print(" =>"); Serial.println(output);
               
        if (input>150 && input<200){//If the Bot is falling 
          
        if (output>0) //Falling towards front 
        Forward(); //Rotate the wheels forward 
        else if (output<0) //Falling towards back
        Reverse(); //Rotate the wheels backward 
        }
        else //If Bot not falling
        Stop(); //Hold the wheels still
        
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    }
    else if (mpuIntStatus & 0x02)
    {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        mpu.dmpGetQuaternion(&q, fifoBuffer); //get value for q
        mpu.dmpGetGravity(&gravity, &q); //get value for gravity
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //get value for ypr

        input = ypr[1] * 180/M_PI + 180;

   }
}

void Forward() //Code to rotate the wheel forward 
{
    analogWrite(6,output);
    analogWrite(9,0);
    analogWrite(10,output);
    analogWrite(11,0);
    Serial.print("F"); //Debugging information 
}

void Reverse() //Code to rotate the wheel Backward  
{
    analogWrite(6,0);
    analogWrite(9,output*-1);
    analogWrite(10,0);
    analogWrite(11,output*-1); 
    Serial.print("R");
}

void Stop() //Code to stop both the wheels
{
    analogWrite(6,0);
    analogWrite(9,0);
    analogWrite(10,0);
    analogWrite(11,0); 
    Serial.print("S");
}