基于Arduino的自平衡抓球机器人

基于Arduino的自平衡抓球机器人

郭旭琨，张为杰，张龙龙

沈阳城市建设学院

摘  要

一种基于Arduino Mega 2560 Pro单片机上的双串级PID算法，通过惯性导航芯片可控制小车能稳定直立行走并且能精准控制小车行走角度与距离的自平衡小车，搭配弹性抓球装置，可设置平衡车按照设定的路线行走。基于K210视觉开发板进行垃圾识别分类。

关键词：自平衡机器人，MPU6050，Arduino，场内定位，机器视觉

1绪论

在当下这个盛行不断创新的时代，高科技技术越来越成为时代发展的“主旋律”。其对社会的发展和人类的生活也产生着积极的作用。本文从双轮平衡车入手，从结构系统设计、软件系统设计、成果展示三大模块进行论述。自平衡车两轮左右对称布置，主要利用陀螺仪模块进行前后倾斜角度的测量，并将信息输出给单片机，通过相应的PID控制算法计算出车辆的控制量，用该控制量驱动电机工作，完成自平衡车前进、后退与转向等操作。

自平衡车系统的设计难点在于尽可能采用较高性价比的惯性传感器进行准确的姿态检测与系统控制算法的设计。

自平衡小车的运作原理主要是建立在一种被称为“动态稳定”(DynamicStabilization)的基本原理上，也就是车辆本身的自动平衡能力。以内置的精密固态陀螺仪（Solid-StateGyroscopes）来判断车身所处的姿势状态，通过精密且高速的中央微处理器计算出适当的指令后，驱动马达来做到平衡的效果。

2平衡车原理

由于两轮自平衡电动车的两轮结构，使得它的重心在上、下两个支点处，故在非控制状态（或静止状态）下为不平稳状态。然而，可以利用倒立摆系统的控制原理，通过微处理器的控制使它能够如倒立摆放一般稳定在一个平衡位置处，并能在保持平衡的状态下按照操作者的指令要求正常运行。两轮自平衡电动车实际上是一级直线式倒立摆和旋转式倒立摆的结合体，它的控制原理与倒立摆原理系统的基本一致。更形象地说，自平衡车的工作原理更像是人行走的过程。

2.1直立控制

我们可以把平衡小车简化为一个倒立摆，而且这个倒立摆只有前后两个方向可以运动（图1.1小车简化物理模型）。

当小车向一侧倾倒时，我们进行受力分析：已知车子自身受到重力作用、杆的支撑力，从而得到合力为。要想使得小车能够保持静止在稳定状态下，必须给予小车一个反方向的力， 那么就可以让小车回复到平衡位置了。小车的动力以及外部所施加的力均是由底部的两个车轮所提供的，小车作变速运动时，车体自然会受到一个力的作用（图1.2小车倾斜受力分析图）。

假设小车向前倾斜时，控制小车的车轮使小车作加速运动，加速度大小为 a。 当小车不以地面为参考系时视为一个非惯性系统，以小车轮子为参考系，小球上方的圆球会受到一个方向与加速度相反且大小与加速度成比例的惯性力， 比例系数为质量m，即F=-ma，把这个水平向左的惯性力分解，可以得到垂直于杆的力（图1.3 施加加速度后物理模型）。 可知小车受到的回复力为，若很小，便可以认为，，并且假设小车的反馈系统中加速度a与小车的倾角成比例关系，比例系数为，那么有。当时，回复力的方向就与倾斜方向相反了，此时小车会回复到平衡的位置。

    图1.1小车简化物理模型                图1.2 小车倾斜受力分析               图1.3 施加加速度后物理模型

2.2速度控制

如果小车产生倾角，在直立控制的作用下就会使小车在倾斜的方向加速，我们可以利用小车的这个特性来进行速度控制，控制速度实际上就变成了控制小车的倾角。

获取车轮的速度可以通过读取编码器的数值来获得。小车的速度控制对快速性要求并不高，但是对于准确性有一定的要求。PID控制中微分(Didderential)控制主要的作用是减少振荡，加快稳定速度，积分(Intergral) 控制主要的作用是减少静态误差，所以我们可以建立速度PI闭环控制，输出一个角度使小车达到目标速度，这其实就是串级PID控制，让速度控制的输出作为角度控制的输入，角度控制的输出直接作用于电机上（图1.4串级PID系统）。

                                                                  图1.4串级PID系统

3平衡车总体结构设计

3.1机械结构设计

小车总体采用国标2020铝型材搭建，由于其拥有较高的拓展性，可非常方便得在小车上放置其他传感器，以方便后续完善小车。

3.2电源部分

电源采用DJI大疆经纬M100/M600/Robmaste 原厂电池 TB47S/D TB48S/D电池，可提供稳定的24v电压，其内置智能电池管理系统，为电池提供更好的保护。在飞行过程中，智能 App 上会实时显示剩余的电池电量，系统会自动分析并计算返航电量和时间，免除时刻担忧电量不足的困扰。智能电池会显示每块电芯的电压，总充放电次数以及整块电池的健康状态等，方便进行小车动力分析。

3.3动力部分

动力部分采用MD36NP51P-24V6000-27F直流有刷电机,功率为35w，额定电流为2.3A，其搭载了霍尔式编码器，可方便得测得其电机速度。测得平衡小车在直线速度为30m/min时，仍有较好的平衡效果，因此在本小车中，我们选用27减速比电机，此电机配合130mm的车轮使小车最高速度可达40m/s，保证小车在有足够向前速度的前提下仍有较好的低速稳定性。

3.4电机驱动部分

电机驱动部分我们采用WheelBoard D50A 12A大电流MOS双路直流电机驱动模块，由于本小车采用的动力部分为35W，额定电流为2.3A的电机，且由于直流电机启动电流较大，因此在选择电机驱动时，每路电机至少需要10A的电流，因此选用此电机驱动，在满足电机正常工作要求的情况下，还有较大的冗余量，可避免小车在电机堵转时，烧坏电机驱动板。且由于我们采用电机电流较大，因此需采用光耦隔离的方式，将单片机与电机隔离开，防止因电机启动电流过大引起倒流，烧坏单片机电路。

且由于平衡小车所需电机控制精度较高，而普通电机驱动模块死区较大，例如常用的L298N电机驱动模块死区就较大，因此我们采用此电机驱动，他有高速PWM隔离输入，最小脉冲宽度3μs，隔离带宽10MHz，且支持满占空比输入。控制精度相对L298N和TB6612等电机驱动芯片较高。

3.5电机速度反馈部分

我们采用增量式霍尔编码器作为小车电机的速度读取传感器，这款编码器通过霍尔元件检测与电机轴连接的磁极磁性变化。通过磁性变化给A，B相产生高低电平脉冲，通过此脉冲，即可分析出小车当前的速度值，通过AB相将小车当前位置和转速情况通过脉冲输出给Arduino，Arduino通过M法测速，即通过求单位时间内脉冲个数，来求出当前电机转速。

图3.1编码器脉冲图

由于此垃圾分类平衡车速度要求并不高，因此我们采用1：27的减速比，可将6210r/min的转速转为230r/min，此转速即可实现平衡小车的要求。需要注意的是，由于Arduino并未含有正交编码的外部中断接口，其采用中断的方式来通过软件模拟正交编码，因此其占用主频较高，如果采用编码器线数较高，则会导致单片机主频不够而产生死机的情况，因此在选择编码器线数时，需考虑单片机的接口情况，例如STM32F103RCT6单片机，其内置正交编码模式的外部中断引脚，其硬件中断系统可使用的编码器线数就很高，可达500线或更高，而Arduino Mega 2560 Pro则只能采用13线或更低线数的编码器。甚至当电机转速提高时，就会导致程序外部中断频率过高而导致程序卡死。

需要注意的是，由于Arduino Mega 2560 Pro单片机只有6个外部中断引脚，其中2，3引脚使用的是Arduino的TIME 2定时器，而9，10引脚的pwm输出也用的是TIME 2定时器，即当我们在使用2，3引脚作为外部中断时，就不能再使用9，10引脚输出PWM，不然就会产生定时器错乱的情况。

3.6主控单片机

主控单片机我们采用Arduino Mega2560 pro模块，他相对于普通的Arduino UNO来说，他的引脚较多，而我们在电路搭建时，单路电机驱动需要5根引脚才能达到精确控制。而Arduino UNO 数字输入引脚不够我们使用，并且uno采用的是8位芯片，主频只有16Mhz。在处理一些较复杂的PID运算，和一些外部中断时，就会出现卡死现象。因此在本小车中，我们选用Arduino Mega 2560 Pro作为主控单片机。

4平衡车行走机构设计

4.1Mpu6050数据读取及滤波处理

4.1.1Mpu6050数据读取

MPU-6000（6050）为全球首例整合性6轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时间轴之差的问题，减少了大量的封装空间。非常适合于平衡车等一些需要高精度，高响应速度，高回传速度的电子设备上。由于其使用方便，数据精度较高，价格低廉，因此其用户在全球范围内极为广泛。在我们的小车上，就使用了基于MPU6050开发的一款高精度陀螺仪模块JY-521六轴传感器。此传感器内置DMP滤波装置，可直接输出四元数或欧拉角。单片机仅需IIC总线的方式，即可将此陀螺仪芯片中的数据读取出来。SCL和SDA是连接MCU的IIC接口，MCU通过这个IIC接口来控制MPU6050，另外还有一个IIC接口:AXCL和 XDA，这个接口可用来连接外部从设备，比如磁传感 器，这样就可以组成一个九轴传感器。VLOGIC是IO口电压，该引脚最低可以到1.8V，我们 一般直接接VDD即可。AD0是从IIC接口(接MCU)的地址控制引脚，该引脚控制IIC地址 的最低位。如果接GND，则MPU6050的IIC地址是:0X68，如果接VDD，则是0X69。

在Arduino中，使用Mpu6050非常方便，只需使用

即可将Mpu6050中的三轴加速度和三轴角速度分别放在ax，ay，az，gx，gy，gz的地址中。

4.1.2滤波算法

在Arduino中，使用MPU6050非常方便，只需调用其I2Cdev.h，MPU6050_6Axis_MotionApps20.h，Wire.h库，即可实现数据的读取，但由于我们所需的三轴角度是通过三轴角速度，三轴加速度积分累加得到的欧拉角，因此三轴角度会随着时间的延长，而产生误差。我们需要通过滤波算法，才能将其数据精确得输出出来。如果采用内置的DMP滤波器输出四元数就不需要此算法。算法我们一般采用一阶低通滤波或卡尔曼滤波技术。在本小车中，我们采用的就是卡尔曼滤波技术。

4.1.3卡尔曼滤波技术使用

卡尔曼滤波技术广泛使用在此类传感器运行极为广泛，上一点我们提到，Mpu6050输出的六轴角度信息分别为三轴的加速度和三轴的角速度，角速度通过对时间积分而获取到角度值，三轴加速度可通过正交分解等方式，解算出三轴角度。而三轴的加速度值没有累积误差，且通过算 tan()  可以得到倾角，但是它包含的噪声太多，因为待测物运动时会产生加速度，电机运行时振动会产生加速度等，不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以，不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角。因此我们可通过卡尔曼滤波算法，将两数据融合，即可将两者优点相结合，缺点互相弥补，从而得出精确数据。

在Arduino中，使用卡尔曼滤波算法非常方便，只需使用

将Mpu6050读取到的数据ax，ay，az，gx，gy，gz，和我们设置的卡尔曼滤波器的参数放入此函数，即可直接输出三轴角度。

4.2平衡车行走代码设计

当读取完Mpu6050数据并进行卡尔曼滤波算法处理过之后获取到准确的当前角度值，并使用增量式霍尔编码器获取到电机当前速度值，即可进行小车的平衡底盘软件设计。其主要包括直立环和速度环，使用串级PID将两环相结合，通过角度，角速度和电机速度的输入，输出两电机的PWM值，然后通过PWM限幅函数，将两PID输出的PWM值限制在0~255（Arduino mega 2560 pro输出精度为8位，即只能输出0~255的PWM），最后将限幅后的PWM输出给电机驱动模块，电机驱动模块通过连续的闭合来控制两电机的速度。

4.2.1直立环代码

Bias = Angle - Mechanical_median;

balanceval = Balance_Kp * Bias + Gyro * Balance_Kd;

4.2.2速度环代码

Encoder_Integral += Encoder;

Velocity = Encoder * Velocity_Kp + Encoder_Integral * Velocity_Ki;

4.2.3转向环代码

Z_error = z_angle - Z_target1;

z_error_i = z_error_i + Z_error;

Turn = Z_error * Kp + z_error_i * ki + gyro * Kd;

4.2.4PWM限幅代码

int Amplitude = 130;  //===PWM满幅是255 限制在250

  if (Motor1 < -Amplitude) Motor1 = -Amplitude;

  if (Motor1 > Amplitude)  Motor1 = Amplitude;

  if (Motor2 < -Amplitude) Motor2 = -Amplitude;

  if (Motor2 > Amplitude)  Motor2 = Amplitude;

4.3场内定位方案设计

4.3.1惯性导航

采用的是FDISYSTEMS九轴AHRS姿态传感器N200，这种惯性导航系统内置陀螺仪和加速度计，磁力计，温度计。由于磁力计受温度影响较大，如果不搭配温度计，那么当此惯性导航传感器工作在温度较高环境时，就会有很大误差，因此搭配一个温度计进行误差分析能实现更高的精度N200相较于N100多搭载一颗航向陀螺，融合算法处理输出零偏精度2°/hr的航向角数据。此高精度惯性导航单元即可配合激光测距系统实现路径规划和场内定位。

4.3.2激光测距

激光测距TOFSense P-UART，这款激光测距传感器优点在于，传输速率较高，且精度相较于其他传感器，毫米级的精度足以实现场内定位的效果。且这款传感器可采用级联的方式，即每个激光测距传感器上都有两组引脚，即四根线即可挂载许多激光测距传感器，可节约大量引脚。

5机器视觉目标分类设计

5.1颜色采集机械结构部分

垃圾采集装置采用舵机控制，小车前方放置了超声波传感器，用来检测小车前挡板与场地前方的距离值，当距离小于设定的阈值后，机械臂即下降并开始进行抓球。抓球装置使用了弹性设计，使用弹性装置将小球挤入抓球装置内。

5.2颜色采集数据展示部分

                        5.1 数据集                                                               5.2 LOSS图

6平衡车成果展示

6.1平衡车三维模型

7总结与展望

7.1总结

自平衡车的自平衡原理探究和论证主要涵盖了物理模型分析、数学建模分析、PID原理分析等部分。基于Arduino Mega 2560 Pro单片机为主控板，并通过双串级PID算法实现了小车的稳定行走并实现精准抓球的效果。

7.2展望

通过改进机械结构，从而实现提高平衡车系统的稳定性；可以采用更高精度的陀螺仪传感器和加速度计传感器，从而实现提高系统姿态测量的精度；可以使用工作频率更高的主控制器，从而实现系统运算的速度。

8参考文献

•[1]乔纬国. 一种采用双PID串级控制的双轮自平衡车的研制[D].吉林大学,2017.