双轮自平衡小车控制系统设计

蚌埠学院电子与电气工程系 孟 帅 常德军 陈章宝

双轮自平衡小车控制系统设计

蚌埠学院电子与电气工程系 孟 帅 常德军 陈章宝

双轮自平衡小车是双轮机器人研究的基础，具有多变量、非线性、强耦合等特性。本文以STC15W4K32S4为主控芯片，MPU6050采集车身的角度和角速度，通过互补滤波进行数据融合，得到优化的角度值，以TB6612FNG作为电机驱动芯片，霍尔元件进行车速测量，构建双轮小车的姿态平衡控制和运动控制系统。通过姿态解算和PID控制，实现了小车的平稳运行。

双轮自平衡小车；STC15W4K32S4；MPU6050；TB6612FNG；PID

引言

双轮自平衡小车因其结构简单、转向灵活等特点，能在狭窄空间中行驶，以及在特殊环境下执行特殊任务，对变化的地形适应性强，引起新一轮研究热潮和广泛的应用开发。双轮自平衡小车作为控制系统的经典模型，是验证控制理论和力学理论的理想实验平台，为科学理论的实验研究起到了硬件平台的作用，有着巨大的研究价值[1]。

1.小车运动平衡分析

设小车模型车体重心到底盘重心长度为l，质量为m，当车轮产生水平向右的加速度α时，对车模受力分析如下图1所示。

图1 车模受力分析

根据小车平衡状态受力和“角加速度与曲率半径的乘积等于切向加速度”可得到车模运动过程中的动态数学模型[2]：

在初始状态时，车轮加速度α为0，并进行工程近似：在θ较小时，，模型可以简化为下式。

对上式微分方程等式两边同时进行拉普拉斯变换，得到静止状态下系统传递函数为：

由式可解系统有一个极点位于复频域实轴的右半部分，所以在静止时车身无法达到平衡，为此需引入比例微分控制，对车模传递函数进行校正，提高系统的稳定性。在增加比例微分负反馈后，相当于增加了系统的开环零点，系统两极点均具有负实部，系统稳定[3]。

2.硬件电路设计

为实现双轮自平衡小车的平衡控制和运动控制，系统硬件设计包括主控模块、姿态检测模块（陀螺仪和加速度计）、测速模块、直流电机驱动模块、电源模块和人机交互模块，控制系统框图如图2所示。电压转换电路采用LM2940-5.0，将电池组的+12V电压转成+5V，用AMS1117-3.3稳压电路将+5V转成+3.3V，人机交互模块使用LCD1602和四个独立按键，实现系统调试时的参数设置和显示。

图2 控制系统方框图

2.1 主控模块设计

系统主控模块采用STC15系列MCU，STC15W4K32S4是宏晶公司生产的增强型8051内核的单片机。工作电压2.5～5.5V，有6路硬件PWM、7个定时器、4组串口、8路高速10位ADC，最大有61k程序Flash和4k字节SRAM，内部集成了时钟和复位电路。通过引脚1、3与MPU6050进行I2C通信，采集姿态数据，引脚30～33、34、35连接电机驱动芯片TB6612，控制电机的转向和转速，通过引脚38、39捕获霍尔传感器输入脉冲；通过引脚21、2、22、4可以对霍尔传感器两相A1(A2)、B1(B2)进行外部中断触发和计数器计数。

2.2 姿态检测电路

姿态传感器使用的是MPU6050，内部集成了三轴加速度计和三轴陀螺仪，所以又称为六轴传感器[4]，具有体积小、功耗低、分辨率高等优点。模块内置三个16位模数转换器，加速度计测量范围±2、±4、±8、±16g，陀螺仪测量范围±250、±500、±1000、±2000°/sec，可编程控制姿态传感器的测量范围，来提高读取数据的最小精度，使用引脚23、24与主控间进行I2C通信。姿态检测电路图如图3所示。

图3 姿态检测电路

2.3 电机驱动电路

电机驱动芯片选用的是东芝生产的TB6612，它的最大工作电压高达15V，平均工作电流可达1.2A，瞬时最大电流可以达到3.2A，而且PWM控制频率可高达100KHz[5]。AIN1、AIN2、BIN1、BIN2脚为输入控制电平，控制电机正、反转，PWM3、PWM4脚与MCU的PWM输出口相连，控制电机加、减速，电机驱动电路如图4所示。TB6612具有大电流H桥结构，双通道电路输出，可同时驱动两个电机，AO1(BO1)、和AO2(BO2)之间可分别接电机。

图4 电机驱动电路

2.4 霍尔测速电路

霍尔传感器是基于霍尔效应的磁场传感器，主要由磁块和磁电检测装置组成[5]。将磁铁粘在非磁性圆盘的边沿，靠近圆盘边沿处放置霍尔传感器，当圆盘转动时，霍尔传感器就输出脉冲，相应的计数变量就加1，在单位时间内测得的脉冲数越多代表速度越快。脉冲采集电路主要由电压比较器LM393、霍尔元件3144和磁感应探头组成，采用非接触检测方式，D6为感应指示灯，当感应到磁场时，输出低电平，信号灯D6亮，3144感应磁场的强度并输出数字信号，再由LM393转化成为脉冲。

3.系统软件设计

采用模块化的设计思想，系统软件设计包括：数据采集与传输，PID控制，PWM驱动及其他模块程序设计。系统上电初始化后，首先检测倾角是否在可控范围内，条件为真时进行数据采集与传输，通过角度PD和速度PI控制PWM输出，调节PWM占空比来调节车速，从而保持车模的动态平衡，通过改变给定的车体倾角θ实现小车的运行。

六轴姿态传感器MPU6050内置了三轴加速度计和三轴陀螺仪，知道六个轴的数据寄存器的地址，就能从数据寄存器中读取数据，MPU6050中自带了I2C程序，利用由数据线SDA和时钟线SCL构成的两线式串行总线，主控单片机就能通过I2C通信读取姿态传感器的未经过滤波的角度，通过互补滤波进行数据融合，得到小车的倾角值。

PID（比例、积分、微分）控制以车身倾角为设定值[6]，以偏差e(t)为输入，构建双闭环PID调节控制器，通过第一级角度PD控制计算出小车保持动态平衡需要的车轮速度值，然后以直流电机上霍尔测速装置构成第二级的速度PI控制。第一级角度反馈不断去补偿角度偏差直至最后接近零，第二级速度反馈调节加到电机上保持小车以特定速度运行。

PWM即脉冲宽度调制，它是指将输出信号的最小周期固定，通过调整最小周期内工作周期的大小来控制输出电压的方法。本设计中，通过PWM调宽可以快速地控制输入直流电机的电压大小来达到调速的目的，设最小周期固定时，输出电压最大为，设占空比为，则平均输出电压为，当改变占空比α时，就可以得到不同大小的输出电压，从而达到调速的目的。

4.总结

本次设计以STC15W4K32S4为主控芯片，通过互补滤波和PID控制算法，设计出能够自平衡的两轮直立前进后退、左右转向的控制系统。通过互补滤波算法，滤波后的角度，既能在静态时紧紧趋近于加速度计的角度，也能在动态时随陀螺仪角度的变化而变化，角度PD控制算法通过互补滤波输出优化的角度并乘以比例增益，再相加未经滤波的角速度乘以微分增益的值，两者共同输出角度PD控制下的电机驱动PWM信号；速度PI控制算法将设定与实测速度值的偏差作为输入，先积分后乘以积分增益，再结合输入乘以比例增益得到的值，两者共同输出速度PI作用下的电机驱动PWM信号，角度与速度输出的PWM值相加得到最终电机驱动PWM信号。实际运行结果表明，小车上电后的动态特性良好，能够有效实现对小车整体平稳运行。本设计实现了小车在平整路面上的运动控制，如何实现小车在不平整及障碍路面执行特定的任务，需在以后的实践中进一步进行研究。

[1]徐国保,尹怡欣等.智能移动机器人技术现状及展望[J].机器人技术与应用,2010.3:29-32.

[2]上海交通大学物理教研室.大学物理学(第四版)[M].上海:上海交通大学出版社,2010:115-117.

[3]高金源,夏洁.计算机控制系统[M].北京:清华大学出版社,2010:159-164.

[4]赖义汉,王凯.基于MPU6050的双轮平衡车控制系统设计[J].河南工程学院学报,2014,26(1):53-57.

[5]刘江.两轮自平衡机器人系统设计及控制研究[D].北京:北京工业大学控制科学与工程系,2007.

[6]孟庆明.自动控制原理(非自动化类)(第二版)[M].北京:高等教育出版社,2008:179-183.

孟帅（1995－），男，本科，电子信息工程，研究方向：嵌入式系统开发，机器人运动控制。

大学生创新创业计划项目（AH201411305036），2016蚌埠学院教学研究项目（2016JYXM28）。