Балансирующий робот на Arduino

Попробуем сделать самобалансирующегося робота с помощью ARDUINO. Надеемся, эта статья поможет новичкам.

Что понадобится

Для механической части нам понадобится:

• Лист ПВХ
• Карандаши
• Мотор-редуктор
• Кронштейн для двигателя
• Колеса

Для части электроники нам понадобится:

• Ардуино Нано
• Драйвер двигателя L298n
• Модуль MPU6050
• 18650 литий-ионный аккумулятор
• 18650 Держатель литий-ионного аккумулятора
• Переключатель

Что такое балансирующий робот?

Самобалансирующийся робот — это двухколесный робот, который балансирует себя так, что предотвращает падение.

Как работает балансирующий робот?

Самобалансирующиеся роботы используют систему «управления с обратной связью по замкнутому контуру»; это означает, что данные датчиков движения в режиме реального времени используются для управления двигателями и быстрой компенсации любого наклона, чтобы удерживать робота в вертикальном положении. Подобные самобалансирующиеся системы управления с обратной связью можно увидеть во многих других приложениях.

Где используются балансирующие роботы?

Среди колесных роботов стали популярны два самобалансирующихся робота, Segway и Ninebot, которые используются для поездок на работу или в качестве патрульных транспортных средств . Кроме того, самобалансирующиеся колесные роботы, такие как Anybots QB, в настоящее время используются в качестве платформы для сервисных роботов.

Какие компоненты потребуются и почему?

Вот в этой части мы обсудим компоненты, почему мы их выбрали.

для лучшего объяснения разделим все на три разные части:

• Механическая часть : здесь мы научимся делать корпус робота очень простым и легким способом.
• Часть электроники : здесь мы узнаем об электронных компонентах, проектировании схем и сборке всех компонентов. В конце этой части наш робот будет готов перейти к следующему и последнему уровню — кодированию и калибровке Arduino.
• Кодирование и калибровка Arduino . Здесь, в этой последней части, мы обсудим код Arduino и способы калибровки робота для самобалансировки.

После выполнения этих трех шагов наш робот будет готов.

Механическая часть

Сначала мы начнем делать нашу механическую часть. Для этого используем несколько листов ПВХ и 4 шт. графитового карандаша, чтобы сделать корпус робота. Пожалуйста, сделайте робота многоуровневым, потому что мы собираемся использовать mpu6050, который лучше всего работает, если его разместить на некоторой высоте выше основания. Мы обсудим больше о mpu6050 в последней части этой статьи, посвященной электронике. На данный момент мы рассмотрим только механическую часть. После этого нам понадобятся два мотор-редуктора, два кронштейна двигателя и два колеса.

Будем ипользовать горячий клей, чтобы соединить все части вместе. Чтобы придать ей идеальный вид, потребуется не более 20-25 минут. Наша механическая часть готова и теперь мы займемся электронной частью.

Электронная часть

В этой части мы обсудим электронные компоненты, которые мы использовали для создания этого робота. Мы также узнаем, почему мы выбрали все эти компоненты. Здесь мы также соединим все компоненты вместе в соответствии со схемой, которую мы приложим к этой статье ниже.

Arduino Nano

Arduino NANO — это мозг робота. Здесь мы выбрали его, потому что это идеальный микроконтроллер для изучения хобби-электроники и программирования, а его размер делает его идеальным для встраивания в проекты, требующие небольшого форм-фактора.

Драйвер двигателя L298n

Драйвер двигателя L298N представляет собой контроллер, который использует H-мост для простого управления направлением и скоростью до 2 двигателей постоянного тока. L298N — это драйвер двигателей с двойным Н-мостом, который позволяет одновременно управлять скоростью и направлением двух двигателей постоянного тока. Модуль может управлять двигателями постоянного тока с напряжением от 5 до 35 В и пиковым током до 2 А.

MPU6050

MPU6050 — это микроэлектромеханическая система (MEMS), состоящая из трехосного акселерометра и трехосного гироскопа. что означает, что он дает шесть значений в качестве вывода:

• три значения с акселерометра
• три из гироскопа

Это помогает нам измерять скорость, ориентацию, ускорение, смещение и другие характеристики движения. Он очень точен, так как содержит 16-битное аналогово-цифровое преобразование для каждого канала. Поэтому он захватывает каналы x, y и z одновременно. Датчик использует шину I2C для взаимодействия с Arduino. MPU6050 может измерять угловое вращение с помощью встроенного гироскопа с четырьмя программируемыми диапазонами полной шкалы: ±250°/с, ±500°/с, ±1000°/с и ±2000°/с.

Это были основные детали компонентов, которые мы использовали в этом роботе. Есть много других компонентов, таких как кулисные переключатели, перемычки и т. д., Многие другие вещи, которые не нуждаются в объяснении.

Сборка компонентов

Как упоминали ранее, мы уже приложил принципиальную схему к этой статье, поэтому, пожалуйста, загрузите ее, прежде чем соединить их все вместе. В данном случае мы сделали индивидуальную плату печатной платы из JLCPCB, чтобы пропустить несколько шагов проводки и упростить соединение между всеми компонентами.Вы можете сделать так же.

Поместим Arduino nano и MPU6050 на одну и ту же печатную плату, а также сделаем отдельную точку для модуля Bluetooth HC-05, чтобы можно было легко модернизировать нашего самобалансирующегося робота до управляемого Bluetooth самобалансирующегося робота в нашем, возможно, следующем проекте с помощью той самой печатной платы.

Идеально установив все компоненты в корпус робота, мы сделали нашего робота. Теперь осталось сделать только последний и ответсвенный шаг — загрузить код Arduino в Arduino Nano.

Кодирование и калибровка Arduino

Необходимо выполнить несколько шагов для калибровки и загрузки кода. Для лучшего понимания мы разделили эту часть кодирования и калибровки Arduino на несколько разных частей.

Измерение угла наклона с помощью акселерометра

MPU6050 имеет 3-осевой акселерометр и 3-осевой гироскоп. Акселерометр измеряет ускорение по трем осям, а гироскоп измеряет угловую скорость по трем осям. Для измерения угла наклона робота нам нужны значения ускорения по осям y и z. Функция atan2 ( y,z) дает угол в радианах между положительной осью z плоскости и точкой, заданной координатами (z,y) на этой плоскости, с положительным знаком для углов против часовой стрелки (правая половина -плоскость, y > 0), и отрицательный знак для углов по часовой стрелке (левая полуплоскость, y < 0). Мы используем эту библиотеку, для чтения данных из MPU6050. Загрузите приведенный ниже код и посмотрите, как меняется угол наклона.

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
MPU6050 mpu;
int16_t accY, accZ;
float accAngle;
void setup() {  
  mpu.initialize();
  Serial.begin(9600);
}
void loop() {  
  accZ = mpu.getAccelerationZ();
  accY = mpu.getAccelerationY();
   
  accAngle = atan2(accY, accZ)*RAD_TO_DEG;
  
  if(isnan(accAngle));
  else
    Serial.println(accAngle);
}
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
MPU6050 mpu;
int16_t accY, accZ;
float accAngle;
void setup() {  
  mpu.initialize();
  Serial.begin(9600);
}
void loop() {  
  accZ = mpu.getAccelerationZ();
  accY = mpu.getAccelerationY();
   
  accAngle = atan2(accY, accZ)*RAD_TO_DEG;
  
  if(isnan(accAngle));
  else
    Serial.println(accAngle);
}

Попробуйте двигать робота вперед и назад, сохраняя при этом его наклон под некоторым фиксированным углом. Вы заметите, что угол, отображаемый на вашем последовательном мониторе, внезапно меняется. Это связано с тем, что горизонтальная составляющая ускорения влияет на значения ускорения по осям y и z.

Измерение угла наклона с помощью гироскопа

3-осевой гироскоп MPU6050 измеряет угловую скорость (скорость вращения) по трем осям. Для нашего самобалансирующегося робота одной только угловой скорости вдоль оси x достаточно, чтобы измерить скорость падения робота.

В приведенном ниже коде мы считываем значение гироскопа относительно оси X, конвертируем его в градусы в секунду, а затем умножаем на время цикла, чтобы получить изменение угла. Мы добавляем это к предыдущему углу, чтобы получить текущий угол.

 	#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 mpu;
int16_t gyroX, gyroRate;
float gyroAngle=0;
unsigned long currTime, prevTime=0, loopTime;
void setup() {  
  mpu.initialize();
  Serial.begin(9600);
}
void loop() {
  currTime = millis();
  loopTime = currTime - prevTime;
  prevTime = currTime;
  
  gyroX = mpu.getRotationX();
  gyroRate = map(gyroX, -32768, 32767, -250, 250);
  gyroAngle = gyroAngle + (float)gyroRate*loopTime/1000;
  
  Serial.println(gyroAngle);
}
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 mpu;
int16_t gyroX, gyroRate;
float gyroAngle=0;
unsigned long currTime, prevTime=0, loopTime;
void setup() {  
  mpu.initialize();
  Serial.begin(9600);
}
void loop() {
  currTime = millis();
  loopTime = currTime - prevTime;
  prevTime = currTime;
  
  gyroX = mpu.getRotationX();
  gyroRate = map(gyroX, -32768, 32767, -250, 250);
  gyroAngle = gyroAngle + (float)gyroRate*loopTime/1000;
  
  Serial.println(gyroAngle);
}

Положение MPU6050 при запуске программы является нулевой точкой наклона. Угол наклона будет измеряться относительно этой точки.

Держите робота неподвижно под фиксированным углом, и вы заметите, что угол будет постепенно увеличиваться или уменьшаться. Он не будет оставаться стабильным. Это связано с погрешностью, присущим гироскопу.

В приведенном выше коде время цикла вычисляется с помощью функции millis(), встроенной в IDE Arduino. На последующих этапах мы будем использовать прерывания по таймеру для создания точных интервалов выборки. Этот период выборки также будет использоваться при генерации выходного сигнала с помощью ПИД-регулятора.

Объединение результатов с дополнительным фильтром

У нас есть два измерения угла из двух разных источников. На показания акселерометра влияют внезапные горизонтальные движения, а показания гироскопа постепенно отклоняются от фактического значения. Другими словами, на показания акселерометра влияют кратковременные сигналы, а на показания гироскопа — длительные сигналы. Эти чтения в некотором роде дополняют друг друга. Объедините их оба с помощью дополнительного фильтра, и мы получим стабильное и точное измерение угла. Дополнительный фильтр представляет собой фильтр верхних частот, воздействующий на гироскоп, и фильтр нижних частот, воздействующий на акселерометр, для фильтрации дрейфа и шума измерения.

 	currentAngle = 0.9934 * (previousAngle + gyroAngle) + 0.0066 * (accAngle)

0,9934 и 0,0066 — коэффициенты фильтра для постоянной времени фильтра 0,75 с. Фильтр нижних частот пропускает через себя любой сигнал длиннее этой длительности, а фильтр верхних частот пропускает любой сигнал короче этой длительности. Отклик фильтра можно настроить, выбрав правильную постоянную времени. Уменьшение постоянной времени позволит пройти большее горизонтальное ускорение.

Устранение ошибок смещения акселерометра и гироскопа

Загрузите и запустите код, указанный на этой странице, для калибровки смещения MPU6050. Любую ошибку из-за смещения можно устранить, определив значения смещения в процедуре setup(), как показано ниже.

mpu.setYAccelOffset(1593); 
mpu.setZAccelOffset(963); 
mpu.setXGyroOffset(40);

ПИД-регулятор для генерации выходного сигнала

PID означает пропорциональный, интегральный и производный. Каждый из этих терминов дает уникальный ответ нашему самобалансирующемуся роботу.

Пропорциональный член, как следует из его названия, генерирует ответ, пропорциональный ошибке . Для нашей системы ошибкой является угол наклона робота.

Интегральный член генерирует ответ на основе накопленной ошибки. По сути, это сумма всех ошибок, умноженная на период выборки. Это ответ, основанный на поведении системы в прошлом.

Член производной пропорционален производной ошибки. Это разница между текущей ошибкой и предыдущей ошибкой, деленная на период выборки. Это действует как прогностический термин, который реагирует на то, как робот может вести себя в следующем цикле выборки.

Умножая каждый из этих членов на соответствующие им константы (т. е. Kp, Ki и Kd) и суммируя результат, мы генерируем выходные данные, которые затем отправляются в виде команды для запуска двигателя.

Настройка констант ПИД

1. Установите Ki и Kd на ноль и постепенно увеличивайте Kp, чтобы робот начал колебаться вокруг нулевого положения.

2. Увеличьте Ki, чтобы робот реагировал быстрее, когда он выходит из равновесия. Ки должен быть достаточно большим, чтобы угол наклона не увеличивался. Робот должен вернуться в нулевое положение, если он наклонен.

3. Увеличьте Kd, чтобы уменьшить колебания. Выбросы также должны быть уменьшены к настоящему времени.

4. Повторите описанные выше шаги, точно настроив каждый параметр для достижения наилучшего результата.

Полный код

//Self Balancing Robot
#include
#include 
#include 
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

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

#define MIN_ABS_SPEED 30

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
double originalSetpoint = 172.50;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;

//adjust these values to fit your own design
double Kp = 60;
double Kd = 2.2;
double Ki = 270;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.5;

//MOTOR CONTROLLER
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 9;
int IN4 = 8;
int ENB = 10;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

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

  mpu.initialize();

  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  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
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    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(")"));
  }
}


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

  }

  // 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);
    input = ypr[1] * 180 / M_PI + 180;
  }
}

Теги: