基于STM32的双轮平衡机器人的控制系统设计

范淇元，覃羡烘，李洪毅

（1.华南理工大学广州学院 机械工程学院，广州 510800；2.广东理工学院 工业自动化系，肇庆 526100）

伴随着计算、传感、控制及执行等技术的快速发展，科研工作者开始越来越重视移动机器人的研究，使得机器人学成为目前学科研究炙手可热的领域之一。社会在发展，人们对移动机器人的应用要求也越来越高。所以机器人面临的环境和任务也越来越复杂，这就要求移动机器人必须能够适应一些复杂的环境和任务[1-2]。比如，户外移动机器人需要在凹凸不平的地面上行走，有时需要在狭窄的空间运行等。如何解决机器人在这些环境中更好更平稳地运行，成为科研工作者须重点考虑的问题。

本双轮平衡机器人完美解决了上述问题。各个模块能够正常并协调工作，机器人可以在无人干预条件下实现自主平衡。同时在引入适量干扰情况下机器人能够自主调整并迅速恢复稳定状态。本控制系统在保证机器人平稳运行的前提下，具备两方面的优点：硬件电路总体体积小，而且其所包含的电子元器件价格低、可靠性高；程序易开发，开发耗时少，修改方便。

1 双轮平衡机器人的数学模型

1.1 双轮平衡机器人的整体结构

如图1所示，是双轮平衡机器人的机械结构三维图。双轮平衡机器人整体结构由机身和车轮两大部分组成。机身由上顶板（亚克力板）与下底板（不锈钢板）通过铜柱联接构成，机身的底板部搭载的是控制系统板。机身下部左右各安装1个370直流电机，2个电机各自驱动左右2个车轮，控制芯片向电机发送指令从而控制车轮实现机器人前进、后退和转向3个动作。经测量，双轮平衡机器人的尺寸为 150 mm×85 mm×110 mm。

图1 双轮平衡机器人整体结构三维图Fig.1 3D structure of two wheeled balancing robot

1.2 平衡的实现

当双轮平衡机器人的控制系统不工作时，不论机身向前或向后倾倒，两轮没有任何转动，这时机身前后摆动与其车轮转动是相互独立的；当控制系统开始控制时，机身的状态变化使机器人有前进、静止、后退3种运动的方式，而机器人上装载的姿态检测系统能够对机器人的倾斜状况进行实时检测并将得到的检测数据发送给单片机，单片机接收到数据后对车轮转动进行适当的控制，便可以抵消在这个维度上的倾斜力矩便可以保持双轮机器人的平衡，如图2所示。

双轮平衡机器人的控制核心思想是，当姿态检测系统检测到机身产生倾斜时，控制系统会根据测得的倾角产生一个相应的力矩，通过控制电机驱动车轮朝车身要倒下的方向运动至合适距离，以保持机身的动态平衡[3-5]。

图2 三种纠正方式Fig.2 Three types of correction

2 双轮平衡机器人控制系统硬件设计

2.1 控制系统硬件总体设计方案

双轮平衡机器人的硬件电路主要由控制芯片STM32F103C8T6、测量模块、直流电机驱动模块、蓝牙模块组成。其中测量模块主要是MPU6050、磁电编码器两部分。MPU6050测量的是机身的倾角θ与角加速度x（t），安装在左右2个电机上的磁电编码器将电机轴转过的角度经放大电路放大，然后经单片机处理后输出两路AB相脉冲信号，最后得到车轮的转速和方向。双轮平衡机器人硬件系统结构如图3所示。

图3 系统硬件结构Fig.3 Hardware structure diagram of the system

2.2 STM32F103C8T6芯片

主控模块的STM32F103C8T6单片机是控制器的核心部分。该单片机采用ST意法半导体公司生产的32位多功能、低成本、低功耗单片机，它的内核采用ARM公司最新生产的Cortex M3架构，最高工作频率可达72 MHz，运算速度可达1.25DMIPS/MHz，片上64 KB的闪存、20 KB RAM，拥有7个定时器。片上集成通信接口有2个I2C、3个USART、2个 SPI、1个 CAN 和 1个 USB。

控制机器人需要的端口如下：①2个PWM输出，用于控制2个电机的转速，用PWMA和PWMB表示。在此使用定时器3的两个输出端口PB0和PB1来输出PWM信号；②4个gpio口用于控制2个电机的正反转与停止。2个电机分别表示为motor1和motor2， 端口PA0、PA1控制motor1， 端口PB6、PB7控制motor2；③2个I2C模拟gpio端口，用于控制获取6轴模块的数据，使用的端口表示为SCL和SDA；④4个编码器输入接口，左轮的编码器使用了定时器2的编码器模式，需连接到定时器2的TIM2 CH1和TIM2 CH2端口（即PA0、PA1的端口）右轮的编码器使用了定时器4的编码器模式，使用的端口为TIM4 CH1和TIM4 CH2端口（即PB6、PB7的端口）；⑤2个蓝牙模块接口，主要用于接收和发送手机遥控信号，分别连接至单片机的PB10、PB11引脚上。

2.3 电源管理模块设计

电源模块由若干相互独立的稳压电源电路组成。这样有利于为各模块提供合适的电源并有利于减少模块相互间的干扰。

整个系统的电源来源为12V 18650锂电池，5 V电源由7805稳压器提供。7805稳压器用的是三端集成稳压电路，只有3条引脚输出，分别是输入端、接地端和输出端。3.3 V电源则由LM2596S提供，LM2596是3 A电流输出降压开关型集成稳压芯片。利用该器件只需极少的外围器件便可构成高效稳压电路，提供的有3.3 V、5 V、12 V及可调（-ADJ）等多个电压档次产品。选择3 V的电压档。

2.4 使用MPU6050测量姿态角度

MPU6050为全球首例整合性6轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器的轴间差问题，减少了大量的包装空间以及计算误差。MPU6050对陀螺仪和加速度计分别用了3个16位的ADC，将其测得的模拟量转化为可输出的数字量[6]。MPU6050的对运动状态的检测比较灵敏，可对快速和慢速的动作进行准确的跟踪，使用者可对传感器的测量范围进行设置，陀螺仪的测量范围为±250°/s，±500°/s，±1000°/s，±2000°/s， 加速度计的测量范围为±2 g，±4 g，±8 g，±16 g。一个片上的 FIFO有1024 B，起到降低系统功耗的作用。本次的MPU6050的通信采用的接口为 400 MHz的I2C。另外，片上内嵌了温度传感器和在工作环境下仅有±1%变动的振荡器。SDA和SCL连接到单片机，INT产生终端信号，连接至单片机。AD0接地。MPU6050综合利用了陀螺仪和加速度计的特点，优势互补获得较准确的姿态角度。方法是将模块的姿态数据输出表示，作为系统的状态量，模块的姿态运动学方程作为滤波的状态转移方程，加速度信息作为滤波的观察量信息，然后利用卡尔曼滤波（本次利用的是简易卡尔曼滤波）的计算方法迭代计算更新，从而可得到精度较高的不漂移的三轴角度。

2.5 电机及驱动设计

电机型号选择JGA25-370，采用齿轮组减速，减速比为1∶34。智能小车常用的直流电机驱动一般是L298N，但本设计使用的电机驱动是TB6612FNG，因为与L298N相比，TB6612FNG在效率上有更显著提高，而且体积也变得更小，其大小与1元硬币相当。

TB6612FNG内部电路是MOSFET的H桥集成电路，效率远高于晶体管H桥驱动器。相比L298N的热耗性和外围二极管续流电路，它无需外加散热片，外围电路简单，只需外接电源滤波电容就可以直接驱动电机，系统尺寸大幅减小。由于启动电机的瞬间电流很大，会瞬间拉低整个系统的电压，导致其他元器件无法正常工作，而且还有可能导致电机电源输入端电路烧坏。所以，需要在电池电源输入端加上较大的滤波电容C2。

2.6 磁电编码器

使用磁电编码器为61302H2系列霍尔永磁编码器。该编码器为两通道增量式磁电编码器，可以直接固定于电机尾部电源脚上，磁栅固定到电机尾轴上。霍尔永磁编码器包含1个磁栅和2个磁敏检测传感器，输出2个通道正交相位角90°的方波。永磁编码器响应速度快、抗震性强、传输距离远，在恶劣的环境下使用有较长的使用寿命和较高的可靠性。

2.7 蓝牙模块选择

蓝牙模块选择BC04，与手机的蓝牙匹配后，通过手机上装载的遥控软件，可实现对双轮平衡机器人的遥控和辅助调试，验证双轮平衡机器人在保持平衡的同时，还能做前进、后退、转弯的动作。

3 控制系统软件总体设计

软件设计是双轮平衡机器人控制系统的关键。本次软件设计实现的主要功能包括系统初始化、机器人姿态信息的采集和卡尔曼滤波处理、速度检测、机器人运动控制（PID）、直立控制、速度控制及转向控制、蓝牙遥控和限幅控制。

图4所示为双轮平衡机器人的主程序流程图，机器人通电运行后，便进行单片机的初始化。初始化包括两部分，第一部分是对单片机所用到的模块进行初始化；第二部分是应用程序初始化，是对于机器人控制程序中应用到的变量值进行初始化。第一部分代码可以通过网上下载的STM32库进行移植实现[7-8]。初始化结束后，先进入机器人直立检测程序，并完成对机器人的姿态信息采集。该段程序中，单片机读取MPU6050的数值判断是否已完成对机器人的姿态信息采集。如果一旦采集完成，则启动机器人直立控制、速度控制及转向控制。程序在控制机器人运动的同时，也向单片机不断地反馈机器人的运动状态，判断机器人是否跌倒。跌倒可通过检测机器人的倾角是否超过一定范围进行判断。如果机器人跌倒，则停止机器人运行，然后重新进入姿态信息采集过程。

图4 主程序框架Fig.4 Main program flow chart

4 系统调试结果

4.1 姿态检测系统调试

如图5所示，串口助手显示的Pitch、Roll、Yaw分别代表三轴的角度，而本设计中，实际计算需要的只有Pitch（俯仰角），其大小等于机身倾角θ（穿过机身的水平坐标轴与水平面的夹角）；显示的ACCEL、GYRO则分别表示三轴加速度、三轴角速度。通过观察Pitch的值变化可以发现，机器人刚开始运行时机身倾角在0°附近变化，在0°附近说明控制系统能保持双轮机器人保持直立。

图5 串口输出数据Fig.5 Serial output data

4.2 实际运行效果

为了更好地测试双轮平衡机器人的动态性能，利用手机蓝牙与其上的蓝牙模块匹配后对其进行遥控，检验机器人的运动性能。蓝牙控制界面如图6所示。

图6 蓝牙控制界面Fig.6 Bluetooth control interface

滑动界面上蓝色的圆圈，可让双轮平衡机器人往滑动的方向移动。经过多次的测试和调整，本次设计的双轮平衡机器人控制系统可以让机器人在受干扰的条件下保持运动的稳定性，其稳定性及抗干扰能力较好。其运行效果如图7所示。

图7 运行效果图Fig.7 Operation effect diagram

5 结语

基于STM32设计的双轮平衡机器人的控制系统在验证试验中取得了良好的结果，各功能模块相互协调并能正常工作，在无人干预和适量干扰的情况下，机器人都能够自主调整并迅速恢复稳定状态，因此为双轮平衡机器人应用到实际提供了较好的依据。

参考文献：

[1]K.Y.Cai，L.Zhang.Fuzzy reasoning as a control problem[J].IEEE Transactions on Fuzzy Systems，2008，16（3）：600-614.

[2]L.A.Zadeh.The concept of a linguistic variable and its application to approximate reasoning-I[J].Springer US，1974，8（3）：199-249.

[3]罗绥辉.四旋翼控制系统的硬件电路设计研究[J].中国机械，2014（16）：201-210.

[4]惠易佳，姜羡，朱再强.基于Arduino平台的自平衡二轮排爆车[J].科技视界，2015（2）：22-30.

[5]胡从坤，余泽宇，陈曦晨.四旋翼飞行器控制系统研究[J].科技广场，2014（6）：61-64.

[6]戴维，岳明，肖鹏程，等.单轴双轮自平衡小车的设计研究[J].物理实验，2014（12）：33-36.

[7]甘家辉.两轮自平衡机器人的系统设计及运动控制研究[J].模式识别与智能系统，2010（22）：45-47.

[8]戴福全，李济泽，朱悦涵，等.两轮自平衡机器人转向稳定性优化研究[J].机械设计与制造，2017（4）：256-258.