ROBOT HAI BÁNH TỰ CÂN BẰNG - 2 WHEEL SELF BALANCING ROBOT

Bạn có một DỰ ÁN hay giống thế này? Chia sẻ nhé!

tuenhi.n2012 gửi vào Thứ năm, 21 Tháng 12, 2017 - 15:30

82771 lượt xem

Hôm nay mình xin chia sẻ với các bạn cách làm một robot tự cân bằng trên hai bánh xe bằng Arduino từ xe đồ chơi bị hỏng của thằng nhóc ở nhà. Tất nhiên, hai động cơ và hai bánh xe chưa bị hỏng nhé. . Để robot tự cân bằng trên hai bánh xe thì chuyển động của nó tương tự như việc giữ thăng bằng một cây gậy trên ngón tay. Điều này chắc các bạn cũng đã từng thử trước đây. Để giữ thăng bằng, chúng ta phải di chuyển ngón tay của mình nhanh hay chậm theo hướng nghiêng và tốc độ nghiêng của cây gậy. Chúng ta bắt đầu tìm hiểu xem làm thế nào mà Arduino có thể tự điều chỉnh được như thế.

I. DẪN NHẬP

Hôm nay mình xin chia sẻ với các bạn cách làm một robot tự cân bằng trên hai bánh xe bằng Arduino từ xe đồ chơi bị hỏng của thằng nhóc ở nhà. Tất nhiên, hai động cơ và hai bánh xe chưa bị hỏng nhé.. Để robot tự cân bằng trên hai bánh xe thì chuyển động của nó tương tự như việc giữ thăng bằng một cây gậy trên ngón tay. Điều này chắc các bạn cũng đã từng thử trước đây. Để giữ thăng bằng, chúng ta phải di chuyển ngón tay của mình nhanh hay chậm theo hướng nghiêng và tốc độ nghiêng của cây gậy. Chúng ta bắt đầu tìm hiểu xem làm thế nào mà Arduino có thể tự điều chỉnh được như thế.

II. B.O.M

Các vật tư cần thiết để làm một robot 2 bánh tự cân bằng như sau:

No.	Item	Spec	Q'ty	Unit	Remarks
1	Arduino Uno		1	pcs
2	MPU-6050		1	pcs
3	L298		1	pcs
4	DC Motor		2	pcs
5	Bánh xe	60mm	2	pcs
6	Biến trở	5Kohm	4	pcs
7	PCB đục lỗ	4x6 cm, màu xanh, hai mặt	2	pcs
8	Trụ đồng		8	pcs
9	Nguồn 5V & 12V		1	pcs
10	Bus 4		1	pcs
11	Mica trong/đục	Dày 5mm	1	pcs	Làm khung
12	Dây – jack nguồn/ hàng rào		1	pcs

III. CHUẨN BỊ

Trước tiên, chúng ta phải bỏ một ít thời gian để tìm hiểu các thông tin cơ bản sau đây trước khi tiến hành làm một robot tự cân bằng.

3.1. Nguyên tắc con lắc ngược (inverted pendulum)

Nó giống như việc giữ thăng bằng một cây gậy trên ngón tay. Để giữ thăng bằng, chúng ta phải di chuyển ngón tay của mình theo hướng nghiêng và tốc độ nghiêng của cây gậy. Để điều khiển động cơ bánh xe cho robot tự cân bằng qua mạch cầu L298N, chúng ta cần một số thông tin về trạng thái của robot như: điểm thăng bằng cần cài đặt cho robot, hướng mà robot đang nghiêng, góc nghiêng và tốc độ nghiêng. Tất cả các thông tin này được thu thập từ MPU6050 và đưa vào bộ điều khiển PID để tạo ra một tín hiệu điều khiển động cơ, giữ cho robot ở điểm thăng bằng.
Về phần lý thuyết và các công thức, các bạn có thể tìm hiểu qua google với các từ khóa: inverted pendulum (con lắc ngược), self-balancing robot hay 2 wheel self-balancing.

3.2. Điều khiển vòng kín P.I.D

Về P.I.D, các bạn có thể tham khảo trên các website hay qua bài viết của tôi tại: http://arduino.vn/result/5401-pid-speed-position-control
Ở đây, giá trị cài đặt bộ P.I.D (SP) là điểm cân bằng được hiểu là góc so với phương thẳng đứng, vuông góc với robot. Nếu phần cứng cho robot hoàn hảo, cân bằng và đối xứng thì với thiết kế của mình góc này sẽ là 180⁰, thực tế điểm SP của mình là 178.70⁰ . Tại sao là 180⁰ hay 178.70⁰, các bạn hãy xem chương trình bên dưới. Tín hiệu hồi tiếp feedback (PV) là sự kết hợp giữa Gyroscope và Accelerometer được thu thập từ MPU-6050. Output của bộ PID là tín hiệu điều xung tốc độ cho hai động cơ DC sao cho PV tiến tới điểm cân bằng SP.

3.3. MPU-6050

MPU-6050 là cảm biến của hãng InvenSense tích hợp 6 trục cảm biến bao gồm:
- Con quay hồi chuyển 3 trục (Gyroscope).
- Cảm biến gia tốc 3 chiều (Accelerometer).
Trên cộng đồng cũng đã có một bài viết rất hay về MPU-6050 của bạn Phan Vu Hoang tại: http://arduino.vn/bai-viet/960-doi-dieu-ve-imu-inertial-measurement-unit. Trong đó, bạn Phan Vu Hoang có đề cập đến sensor fusion – sự kết hợp cảm biến. Đó là dùng cả gyro và accel để đo góc và dùng một số loại thuật toán để gộp 2 giá trị với nhau, bù trừ nhau để đưa ra kết quả chính xác nhất. Ở dự án này, mình không dùng bộ lọc do kết quả đạt được từ sensor fusion là tương đối ổn định nhưng mình sẽ thử với bộ lọc Kalman.
Khi tìm hiểu về MPU-6050, các bạn sẽ gặp phải thuật ngữ QUATERNION, YAW, PITCH, ROLL. Và theo mình, đây là cách giải thích đơn giản và dể hiểu nhất:
Về Quaternion, các bạn tham khảo bài viết của tác giả Bùi Quang Minh tại địa chỉ này https://minhcly.wordpress.com/2014/01/30/quarternion-va-phep-quay-khong-gian-phan-1/
Về Yaw, Pitch, Raw, các bạn tham khảo bài viết của tác giả Nguyễn Bình Long tại: https://kienthucbay.wordpress.com/tag/ailerons/

Xin phép tác giả trích lược lại như sau:

Một máy bay có thể thực hiện bao nhiêu kiểu chuyển động. Các loại chuyển động đó xảy ra xung quanh những trục nào?

Một máy bay có thể thực hiện 3 kiểu chuyển động. Nó có thể gọi pitch, roll và yaw.

Pitch là kiểu chuyển động khi mũi của máy bay chúc lên trên hoặc chúi xuống dưới. Chuyển động pitch diễn ra xung quanh trục ngang của máy bay.

Roll là kiểu chuyển động khi một trong hai cánh của máy bay liệng xuống còn cánh còn lại thì liệng lên. Ví dụ, nếu máy bay đang roll sang bên trái thì cánh trái sẽ liệng xuống còn cánh phải thì liệng lên. Chuyển động roll diễn ra xung quanh trục dọc thân máy bay.

Yaw là kiểu chuyển động khi mũi của máy bay di chuyển qua phải hoặc qua trái. Chuyển động yaw diễn ra xung quanh trục thẳng đứng, vuông góc với thân máy bay.

Hiểu được các chuyển động của máy bay ở trên sẽ giúp chúng ta dễ dàng hình dung ra được robot tự cân bằng sẽ hoạt động như thế nào. Và chắc các bạn cũng đoán được nó chuyển động theo kiểu nào rồi!
Ngoài ra, các bạn còn phải tìm hiểu về cách calibrate MPU-6050 (Việt Nam mình hay gọi là “ca líp” như là “ca líp cân”, thường được dùng để hiệu chỉnh các sensor như loadcell, pressure transmitter, flow transmitter …)
Các bạn có thể xem các chuyển động yaw, pitch, roll của robot tự cân bằng tại địa chỉ:

IV. SƠ ĐỒ MẠCH VÀ KHUNG ROBOT

4.1. Phần cứng và khung robot

Khi làm phần cứng các bạn lưu ý làm phần khung cho robot phải cứng cáp, chịu được va đập trong quá trình test và đối xứng thì robot sẽ đẹp và dễ cân bằng hơn.
Phần khung robot: bằng mica, thiết kế của mình còn thiếu một tầng chứa pin do chưa có đủ tiền để mua nó. .
Robot bằng đồ chơi dễ tìm được điểm cân bằng do bánh của nó có các gai nhỏ. Do đây là đồ chơi bị hỏng nên nó bị vẹo một tí. Sau đó mình đã mua 2 động cơ DC và bánh xe khác để thử. Kết quả thật tuyệt vời! Đây là hai phiên bản của em nó.

4.2. Sơ đồ hiệu chỉnh P, I, D bằng chương trình

Với sơ đồ này, các bạn phải tìm các thông số P, I, D bằng các phép thử và theo mình sẽ mất nhiều thời gian. Nhưng Arduino sẽ còn nhiều chân Analog để có thể làm việc khác. Một cách khác là điều chỉnh P, I, D qua Serial nhưng nó sẽ không hiệu quả lắm do Arduino thường xuyên bị treo (pending/ freezing). Qua tìm hiểu, mình thấy có rất nhiều người trên các diễn đàn than phiền về vấn đề này nhưng vẫn chưa có cách giải quyết....

4.3. Sơ đồ hiệu chỉnh P.I.D qua biến trở

Với việc có thêm các biến trở để hiệu chỉnh các hệ số P, I, D sẽ làm giảm rất nhiều thời gian mò mẫm các hệ số này sao cho robot hoạt động ổn định, mượt mà. Các hệ số P, I và D được tìm như sau:

Đặt tất cả các biến trở P, I, D về 0.
Tăng dần biến trở P cho đến khi robot bắt đầu dao động qua lại xung quanh điểm cân bằng nhưng robot vẫn không bị ngã.
Tăng dần biến trở D cho đến khi robot không còn dao động. Lúc này, robot hoạt động tương đối ổn định nhưng sẽ bị khựng khựng khi bị tác động bằng tay.
Tăng dần biến trở I từ từ cho đến khi hệ thống hoạt động ổn định mượt mà ngay cả khi đẩy mạnh robot về một phía. Nếu giá trị biến trở I lớn nó sẽ làm cho robot đáp ứng chậm.

Với robot của mình, các thông số PID tìm được là:

K_P = 10.50.
K_I = 67.44.
K_D= 0.88.

V. THƯ VIỆN VÀ CHƯƠNG TRÌNH

5.1. Chương trình chính

Chương trình chính và việc áp dụng các thư viện được tham khảo từ nhiều nguồn khác nhau, trang tham khảo chính: https://github.com/lukagabric/Franko

#include "PID_v1.h"
#include "LMotorController.h"
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

#define LOG_INPUT 0
#define MANUAL_TUNING 1
#define LOG_PID_CONSTANTS 1 //MANUAL_TUNING must be 1
#define MOVE_BACK_FORTH 0

#define MIN_ABS_SPEED 5

//MPU

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

//PID

#if MANUAL_TUNING
  double kp , ki, kd;
  double prevKp, prevKi, prevKd;
#endif
double originalSetpoint = 178.70;// 181.13
double setpoint = originalSetpoint;
double movingAngleOffset = 0.15;// 0.3- OK, 0.15 - OK
double input, output;
int moveState=0; //0 = balance; 1 = back; 2 = forth

#if MANUAL_TUNING
  PID pid(&input, &output, &setpoint, 0, 0, 0, DIRECT);
#else
  PID pid(&input, &output, &setpoint, 10.50, 67.44, 0.88, DIRECT);// time 5ms & 10ms, sometimes Kp(17.35, 16.86) Ki(302.05, 301.05) Kd(1.21)
#endif

//MOTOR CONTROLLER

int ENA = 3;
int IN1 = 4;
int IN2 = 8;
int IN3 = 5;
int IN4 = 7;
int ENB = 6;

LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 1, 1);

//timers

long time1Hz = 0;
long time5Hz = 0;

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

void setup()
{
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    Serial.begin(115200);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // 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
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(39);
    mpu.setYGyroOffset(14);
    mpu.setZGyroOffset(6);
    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

    // 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(5);// 10 - OK, 5 - GOOD, 1- CHANGE PID
        pid.SetOutputLimits(-255, 255);// 80 - OK Strong enough  
    }
    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(")"));
    }
}

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();
        motorController.move(output, MIN_ABS_SPEED);
        
        unsigned long currentMillis = millis();

        if (currentMillis - time1Hz >= 1000)
        {
            loopAt1Hz();
            time1Hz = currentMillis;
        }
        
        if (currentMillis - time5Hz >= 5000)
        {
            loopAt5Hz();
            time5Hz = currentMillis;
        }
    }

    // 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);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        #if LOG_INPUT
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif
        input = ypr[1] * 180/M_PI + 180;
   }
}

void loopAt1Hz()
{
#if MANUAL_TUNING
    setPIDTuningValues();
#endif
}

void loopAt5Hz()
{
    #if MOVE_BACK_FORTH
        moveBackForth();
    #endif
}

//move back and forth


void moveBackForth()
{
    moveState++;
    if (moveState > 2) moveState = 0;
    
    if (moveState == 0)
      setpoint = originalSetpoint;
    else if (moveState == 1)
      setpoint = originalSetpoint - movingAngleOffset;
    else
      setpoint = originalSetpoint + movingAngleOffset;
}

//PID Tuning (3 potentiometers)

#if MANUAL_TUNING
void setPIDTuningValues()
{
    readPIDTuningValues();
    
    if (kp != prevKp || ki != prevKi || kd != prevKd)
    {
#if LOG_PID_CONSTANTS
        Serial.print(kp);Serial.print(", ");Serial.print(ki);Serial.print(", ");Serial.println(kd);
#endif

        pid.SetTunings(kp, ki, kd);
        prevKp = kp; prevKi = ki; prevKd = kd;
    }
}

void readPIDTuningValues()
{
    int potKp = analogRead(A0);
    int potKi = analogRead(A1);
    int potKd = analogRead(A2);
        
    kp = map(potKp, 0, 1023, 0, 25000) / 100.0; //0 - 250
    ki = map(potKi, 0, 1023, 0, 100000) / 100.0; //0 - 1000
    kd = map(potKd, 0, 1023, 0, 500) / 100.0; //0 - 5
}
#endif

5.2. Các thư viện cho dự án

Các bạn phải inlcude các thư viện sau vào chương trình chính:

Thư viện I2Cdev
Thư viện MPU6050
Thư viện LMotorController
Thư viện PID_v1

V. LỜI KẾT

Việc tinh chỉnh các thông số PID qua các biến trở sẽ giúp chúng ta hiểu hơn về điều khiển vòng kín có hồi tiếp vốn dĩ là rất phức tạp và các mô phỏng vật thể qua MPU-6050 cho ta cảm nhận thực về các chuyển động trong không gian. Các chia sẻ ở trên trong phạm vi hiểu biết của tôi, chắc chắn sẽ có nhiều sai sót, mong được góp ý để có thể làm tốt hơn.
Robot hai bánh tự cân bằng hoạt động khá tốt ngay cả ở mặt đường gồ ghề, nghiêng, trên thảm, trên nệm và mang thêm các vật nhẹ trên nó. Nó cũng có thể lấy lại cân bằng khi bị xoay trái - phải hay đẩy tới - lui.
Dự án robot hai bánh tự cân bằng là một trải nghiệm thú vị vì lần đầu tiên con trai tôi thích món đồ chơi do tôi làm ra và bắt đầu có các câu hỏi về Arduino (cu cậu mới học lớp 5 à). Nó cũng giúp tôi giảm bớt căng thẳng và dường như tìm lại được sự cân bằng cho chính mình..