design and implementation of twowheeled selfbalancing vehicle.docx

1、design and implementation of twowheeled selfbalancing vehicle摘要近年来，两轮自平衡机器人的研究取得了快速的发展，两轮自平衡小车的动力学系统是一种多变量、非线性、强耦合的系统，是检验各种控制方法的典型装置。同时由于它具有体积小、运动灵活、零转弯半径等特点，必将会在军用和民用领域有着广泛的应用前景。本文主要介绍了基于Freescale MC9S12XS128单片机为控制核心的两轮自平衡小车系统，以验证经典的PID控制在动态平衡系统上的控制效果。在该系统上，姿态传感器采用MPU6050，单片机在采集到姿态数据后，采用Kalman滤波器对得

2、到的数据进行融合，并在此基础上分析不同滤波方法的效果。借助增量式PID控制PWM的输出和利用TB6612FNG控制电机的转向以及转速，从而实现了小车的自平衡控制。关键词：两轮自平衡系统； Kalman滤波； 数据融合； HCS12； MPU6050 . Design and implementation of two-wheeled self-balancing vehicleAbstractIn recently years, the research of two-wheeled self-balancing robot has made a rapid development, the

3、dynamics system of two-wheeled self-balancing vehicle is a multivariable, nonlinear, strong coupling system, and also ，its a typical devices to test a variety of control methods. Because of it has a small, flexible movement and zero turning radius. It will have a wide range of applications in milita

4、ry and civilian fields.In the article, it describes the Freescale MC9S12XS128 microcontroller-based control of two-wheeled self-balancing vehicle system to verify the classic PID control system in the dynamic balance . On this system, It used MPU6050 as the car state sensing system, and it used the

5、Kalman filter to fuse the obtained angle data, and analyzed the effect of different filtering methods based on this. With incremental PID control PWM output and use TB6612FNG steering and speed control motors, enabling the cars self-balance control finally.Keywords: two-wheeled self-balancing system

6、; the Kalman filter;HCS12;MPU6050目录摘要 2第1章 绪论 61.1 两轮自平衡机器人概述 61.2 两轮自平衡机器人的发展 61.3 方案论证及选择 71.4 关键技术及目标 81.4.1 姿态数据处理 81.4.2 控制算法的实现 91.4.3 目标 9第2章 两轮自平衡小车的原理 102.1 小车的直立控制 102.2 倾角与角速度的测量 132.3 本章小结 14第3章 电路设计 153.1 整体电路框图 153.2 电源电路 163.3 单片机最小系统 173.3.1 S12单片机简介 173.3.2 MC9S12XS128最小系统电路 173.4 M

7、PU6050 183.4.1 MPU6050简介 183.4.2 I2C通信 193.5 电机驱动电路 193.5.1 驱动芯片介绍 193.5.2 驱动电路设计 203.6 速度传感器电路 213.6.1 光电编码器介绍 213.7 无线遥控电路 213.7.1 Pt2262简介 213.7.2 Pt2262应用 223.8 液晶显示电路 233.8.1 LCD1602简介 233.8.2 LCD1602电路 233.9 车模控制电路全图 243.10 本章小结 26第4章 系统软件设计 274.1 控制算法介绍 274.2 S12单片机初始化 284.2.1 锁相环初始化 284.2.2

8、PWM模块初始化 284.2.3 串行通信初始化 284.2.4 外部中断初始化 294.3 PID控制的实现 294.4 姿态数据处理 294.4.1 角度计算函数 294.4.2 滤波方法分析与选择 294.5 小车的运动控制 334.6 无线遥控 334.7 本章小结 33第5章 系统调试 355.1 软件调试工具 355.2 系统调试工具 355.3 系统硬件电路调试 355.4 姿态检测模块调试 355.5 Kalman滤波器参数的整定 375.6 PID参数的整定 375.7 本章小结 38第6章 总结 396.1 总结与展望 39参考文献 41附录 43附录一 系统主控板 43附

9、录二 系统核心源码 44致谢 47第1章 绪论两轮自平衡系统最早可追溯到上世纪80年代，日本电气通信大学的山藤一雄教授提出的基于倒立摆原理的自动站立机器人的模型被认为是两轮自平衡小车的构思起源。本章主要介绍两轮自平衡小车的起源与发展、研究意义以及国内外的研究现状，并依此提出了本论文研究的主要内容。1.1 两轮自平衡机器人概述两轮自平衡小车是一种只有两个车轮且能在没有人为因素干预的条件下，能够自动保持的平衡的这样一个装置。自平衡小车系统的运作原理主要是飞机平衡的原理，也就是车辆本身的自动平衡能力1。自平衡小车系统通过陀螺仪和加速度计模块来检测小车的加速度以及角速度，MCU采集相关数据并进行运算，

10、产生与当前系统姿态所适应的信号来驱动电机运动以达到平衡的效果。例如当系统向前倾时，经过姿态检测传感器检测出当前系统的姿态，MCU提取有关数据以后进行计算，驱动电机以一个合适的加速度向前运动，以此来使系统处于平衡状态。相反的，如果系统向后倾斜，那么小车将会以某个速度向后运动，保证小车处于直立状态。虽然两轮自平衡系统提出的时间并不是很长，可是由于对两轮自平衡系统的研究有很重要的意义而得到了快速的发展。两轮自平衡小车的数学模型类似一阶倒立摆，它是一个典型多变量、存在严重非线性、非自治不稳定系统。这个系统是检验各种控制方法的理想平台。例如我们可以用经典的PID控制对其进行控制2，也可使用模糊控制理论对

11、系统进行控制。另外两轮自平衡系统具有体积小、硬件易实现、运动灵活、零转弯半径等特点，这将会使其在未来得到广泛的应用。1.2 两轮自平衡机器人的发展本世纪初，由于控制理论趋于完善，而且电子技术得到飞跃式的发展，实现两轮自平衡系统成为可能，于是该项技术迅速引起了研究机构和机器人爱好者极大地关注。出于不同的应用与目的，不同设计方案和控制策略的自平衡系统相继诞生。在两轮自平衡系统的研究领域，国外的研究起步较国内早，国外已经有公司做出了非常成熟的产品，而且其技术己经达到了一个很高的水平，国内的研究相对较少。下面分别介绍国内外一些主要的两轮自平衡系统的发展：1. SEGWAY HT2001年，美国的科学家

12、Kamen发明了一种新型的两轮交通工具“Segway”3 。这种交通工具节能环保、方便快捷以及更为实用，而且由于其体积较一般的交通工具要小很多，所以在解决交通拥堵方面具有得天独厚的优势。图1.2.1为Segway公司生产的一种两轮自平衡电动车。Segway的运作基本原理主要是建立在“动态稳定”的基本原理之上的，它通过高精度的陀螺仪和其他角度传感器来检测车身的姿态，并通过高速的中央处理器发出指令，驱动电机以保持车身出于直立的平衡状态。图 1.2.1 Segway2. Nbot由美国科学家David P.Anderson研发的两轮自平衡机器人Nbot也是非常成功的应用例子。Nbot通过使用惯性传感

13、器和电机编码器来实现系统的平衡。3. Unibot2007年，Unibot诞生于加州大学圣地亚哥分校，Unibot是两轮自平衡系统的最新研究成果，采用了独轮自平衡机器人控制，结合了倒立摆和惯性系统的驱动方式。在Unibot这个系统中，采用了陀螺仪和加速度计作为姿态检测传感器，在车轮方向上利用电机驱动控制，在垂直于车轮方向上，利用不平衡体旋转来提供补偿力矩。虽然目前只实现了倾角小范围控制，但具有很重要的指导意义4。国内在这方面的研究主要成果有：台湾国立中央大学在2004年利用模糊控制实现了两轮自平衡小车的控制5。在2009年，北京工业大学电子工程与智能信息研究所研制出了“原人3号”机器人6，它是

14、一种基于红外姿态检测的两轮自平衡机器人。通过设置机器人上部的红外传感器检测其与地面的距离，得出姿态信号，经过相应的处理之后，产生驱动信号。这种方法简单易行且有优良的控制效果。1.3 方案论证及选择由于两轮自平衡系统完美的综合了控制技术与电子技术知识，对加深所学的理论知识有很大的帮助。实现两轮自平衡系统的主要方式就是通过单片机采集姿态传感器的数据来控制电机的运动。姿态传感器选用数字输出的六轴姿态传感器MPU6050。方案上的选择主要就是对MCU、电机驱动单元及控制算法的选择。首先，常见的单片机有8位的51单片机、8位的ATmega16单片机以及16位的Freescale单片机。那么两轮自平衡小车

15、系统有三种方案可供选择。方案一，选用51单片机作为主控芯片。51单片机时目前发展最成熟的单片机，且型号众多、价格相对较低。可是由于其片内资源较少，而两轮自平衡小车系统需要A/D转换模块、PWM模块。如果选用51单片机的话，会增加外围电路的设计难度，而且由于51单片机的运行速度相对较慢，这将导致系统的整体性能不高。方案二，选用ATmega16单片机作为两轮自平衡系统的主控芯片。虽然ATmega16单片机片内集成了A/D等资源，而且运算速度相比51单片机会快很多。但是由于ATmega16单片机只有32路I/O端口，若使用该单片机作为主控芯片，会导致I/O资源紧张。所以也不建议采用ATmega16作

16、为主控芯片。方案三，使用Freescale的16位单片机S12XS128作为主控芯片。Freescale单片机运用在汽车控制方面有独特的优势，片内资源丰富、运算速度快、而且有非常丰富的I/O端口资源，便于两轮自平衡小车系统的扩展。因此，以Freescale的16位S12单片机作为两轮自平衡系统的主控芯片是极其合适的7。第二，在两轮自平衡小车系统中，单机驱动单元影响着整个系统的性能。如果电机驱动单元效率高而且功率较大，那么整个系统的性能就得到了保障。常见的电机驱动芯片有L298N及东芝半导体公司的TB6612FNG等。本小车系统选用TB6612FNG芯片驱动电机8。TB6612FNG与L298N

17、相比，除了具有L298N所具有的可以同时驱动两个直流电机之外，还有以下三个优势：TB6612FNG所能提供的驱动电流比L298N更大。TB6612FNG的效率比L298N更高。TB6612FNG有很小的发热量。因此选择TB6612FNG作为电机驱动芯片是不错的选择。第三，两轮自平衡车属于本质不稳定系统，因此其实现的平衡是一种动态平衡。在遇到外界干扰如何快速恢复，保持自平衡等问题是控制算法需要考虑的问题。传统的PID控制完全可以满足两轮自平衡车的控制系统要求。因此核心控制算法采用PID控制即可。综上所述，整个系统的实施方案就是以MPU6050来获得姿态参数，然后送给Freescale S12单片

18、机进行相关计算处理9，产生PWM信号。通过电机驱动芯片TB6612FNG驱动电机完成相应的控制要求，最终使小车能够直立。1.4 关键技术及目标1.4.1 姿态数据处理两轮自平衡小车系统中，姿态数据的处理包括角度、角速度等数据的采集，数据的转换与计算以及相应的滤波处理。姿态数据的处理之于两轮自平衡小车系统犹如地基之于建筑。如果不能获得正确的姿态数据，那么系统将不能达到期望的控制要求。而且在两轮自平衡小车系统中，对数据的准确性、实时性要求非常高。所以在两轮自平衡小车系统中，能够获取正确的、具有控制意义的姿态数据也就成功了一半。在得到姿态数据之后，由于得到的数据中包含了一些噪声，所以并不能直接用来控

19、制小车。需要进行相应的滤波处理。常用的软件滤波手段主要有平均值滤波、互补滤波以及Kalman滤波。其中，Kalman滤波效果最好，可是原理较难，单片机运算时间较长。互补滤波实现比较容易，但是效果不及Kalman滤波10。关于滤波的方法在后面的章节中将详细介绍。1.4.2 控制算法的实现PID控制算法的实现又可以分为增量式PID和位置式PID。其中，增量式PID和位置式PID相比，具有运算量小的特点，非常适合在单片机中实现。PID控制算法也可以做的很深很广，但是在两轮自平衡小车系统中，只需要简单入门算法即可承担控制任务。PID算法的实现根据增量式PID的表达式书写即可11。1.4.3 目标本文主

20、要介绍制作两轮自平衡小车系统的实现原理及方法，目标是：（1） 完成两轮自平衡小车的硬件设计与程序设计。（2） 能够通过无线遥控器对小车的运动方式进行控制，并将其运动状态显示出来。（3） 软件滤波的实现及结果分析，并分析Kalman滤波与互补滤波的优劣势。（4） PID控制算法的实现。（5） 检测PID的控制效果，并在理论上与其他控制方法进行对比分析。第2章 两轮自平衡小车的原理本章主要介绍两轮自平衡小车系统的原理，结合相应的数学公式说明及简单的推导过程说明自平衡小车实现直立的原理及可操作性。2.1 小车的直立控制在杂技表演里经常可以看到这样的表演，一个杂技演员可以用头或者身体的其他某个部位支起

21、一个物体，并能使物体保持垂直于地面的状态。这也是两轮自平衡小车的最直观的体验，所以控制车模直立的直观经验来自于人们的生活经验。一个人通过练习可以让一支笔在手指尖上保持直立，不过这需要两个条件：其一，手指可移动。其二，人眼可看到笔的运动趋势。其实，人眼、大脑和手构成了一个负反馈系统。同理，两轮自平衡小车系统的直立控制也是通过负反馈来实现的。由于小车的两个轮子有两个共轴电机连接，所以小车只能在轮子滚动的方向上发生倾斜，那么这个时候只要控制车轮向与倾斜方向相同的方向运动，就可以抵消小车的倾斜趋势，也就可以使小车保持平衡了13，如图2.1.1所示。图 2.1.1 小车保持平衡示意图 。 当车体发生倾斜

22、时，为了确定车轮的运动方式，下面对小车的模型进行分析。两轮自平衡小车系统可以看作是一个底部可移动的单摆模型14，通过对比单摆系统的特性进行对比分析。我们都知道单摆能够稳定在竖直位置，其条件有两个： （1） 系统受到的回复力与位移相反； （2） 系统受到的阻尼力与速度的方向相反。 正是因为有了则两个力的存在，所以单摆才能稳定在平衡位置。同理，如果小车要像单摆那样能够保持平衡，那么小车也要满足上面的两个条件。下面分析一下小车处于直立状态时的受力情况。如下图所示：图 2.1.3 小车受力分析图 2.1.2 小车受力分析通过分析可以发现，两轮自平衡小车之所以不能稳定在竖直方向上，是因为当小车偏离平衡位

23、置时，小车受到的回复力与位移方向相同，而不是相反。因此，当小车偏离平衡位置时，小车会加速偏离平衡位置直到倒下。如果要通过控制是小车能够像单摆一样稳定在平衡位置，有两个办法可以达到目标：（1） 改变重力的方向。（2） 施加另外一个力，使得小车的回复力与位移相反。显然，只有第二种办法可行。当小车偏离平衡位置时，控制小车向其倾斜的方向做加速运动，这样小车就会受到一个额外的力，这个力与车轮的加速度方向相反，大小成正比。此时，小车所受到的回复力变为： (2-1)如果小车的加速度a与偏角成正比，比例为k1。当k1g时，那么回复力的方向便和位移的方向相反，小车就可保持平衡。此外，为了使能小车能够尽快的在垂直

24、位置稳定，根据单摆的运动特性，还需要增加一个阻尼力。根据单摆的特性，增加的阻尼力大小应与偏角的速度成正比，方向相反。式2-1可以转换为： (2-2)按照上面的控制方法，两轮小车能够稳定在竖直方向上。因此，可得到控制车轮加速度的控制算法： (2-3)式中，为车模倾角；为角速度；k1、k2均为比例系数；k1、k2两个参数的作用如下图所示。两项相加后作为车轮加速度的控制量。图 2.1.3 小车平衡控制的两个系数为了进一步确定比例系数k1、k2与小车稳定的关系，下面将通过对小车模型建模来确定其应满足的条件。在进行建模之前，设定以下假设条件：假设小车的高度为L，质量为m，外力干扰引起车模产生的角加速度为

25、x(t)。沿着垂直于小车地盘方向进行受力分析，可以得到小车倾角与车轮运动加速度a(t)以及外力干扰加速度x(t)之间的运动方程。图 2.1.4 小车运动方程 通过分析可以发现，当车模静止时，系统的输入输出传递函数为： (2-4) 此时系统具有两个极点。显然，一个极点位于s平面的右半平面。根据控制理论可知，此时的系统是不稳定。引入上面得到的加速度负反馈控制后，得到新的系统如下图所示：图 2.1.5 小车直立控制框图此时系统的传递函数为： (2-5) 系统的两个极点为：。系统稳定的条件是其两个极点都位于s平面的左半平面。因此，当时，小车能够在竖直方向上稳定。 至此，可以得到要实现小车直立控制的条件

26、有两个： （1） 能够准确的测量小车的倾角及角速度的大小。 （2） 可以控制小车的加速度。 关于小车倾角及角速度的测量会在后面的章节中介绍。对小车加速度的控制实际上也是对电机的的运行状态进行控制。而电机的运行状态是与施加在电机上的电压相关的。通过对电机模型的简化，可以得到电机转速与电压之间的关系，即电机模型可看成一个一阶惯性环节。 (2-6) 式中，E为电压；u(t)为单位阶跃函数；T1为惯性环节的时间常数；km为电机转速常数。对应不同的电压时，电机的转速变化曲线如图所示：E=5VE=4V E=3VE=2V E=1V图 2.1.6 电机在不同电压下的速度曲线 通过图中我们可以看到，电机的运行过

27、程明显的分为两个阶段：加速阶段和恒速阶段。通过控制电机运行过程中的加速阶段的加速度就可以控制小车保持平衡。而此时施加的电压大小等于上面计算的得到的加速度乘以一个比例系数的值。施加这样一个电压给电机，那么小车就可以保持平衡了15。2.2 倾角与角速度的测量倾角与角速度的测量是利用姿态传感器MPU6050得到的，将MPU6050安装好之后，当其敏感轴产生可测量的旋转时，相应轴的输出电压发生变化，经过MPU6050内部的16位AD转换器转换为数字量后，通过I2C总线输出，单片机直接采集相应的信号即可。在这里，需要采集一轴的加速度与角速度。将采集到的数据乘以相应的量纲过后，就得到了小车的倾角。2.3

28、本章小结本章主要从原理上对两轮自平衡小车的直立控制方法进行了分析。其中最重要的就是能够准确的、快速的获得小车当前的角度与角速度。然后通过PID控制控制方法控制单片机PWM的输出来驱动电机运动，从而使小车能够自平衡。第3章 电路设计在了解了两轮自平衡小车系统的原理之后，接下来的关键问题就是硬件的设计。合理的电路设计能够使小车系统更加的稳定可靠，也会使调试过程变的相对容易。本章将介绍两轮自平衡小车的电源电路、单片机最小系统、测速电路、MPU6050模块以及无线遥控电路。3.1 整体电路框图在设计两轮自平衡小车系统之前，要先明确系统的输入量与输出量，并分配好I/O端口。这样才能有目的的设计硬件电路。

29、在两轮自平衡系统中，系统的输入输出有：（1） 外部中断接口（3路）a) 测速：左右两路，光电编码器B相脉冲输入。b) 无线遥控有效信号确认：一路，无线遥控中断信号。（1） （2） PWM接口（2路）a) 控制左右两个电机双向运行。（3） 通讯接口（9路）a) SCI（UART）：三路，用于串口通信，调试使用。b) BDM：六路，用于下载程序。（4） 通用I/O接口a) I2C：两路，模拟I2C通信接口，用于与MPU6050通信。b) 无线遥控接收数据：四路，用于接收无线遥控解码数据。c) LCD1602接口：十一路，其中8路用于1602数据线，三路用于1602控制线号线。d) 电机控制信号：五

30、路，一路用于电机运行状态信号STBY。另外四路用于电机转动方向控制。e) 方向判别：两路，用于连接光电编码器B相脉冲，进行方向判断。可以看出，两轮自平衡小车系统用到的I/O资源是比较多的。MC9S12XS128具有丰富的I/O资源，能够满足系统要求，而且剩下的I/O资源还方便以后的拓展，比如加上超声波避障等功能。小车控制电路整体框图如下：图 3.1.1 小车整体电路图根据小车整体电路框图可知，控制电路分为以下几个模块：（1） 电源电路：电源电压降压稳压与滤波。（2） 单片机最小系统：包括时钟电路、复位电路、BDM下载接口等。（3） 电机驱动：驱动两个电机的功率电路。（4） 速度检测：光电编码器

31、检测电机的转速。（5） 姿态数据采集电路：（6） 无线遥控：Pt2262/Pt2272发码、解码电路。下面分别介绍各模块电路的详细设计方法。3.2 电源电路如果说单片机是两轮自平衡小车系统的大脑的话，那么电源无异于小车的心脏。电源系统设计的质量关系到整个系统的运行情况。只有一个稳定的电源，整个系统才能正常的工作。尤其是给单片机供电的电源，对电源质量的要求更高，如果电源波动很大，单片机就会出现复位的情况，由于电池的供电能力有限，所以当电机反向制动时，耗电非常大，这时电池电压就会被拉得很低，这时就要对单片机的供电进行保护。一种简单实用的方式就是用大电容给单片机储存电能，在电池电压拉低的短时间内给单片机提供电源，防止由于电压过低导致单片机复位。在两轮自平衡小车系统中，需要12V、5V两种电压大小的电源。其中12V电压为电机的驱动电压，5V的电压是单片机以及姿态传感器、光栅编码器以及无线遥控模块的工作电压。整个系统的电源由一个大容量的12V锂电池提供，然后经过稳压芯片降压后得到5V的电压。用于产生5V电压的芯片选用常用的L7805三端稳压电压。使用L7805具