两轮自平衡小车系统设计

两轮自平衡小车是一种非线性、多变量、强耦合、参数不确定的复杂系统，他体积小、结构简单、运动灵活，适合在狭小空间内工作。两轮自平衡小车系统是一种两轮左右并行布置的系统。像传统的倒立一样，其工作原理是依靠倾角传感器所检测的位姿和状态变化率结合控制算法来维持自身平衡。本设计通过对倒立摆进行动力学建模，类比得到小车平衡的条件。从加速度计和陀螺仪传感器得出的角度。运用卡尔曼滤波优化，补偿陀螺仪的漂移误差和加速度计的动态误差，得到更优的倾角近似值。通过光电编码器分别得到车子的线速度和转向角速度，对速度进行PI控制。根据PID控制调节参数，实现两轮直立行走。通过调节左右两轮的差速实现小车的转向。实验前期，对各个模块进行了解，例如电机、STM32、陀螺仪、蓝牙模块、避障模块等等，对这些进行学习程序并编写。接着研究了小车的自平衡、手机蓝牙遥控、超声波避障。其主要工作内容包括：小车机械本体设计、硬件设计、电气原理图、小车的直立、速度、转向平衡和小车的手机蓝牙控制试验、超声波避障实验。后期进行对小车的机械结构进行完善，并进行三维图二维图的绘制。制作完成小车样品后，小车实现了前进、后退、和左右转向的基本动作。此外小车能在正常条件下达到自主平衡状态。并且在适量干扰下，小车能够自主调整并迅速恢复稳定状态。小车能够实现手机蓝牙控制前后左右移动的功能，以及能够智能实现避障功能。关键词：自平衡 蓝牙遥控 STM32 避障 前言移动机器人是机器人学的一个重要分支，对于移动机器人的研究，包括轮式、腿式、履带式以及水下式机器人等，可以追溯到20世纪60年代。移动机器人得到快速发展有两方面原因：一是其应用范围越来越广泛；二是相关领域如计算、传感、控制及执行等技术的快速发展。移动机器人尚有不少技术问题有待解决，因此近几年对移动机器人的研究相当活跃。近年来，随着移动机器人研究不断深入、应用领域更加广泛，所面临的环境和任务也越来越复杂。机器人经常会遇到一些比较狭窄，而且有很多大转角的工作场合，如何在这样比较复杂的环境中灵活快捷的执行任务，成为人们颇为关心的一个问题。双轮自平衡机器人概念就是在这样的背景下提出来的。两轮自平衡小车是一个高度不稳定两轮机器人，是一种多变量、非线性、强耦合的系统，是检验各种控制方法的典型装置。同时由于它具有体积小、运动灵活、零转弯半径等特点，将会在军用和民用领域有着广泛的应用前景。因为它既有理论研究意义又有实用价值，所以两轮自平衡小车的研究在最近十年引起了大量机器人技术实验室的广泛关注。II目录摘要I前言II第一章 设计综述11.1 自平衡车发展现状及市场分析11.1.1 市场调查分析11.1.2 市场背景11.1.3 目标市场的确定21.2 智能机器人系统设计任务概述21.3 课程设计主要内容2第二章 方案设计22.1 机械结构方案设计22.2驱动方案选择32.3 传感器的选择32.3.1主芯片的选择42.3.2电机驱动的选择42.3.3 陀螺仪芯片的选择52.3.4 降压模块的选择52.3.5超声波模块的选择62.3.6蓝牙模块的选择62.4 结构的合理性7第三章 机械系统设计73.1 机械系统总体方案73.2关键零部件结构尺寸设计73.3 驱动关节运动轨迹规划83.3.1运动学方程83.3.2速度分析93.3.3 运动学反解10第四章 控制系统设计104.1 控制系统总体方案104.2 选型114.3 STM32使用的资源114.4 电机驱动的电路设计114.5 传感检测外围电路设计124.6 电源供电电路设计134.7 总电路图14第五章 软件系统设计145.1 软件系统总体方案145.2位姿控制方案145.2.1 PID控制145.2.2 应用现状155.2.3 PID调节规律155.3 位姿控制流程图175.4 速度控制方案185.5 手机蓝牙遥控流程图195.6 超声波避障流程图20第六章 装配与调试216.1 机器人系统整体装配216.2系统调试流程与状况216.3 设计创新点216.4 系统缺陷与改进21第七章 市场应用前景分析227.1 市场前景227.2 产品优势22项目心得22参考文献23附录一 成本分析24附录二 程序源代码25 第一章 设计综述1.1 自平衡车发展现状及市场分析1.1.1 市场调查分析我国人口众多，交通拥堵问题严重，电动平衡车轻便、灵活的特点非常好的满足了年轻学生族、上班族对于解决短途出行的需求，不仅节省了金钱成本，也有效的缩短了上下班在路途花费的时间和体力，而且两轮自平衡电动车较广泛的被用来商用和警用，起到了非常不错的效果，因此电动平衡车市场需求很大。 图1.1 市场平衡车产品1.1.2 市场背景随着智能硬件技术的快速发展，电动平衡车产业如雨后春笋，行业内生产商从数十家增长至万余家，国内平衡车产能从2011年的3.2万台激增至2015年近1600万台。电动平衡车在2015年经历了爆发式的增长，其高产量的背后也反映出了市场对产品极大的需求，消费者对这种出行方式的认可度和接受能力在不断提高。目前，国内的平衡车主要是经出口销售到海外市场，2015年的高峰期，电动平衡车的月出口量就超过200万台。同时，国内消费市场也在崛起，越来越多的消费者开始尝试使用这一新颖的短途出行工具。但由于电动平衡车的安全性问题，2016年，中国电动平衡车行业不仅未得秋收反而提早入冬，订单锐减。从在美国为首安全事故至亚马逊网站下架，从三次美国ITC“337调查”至京沪的禁行政策，多方面接踵袭击电动平衡车行业。而且，我国两轮平衡行业发展滞后，以次充好现象严重，成本无法控制，市场平衡车价格高居不下，因此，平衡车市场需要安全性高让人放心的产品出现，具有极大的拓展空间。 1.1.3 目标市场的确定两轮自平衡电动车是一种新型代步工具，是依靠重力操控、电力驱动的智能交通工具，在安保巡逻、广告宣传、短途代步等领域具有广泛的用途。因此目标市场主要集中在政府机构、保安公司和广告商，以及学生族和上班族等。1.2 智能机器人系统设计任务概述a. 具备自平衡功能，能够实现直立、速度、转向平衡等基本动作b. 具备完整检测系统，机器人能够充分检测外部接触环境（超声波避障）；c. 具备足够负载能力，可携带移动电源；d. 具备无线通讯功能，能够实现小车的无线远程控制（手机蓝牙控制）。1.3 课程设计主要内容（1）资料分析：查阅相关文献资料，对资料进行分析总结。（2）机器人总体设计：根据机器人的具体任务要求，初步拟定小车的技术参数、电路部分设计、机械结构设计、控制方案选择等，制造出样品。（3）机器人机械结构设计：将机器人分解为车身结构、运动机构等若干部分，绘制机器人总装图和关键零部件图。（4）各个芯片的选择与调试：根据任务要求，选择各个模块进行相关信息检测、处理，并完成信息的正确传输。（5）运动控制方案设计：基于传感信息，采用STM32单片机，进行小车控制系统硬件和软件程序的设计和系统调试。（6）编制课程设计说明书。 第二章 方案设计2.1 机械结构方案设计 本文是对本质不稳定两轮小车自平衡控制问题的研究。实现小车直立、速度、转向平衡等基本动作以及蓝牙遥控、自动避障的功能。整个小车由机械部分和控制系统两大部分组成。机械部分包括3个亚克力板、两个驱动轮。它们是电池以及控制系统的载体。机器人的底板大小为15CM8CM，机器人有两个电机对称联接于机器人底板上，底板上方则时放置超声波避障模块、电池等，中间亚克力板则放置控制模块、陀螺仪芯片等。2.2驱动方案选择 利用PWM原理来控制直流减速电机的转动 对于电机的转速调整，我们是采用脉宽调制（PWM）办法，控制电机的时候，电源并非连续地向电机供电，而是在一个特定的频率下以方波脉冲的形式提供电能。不同占空比的方波信号能对电机起到调速作用，这是因为电机实际上是一个大电感，它有阻碍输入电流和电压突变的能力，因此脉冲输入信号被平均分配到作用时间上，这样，改变在使能端上输入方波的占空比就能改变加在电机两端的电压大小，从而改变了转速。PWM不管是高电平还是低电平时电机都是转动的，电机的转速取决于平均电压。脉冲宽度越大即占空比越大，提供给电机的平均电压越大，电机转速就高。 图2.1 12V直流减速电机2.3 传感器的选择 2.3.1 主芯片的选择 STM32单片机作为主控芯片。此芯片是以ARM的Cortex-M系列为内核的单片机，相对其他单片机，外设丰富，主频高，价格便宜，有专门的软件库，操作简单，调试方便，低功耗。强型系列时钟频率达到72MHz，是同类产品中性能最高的产品；基本型时钟频率为36MHz，以16位产品的价格得到比16位产品大幅提升的性能，是16位产品用户的最佳选择。 图2.2 STM32芯片 2.3.2电机驱动的选择 TB6612FNG是东芝半导体公司生产的一款直流电机驱动器件，它具有大电流MOSFET-H桥结构，双通道电路输出，可同时驱动2个电机。 TB6612FNG每通道输出最高1.2 A的连续驱动电流，启动峰值电流达2A/3.2 A(连续脉冲/单脉冲);4种电机控制模式：正转/反转/制动/停止;PWM支持频率高达100 kHz;待机状态;片内低压检测电路与热停机保护电路;工作温度：-2085;SSOP24小型贴片封装。 图2.3 驱动模块TB6612FNG 2.3.3 陀螺仪芯片的选择 MPU-6050集成了3轴MEMS陀螺仪，3轴MEMS加速度计，以及一个可扩展的数字运动处理器DMP，可用I2C接口连接一个第三方的数字传感器，比如磁力计。扩展之后就可以通过其I2C或SPI接口输出一个9轴的信号。MPU-6050也可以通过其I2C接口连接非惯性的数字传感器，比如压力传感器。MPU-6050对陀螺仪和加速度计分别用了三个16位的ADC，将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的，陀螺仪可测范围为250，500，1000，2000/秒（dps），加速度计可测范围为2，4，8，16g。另外，片上还内嵌了一个温度传感器和在工作环境下仅有1%变动的振荡器。芯片尺寸440.9mm，采用QFN封装（无引线方形封装），可承受最大10000g的冲击，并有可编程的低通滤波器。关于电源，MPU-6050可支持VDD范围2.5V5%，3.0V5%，或3.3V5%。另外MPU-6050还有一个VLOGIC引脚，用来为I2C输出提供逻辑电平。VLOGIC电压可取1.85%或者VDD。 图2.4 Mpu6050陀螺仪 其有以下优点：（）集角度测量与加速度测量于一体（2）其那同时测量三轴上的角度与加速度测量（3）其输出为数字信号，便于处理于存储与传输（4）测量范围大，反应快。 2.3.4 降压模块的选择 LM2596数显可调降压模块是直流降压稳压模块，带有电压显示功能，按键可切换输入或输出电压测量；具有输入反接保护，输出短路保护，过流过热保护等措施；模块全部采用优质元器件，用料讲究，布局合理，外观精美，性能稳定可靠。带电压表显示，电压表量程040V，误差0.1V；轻触按键（短按）即可切换测量输入或输出电压，并有指示灯显示正在测量的是哪路电压；特有的输入和输出自动转换显示功能，可实时监控电压；带电源指示灯，可指示是否有电源输入；带反接保护，反接不会损坏模块；带接线端子，方便使用； 图2.5 降压模块2.3.5超声波模块的选择DYP-ME007超声波测距模块可提供3cm-3.5m的非接触式距离感测功 能，图1为DYP-ME007外观，包括超声波发射器、接收器与控制电路。其基本工作原理为给予此超声波测距模块一触发信号后发射超声波，当超声波投射到物体而反射回来时，模块输出一回响信号，以触发信号和回响信号间的时间差，来判定物体的距离。 图2.6 超声波测距模块 2.3.6蓝牙模块的选择 HC-05 无线蓝牙串口透传模块引出接口包括VCC,GND,TXD,RXD,预留LED状态输出脚，单片机可通过该脚状态判断蓝牙是否已经连接，KEY引脚对从机无效。底板3.3V LDO，输入电压3.66V，未配对时电流约30mA，配对后约10mA，输入电压禁止超过7V。空旷地有效距离10米，超过10米也是可能的，但不对此距离的连接质量做保证。配对以后当全双工串口使用，无需了解任何蓝牙协议，但仅支持8位数据位、1位停止位、无奇偶校验的通信格式，这也是最常用的通信格式，不支持其他格式。 图2.7 无线蓝牙模块2.4 结构的合理性本机构采用两轮驱动，通过调节左右两轮的差速实现小车的转向，能更好的实现小车的换向。 第三章 机械系统设计3.1 机械系统总体方案双轮自平衡小车身上装有2个电机，用来带动车轮转动能够实现直立、速度、转向平衡等基本动作，小车车身装有蓝牙模块、陀螺仪模块、驱动模块、降压模块、和超声波模块。小车电机控制通过单片机来实现控制。底面的陀螺仪模块作为角度检测传感器，可以实现自平衡功能。还有蓝牙模块，实现手机蓝牙控制，超声波模块实现避障功能。 图3.1 小车初步设计3.2关键零部件结构尺寸设计通过在查阅相关资料和比较现有平衡车产品，初步设定平衡车机器人的外形机构如图3.1所示，其中小车的3个亚克力板大小均为150mm80mm。 图3.2 亚克力板 3.3 驱动关节运动轨迹规划 3.3.1运动学方程图 3.2 小车的受力分析图如图 3.2所示，小车的本质不稳定，控制系统根据测得的倾角产生一个相应的力矩，通过控制电机驱动两个轮朝车身要倒下的方向运动，以保持小车自身的动态平衡。控制量是轮子的转动力矩。小车在水平方向上位移为： 竖直方向上位移为： 在各个方向上的运动方程可以写为： 3.3.2速度分析1.驱动电机为直流减速电机，电机的动态方程为： 电机转角与小车的位置关系，电机转子的角位移为，电机的齿轮比为，则小车的驱动轮可以表示为： 设小车的质量为M, 为电机提供给方向上的驱动力，则小车的运动学方程为： 联立上式可以得：为了保持平衡，小车驱动力，必须大于或等于重力沿水平方向上的分力 将两轮的转动力矩作为系统输入量u，建立平衡小车的系统方程为： 3.3.3 运动学反解假设小车作匀加速直线运动，小车的轮子在地面上作纯滚动，由此可得小车的加速度为： 时小车运动的位置为，车与竖直方向的夹角为，由此可得，车轮的输入角度为： 第四章 控制系统设计4.1 控制系统总体方案 自平衡小车由多个功能模块共同组成，这几个模块共同工作、相互协调、相互作用，保证了小车的直立、速度、转向平衡。具体的工作原理如下：由STM32作为控制芯片，它对其它各个功能模块进行控制。信息采集模块负责采集周围环境以及机器人本身的各种信息。其中陀螺仪、角速度编码器，主要负责小车的平衡的功能，超声波测距模块实现避障。蓝牙遥控接收模块可以接收人们对机器人的控制信息，然后把信息传给STM32进行处理。4.2 选型1) 电机选择12V直流减速电机2) HC-05蓝牙模块3) DYP-ME007超声波测距模块4) LM2596数显可调降压模块5) MPU-6050陀螺仪6) TB6612FNG电机驱动4.3 STM32使用的资源 表4-1 STM32资源TIM15ms定时中断 采集小车位姿TIM2初始化为正交编码器模式，硬件采集编码器1数据（采集超声波脉宽）TIM3初始化为PWM输出，CH3 ，CH4输出20KHZ的PWM控制电机TIM4初始化为正交编码器模式，硬件采集编码器2数据USTART3通过串口3接受蓝牙遥控数据，接受方式为中断接收IIC利用IO模拟IIC接收MPU6050数据GPIO 输入和输出4.4 电机驱动的电路设计 图4.1 TB6612FNG引脚 STBY口接单片机的IO口清零电机全部停止，置1则通过AIN1 AIN2，BIN1，BIN2 来控制正反转，VM接12V以内电源，VCC接5V电源，GND接地，PWMA和PWMB分别接接单片机的PWM口，A01和AO2接电机1的两个脚。BO1和BO2接电机2的两个脚。表4-2 电机1真值表 AIN1 0 0 1 AIN2 0 1 0 停止正转反转表4-3 电机2真值表 BIN1 0 0 1 BIN2 0 1 0 停止正转反转 4.5 传感检测外围电路设计 图4.2 驱动电机控制电路 图4.3 蓝牙 图4.4 超声波 图4.5 陀螺仪 4.6 电源供电电路设计 图4.6 电源电路4.7 总电路图 图4.7 总电路图 第五章 软件系统设计5.1 软件系统总体方案1. 通过陀螺仪与编码器实现小车的自平衡功能。2. 通过蓝牙模块实现手机端的蓝牙遥控，手动实现小车的前后左右。3. 通过超声波模块实现小车的避障功能。5.2位姿控制方案5.2.1 PID控制控制技术是运动控制的核心，各种先进控制技术的研究不断推动着运动控制的发展，比如自适应控制技术和以神经网络和模糊控制为代表的智能控制技术，但在实际生产实践中应用最普遍的还是各种以PID为代表的基本控制技术按照偏差的比例、积分和微分进行控制的调节器，简称为PID调节器，是连续系统中技术成熟且应用广泛的一种调节器。本节将对系统用到的PID控制技术做相应的介绍和研究，当小车开始倾斜时，陀螺仪及时地将采集的小车倾斜角度、瞬时角加速度传给单片机，同时，编码器也将车子的实时速度采集后传给单片机，单片机系统收集到以上三组数据，对数据进行量化处理后，在PID平衡算法的控制下，控制电机及时地做出前进或后退或加速或减速的反应，使车子在一个小角度范围内做平衡地来回摆动，以保持车子的不倒。5.2.2 应用现状在电机伺服系统的控制中，经典的PID控制具有其结构简单、鲁棒性强以及现场对Pro的广泛使用积累了丰富的经验等优点，在无刷直流电动机的控制方面一直占有很重要的地位。PID的引入保证了其系统响应的快速性，稳定了闭环控制器，补偿了由逆变器引起的控制误差。PID控制器就是将偏差的比例(Proportional)、积分(Integral)和微分(Differential)通过线性组合构成控制量，用这一控制量对被控对象进行控制。 PID算法是目前工业过程控制中应用最广泛的控制算法。PID算法应用如此广泛，是因为它具有如下优点:(1)算法较为简单，易于实现；(2)基于线性控制理论，具备许多成熟的稳定性分析方法，有较高的可靠性；(3)可以在很宽的操作条件内保持较好的鲁棒性，对于控制对象模型参数小范变化不敏感；(4)不要求了解控制对象的精确数学模型。利用许多成熟的参数整定方法，可以根据控制对象的实际响应曲线来计算PID控制器的参数；(5)允许工程技术人员以一种简单直接的方式来调节控制系统，以达到希望得到的控制性能，如上升时间、最大超调量和稳态误差等。当被控对象的结构和参数不能完全掌握，或得不到精确的数学模型时，系统控制器的结构和参数必须依靠经验和现场调试来确定，这时应用PID控制技术最为方便。即当我们不完全了解一个系统和被控对象，或不能通过有效的测量手段来获得系统参数时，最适合用PID控制技术。5.2.3 PID调节规律PID控制分为两大类，一个是模拟PID控制，一个是数字PM控制。在模拟控制系统中，PID是最常用的控制方法。图5.1所示为模拟PID控制系统原理框图。比例系数Kp积分系数Ki微分系数Kd被控对象r(t)c(t) + _ + + u(t) y(t) + 图 图5.1 模拟PID控制系统原理框图计算机控制系统中，使用的是数字PID控制器。将连续的模拟量进行离散化处理，则可得在数字PID控制中，采样周期相对于系统的时间常数来说一般是很短。因此其参数可按模拟PID控制器中的方法来选择。由于要保持动态的平衡，则小车的倾角在一定的范围内要求可控。故本文选择系统的小车的倾角作为输出量，利用PID技术对其进行分析。由上述理论分析可以看出:其比例、积分、微分三者是彼此影响；同时要使其系统保持稳定，PID三参数必须满足上述关系；为了让系统能够实现其动态的平衡，需要通过反复试凑的方法来解决，而且整定的参数多导致反复试凑的次数极大的增加，控制器的参数较难选取。加之系统经过线性化处理，即使所选参数满足上述的关系式，仍需要根据系统的实际模型进行调整，这大大增加了调试的难度。5.3 位姿控制流程图 图5.2 位姿控制流程图5.4 速度控制方案利用PWM原理来控制直流减速电机的转动，PWM主要就是控制频率和占空比的：这两个因素分别通过两个寄存器控制：TIMX_ARR和TIMX_CCRX。ARR寄存器就是自动重装寄存器，也就是计数器记到这个数以后清零再开始计，这样PWM的频率就是tim_frequency/（TIMX_ARR-1）。在计数时会不停的和CCRX寄存器中的数据进行比较，如果小于的话是高电平或者低电平，计数值大于CCRX值的话电平极性反相。所以这也就控制了占空比。5.5 手机蓝牙遥控流程图 图 5.3 手机蓝牙遥控流程图5.6 超声波避障流程图 图5.4 超声波避障流程图 第六章 装配与调试6.1 机器人系统整体装配 图6.1 小车实物装配图6.2系统调试流程与状况1) 根据PID控制调节参数，如和来实现两轮直立平衡。同理，需调节相应参数实现速度与转向平衡。由于平衡的角度中值和机械相关，把平衡小车放在可以让轮子悬空的台子上，需要通过不断调试找出小车接近平衡的角度。2) 由于陀螺仪的漂移误差和加速度计的动态误差、相应参数调试的不完美、齿轮减速器存在死区、小车存在比较大的惯性等原因导致小车不能完全静止平衡，产生抖动。通过互补滤波可以得到更优的倾角近似值，有利于平衡。3) 超声波模块的响应与避障，设定距离不能太短，使小车在一定速度下可以避免碰撞。4) 蓝牙模块与手机连接的时候，蓝牙遥控的有效距离在空旷地为10米左右，所以实际情况下超过10米或小于10米也是可能的。6.3 设计创新点 在小车平衡的基础上附加了遥控和避障功能，以及通过调节左右两轮的差速实现小车的转向，能更好的实现小车的换向。6.4 系统缺陷与改进 小车平衡性能还是不完美，存在抖动，同时速度过快很容易摔倒，还需后续的调试与改进。21 第七章 市场应用前景分析7.1 市场前景 电动平衡车是我国新兴产业之一，开始较广泛的被用来商用和警用，起到了非常不错的效果，其次，自平衡电动车在中国的民用销量也是很不错的，很多人对它的特性所着迷。总体来说，自平衡电动车在中国的市场还是比较乐观的，随着民众生活水平的提高，自平衡电动车的销量一定会出现成倍的增加，这都是完全可能的。但由于电动平衡车在国内的技术已经成熟，我们需要对其进行不断地改进，不断地创新，争取把产品最优化，实现人们的需求。7.2 产品优势目前大部分市面上的电动平衡车，占道面积等同中等体型人的宽度，因此对于现代拥挤的交通状况来说确实满足最初的设计理念：流畅平顺的人类运输器。平衡车采用锂电池能量驱动，由普通民用电进行补足，很方便人们使用。相对传统代步工具，电动平衡车的机动性比较强。 项目心得本设计主要研究两轮自平衡小车的设计与实现。通过相应硬件与软件的设计，最终实现了对两轮自平衡小车电机的的动态平衡与运动控制。系统硬件结构以stm32为控制核心，采用mpu6050构成了姿态检测系统，当我们轻轻将陀螺仪装懂一个角度时，电机就会带动车轮转动，当我们向着反方向轻轻转动时，电机会带动车轮向相反的方向转动，陀螺仪在次处于水平，电机会停止转动。 最终，我们要完成实现了两轮自平衡小车的平衡控制及运动控制。本设计的控制策略采用PID控制算法。通过对小车的运动建模，构建了小车运动控制的PD控制算法，并对PID参数进行了整定,使小车能够稳定地平衡。并且在适量干扰下，小车能够自主调整并迅速恢复稳定状态。小车能够实现手机蓝牙控制前后左右移动的功能，以及能够智能实现避障功能。设计过程中，在老师、助教以及同学的帮助和配合下，才使得项目如期基本完成，这体现了团队精神的重要性。本次课程实践项目无论在专业知识还是在学习能力上都对我们有很大的启发，也非常感谢老师和助教的指导。 参考文献1 张洋 刘军 严汉宝.原子教你玩STM32（库函数版）M.北京航空航天大学出版社,20072 刘军 张洋 原子教你玩STM32（寄存器版）M.北京：北京航空航天大学出版社，20133 熊有伦.机器人技术基础,华中科技大学出版社,19954 李凡红 两轮自平衡小车系统J 北京交通大学学报，2011年03期附录一 成本分析 表1 成本电机2个STM32芯片1个驱动模块1个陀螺仪1个降压模块2个车轮及联轴器2个亚克力板3个电机支架2个电路板2个手工费其他蓝牙模块 超声波模块总计49元/个29.8元/个12.9元/个6.7元/个20元/个20元/个9元/个3.8元/个2元/个50元15元10元/个10元/个356元附录二 程序源代码主程序：u8 Way_Angle=2; /获取角度的算法，2：卡尔曼 可以改成3 即互补滤波，但是在顶配版程序中无法使用DMP,因为时序和超声波有冲突 u8 Flag_Qian,Flag_Hou,Flag_Left,Flag_Right,Flag_sudu=2; /蓝牙遥控相关的变量u8 Flag_Stop=1,Flag_Show=1; /停止标志位和 显示标志位 默认停止 显示打开int Encoder_Left,Encoder_Right; /左右编码器的脉冲计数int Moto1,Moto2; /电机PWM变量 Motorint Temperature; /显示温度int Voltage; /电池电压采样相关的变量float Angle_Balance,Gyro_Balance,Gyro_Turn; /平衡倾角 平衡陀螺仪 转向陀螺仪float Show_Data_Mb; /全局显示变量，用于显示需要查看的数据u32 Distance; /超声波测距int main(void) Stm32_Clock_Init(9); /系统时钟设置delay_init(72); /延时初始化JTAG_Set(JTAG_SWD_DISABLE); /=关闭JTAG接口JTAG_Set(SWD_ENABLE); /=打开SWD接口 可以利用主板的SWD接口调试LED_Init(); /初始化与 LED 连接的硬件接口KEY_Init(); /按键初始化uart_init(72,115200); /初始化串口1uart3_init(36,9600); /串口3初始化Adc_Init(); /ADC初始化MiniBalance_PWM_Init(7199,0); /=初始化PWM 10KHZ 高频可以防止电机低频时的尖叫声#if MODE_BIZHANG=1 TIM2_Cap_Init(0XFFFF,72-1); /=如果MODE_BIZHANG置1，则把TIM2初始化为超声波接口 然后不采集左路编码器，通过右路编码器近似左路车轮速度#else Encoder_Init_TIM2(); /=如果MODE_BIZHANG置0，则把TIM2初始化为编码器接口#endifEncoder_Init_TIM4(); /初始化编码器2 IIC_Init(); /模拟IIC初始化 MPU6050_initialize(); /=MPU6050初始化DMP_Init(); /初始化DMP Timer1_Init(49,7199); /=5MS进一次中断服务函数#if MODE_BIZHANG=1 Read_Distane(); /BIZHANG_MODE=1才能读取超声波#endif角度测量int Balance_Pwm,Velocity_Pwm,Turn_Pwm;u8 Flag_Target;int TIM1_UP_IRQHandler(void) if(TIM1-SR&0X0001)/5ms定时中断 TIM1-SR&=(10); /=清除定时器1中断标志位 Flag_Target=!Flag_Target; if(Flag_Target=1) /=5ms读取一次陀螺仪和加速度计的值，更高的采样频率可以改善卡尔曼滤波和互补滤波的效果Get_Angle(Way_Angle); /=更新姿态return 0; /=10ms控制一次，为了保证M法测速的时间基准，首先读取编码器数据Encoder_Right=Read_Encoder(4); /=读取编码器的值#if MODE_BIZHANG=1 Encoder_Left=Encoder_Right; /=避障模式，左路编码器直接由右路编码器近似，并取消转向环中的编码器使用，只用Z轴陀螺仪#else Encoder_Left=-Read_Encoder(2); /=读取编码器的值，因为两个电机的旋转了180度的，所以对其中一个取反，保证输出极性一致 #endif Get_Angle(Way_Angle); Led_Flash(100); /=LED闪烁;指示单片机正常运行 Voltage=Get_battery_volt(); /=获取电池电压 Key(); /=扫描按键状态 单击双击可以改变小车运行状态 Balance_Pwm =balance(Angle_Balance,Gyro_Balance); /=平衡PD控制 Velocity_Pwm=velocity(Encoder_Left,Encoder_Right); /=速度环PI控制 记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点 Turn_Pwm =turn(Encoder_Left,Encoder_Right,Gyro_Turn); /=转向环PD控制 Moto1=Balance_Pwm-Velocity_Pwm+Turn_Pwm; /=计算左轮电机最终PWM Moto2=Balance_Pwm-Velocity_Pwm-Turn_Pwm; /=计算右轮电机最终PWM Xianfu_Pwm(); /=PWM限幅 if(Turn_Off(Angle_Balance,Voltage)=0) /=如果不存在异常 Set_Pwm(Moto1,Moto2); /=赋值给PWM寄存器 return 0; 直立PD控制int balance(float Angle,float Gyro) float Bias,kp=390,kd=1.2; int balance; Bias=Angle+9; /=求出平衡的角度中值 和机械相关 balance=kp*Bias+Gyro*kd; /=计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数 return balance;速度PI控制int velocity(int encoder_left,int encoder_right) static float Velocity,Encoder_Least,Encoder,Movement; static float Encoder_Integral; float kp=-80,ki=-0.4; /遥控前进后退部分if(1=Flag_Qi