平衡小車(balance-Robot)-基本平衡-Arduino

 平衡小車(balance-Robot)-基本平衡

參考-https://circuitdigest.com/microcontroller-projects/arduino-based-self-balancing-robot

前言

申明一下，有些原理和程式是借助一些原創者的文章做為參照，因為平衡車

如果真的要一步步從硬體和程式全包，須要花很多時間學習，尤其是ＰＩＤ

的寫法算法，慶幸的是有很多創者前人不吝分享，所以我這後人才得已少走

歧途。

平衡車原理

平衡小車是通過兩個電機運動下實現小車不倒下直立行走的多功能智能小

車，在外力的推拉下，小車依然保持不倒下。通過負反饋實現平衡。與上面保

持木棒直立比較則相對簡單，因為小車有兩個輪子著地，車體只會在輪子滾動

的方向上發生傾斜。控制輪子轉動，抵消在一個維度上傾斜的趨勢便可以保持

車體平衡了。

所以根據上述的原理，通過測量小車的傾角和傾角速度控制小車車輪的加速度

來消除小車的傾角。因此，小車傾角以及傾角速度的測量成為控制小車直立

的關鍵。我們的亞博智能平衡小車使用了測量傾角和傾角速度的集成傳感器陀

螺儀-MPU6050。

PID基礎原理

PID調節系統PID功能由PID調節器或DCS系統內部功能程序模塊實現，了解與PID調節相關的一些基本概念，有助於PID入門新手快速熟悉調節器應用，在自動調節系統中成功整定PID參數。

1、被調量

被調量就是反映被調對象的實際波動的量值。被調量是經常變化的。

2、設定值

PID調節器設定值就是人們期待被調量需要達到的值。設定值可以是固定的，也可以是變化的。

3、控制輸出

    控制輸出指PID調節器根據被調量的變化情況運算之後發出的讓外部執行結構按照它的要求動作的指令。在PID調節器和執行機構之間還會有其他環節，比如限幅、伺服放大器等。限幅功能通常在PID調節器內完成；如果如果將PID、限幅和伺服放大器功能做在一台儀表內就構成閥位控制PID調節器；將伺服放大器和限幅做在執行機構裡就構成智能執行機構。

4、輸入偏差

輸入偏差時被調量和設定值之間的差值

5、P（比例）

P就是比例作用，簡單說就是輸入偏差乘以一個係數。

6、I（積分）

I就是積分，簡單說就是將輸入偏差進行積分運算。

7、D（微分）

D就是微分，簡單說就是將輸入偏差進行微分運算

8、PID基本公式

PID調節器參數整定過程通俗講就是先把系統調為純比例作用，逐步增強比例作用讓系統振盪，記錄下比例作用和振盪週期，然後這個比例作用乘以0.6，積分作用適當延長

KP=0.6 Km

KD= KP×π/4ω

KI= KP×ω/π

     公式中KP為比例控制參數；KI為積分控制參數；KD為微分控制參數；Km為系統開始振盪是的比例值；ω為極坐標下振盪時的頻率

9、動態偏差

在調節過程中，被調量和設定值之間的偏差隨時改變，任意時刻兩者之間的偏差叫做動態偏差。

10、靜態偏差

    調節趨於穩定之後，被調量和設定值之間還存在的偏差交靜態偏差。消除靜態偏差是通過PID調節器積分作用來實現的。

其實一開始看起來很複雜，但原理學習是必須的，還好現在有一種PID的函式開源，減輕了想學平衡車的小白壓力。

小車結構

平衡車主要用的電機馬達常見的是編碼式的，其實用一般馬達也可以，差別在於穩定性，因為第一次做，所以我選用一般電機馬達。

實體如果是自己自制，那配置平衡就要留意些，也就是說在配置硬體時，靜態的狀態下最好能有一定的平衡點。

馬達的部份，在安置兩邊馬達，減速組和地面呈垂直，車體才不會偏向前後變得不平衡。

接線方式

這裡要特別注意，雖然圖上畫的L298N用一組電池供應L298N和Arduino，但我個人搞了一天才發現，最好L298N和UNO分開供電。

元件引脚	Arduino引脚
MPU6050
VCC	+ 5V
Ground	GND
SCL	A5
SDA	A4
INT	D2
L298N
IN1	D6
IN2	D9
IN3	D10
IN4	D11

編輯程式

來源參考:https://www.yiboard.com/thread-939-1-1.html

現在我們需要對Arduino UNO開發板進行編程以平衡機器人。這就是所有神奇魔法發生的地方；它背後的概念很簡單。我們需要通過使用MPU6050來檢查機器人是否向前傾斜或向後傾斜，然後如果它向前傾斜我們必須向前旋轉車輪，如果它向後傾斜我們必須在相反的方向旋轉車輪。

同時我們還必須控制車輪旋轉的速度，如果機器人從中心位置略微迷失方向，車輪會緩慢旋轉，並且當車輛離中心位置越來越遠時速度會增加。為了實現這個邏輯，我們使用PID算法，其中心位置為設定點，迷失方向的強度為輸出。

要知道機器人的當前位置，我們使用MPU6050，它是一個6軸加速度計和陀螺儀傳感器相結合。為了從傳感器獲得可靠的位置值，我們需要使用加速度計和陀螺儀的值，因為加速度計的值存在噪聲問題，陀螺儀的值會隨時間漂移。因此，我們必須結合兩者並獲得我們的機器人的偏航俯仰和滾動的值，其中我們將僅使用偏航的值。

聽起來有點頭暈嗎？但是不用擔心，在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示例代碼之上的，我們只是為了目的優化代碼，並為我們的自平衡機器人添加PID和控制技術。

首先，我們包括此程序工作所需的庫。包括內置I2C庫、我們剛剛下載的PID庫和MPU6050庫。

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

然後我們聲明從MPU6050傳感器獲取數據所需的變量。我們讀取了重力矢量和四元數值，然後計算機器人的偏航俯仰和滾轉值。浮點數組ypr [3]將保存最終結果。

1. // MPU control/status vars
   
2. bool dmpReady = false;  // set true if DMP init was successful
   
3. uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
   
4. uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
   
5. uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
   
6. uint16_t fifoCount;     // count of all bytes currently in FIFO
   
7. uint8_t fifoBuffer[64]; // FIFO storage buffer
   
8. 
   
9. // orientation/motion vars
   
10. Quaternion q;           // [w, x, y, z]         quaternion container
    
11. VectorFloat gravity;    // [x, y, z]            gravity vector
    
12. float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

接下來是代碼中非常重要的部分，這是您在調整正確的值集時花費很長時間的地方。如果您的機器人具有非常好的重心並且組件對稱排列（在大多數情況下不是），那麼您的設定點的值將為180。否則將您的機器人連接到Arduino串行監視器並將其傾斜直到你找到了一個很好的平衡位置，讀取串行監視器上顯示的值，這是你的設定值。 Kp、Kd和Ki的值必鬚根據您的機器人進行調整。沒有兩個相同的機器人將具有相同的Kp、Kd和Ki值，因此沒有方法避免。

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

在接下來的代碼中，我們通過傳遞輸入變量輸入、輸出、設定點、Kp、Ki和Kd來初始化PID算法。其中我們已經在上面的代碼片段中設置了設定點Kp、Ki和Kd的值。輸入值將是從MPU6050傳感器讀取的偏航的當前值，輸出值將是PID算法計算的值。因此，基本上PID算法將為我們提供一個輸出值，該輸出值應該用於將輸入值校正為接近設定點。

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

在void setup()函數中，我們通過配置DMP（數字運動處理器）初始化MPU6050。這將有助於我們將加速度計數據與陀螺儀數據相結合，並提供可靠的偏航、俯仰和滾轉值。我們不會深入研究這一點，因為它遠遠超出了主題。無論如何，您必須在setup函數中查找的一段代碼是陀螺儀偏移值。每個MPU6050傳感器都有自己的偏移值，您可以使用此Arduino範例計算傳感器的偏移值，並在程序中相應更新以下行。

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

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

1. //Initialise the Motor outpu pins
   
2.     pinMode (6, OUTPUT);
   
3.     pinMode (9, OUTPUT);
   
4.     pinMode (10, OUTPUT);
   
5.     pinMode (11, OUTPUT);
   
6. 
   
7. //By default turn off both the motors
   
8.     analogWrite(6,LOW);
   
9.     analogWrite(9,LOW);
   
10.     analogWrite(10,LOW);
    
11.     analogWrite(11,LOW);

在loop函數內部，我們檢查MPU6050的數據是否可以讀取。如果是，則我們使用它來計算PID值，然後在串行監視器上顯示PID的輸入和輸出值，以檢查PID的響應方式。然後根據輸出值我們決定機器人是向前還是向後移動還是靜止不動。

因為我們假設當機器人直立時MPU6050將返回180。當機器人向前傾斜時，我們將獲得正值校正值，如果機器人向後傾斜，我們將得到負值。所以我們檢查這個條件並調用相應的函數來移動機器人前進或後退。

1.  while (!mpuInterrupt && fifoCount < packetSize)
   
2.     {
   
3.         //no mpu data - performing PID calculations and output to motors   
   
4.         pid.Compute();  
   
5.       
   
6.         //Print the value of Input and Output on serial monitor to check how it is working.
   
7.         Serial.print(input); Serial.print(" =>"); Serial.println(output);
   
8.               
   
9.         if (input>150 && input<200){//If the Bot is falling
   
10.          
    
11.         if (output>0) //Falling towards front
    
12.         Forward(); //Rotate the wheels forward
    
13.         else if (output<0) //Falling towards back
    
14.         Reverse(); //Rotate the wheels backward
    
15.         }
    
16.         else //If Bot not falling
    
17.         Stop(); //Hold the wheels still
    
18.     }

PID輸出變量還決定了電機旋轉的速度。如果機器人即將摔倒，那麼我們通過緩慢旋轉車輪進行微小修正。如果這些小的校正工作仍然有效，但如果機器人跌落，我們會提高電機的速度。車輪旋轉的速度值將由PI算法決定。請注意，對於Reverse函數，我們將輸出值乘以-1，以便我們可以將負值轉換為正值。

1. void Forward() //Code to rotate the wheel forward
   
2. {
   
3.     analogWrite(6,output);
   
4.     analogWrite(9,0);
   
5.     analogWrite(10,output);
   
6.     analogWrite(11,0);
   
7.     Serial.print("F"); //Debugging information
   
8. }
   
9. 
   
10. void Reverse() //Code to rotate the wheel Backward
    
11. {
    
12.     analogWrite(6,0);
    
13.     analogWrite(9,output*-1);
    
14.     analogWrite(10,0);
    
15.     analogWrite(11,output*-1);
    
16.    Serial.print("R");
    
17. }
    
18. 
    
19. void Stop() //Code to stop both the wheels
    
20. {
    
21.     analogWrite(6,0);
    
22.     analogWrite(9,0);
    
23.     analogWrite(10,0);
    
24.     analogWrite(11,0);
    
25.     Serial.print("S");
    
26. }

完整程式碼

/*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;  // 如果DMP初始化成功，则设置为1

uint8_t mpuIntStatus;   // 保存mpu中断状态

uint8_t devStatus;      // 运行后返回状态0表示成功，!0有错误

uint16_t packetSize;    // DMP数据包的大小

uint16_t fifoCount;     // 统计当前在FIFO的字节数

uint8_t fifoBuffer[64]; // FIFO储存缓冲区

// orientation/motion vars

Quaternion q;           // [w, x, y, z] // (方位,运动)变量

VectorFloat gravity;    // [x, y, z] //重力矢量

float ypr[3];           //yaw/pitch/roll（偏航/俯仰/滚动）数组

 

/*********Tune these 4 values for your BOT*********/

double setpoint= 182; //平衡车垂直于地面时的值（目标值）,從序列監控取得小車在直立平衡狀況下的值

//(依照P->D->I順序調參)

double Kp = 20; //1.设置偏差比例系数(調節施予外力的直立,給的過大會震盪)

double Kd = 0.8; //2.调积分(消抖,給的過大會震盪)

double Ki = 140; //3.调微分(調節快速平衡)

/******End of values setting*********/

double input, output;

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

 

volatile bool mpuInterrupt = false;     // MPU中断

void dmpDataReady()

{

    mpuInterrupt = true;

}

void Forward() //電機前進

{

    analogWrite(6,output);

    analogWrite(9,0);

    analogWrite(10,output);

    analogWrite(11,0);

    Serial.print("F"); //Debugging information

}

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");

}

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(57);

    mpu.setYGyroOffset(-29);

    mpu.setZGyroOffset(3);

    mpu.setZAccelOffset(967);

      // 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(")"));

    }

//初始化電機輸出引腳

    pinMode (6, OUTPUT);

    pinMode (9, OUTPUT);

    pinMode (10, OUTPUT);

    pinMode (11, OUTPUT);

//默認情況下關閉電機

    analogWrite(6,LOW);

    analogWrite(9,LOW);

    analogWrite(10,LOW);

    analogWrite(11,LOW);

}

void loop() {

 

    // 如果程序失败直接return。停止

    if (!dmpReady) return;

    // 等待知道MPU中断返回值可用、数据包正常

    while (!mpuInterrupt && fifoCount < packetSize)

    {

        //无mpu数据,运行PID并输出到电机

        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

       

    }

    // 重置中断标志,并获取INT_STATUS数据

    mpuInterrupt = false;

    mpuIntStatus = mpu.getIntStatus();

    // 获取当前FIFO计数

    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)

    if ((mpuIntStatus & 0x10) || fifoCount == 1024)

    {

       // 重置

        mpu.resetFIFO();

        Serial.println(F("FIFO overflow!"));

    // 否则，检查DMP数据准备中断

    }

    else if (mpuIntStatus & 0x02)

    {

        // 等待正确的可用的数据

        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // 读取先进先出的包

        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); //获取q值

        mpu.dmpGetGravity(&gravity, &q); //获取重力值

        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //获取ypr值

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

   }

}