基于MPU6050的双轮平衡车控制系统设计

赖义汉，王 凯

(龙岩学院 物理与机电工程学院 ，福建 龙岩 364012)

基于MPU6050的双轮平衡车控制系统设计

赖义汉，王 凯

(龙岩学院 物理与机电工程学院 ，福建 龙岩 364012)

双轮平衡车是一个高度不稳定两轮机器人，单独使用陀螺仪或者加速度计都不能提供有效而可靠的信息来保证车体的平衡.介绍了以STC12C5A60S2单片机作为核心控制器的双轮平衡车控制系统设计方案，采用MPU6050六轴传感器采集角度和角速度信号，应用卡尔曼融合滤波方式和PID控制算法，以实现双轮平衡车的平衡和简单的直立行走.

双轮平衡车；MPU6050；控制系统

图1 系统总体框图Fig.1 System structure diagram

1 系统总体框图

双轮平衡车控制系统主要由控制核心(MCU)模块、姿态传感器模块、无线遥控模块、电源模块、电机驱动模块、车速检测模块及显示模块等组成，系统总体框图如图1所示.姿态传感器采用MPU6050，主要采集小车的角度和角速度信号；MCU采用宏晶科技有限公司的STC12C5A60S2单片机[2]作为控制芯片，该芯片具有I2C接口和PWM输出，抗干扰能力强；显示模块的主要功能为显示车体的倾角等相关信息.

2 姿态传感器信号处理

在双轮平衡车姿态检测系统中，加速度计、陀螺仪主要用于检测车体倾斜角和倾斜角的变化速度，其中加速度计属于测量线性运动的传感器，其静态响应好，能够准确测量车体静态时的角度，但对震动较敏感，受动态加速度的影响较大.在角度测量时，除了小车角度变化的信号外，还伴随着因车体运动而产生的噪声，这个噪声会随着车体运动速度的增加而增大.陀螺仪属于测量旋转运动的传感器，其输出值围绕某个轴向的旋转角速率，通过角速率对时间积分即可得到角度值.系统采用微控制器循环采样获取陀螺仪的角速率信息，即每隔一段很短的时间采样一次，采用累加的方法实现积分的功能来计算角度值，所以存在累积漂移误差，不适合长时间单独工作[3].

为获得准确的车体倾角值，需对加速度计和陀螺仪的输出值进行融合，系统采用一种简易的卡尔曼滤波方法进行数据融合.卡尔曼滤波器是一种递归的估算，例如要估算K时刻车体的实际角度值，首先要根据K-1时刻的角度值预测得到K时刻的角度值，再根据K时刻的预测角度值和高斯噪声的方差，进行递归运算，直到获得最优的车体角度值[4].

图2 角度与角速度数据融合及输出控制框图Fig.2 The output control diagram of angle and angular velocity data fusion

当双轮平衡车车体产生倾斜时，系统采用PID控制算法[5]，通过整合车体角度、角速度、车体速度和位置等参数值，输出PWM信号驱动电机，产生相应的力矩，从而保持车体的动态平衡,其结构框图如图2所示，图中的Kd，Kp，Ksp，Kis是PID控制器的相关参数.

表1 MPU6050的主要管脚功能Tab.1 The functions of MPU6050 pins

3 主要硬件电路设计

3.1MPU6050传感器电路

MPU6050六轴传感器芯片集成了3轴MEMS陀螺仪和3轴MEMS加速度计，每个轴对应有一个16位AD转换器，测量范围达±16 g，其高分辨率(3.9 mg/LSB)能够测量不到1.0°的倾斜角度变化.芯片正常工作时，陀螺仪和加速度计分别采集x轴、y轴和z轴的电压值，然后通过AD转换，转换成数字信号，最后通过I2C总线传送到控制芯片，但此时得到的值并不是实际的角度和角速度值，还必须经过一定的比例关系进行转换，才能得到实际的角度和角速度值，其电路如图3所示.表1为MPU6050芯片主要的引脚功能.

图3 MPU6050接口电路Fig.3 Interface circuit of MPU6050

图4 车速检测电路Fig.4 Detection circuit of vehicle speed

3.2车速检测电路

在小车的车轮上安装码盘，通过红外光电传感器GK102组成测速电路，利用控制单片机的计数器测量在固定时间间隔内速度脉冲信号的个数来得到电机的转速，再通过变换得到车体的速度.光电传感器GK102外接电路如图4所示.

P2P网贷平台的刑事治理有一定困难之处:第一，P2P平台非法集资行为具有一定的隐蔽性，所以发现难;第二，对P2P平台非法集资行为的罪与非罪、此罪与彼罪和共同犯罪的成立等问题的定性难;第三，对P2P网络集资相关犯罪的追赃难，一方面是定性赃款的范围有难度，另一方面案件发生后，非法所得都被挥霍一空;第四，由于犯罪成本低、司法机关重视事后制裁，所以对P2P网络非法集资行为预防难。

图5 无线遥控模块电路Fig.5 Circuit of wireless remote control module

3.3无线遥控模块电路

无线遥控模块采用nRF24L01+，该芯片是一款工作在2.4～2.5 GHz世界通用ISM 频段的单片无线收发器芯片.无线收发器包括频率发生器、增强型SchockBurst模式控制器、功率放大器、晶体振荡器、调制器、解调器等.输出功率频道选择和协议的设置可以通过SPI接口进行设置，其外围接口电路如图5所示，该模块主要用来遥控小车的前进和后退等工作方式.

3.4电机的驱动电路

双轮平衡小车执行机构由两个减速直流电机构成，其驱动电路由L298N组成，电路如图6所示.其中,OUT1和OUT2以及OUT3和OUT4之间分别接两直流电机M1和M2，IN1～IN4脚为输入控制电平，通过光电耦合器与单片机相连，控制电机正反转，ENA和ENB为控制使能端，与单片机的PWM 输出口(P1.2和P1.3)相接，控制电机转速， L298N的 1脚和15脚是输出电流反馈引脚，通常可直接接地,其电机控制功能模式如表2所示.

表2 直流电机控制功能表Tab.2 The table of DC motor control function

图6 电机驱动电路Fig.6 The motor drive circuit

4系统软件设计

系统软件主要完成传感器数据的采集及相关数据的处理并输出PWM控制信号控制电机的运行，主程序流程如图7所示.其中，Angle_Calcu()为角度和角速度处理函数，主要读取传感器MPU6050的角度和角速度信号并进行融合滤波，得到车体的角度值，其程序流程如图8所示.Psn_calcu()为速度位置处理函数， PWM_Calcu()函数主要完成PID控制算法，并输出PWM信号.

图7 系统主流程Fig.7 The main flow chart of system

图8 角度、角速度处理函数流程Fig.8 Function flow chart of angle and angular velocity

5 系统测试

5.1角度和角速度的零点偏移值测定

根据MPU6050传感器摆放的位置，选择加速度计y轴数据进行角度计算，选择陀螺仪x轴数据进行角速度计算.单片机通过I2C总线分别读取MPU6050加速度计的角度值Accel_y和陀螺仪的角速度值Gyro_x.在静止状态下把车放置在平衡位置，待传感器稳定后，测量MPU6050传感器的10组数据值，如表3所示，通过求平均值计算此时角度和角速度的零点偏移值分别为大约1 100 LSB和8 LSB.

表3 角度和角速度零点偏移Tab.3 Zero offset of angle and angular velocity

当偏转角度不太大时，可通过公式(1)和(2)转换成实际的角度和角速度:

(1)

Gyro_x=(Gyro_x-8)/16.4.

(2)

公式(1)中Accel_y为MPU6050传感器加速度计y轴的数据，1 100为加速度计的零点偏移值，加速度计选±2 g，通过查MPU6050传感器数据手册可知，其对应的灵敏度为16 384 LSB/g，而180/3.14是为了把弧度转换成度，此时得到的值不是它真正的角度值，需适当放大1.2倍，近似得到它的角度值.公式(2)中Gyro_x为MPU6050传感器陀螺仪x轴的数据，8为陀螺仪的零点偏移值，陀螺仪选±2 000 (°/s).查MPU6050传感器数据手册可知，其对应的灵敏度为16.4 LSB/(°/s).此时的Angle_ay为(-90,90),平衡位置为0度.Gyro_x的值在静止时为0，摆动时不为0，方向与角度的微分方向一致，符合要求.

5.2PID控制器的参数整定

通过对PID公式进行变换，得到PWM输出表达式，驱动直流电机，其计算公式为

PWM=Kp×Angle+Kd×Gyro_x+Kis×Position+Kps×Speed，

(3)

公式(3)中，Angle代表角度，Gyro_x代表x轴角速度，Position代表车体速度的积分得到的位置，Speed代表测得的车体速度.

首先,通过Matlab进行仿真，确定PID控制器4个参数的大致范围及比例关系，再通过实验法来确定PID算法中的各个参数，具体步骤如下：

(1)首先确定角度的比例参数Kp和微分参数Kd，可以遵循先比例后微分的过程.首先,调整比例参数，使车模能够保持直立并且开始来回摆动.然后,逐步增加微分参数，车模逐步保持直立稳定.增大微分参数直到车模开始共振，这样就可以确定微分参数的最大值.然后，适当减少微分参数、逐步增加比例参数，直到车模又开始震荡，这样便可以确定比例参数的最大值.最后，适当减少比例参数.在这些参数附近进行实验，直到寻找到一组车模直立控制的最优参数为止.

(2)在调整车速的微分参数Kis和比例参数Kps时，采用类似方法，先逐步增加比例参数，使车模能够在一定平衡点附近来回运动，然后再逐步增加微分参数，使车模能够较快停留在平衡点处，再适当调整微分和比例控制参数，使车模在外力冲击下能够很快趋于静止.通过几次调整测试，可以初步确定速度控制参数的最佳组合.

6 结语

双轮平衡车控制系统以STC12C5A60S2单片机为控制芯片，采用卡尔曼融合滤波方式对MPU6050传感器的角度和角速度信号进行融合处理，从而得到实时、稳定的角度和角速度信号，应用实验测试法确定PID算法中的各个参数，从而实现对双轮平衡车的控制.经多次测试，能初步实现车体的平衡和简单的直立行走.

[1] 张伟民,段晓明,赵艳花.两轮自平衡小车控制研究[J].自动化技术与应用,2011，30(4)：10-13.

[2] 陈桂友,柴远斌.单片机应用技术[M].北京：机械工业出版社,2008：214-220.

[3] 郜园园，阮晓钢，宋洪军,等．两轮自平衡机器人惯性传感器滤波问题的研究[J].传感器技术学报，2010,23(5):696-699.

[4] 冯智勇，曾瀚，张力,等.基于陀螺仪及加速度计信号融合的姿态角度测量[J].西南师范大学学报:自然科学版，2011，36(4):137-140.

[5] 何跃,林春梅.PID控制系统的参数选择研究及应用[J].计算机工程与设计,2006,27(8)：1496-1498 .

Thedesignofthetwo-wheeledbalancingvehiclecontrolsystembasedonMPU6050

LAI Yi-han，WANG kai

(PhysicsandMechanicsCollege,LongyanUniversity,Longyan364012,China)

Two-wheeled balancing car is a highly unstable two-wheeled robot, the information depending on the gyroscope or the accelerometer couldn't make sure the balance of the vehicle．The paper mainly introduced the design of the two-wheel balancing car control system, as the core controller of the STC12C5A60S2 microcontroller. System collects angle and angular velocity base on MPU6050 six axis sensors. Using Kalman filtering fusion method and PID control algorithm, the balance and walking upright of two-wheeled balancing is realized.

two-wheeled balancing vehicle；MPU605；control system

2013-10-28

赖义汉(1968-)，男，福建龙岩人，副教授，主要从事嵌入式系统与应用方面的研究.

TP212.9

A

1674-330X(2014)01-0053-05