基于单片机的两轮自平衡小车的设计∗

魏 雅

基于单片机的两轮自平衡小车的设计∗

魏 雅

（陕西工业职业技术学院 咸阳 712000）

系统采用陀螺仪MPU6050构成小车姿态检测装置，使用卡尔曼滤波完成陀螺仪数据与加速度计数据的融合，选用STM32F103单片机为控制核心，完成了传感器信号的处理，实现了车身的控制。整个系统的各个模块能够正常、协调工作，小车可以在无人干预条件下实现自主平衡。同时在引入适量干扰情况下小车能够自主调整并迅速恢复稳定状态。

两轮自平衡；陀螺仪；姿态检测；卡尔曼滤波；数据融合

AbstractThe system uses gyroscope MPU6050 car attitude detection device，uses the kalman filter to complete the data fu⁃sion of gyroscope and accelerometer data，uses STM32F103 microcontroller as control core，completes the processing of the sensor signal，realizes the control of the body.Each module of the whole system can work normally and coordinate，and the car can realize independent balance under the condition of no intervention.At the same time，in the case of the introduction of appropriate interfer⁃ence，the car can be adjusted independently and quickly restored to a stable state.

Key Wordstwo wheel self balancing，gyroscope，attitude detection，kalman filter，data fusion

Class NumberTP3-02

1 引言

近年来，随着电子技术的发展与进步，移动机器人的应用范围越来越广泛，但是怎样让移动机器人在凹凸不平的地面上行走，或者在较狭窄的地方行走等，逐渐成为研究者关心的问题。系统设计的两轮自平衡机小车［1］，采用了两轮共轴、各自独立驱动的工作方式，车身的重心位于车轮轴的上方，通过轮子的前后移动来保持车身的平衡，并且还能够在直立平衡的情况下行驶。由于特殊的结构，其适应地形变化能力强，运动灵活，可以在复杂的环境里工作。

2 系统硬件电路设计

系统硬件设计总框图如图1所示。在硬件电路设计时，对电路的所有环节都进行了电磁兼容性设计，做好各部分的接地、屏蔽、滤波等工作，将高速数字电路与模拟电路分开，从而大大提高本系统工作的可靠性和高效性。

图1 硬件设计总框图

2.1 单片机最小系统电路

系统采用 STM32F103 增强型系列单片机［2～3］为控制核心，采用的是5V供电，由它构成的最小系统［4～5］主要包括单片机供电、复位电路、时钟电路。由于单片机内部集成了多个模块，因此使用方便。

设计中时钟电路采用的外部晶振为16MHz，电容C11和C12为外部时钟的起振电容。在单片机进入睡眠模式时，时钟振荡电路输入端被内部200Ω的下拉电阻拉低，振荡电路停振，从而达到省电的目的。图2为单片机最小系统时钟电路原理图。

图2 时钟电路原理图

图3 复位电路

设计中采用了由电阻电容构成的简易复位电路，如图3所示。加电后，由于电容的充电，RESET保持低电平，单片机复位；一段时间后，电容电量充满，RESET端输出高电平，此时单片机运行。手动复位时，按下手动复位按钮，RESET端保持低电平，单片机复位；释放手动复位按钮后，RESET端输出高电平，单片机工作。

2.2 电源模块设计

电源模块［6］由若干相互独立的稳压电路模块组成，这样做可以减少各模块之间的相互干扰。整个系统需要三种电源：1）7.2V电源，为驱动电机供电；2）5V电源，为单片机及相关外设供电；3）3.3V电源，为陀螺仪及加速度计供电。

整个系统电源来源为7.2V镍氢电池，5V电源由LM2940提供，3.3V电源采用AMS1117。电机供电直接采用电池供电，如图4所示为电源模块电路。

图4 电源模块电路图

2.3 倾角传感器信号调理电路

系统采用MPU6050陀螺仪，其输出为0.67mv/deg/sec。ATD模块最高采集精度为12bit，AD基准电压为3.3V，计算得出最小分辨电压为0.8mv，因此不能直接对陀螺仪输出信号进行采集，需要设计放大电路。系统采用LM358（图5 U1B）设计负反馈放大电路，放大倍数为+1，即放大10倍。同时，由于陀螺仪输出会随温度而变化，影响系统倾角检测精度，为抑制陀螺仪温漂，需要在放大电路中设计零点偏置电压调整电路。系统中利用LM358（图5 U1A）构成电压跟随器，输出电压通过电位器调节，使零点偏置电压保持在陀螺仪工作电压的一半（1.65V），这样可以有效抑制陀螺仪的温漂，放大电路如图5。

图5 放大电路

2.4 电机驱动电路设计

本系统中，根据功能要求，采用TB6612电机驱动电路。由于电机启动瞬间电流很大，会将整个系统电压拉低，造成其他设备如单片机的工作不正常，因此要在电池电源输入侧加上较大滤波电容。

如图6所示，PWM1和PWM2分别为两个半桥的控制端口。当PWM1为高电平，PWM2为低电平时，MOTOR1口即输出高电压，MOTOR2输出低电压，此时电机正转；当PWM1为低电平而PWM2为高电平时，MOTOR1口即输出低电压，MOTOR2输出高电压，此时电机反转。通过改变PMW 1和PWM2端口的驱动波形占空比改变输出端电压，从而实现电机调速的目的。

2.5 速度检测模块设计

两轮自平衡小车的原理［7～9］是利用地面对车轮的摩擦力抵消车受到的重力，在系统的控制环节中有两路闭环控制，即倾角闭环控制以及速度闭环控制。为实现速度的闭环控制，必须加入速度检测装置实现速度闭环控制中的反馈环节。系统测速模块采用旋转霍尔编码器。由于编码器采用集电极开路输出，输出波形为矩形波，因此编码器外围电路较为简单。需要在信号输出端接入一个上拉电阻，即可将信号提供给单片机采集数据。如图7所示，PULSE引脚为编码器A相，接单片机的脉冲计数口，通过单片机的PACNT模块对输入脉冲进行计数从而获取电机转速。DIR为编码器B相输出，接单片机I/O口，通过A、B相位差进行软件鉴相，从而判断电机转动方向。

图6 TB6612电机驱动电路

图7 编码器接口电路

3 系统软件设计

软件设计［10～11］是两轮自平衡小车设计中的关键。根据系统要求，软件需要完成单片机初始化、姿态信息采集、卡尔曼滤波、速度检测、直流电机PID控制算法等。系统软件流程如图8所示。由于篇幅问题，在此重点介绍姿态信息采集软件设计。姿态信息采集包括陀螺仪与加速度计输出值转换及卡尔曼滤波器软件实现两部分。

图8 系统总体软件流程图

3.1 陀螺仪与加速度计输出值转换

本系统采用的惯性传感器为模拟量输出式，利用单片机自带ATD模块进行模数转换，从而检测出传感器输出电压。此外还需要对陀螺仪及加速度计的输出进行转换运算，使最终送入卡尔曼滤波器的为陀螺仪检测的角速度以及加速度计检测的角度。

陀螺仪输出电压与检测角速度转换公式如下

式中：G为陀螺仪检测的角速度；Vout为陀螺仪输出电压；Voffest为陀螺仪静止时的输出电压；Vsen为陀螺仪灵敏度；K为放大电路放大系数。加速度计输出电压与检测的角度转换公式如下

式中：A为加速度计检测的重力加速度；Vout为加速度计输出电压；Voffest为加速度计在平衡位置的输出电压；Vsen为加速度计灵敏度。再利用反三角函数求出与重力方向的倾角。

根据式（1）将陀螺仪的输出电压转换为角速度；根据式（2）求出加速度计的输出角度acc（此时得出的角度为弧度），为了将加速度计的输出角度与陀螺仪的积分角度单位统一，需要对加速度计的输出角度进行单位换算，通过式（3）进行换算得出角度。

3.2 卡尔曼滤波器的软件实现

由卡尔曼滤波［12］原理可知，卡尔曼滤波器是一种高效率的递归滤波器（自回归滤波器）。其主要原理是利用前一状态的估计值和当前状态的观测值更新对状态变量的估计，从而得到当前状态的估计值。卡尔曼滤波器的工作流程如图9所示。

图9 卡尔曼滤波器预估-更新流程图

通过卡尔曼增益计算出最优估计值及预测值偏差，得到最优角度值及角速度值。

4 结语

通过相应硬件与软件的设计，最终实现了两轮自平衡小车的平衡控制及运动控制。小车可以在无人干预条件下实现自主平衡。同时在引入适量干扰情况下小车能够自主调整并迅速恢复稳定状态。

［1］季鹏飞，朱燕，程传统，等.基于STM32的两轮自平衡小车控制系统设计［J］.电子科技，2014，27（11）：96-99.

JI Pengfei，ZHU Yan，CHENG Jing，et al.Design of con⁃trol system for two wheeled self balancing vehicle based on［J］.Electronic Science and technology，2014，27（11）：96-99.

［2］郝芸，杨奇，佟皓萌.基于STM32的四轴飞行器设计［J］.电子测试，2015（18）：6-8.

HAO Yun，YANG Qi，TONG Haomeng.four axis aircraft design based on STM32［J］.Electronic Test，2015（18）：6-8.

［3］常国权，戴国强.基于STM32的四轴飞行器飞控系统设计［J］.单片机与嵌入式系统应用，2015，15（2）：29-32.

CHANG Guoquan，DAI Guoqiang.STM32 four axis air⁃craft flight control system design of［J］.microcontroller and embedded systems based on STM32，2015，15（2）：29-32.

［4］王素青，熊维堂.基于STM32的两轮自平衡小车系统设计［J］.实验室研究与探索，2016，35（5）：146-150.

WANG Suqing，XIONG Weitang.Two STM32 based on self research and exploration，the design of［J］.car system laboratory balance，2016，35（5）：146-150.

［5］张志强.基于STM32的双轮平衡车［J］.电子设计工程，2011，19（13）：103-106.

ZHANG Zhiqiang.STM32.The two wheeled balancing car［J］.Electronic design project，based on the，2011，19（13）：103-106.

［6］王振运.基于RT-Thread和STM32的双轮自平衡机器人的设计与实现［D］.太原：中北大学，2016.

WANG Zhenyun.A RT-Thread and STM32 based on the design and implementation of the self balancing robot［D］.Taiyuan：North Central University，2016.

［7］梁华，李晓虹，杨光祥.两轮自平衡机器人动力学模型分析及PID控制方法研究［J］.重庆师范大学学报（自然科学版），2016（1）：163-167.

LIANG Hua，LI Xiaohong，YANG Guangxiang.Study on dynamic model of two wheeled self balancing robot and PID control method［J］.Journal of Chongqing Normal Uni⁃versity（Natural Science Edition），2016（1）：163-167.

［8］冯智勇，曾瀚，张力，等.基于陀螺仪及加速度计信号融合的姿态角度测量［J］.西南师范大学学报自然科学版，2011，36（4）：137-141.

FENG Zhiyong，ZENG Han，ZHANG Li，et al.The gyro⁃scope and accelerometer based on attitude angle measure⁃ment signal fusion［J］.Natural Science Journal of South⁃western Normal University，2011，36（4）：137-141.

［9］阮晓钢，任红格.两轮自平衡机器人动力学建模及其平衡控制［J］.计算机应用研究，2009，26（1）：99-101.

RUAN Xiaogang，REN Hongge.Dynamic modeling and balance control of two wheeled self balancing robot［J］.Computer Application Research，2009，26（1）：99-101.

［10］华瑾，高嵩，秦刚，等.两轮自平衡机器人动力学建模与运动控制［J］.西安工业大学学报，2014（10）：851-855.

HUA Jin，GAO Song，QIN Gang，et al.The two wheeled self balancing robot dynamics modeling and motion con⁃trol［J］.Journal of Xi’an Technological University，2014（10）：851-855.

［11］张洪伟.基于STM32的平衡车设计［J］.科技创新与应用，2016（2）：78-79.

ZHANGHongwei.Design of balance car based on STM32［J］.Science and Technology Innovation and Applica⁃tion，2016（2）：78-79.

［12］王学斌，徐建宏，张章.卡尔曼滤波器参数分析与应用方法研究［J］.计算机应用与软件，2012，29（6）：212-215.

WANG Xubin，WU Jianhong，ZHANG Zhang.On Analy⁃sis and Application Approach for kalman Filtar Parame⁃ters［J］.Computer Application and Software，2012，29（6）：212-215.

Design of Two W heeled Self Balancing Car Based on MCU

WEI Ya
（Shanxi Polytechnic Institute，Xianyang 712000）

TP3-02

10.3969/j.issn.1672-9722.2017.09.038

2017年3月9日，

2017年4月22日

国家自然科学基金项目“基于空间打压矢量的异步电动机软启动控制理论与方法研究”（编号：51577110）资助。

魏雅，女，硕士，副教授，研究方向：电子信息和计算机。