1、双轮自平衡小车机器人专业系统设计与制作双轮自平衡小车机器人专业系统设计与制作燕山大学课 程 设 计 说 明 书题目：双轮自平衡小车机器人系统设计与制作学院（系）：机械工程学院年级专业：12级机械电子工程组 号：3学生姓名： 指导教师：史艳国 姚建涛 李艳文 史小华 张庆玲唐艳华 李富娟 刘晓飞 刘正操 胡浩波日 期： 2015.11 燕山大学课程设计（论文）任务书院（系）：机械工程学院 基层教学单位：机械电子工程系组号学生姓名设计题目双轮自平衡小车机器人系统设计与制作设计要求在课程研究项目所搭建的伺服控制系统的基础上，自主设计加工双轮车的机械系统，并完成智能双轮自平衡车系统装配与调试。技术参数
2、所搭建的双轮车系统需要具备以下基本功能：a. 具备一定的自平衡能力，自动检测自身机械系统的倾角并完成姿态的调整；b. 具备一定的负载承载能力，在加载一定重量的重物时能够快速做出调整并保证自身系统的自我平衡；c. 具备速度调节能力，能够以不同的运动速度实现双轮车系统的前进、后退、左转与右转等动作；d. 具备无线通讯功能，能够实现双轮自平衡车系统的无线远程操作控制。工作量（1）资料分析：查阅相关文献资料，对资料进行分析总结。（2）机器人总体设计：确定机器人的具体任务要求，根据任务初步拟定机器人的技术参数、运动形式、机械结构、驱动方案、传动方案、控制方案等。（3）机器人机械结构设计：将机器人分解为车
3、身结构、机械臂和手爪等若干部分，分别对各个结构的关键部件进行详细设计并校核，绘制机器人总装图和关键零部件图。（4）传感和信息检测及信息传输：根据任务要求，完成相关信息检测、处理，并完成信息的正确传输。（4）运动控制方案设计；基于传感信息，采用单片机完成机器人控制系统硬件和软件的设计和系统调试。（5）编制课程设计说明书工作计划（1）资料查阅、分析总结，所需天数1天 （2）总体方案设计，所需天数2天（3）机械结构设计，所需天数2天 （4）传感、信息传输和运动控制系统的设计，所需天数2天（5）关键零部件的设计制作，所需天数2天 （6）控制程序编码与调试，所需天数3天（7）绘制总装图和关键零件图，所需
4、天数2天（8）系统整体装配与调试 所需天数3天（9）编制课程设计说明书 所需天数2天（10）答辩考核、演示，所需天数1天 参考资料通过校园网在我校订阅的电子资料库中可以搜索到大量的有关机电一体化以及电机和电力拖动的参考资料。同学们也可到学校的图书馆查找纸质期刊资料。机械学院的计算机中心上班时间免费对本院学生开放。指导教师签字基层教学单位主任签字摘要两轮自平衡小车是一种非线性、多变量、强耦合、参数不确定的复杂系统，他体积小、结构简单、运动灵活，适合在狭小空间内工作，是检验各种控制方法的一个理想装置，受到广大研究人员的重视，成为具有挑战性的课题之一。两轮自平衡小车系统是一种两轮左右并行布置的系统。
5、像传统的倒立一样，其工作原理是依靠倾角传感器所检测的位姿和状态变化率结合控制算法来维持自身平衡。本设计通过对倒立摆进行动力学建模，类比得到小车平衡的条件。从加速度计和陀螺仪传感器得出的角度。运用卡尔曼滤波优化，补偿陀螺仪的漂移误差和加速度计的动态误差，得到更优的倾角近似值。通过光电编码器分别得到车子的线速度和转向角速度，对速度进行PI控制。根据PID控制调节参数，实现两轮直立行走。通过调节左右两轮的差速实现小车的转向。制作完成后，小车实现了在无线蓝牙通讯下前进、后退、和左右转向的基本动作。此外小车能在正常条件下达到自主平衡状态。并且在适量干扰下，小车能够自主调整并迅速恢复稳定状态。关键词： 自
6、平衡 陀螺仪 控制调试前言移动机器人是机器人学的一个重要分支，对于移动机器人的研究，包括轮式、腿式、履带式以及水下式机器人等，可以追溯到20世纪60年代。移动机器人得到快速发展有两方面原因：一是其应用范围越来越广泛；二是相关领域如计算、传感、控制及执行等技术的快速发展。移动机器人尚有不少技术问题有待解决，因此近几年对移动机器人的研究相当活跃。 近年来，随着移动机器人研究不断深入、应用领域更加广泛，所面临的环境和任务也越来越复杂。机器人经常会遇到一些比较狭窄，而且有很多大转角的工作场合，如何在这样比较复杂的环境中灵活快捷的执行任务，成为人们颇为关心的一个问题。双轮自平衡机器人概念就是在这样的背景
7、下提出来的。两轮自平衡小车是一个高度不稳定两轮机器人，是一种多变量、非线性、强耦合的系统，是检验各种控制方法的典型装置。同时由于它具有体积小、运动灵活、零转弯半径等特点，将会在军用和民用领域有着广泛的应用前景。因为它既有理论研究意义又有实用价值，所以两轮自平衡小车的研究在最近十年引起了大量机器人技术实验室的广泛关注。本论文主要叙述了基于stm32控制的两轮自平衡小车的设计与实现的整个过程。主要内容为两轮自平衡小车的平衡原理，直立控制，速度控制，转向控制及系统定位算法的设计。通过此设计使小车具备一定的自平衡能力、负载承载能力、速度调节能力和无线通讯功能。小车能够自动检测自身机械系统的倾角并完成姿
8、态的调整，并在加载一定重量的重物时能够快速做出调整并保证自身系统的自我平衡。能够以不同运动速度实现双轮车系统的前进、后退、左转与右转等动作，同时也能够实现双轮自平衡车系统的无线远程控制操作第1章 项目任务1.1 项目概述移动机器人是机器人学的一个重要分支，对于移动机器人的研究，包括轮式、腿式、履带式以及水下式机器人等，可以追溯到20世纪60年代。移动机器人得到快速发展有两方面原因：一是其应用范围越来越广泛；二是相关领域如计算、传感、控制及执行等技术的快速发展。移动机器人尚有不少技术问题有待解决，因此近几年对移动机器人的研究相当活跃。近年来，随着移动机器人研究不断深入、应用领域更加广泛，所面临的
9、环境和任务也越来越复杂。机器人经常会遇到一些比较狭窄，而且有很多大转角的工作场合，如何在这样比较复杂的环境中灵活快捷的执行任务，成为人们颇为关心的一个问题。双轮自平衡机器人概念就是在这样的背景下提出来的。两轮自平衡小车是一个高度不稳定两轮机器人，是一种多变量、非线性、强耦合的系统，是检验各种控制方法的典型装置。同时由于它具有体积小、运动灵活、零转弯半径等特点，将会在军用和民用领域有着广泛的应用前景。因为它既有理论研究意义又有实用价值，所以两轮自平衡小车的研究在最近十年引起了大量机器人技术实验室的广泛关注。本文是对本质不稳定两轮小车自平衡控制问题的研究。并通过无线蓝牙远程控制小车前进、后退、和左
10、右转向等基本动作。1.2 项目内容（1）设计两轮自平衡小车驱动电路。选择合适的电机、传感器和微控制单元并合理设计相应的外围电路，最终完成两轮自平衡小车系统的硬件设计。（2）完成驱动板的调试。通过对驱动触发脉冲、D/A输出以及电机空载进行实验，从功能上验证了其驱动板基本能够满足系统的要求。（3）处理传感器数据。选用惯性导航器件陀螺仪和加速度计，详细分析两者的工作原理和各自单独使用所存在的问题。（4）研究自平衡小车的控制策略。最终实现平衡车的左转、右转、前进、后退等功能。1.3 预期结果（1）具备一定的自平衡能力，自动检测自身机械系统的倾角并完成姿态的调整；（2）具备一定的负载承载能力，在加载一定
11、重量的重物时能够快速做出调整并保证自身系统的自我平衡；（3）具备速度调节能力，能够以不同运动速度实现双轮车系统的前进、后退、左转与右转等动作；（4）具备无线通讯功能，能够实现双轮自平衡车系统的无线远程控制操作。第2章 方案设计2.1 主控芯片的选择STM32单片机作为主控芯片。此芯片是以ARM的Cortex-M系列为内核的单片机，相对其他单片机，外设丰富，主频高，价格便宜，有专门的软件库，操作简单，调试方便，低功耗。强型系列时钟频率达到72MHz，是同类产品中性能最高的产品；基本型时钟频率为36MHz，以16位产品的价格得到比16位产品大幅提升的性能，是16位产品用户的最佳选择。2.2 电机驱
12、动的选择TB6612FNG是东芝半导体公司生产的一款直流电机驱动器件，它具有大电流MOSFET-H桥结构，双通道电路输出，可同时驱动2个电机。 TB6612FNG每通道输出最高1.2 A的连续驱动电流，启动峰值电流达2A/3.2 A(连续脉冲/单脉冲);4种电机控制模式：正转/反转/制动/停止;PWM支持频率高达100 kHz;待机状态;片内低压检测电路与热停机保护电路;工作温度：-2085;SSOP24小型贴片封装。TB6612FNG的主要引脚功能（如图2.2.1）：AINl/AIN2、BIN1/BIN2PWMA/PWMB为控制信号输入端;AO1/A02、B01/B02为2路电机控制输出端;
13、STBY为正常工作/待机状态控制引脚;VM(4.515V)和VCC(2.75.5V)分别为电机驱动电压输入和逻辑电平输入端。图2.1 TB6612FNG引脚TB6612FNG是基于MOSFET的H桥集成电路，其效率高于晶体管H桥驱动器。相比L293D每通道平均600mA的驱动电流和1.2A的脉冲峰值电流，它的输出负载能力提高了一倍。相比L298N的热耗性和外围二极管续流电路，它无需外加散热片，外围电路简单，只需外接电源滤波电容就可以直接驱动电机，利于减小系统尺寸。对于PWM信号，它支持高达100kHz的频率，相对以上2款芯片的5kHz和40kHz也具有较大优势。2.3 陀螺仪的选择MPU-60
14、X0是全球首例9轴运动处理传感器。它集成了3轴MEMS陀螺仪，3轴MEMS加速度计，以及一个可扩展的数字运动处理器DMP（DigitalMotionProcessor），可用I2C接口连接一个第三方的数字传感器，比如磁力计。扩展之后就可以通过其I2C或SPI接口输出一个9轴的信号（SPI接口仅在MPU-6000可用）。MPU-60X0也可以通过其I2C接口连接非惯性的数字传感器，比如压力传感器。 MPU-60X0对陀螺仪和加速度计分别用了三个16位的ADC，将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的，陀螺仪可测范围为250，500，100
15、0，2000/秒（dps），加速度计可测范围为2，4，8，16g。一个片上1024字节的FIFO，有助于降低系统功耗。和所有设备寄存器之间的通信采用400kHz的I2C接口或1MHz的SPI接口（SPI仅MPU-6000可用）。对于需要高速传输的应用，对寄存器的读取和中断可用20MHz的SPI。另外，片上还内嵌了一个温度传感器和在工作环境下仅有1%变动的振荡器。芯片尺寸440.9mm，采用QFN封装（无引线方形封装），可承受最大10000g的冲击，并有可编程的低通滤波器。 关于电源，MPU-60X0可支持VDD范围2.5V5%，3.0V5%，或3.3V5%。另外MPU-6050还有一个VLOG
16、IC引脚，用来为I2C输出提供逻辑电平。VLOGIC电压可取1.85%或者VDD。第3章 平衡车控制原理3.1 控制系统要求分析根据系统要求，小车必须要能够在无外界干预下依靠一对平行的车轮保持平衡，并完成前进，后退，左右转弯等动作。分析系统要求可知，保持小车直立和运动的动力都来自于小车的两只车轮，车轮由两只直流电机驱动。因此，从控制角度来看，可以将小车作为一个控制对象，控制输入量是两个车轮的转动速度。整个控制系统可以分为三个子系统： （1） 小车平衡控制以小车倾角为输入量，通过控制两个电机的正反转保持小车平衡。 （2） 小车速度控制：在保持平衡的基础上，通过调节小车倾角实现对速度的控制，实际上
17、还是演变为对电机的控制实现小车的速度控制。 （3）小车方向控制：通过控制两个电机间的转速不同实现转向。 小车直立和方向控制任务都是直接通过控制车模两个后轮驱动电机完成的，而速度控制则是通过调节小车倾角完成的。小车不同的倾角会引起车模的加减速，从而达到对小车速度的控制。三个子系统各自独立进行控制。由于最终都是对同一个控制对象（小车的电机）进行控制，所以各个子系统之间存在着耦合。为了方便分析，在分析其中之一时，假设其它控制对象都已经达到稳定。比如在速度控制时，需要小车已经能够保持直立控制；在方向控制时，需要小车能够保持平衡和速度恒定；同样，在小车平衡控制时，也需要速度和方向控制已经达到平稳。这三个
18、任务中保持小车平衡是关键。由于小车同时受到三种控制的影响，从小车平衡控制的角度来看，其它两个控制就成为干扰。因此对小车速度、方向的控制应该尽量保持平滑，以减少对平衡控制的干扰。以速度调节为例，需要通过改变车模平衡控制中小车倾角设定值，从而改变车模实际倾斜角度，达到速度控制的要求。为了避免影响车模平衡控制，这个车模倾角的改变需要非常缓慢的进行。其中平衡控制是系统的最基本要求，也是整个控制系统的难点。3.2 平衡控制原理控制小车平衡的直观经验来自人类日常生活经验。如人类身体拥有丰富的感知器官，通过大脑调节便可以控制腰部及腿部肌肉保持人体的直立。而一般人通过简单训练就可以让一根直木棍在手指尖保持直立
19、不倒。这需要两个条件：一个是托着木棍的手指可以自由移动；另一个是人的眼睛可以观察木棍的倾斜角度与倾斜趋势(角速度)。这两个条件缺一不可，实际上这就是控制系统中的负反馈机制。自平衡车的控制也是通过负反馈来实现的，与在指尖保持木棍直立比较则相对简单。由于小车只依靠两个车轮着地，车轮与地面会发生相对滚动使得小车倾斜。而小车上装载的姿态检测系统能够对小车的倾斜状况进行实时检测，通过控制器控制车轮转动，抵消在这个维度上的倾斜力矩便可以保持小车平衡，如图3.1所示。图3.1 小车平衡原理3.3 平衡车数学模型为了准确控制车轮转动，保持小车始终稳定的直立平衡，需要对自平衡车进行运动学分析并建立其数学模型，从
20、而更好的设计控制系统。为了更加直观的分析系统受力情况，下面将直立小车与单摆模型进行对比说明小车的受力情况。重力场中使用细线悬挂的重物经抽象化便形成理想化的单摆模型，两轮自平衡车可以看作一级倒立摆模型进行分析，如图3.2所示。图3.2 一级倒立摆模型示意图对普通单摆进行受力分析如图3.3所示。图3.3 单摆受力分析当物体离开平衡位置后便会受到重力与线的合作用力，驱使物体回复至平衡位置。这个力称为回复力，其大小为：F=mgsin (式3-1)在偏移角很小情况下，回复力与偏移角之间的大小成正比，方向相反。在此回复力的作用下，单摆进行周期运动。由于空气阻力的存在，单摆最终会停止在平衡位置。空气阻尼力与
21、单摆的速度成正比，方向相反。阻尼力越大，单摆会越快停止在平衡位置。可得出，单摆保持平衡的条件有两点：（1）受到与偏移相反的回复力作用；（2）受到与运动速度相反的阻尼力作用。如果没有阻尼力，单摆会在平衡位置左右晃动而无法停止。如果阻尼力过小（欠阻尼），单摆会在平衡位置震荡。阻尼力过大（过阻尼）则导致单摆恢复平衡时间加长。因而存在一个临界阻尼系数，使单摆稳定在平衡位置所需时间最短。对静止的一级倒立摆模型进行受力分析（不考虑车轮与地面的滚动摩擦力），如图3.4所示。图3.4 静止受力分析由一级倒立摆模型静止时的受力分析可知，其回复力大小为：F=mgsin (式3-2)静止的倒立摆之所以不能像单摆一样
22、可以稳定在平衡位置，是因为在偏离平衡位置时所受回复力与其偏移方向相同，而不是相反，因此不能回复至平衡位置，而是加速偏离垂直位置直至倾倒。经分析可知，要控制倒立摆使其能够与单摆一样能够回复至平衡位置并保持稳定有两种方案。一种是改变重力方向；另一种是在系统中增加另外一种力使合回复力与偏移方向相反。显然，只能通过第二种方法实现倒立摆的平衡，即在系统中额外增加一种力使合回复力与偏移方向相反。根据牛顿第一运动定律（即惯性定律），任何一个物体在不受外力或受平衡力的作用时，总是保持静止状态或匀速直线运动状态，直到有作用在其上面的外力迫使其改变这种状态为止。牛顿运动定律只在惯性参考系中成立。在非惯性参考系中，
23、由于惯性的存在，物体会受惯性力。通过控制倒立摆底部车轮，使其做加速运动。在此条件下再次分析倒立摆受力情况，如图3.5所示。图3.5 外力干扰下受力分析倒立摆之所以不能像单摆一样可以稳定在垂直位置，就是因为在它偏离平衡位置的时候，所受到的回复力与位移方向相同，而不是相反！因此，倒立摆便会加速偏离垂直位置，直到倒下。如何通过控制使得倒立摆能够像单摆一样，稳定在垂直位置呢？要达到这一目的，只有两个办法：一个是改变重力的方向；另一个是增加额外的受力，使得恢复力与位移方向相反才行。显然能够做到的只有第二种方法。控制倒立摆底部车轮，使得它作加速运动。这样站在小车上（非惯性系，以车轮作为坐标原点）分析倒立摆
24、受力，它就会受到额外的惯性力，该力与车轮的加速度方向相反，大小成正比。这样倒立摆所受到的回复力为： (式3-3)式中，由于很小，所以进行了线性化。假设负反馈控制是车轮加速度a与偏角成正比，比例为。如果比例g（g是重力加速度）那么回复力的方向便于位移方向相反了。此外，为了使得倒立摆能够尽快地在垂直位置稳定下来，还需要增加阻尼力。虽然存在着空气和摩擦力等阻尼力，相对阻尼力比较小。因此需要另外增加控制阻尼力。增加的阻尼力与偏角的速度成正比，方向相反。因此，上式可变为： (式3-4)按照上面的控制方法，可把倒立摆模型变为单摆模型，能够稳定在垂直位置。因此，可得控制车轮加速度的控制算法： (式3-5)式
25、中，为小车倾角；为角速度； 均为比例系数；两项相加后作为车轮加速度的控制量。只要保证在g，0条件下，可以使得小车像单摆一样维持在直立状态。总结控制小车直立稳定的条件如下：（1）能够精确测量小车倾角的大小和角加速度的大小；（2）可以控制车轮的加速度。3.4 PID控制原理首先给出一个典型的PID算法框图，如下图所示：图3.6 PID算法框图在这里我们先将此图简化为仅剩P控制部分的框图：图3.7 P算法框图 比例调节的公式是：u(t) = Kp * e(t)，e(t) = r(t) c(t) (式3-6) 其中：r(t) 是设定值，就是你想让被控系统某个参数所要保持的状态值； c(t)是系统的这个
26、参数的实际状态值。 比例调节的过程就是即时成比例地反应控制系统的偏差信号e(t)，偏差一旦产生，通过 Kp * e(t) 产生控制作用以减小偏差。理想的情况是有多大的误差，通过这个调节作用之后，就能将误差消除。但实际情况并非都是那么理想，Kp小了，修正不到位，Kp大了就会出现矫枉过正的现象。 对于自平衡小车来说，要控制的这个参数就是小车的倾斜角度，就是想办法让这个小车停在你所设定的角度上，这样小车就能稳定平衡了。 这样我们就知道了，控制的作用就是想让被控系统稳定在你所设定的某个值上。拿小车的平衡来说就是让小车的倾斜角度与其物理（就是小车不控制时，能够自然稳定）平衡角度一致，始终保持在这个倾斜状
27、态。 对于有些结构比较好的小车，仅比例这一项，在没有大的扰动的情况下，的确就能稳定的控制小车平衡。但往往情况并非这么理想，所以还要用到积分项I或微分项D。那么实际会出现小车来回摆动，或者始终往某个方向倾斜，造成小车往一边跑，最后控制不住就倒地了。 先说说来回摆动的问题吧，来回摆动是不是就是小车有一定的倾斜转动角速度，角速度就是角度的微分。要不想让他摆动，就是控制这个角速度为0，这就不摆动了。那么，我们增加D项，就能达到这个目的，虽然不是绝对的解决，但还是明显的改善了很多的。 下面说下始终往一边偏的问题，如果能不往一边偏，仅来回摆动，也就是这个角度一会大、一会小，一会正、一会负，经过积分，这个角
28、度平均值几乎是0，如果往一边偏的话，这个平均值就是某个不为0的值了，这是我们加上I项就能克服这个问题了。 总结一下PID控制：对于角度环来说：P是消除角度的误差，I是消除角度累积的误差，D是消除角度变化率（角速度）的误差；同理，对于速度环来说：P是消除速度的误差，I是消除速度累积（位置）的误差，D是消除速度变化率（加速度）的误差。3.5 PWM调速原理对于电机的转速调整，我们是采用脉宽调制（PWM）办法，控制电机的时候，电源并非连续地向电机供电，而是在一个特定的频率下以方波脉冲的形式提供电能。不同占空比的方波信号能对电机起到调速作用，这是因为电机实际上是一个大电感，它有阻碍输入电流和电压突变的
29、能力，因此脉冲输入信号被平均分配到作用时间上，这样，改变在始能端PE2和PD5上输入方波的占空比就能改变加在电机两端的电压大小，从而改变了转速。此电路中用微处理机来实现脉宽调制，通常的方法有两种：（1）用软件方式来实现，即通过执行软件延时循环程序交替改变端口某个二进制位输出逻辑状态来产生脉宽调制信号，设置不同的延时时间得到不同的占空比。（2）硬件实验自动产生PWM信号，不占用CPU处理的时间。第4章 硬件设计4.1 电路原理图图4.1 电路原理图4.2 主控芯片的电路设计（stm32）ARM公司的高性能”Cortex-M3”内核1.25DMipsMHz,而ARM7TDMI只有0.95DMips
30、MHz1s的双12位ADC，4兆位秒的UART，18兆位秒的SPI，18MHz的IO翻转速度一流的外设，低功耗，最大的集成度，简单的结构和易用的工具在72MHz时消耗36mA(所有外设处于工作状态)，待机时下降到2A复位电路、低电压检测、调压器、精确的RC振荡器等图4.2 STM32引脚4.3 电机驱动的电路设计（TB6612FNG）图4.3 TB6612FNG引脚STBY口接单片机的IO口清零电机全部停止，置1则通过AIN1 AIN2，BIN1，BIN2 来控制正反转，VM接12V以内电源，VCC接5V电源，GND接地，PWMA和PWMB分别接接单片机的PWM口，A01和AO2接电机1的两个
31、脚。BO1和BO2接电机2的两个脚。表4-1 电机1真值表 AIN1 0 0 1 AIN2 0 1 0 停止正转反转表4-2 电机2真值表 BIN1 0 0 1 BIN2 0 1 0 停止正转反转4.4 陀螺仪的电路设计MPU-60X0是全球首例9轴运动处理传感器。它集成了3轴MEMS陀螺仪，3轴MEMS加速度计，以及一个可扩展的数字运动处理器DMP，可用I2C接口连接一个第三方的数字传感器。扩展之后就可以通过其I2C或SPI接口输出一个9轴的信号。MPU-60X0也可以通过其I2C接口连接非惯性的数字传感器。图4.4 Mpu6050引脚接线方法：如图4.4所示，VCC接5V电源，GND接地，SDA是数据线，接stm32单片机的PB9引脚，SCL是时钟线，接stm32单片机的PB8引
