你好，每个人都！

在下面，我会告诉你如何建立一个小型的自平衡机器人，可以左右移动避开障碍物。这是一个小机器人测量4英寸宽，4英寸高，是基于Arduino Pro Mini开发板和MPU6050加速度计陀螺仪模块。

在接下来的步骤中，我们将看到如何用Arduino MPU6050的界面，如何测量机器人的倾斜角度，如何使用PID控制使机器人保持平衡。超声波测距仪也加入到机器人，防止撞到障碍，流连。

零件清单

我买的这些部分来自速卖通但你可以在任何电子商店以及找到他们。

1。Arduino Pro Mini

2。随着gy-521 MPU-6050模块

三.drv8833 pololu电机驱动器

4。2，5v升压转换器

5。us-020超声波距离传感器

6。NCR18650电池持有人

7。双微型减速电机（N20，6V，200转）和托架

8。双42x19mm轮

9。三,双面PCB原型（4cm×6cm）

10。8，25cm尼龙垫片和4，尼龙螺母

除了上面提到的，你将需要一些电缆，伯格连接器和一个开关。

让我们先让我们的手脏一些基本面开始。

自平衡机器人类似于倒立摆。与普通的钟摆一直在摆动一次给出一个轻推，这个倒立摆不能保持平衡，对自己的。它只会摔倒。那么我们如何平衡呢？考虑平衡我们的食指是一种平衡倒立摆的经典例子，一把扫帚。我们将我们的手指在棒的方向落下。用自平衡机器人的情况类似，只有机器人会向前或向后。就像我们如何平衡我们的手指一根棍子，我们平衡机器人的驱动车轮的方向，它会。我们要做的就是保持<em>重心</em>的机器人正上方的支点。

驱动我们需要在机器人的状态信息的汽车。我们需要知道，机器人是下降方向，机器人已经倾斜，它的速度是下降多少。所有这些信息可以从MPU6050读数得到了。我们把所有的这些输入并产生信号驱动电机，使机器人保持平衡。

我们将首先完成对机器人的电路结构。机器人是建立在三层perfboards相隔25mm分开使用尼龙垫片。底层包含两个电机和电机驱动器。中间层的控制器，IMU，和5V升压稳压器模块。最上面的层的电池，一个开关和超声波距离传感器（我们将安装这最后我们一旦得到机器人的平衡）。

在我们开始原型试验电路板上我们应该有一个清晰的画面，每个部分应放置。使成型容易，它始终是更好地绘制各部件的物理布局，以此为参考来放置元件和路线的试验电路板上的跳线。当所有的部分放置和焊接，连接三板采用尼龙垫片。

你可能已经注意到，我使用了两个独立的电压调节模块驱动电机和控制器，即使他们都需要一个5V电源。这是非常重要的。在我的第一个设计，我用单5V升压稳压器电源控制器以及电机。当我打开机器人程序冻结间歇。这是由于从作用于控制器和IMU的电机电路产生的噪声。这是有效地将电压调节器的控制器和电机，增加一个10uF电容在电机电源端子的消除。

有一个的MPU6050三轴加速度计和三轴陀螺仪。加速度传感器测量加速度沿三轴陀螺仪和三轴角速度的措施。衡量我们需要沿着Y轴和Z轴加速度值的机器人倾斜角度。这个但（Y，Z）函数给出了在平面的坐标点的z轴正方向之间的角弧度（Z，Y）上飞机，以逆时针的角度积极的迹象（右半平面，y > 0），并顺时针角度负号（左半平面，y＜0）。我们使用这个图书馆Jeff Rowberg写的从MPU6050读取数据。上传下面给出的代码看看倾角变化。

#包括“丝。” #包括“i2cdev。H” #包括“MPU6050。H” #包括“数学。”

MPU6050微处理器；

int16_t配件，accz； 浮；

无效setup() { 微处理器。initialize()； 系列。开始（9600）；

无效loop() { accz =微处理器。getaccelerationz()； 配件=微处理器。getaccelerationy()； accangle = atan2（配件，accz）* rad_to_deg； 如果（isnan（accangle））； 别的 系列。

尝试移动机器人的前进和后退的同时保持在一定的角度倾斜。你会发现在你的串口监视器显示角度的突然变化。这是由于加速度干扰与Y和Z轴的加速度的水平分量。

MPU6050措施的3轴陀螺仪角速度（转速）沿三个轴的。我们的自平衡机器人，独自沿着X轴的角速度是足够的测量机器人的下降率。

在下面的代码中，我们读到的关于X轴陀螺的价值，将其转换为度每秒然后乘以它的循环时间获得角的变化。我们把这一角获得当前角。

#包括“丝。” #包括“i2cdev。H” #包括“MPU6050。”

MPU6050微处理器；

int16_t捷瑞士，gyrorate； 浮gyroangle = 0； unsigned long currtime，prevtime = 0；

无效setup() { 微处理器。initialize()； 系列。开始（9600）；

无效loop() { currtime = millis()； looptime = currtime - prevtime； prevtime = currtime； 捷瑞士=微处理器。getrotationx()； gyrorate =地图（捷瑞士，32768，32767，250，250）； gyroangle = gyroangle +（浮动）gyrorate * looptime／1000；

位置的MPU6050程序开始运行时是零倾斜点。倾斜角度将尊重这一点测量。

让机器人稳定在一个固定的角度，你会观察到的角度将逐渐增加或减少。它不会保持稳定。这是由于漂移是陀螺固有。

在上述代码中，循环时间使用millis()功能是内置的Arduino IDE计算。在后面的步骤中，我们将使用定时器中断来创建精确的采样间隔。本采样周期也将使用一个PID控制器产生的输出。

谷歌定义为“互补<em>结合在这样一种方式来加强或强调素质对方或另一个</em>R”。

我们从两个不同来源的两个测量的角。从加速度计测量得到的突然的水平运动和陀螺的测量逐渐偏离实际值的影响。换句话说，加速度计的读数被短时间信号的影响和持续时间长的陀螺信号阅读。这些读数，在某种程度上，相辅相成。把它们都使用互补滤波器我们得到一个稳定的角度，准确测量。互补滤波实质上是一个高通滤波器对陀螺和一个低通滤波器的作用在加速度计的测量的漂移和噪声的滤除。

currentangle = 0.9934 *（previousangle + gyroangle）+ 0.0066 *（accangle）

0.9934和0.0066是一个常数时间0.75s滤波器的滤波器系数，低通滤波器允许任何信号比这更长时间的通过和高通滤波器允许任何信号比这更短的时间通过。该滤波器的响应可以通过选择正确的时间常数调整。降低时间常数将允许更多的横向加速度，通过。

消除加速度计和陀螺仪偏置误差 下载并运行了代码网页校准MPU6050的偏移。由于偏移误差可以通过定义的偏移值在setup()程序如下图所示消除。

（1593）setyacceloffset微处理器； 微处理器。setzacceloffset（963）； 微处理器。setxgyrooffset（40；

PID是比例、积分和微分。这些条款提供了我们的自平衡机器人独特的反应。

的<strong>比例</strong>来看，正如它的名字所暗示的，生成的响应与误差。我们的系统中，错误的是机器人的倾斜角度。

生成响应的<strong>积分</strong>项基于累积误差。这基本上是所有错误的总和乘以采样周期。#p#分页标题#e#这是一个基于过去的系统行为。

的<strong>衍生</strong>词是成正比的导数的误差。这是当前的错误和以前的误差除以采样周期之间的差异。这作为一个预测的术语，对机器人如何可能表现在下一个采样回路。

将这些词语用相应的常数（即，Kp，Ki和Kd）和总结的结果，我们产生的输出，然后命令驱动电机发。

1。集Ki和Kd为零并逐步增加KP让机器人开始振荡的零位。

2。增加Ki使机器人的响应越快，当它失去了平衡。王下应足够大，使倾斜角度不增加。机器人必须回到零位，如果它是倾斜的。

三.增加KD以减少振荡。的超调量也应减少到现在。

4。重复上述步骤，通过微调每个参数以达到最好的效果。

我用超声波距离传感器是us-020。它有四个引脚分别是VCC，制动，回声，和GND。它是由一个5V电源供电。触发和回声引脚分别连接到数字引脚9和8的Arduino。我们将使用newping图书馆从传感器得到的距离值。我们将阅读距离每隔100毫秒，如果值为0和20cm之间，我们将指挥机器人进行旋转。这应该足以使机器人远离障碍。

#包括“丝。”

#包括“i2cdev。H” #包括“MPU6050。H” #包括“数学。” #包括<< newping。”

#定义leftmotorpwmpin 6 #定义leftmotordirpin 7 #定义rightmotorpwmpin 5 #定义rightmotordirpin 4

#定义trigger_pin 9 #定义echo_pin 8 #定义max_distance 75

#定义KP 40 #定义KD 0.05 #定义Ki 40 #定义sampletime 0.005 #定义targetangle - 2.5

MPU6050微处理器； newping声纳（trigger_pin，echo_pin，max_distance；

int16_t配件，accz，捷瑞士； 挥发性int电机功率，gyrorate； 挥发性浮accangle，gyroangle，currentangle，prevangle = 0，错误，preverror = 0，errorsum = 0； 挥发性的字节数= 0；

无效setmotors（int leftmotorspeed，int rightmotorspeed）{ 如果（leftmotorspeed > = 0）{ analogWrite（leftmotorpwmpin，leftmotorspeed）； digitalwrite（leftmotordirpin，低）； } { 其他值（leftmotorpwmpin，255 + leftmotorspeed）； digitalwrite（leftmotordirpin，高）； } 如果（rightmotorspeed > = 0）{ analogWrite（rightmotorpwmpin，rightmotorspeed）； digitalwrite（rightmotordirpin，低）； } { 其他值（rightmotorpwmpin，255 + rightmotorspeed）； digitalwrite（rightmotordirpin

无效init_pid() { //初始化定时器1 cli()；/ /禁用全局中断 tccr1a = 0；//设置整个tccr1a登记0 tccr1b = 0；//同tccr1b /设置比较匹配登记设定采样时间5ms ocr1a = 9999； / /打开 CTC模式tccr1b | =（1 << wgm12）； /集cs11位预分频的8 tccr1b | =（1 << cs11）； /使定时器中断 比较timsk1 | =（1 << ocie1a）； sei()；/使

无效setup() { /设定电机控制和PWM引脚输出模式 pinmode（leftmotorpwmpin，输出）； pinmode（leftmotordirpin，输出）； pinmode（rightmotorpwmpin，输出）； pinmode（rightmotordirpin，输出）； /设置状态导致输出模式 pinmode（13，输出 ）；//初始化MPU6050，设置偏移值 微处理器。initialize()； 微处理器。setyacceloffset（1593）； 微处理器。setzacceloffset（963）； 微处理器。setxgyrooffset（40）； /

无效loop() { /读值加速度和陀螺仪 配件=微处理器。getaccelerationy()； accz =微处理器。getaccelerationz()； 捷瑞士=微处理器。getrotationx()；设置电机功率经过约束它 /功率=约束（电机功率，255，255）； setmotors（电机功率，电机功率）； /测距离每100毫秒 如果（（计数为20）= = 0）{ distancecm =声纳。ping_cm()； } 如果（（distancecm <！= 0)) { setMotors(-motorPower, motorPower); } } // The ISR will be called every 5 milliseconds ISR(TIMER1_COMPA_vect) { // calculate the angle of inclination accAngle = atan2(accY, accZ)*RAD_TO_DEG; gyroRate = map(gyroX, -32768, 32767, -250, 250); gyroAngle = (float)gyroRate*sampleTime; currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle); error = currentAngle - targetAngle; errorSum = errorSum + error; errorSum = constrain(errorSum, -300, 300); //calculate output from P, I and D values motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime; prevAngle = currentAngle; // toggle the led on pin13 every second count++; if(count == 200) { count = 0; digitalWrite(13, !//读出数字接口的值（13））；}

在调整PID常数花多一点的时间会给我们一个更好的结果。我们机器人的尺寸也限制了我们所能达到的稳定水平。这是比建立一个小的像我们这样的一个容易的建立一个全尺寸的平衡机器人。不过，我想，我们的机器人在各种表面上的平衡，如视频所示是一个相当不错的工作。