本文將介紹如何使用Arduino來制造一個自動平衡機器人，并介紹其控制原理。

自平衡機器人的控制原理

自平衡機器人

自平衡機器人聽名字比較高大上，但實際生活中其中已經可以看到很多成品了，像比較早期高端的賽格威，滿大街跑的小米平衡車等，都屬于自平衡機器人的范疇。那他是如何工作的呢？首先我們要知道，自平衡機器人為了讓機器人保持平衡，電機的運動必須要能抵消機器人的自然重力導致的姿態變化，比如傾倒、后仰等。要實現這個抵消的動作平衡機器人就需要能反饋并糾正這些變化的因素。在本項目中，反饋元件是MPU6050及其內置的陀螺儀和加速度計，它在三個軸上都提供加速度和旋轉反饋功能。

Arduino控制器通過它來知道機器人當前的姿態信息，從而通過這些信息控制電機和車輪的運動，使機器人能夠保持平衡。

感知到機器人的傾斜，通過驅動車輪來保持機器人直立。

自平衡機器人的電路原理圖

自平衡機器人的電路原理圖

在電路連接方面， 首先我們要將MPU6050連接到Arduino，可查閱：Arduino如何使用MPU6050，然后參照上圖所示連接其余的組件。L298N模塊可以為Arduino提供所需的+5V（詳情見： Arduino直流電機控制教程 中的相關內容 ）。本文中，我們選擇了單獨的電源為電機和電路供電。請注意，如果您計劃為L298N模塊使用大于+12V的電源電壓，您需要移除剛好位于+12V輸入上方的跳線。

構建機器人

機器人框架

機器人框架采用一些簡單的材料黏貼完成，如果有3D打印機也可以嘗試設計一款適合自己風格外殼框架。具體可參閱：設計自平衡機器人。

機器人主電路板

主電路板主要包括Arduino Nano和MPU6050和一塊L298N電機驅動板。

自平衡機器人本質上是一個倒立鐘擺。如果質量中心相對于輪軸較高，則可以更好地平衡。較高的質心意味著較高的質量慣性矩，這對應于較低的角加速度（下降較慢）。這就是為什么我把電池組放在上面。然而，機器人的高度是根據材料的可用性來選擇的。 制作時不必過分強求。

PID控制原理

在控制中，保持某個變量（機器人的位置）的穩定需要一個稱為PID（比例積分導數）的特殊控制器。這些參數都存在“增益”的問題，通常稱為Kp、Ki和Kd。PID需要在期望值（輸入值）和實際值（輸出值）之間進行校正。輸入和輸出之間的差異稱為“誤差”。PID控制器通過不斷調整輸出將誤差減小到最小值。在本文的Arduino自平衡機器人中，這些值的設置遵循以下原則：

使Kp Ki Kd等于0。

Kp的調整：Kp值太低會使機器人摔倒，因為沒有足夠的校正值。太大的Kp會使機器人瘋狂地來回移動。一個合適的Kp值會使機器人稍微來回移動或者稍微擺動即可。

設定好Kp值后，需要調整Kd值。適當的Kd值可以減小振動，直到機器人基本穩定。同時，即使它被推動，適當的Kd值也能讓機器人始終保持站立。

最后就是設置Ki值。因為我們即使設定了Kp和Kd值，機器人在打開電源時也會發生振動，但它會在一定的時間后穩定下來。正確的Ki值可以縮短機器人穩定的時間。

Arduino自平衡機器人代碼

要成功實現代碼安裝，需要四個外部庫文件來讓Arduino自平衡機器人正常工作。PID庫的引入使得計算P、I和D值變得相對容易。LMotorController庫文件用于L298N模塊驅動兩個電機，I2Cdev庫和MPU6050_6_Axis_MotionApps20庫用于從MPU6050讀取數據。你可以在這里下載包含這些庫文件的代碼。

#include

#include

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

#include "Wire.h"

#endif

#define MIN_ABS_SPEED 20

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 = 173;

double setpoint = originalSetpoint;

double movingAngleOffset = 0.1;

double input, output;

//adjust these values to fit your own design

double Kp = 50;

double Kd = 1.4;

double Ki = 60;

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 = 8;

int IN4 = 9;

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;

}

}

本文中的Kp、Ki、Kd的值可能適用也可能不適用你的機器人。如果不適用，那么按照上面PID控制原理中提到的方法來設當調整。注意，代碼中的傾斜值設置為173度。如果有必要，你可以更改這個值，這是機器人必須保持的傾斜角度。此外，如果你的電機速度太快，你可以適當調整motorSpeedFactorLeft和motorSpeedFactorRight值，一個成功的DIY是需要不斷調整的，重點是要有耐心，最后希望大家都能完成自己的自平衡機器人。