常熟理工学院学报（自然科学）Journal of Changshu Institute Technology （Natural Sciences ）第26卷第10Vol.26No.102012年10月Oct.,2012收稿日期：2012-09-07基金项目：江苏省大学生实践创新训练计划项目“两轮自平衡机器小车的设计”（jx110152011）作者简介：李荣伟（1989—），男，江苏东海人，常熟理工学院电气与自动化工程学院测控技术与仪器专业2009级学生.通讯作者：李鑫（1983—），男，安徽亳州人，实验师，硕士，研究方向：智能控制技术与现代检测技术，E-mail:lixin_yy@．两轮自平衡小车的设计李荣伟，李鑫，孙传开，张冬林，江振峰（常熟理工学院电气与自动化工程学院，江苏常熟215500）摘要：设计了以陀螺仪ENC-03以及MEMS 加速度计MMA7260为传感器的姿态感知系统，选用16位单片机MC9S12XS128为控制核心处理器，完成对传感器信号的采集处理、车身控制以及人机交互的设计，实现小车自主控制平衡状态、运行速度以及转向角度大小等功能.实验结果表明该系统的性能满足设计要求.关键词：两轮自平衡；姿态检测；卡尔曼滤波；数据融合；PID 控制器中图分类号：TP242.6文献标识码：A 文章编号：1008-2794（2012）10-0070-06近年来，随着电子技术的发展与进步，移动机器人的研究已成为目前科学研究最活跃的领域之一，移动机器人经常会遇到在较为狭窄复杂的环境中如何灵活快捷地执行任务的问题.两轮自平衡机器人的概念就是在此背景下提出来的，这种机器人区别于其他移动机器人最显著的特点是：采用了两轮共轴、各自独立驱动的工作方式（又称差分式驱动），车身的重心位于车轮轴的上方，通过轮子的前后移动保持车身的平衡、行驶[1].因为具有体积小、运动灵活、零转弯半径等特点，所以在军用和民用领域有着广泛的应用前景，更重要的是系统具有多变量、非线性、强耦合、不稳定性的特性，使其成为很好地验证控制理论及方法的平台，具有很高的研究价值.1系统整体设计本文设计的自平衡车采用姿态传感器（加速度计和陀螺仪）监测车身所处的俯仰状态和状态变化率，通过高速微控制器（MC9S12XS128）完成数据融合处理，得到平滑而稳定车体姿态信息，然后驱动电动机产生前进或后退的加速度来控制车体保持平衡，同时系统还要根据速度的反馈量来完成对车体速度和方向的控制，微控制器还需构建相关输入输出模块和人机交互设备.系统设计总体结构框图如图1所示.已知自平衡车高度为l，质量为m ，将其抽象为一级倒立摆，并将倒立摆置于可水平移动的小车上.假设其受外力干图1系统设计总体结构框图10李荣伟，李鑫，孙传开，等：两轮自平衡小车的设计扰引起的车体角加速度为x (t )，沿垂直于车体方向进行受力分析（见图2），可以得到自平衡车倾角与车轮移动加速度为a (t )以及外力干扰带来的加速度x (t )之间的运动方程[2-3].小车运动微分方程表达式为l d 2θ()t d t 2=g sin []θ()t -a ()t cos []θ()t +lx ()t ,当倾角θ很小的时候，可以进行线性化处理：cos(θ)≈1、sin(θ)≈θ，运动微分方程可简化为l d 2θ()t d t 2=gθ()t -a ()t +lx ()t ,自平衡车静止时a ()t =0，其运动微分方程为l d 2θ()t d t 2=gθ()t +lx ()t ,由小车静止时的运动方程可得到系统的传递函数：H ()s =Θ()s X ()s =1s 2-g l .此时系统具有两个极点：s p =±.其中一个极点位于s 平面的右半平面.根据奈奎斯特稳定判据可知系统不稳定，因此小车在静止状态不能保持平衡.由倒立摆系统的受力分析可知，车轮提供的加速度大小是根据角度θ及角速度θ′的反馈得出的，因此需要在控制系统中引入角度θ及角速度θ′构成比例（P ）微分（D ）反馈环节，如图3所示.加入比例微分反馈后的系统传递函数为H ()s =Θ()s X ()s =1s 2+k 2l s +k 1-g l .此时，系统的两个极点为s p.根据奈奎斯特稳定判据可知，系统稳定需要两个极点都位于s 平面的左半平面.要满足这一点，需要k 1>g ，k 2>0.由此可得出结论，当k 1>g ，k 2>0时，小车可以保持平衡.2硬件模块设计系统硬件主要由以下几个模块组成：MC9S12XS128单片机最小系统、电源模块、姿态检测模块、电机驱动电路和速度检测电路.本设计采用Freescale 公司16位单片机MC9S12XS128为控制器，工作电压为5V.其丰富的内置模块资源可以为姿态检测系统提供12位精度的A/D 转换，为速度检测模块提供脉冲计数，同时拥有独立的PWM 输出模块可以控制电机输出.姿态检测模块能够精确并稳定地检测出当前的车身姿态，这是实现有效控制的关键，本设计选用惯性传感器MEMS 陀螺仪ENC-03和加速度计MMA7260作为姿态检测传感器.加速度MMA7260传感器是一种图3加入比例微分环节后的控制系统结构图图2外力干扰条件下的小车受力分析71常熟理工学院学报（自然科学）2012年低g 值的传感器，根据物件运动改变各轴的信号输出.各轴的信号在不运动或不受重力作用的状态下（0g ），其输出为1.65V ．MMA7260的灵敏度可通过gs_1和gs_2的引脚电平来设定.分别为800mv/g 、600mv/g 、300mv/g 、200mv/g ，为了很好地检测车体微小变化，本电路通过gs_1和gs_2脚同时接地，将其灵敏度设定为800mv/g.本系统采用的陀螺仪为村田公司生产的ENC-03[4]，其输出为0.67mv/deg/sec.采用LM358构成负反馈同相放大电路，增益为R 3R 4+1，即放大10倍左右.由于陀螺仪静态输出会随温度等因素变化，影响系统倾角检测，为此在放大电路中利用LM358设计了零点偏置电压校正电路，并将陀螺仪静态输出电压调整为模拟电压的一半（此处为1.65V ）.姿态检测模块电路原理如图4和图5所示.图4MMA7260三轴加速度计电路原理图图5ENC-03放大电路设计原理图电源管理模块是整个硬件电路稳定可靠运行的基础，整个系统需要3种电源：为驱动电机供电的7.2V 电源，为单片机及相关外设供电的+5V 数字电源，为姿态传感器和A/D 基准电压供电的3.3V 模拟电源.整个系统电源来源为7.2V 镍氢电池，5V 电源由LM2940提供.LM2940是一种线性低压差三端稳压器件，其输出纹波较小，适合单片机供电.3.3V 电源采用AMS1117转换而得.电机驱动采用全桥式驱动电路实现电机的正反转，系统采用两片专用半桥驱动芯片BTS7960构成全桥式驱动电路.速度检测模块采用欧姆龙公司500线增量式旋转编码器，通过单片机的PACNT 模块对输入脉冲进行计数，从而为处理器提供速度反馈量.辅助调试模块由按键、RS-232串口通信模块、电脑等组成.3系统软件设计3.1软件设计框架系统软件设计框架主要包括：资源模块初始化、姿态信息采集、数据融合、速度检测、直流电机PID 控制.系统软件流程如图6所示.3.2资源模块初始化单片机系统初始化主要完成以下工作：通过锁相环模块（PLL ），设定单片机的工作频率为64MHz ；将模数转换模块（ATD ）设置为12位精度、4通道连续采样、采样时间4MHz 、非中断触发；串行通信模块（SCI ）设置为波特率9600、8个数据位、1个停止位、允许收发；ECT 中断使能8路输入捕捉（IC ）/输出比较（OC ）通道、下降沿捕图6软件流程设计7210李荣伟，李鑫，孙传开，等：两轮自平衡小车的设计捉、使能捕捉中断；定时器（PIT ）产生10ms 的定时中断；PWM 模块使用0通道和1通道级联、2通道和3通道级联产生两路10kHz 、左对齐的PWM 等.3.3车模姿态信息检测本系统采用的惯性传感器为模拟量输出式，利用xs128自带的ATD 模块以3.3V 的基准电压进行模数转换，从而量化出陀螺仪和加速度计的输出电压.当车模倾角发生变化时，重力加速度g 便会在加速度传感器的Z 轴上形成加速度分量，从而引起该轴输出电压的变化：Δu =kg sin θ≈kgθ，式中g 为重力加速度，±900为车模倾角，k 为加速度传感器灵敏度系数.当倾角θ()-100≤θ≤+100比较小的时候，输出电压的变化近似与倾角成正比.故车模加速度计的可以表达为θz =V out -V offest V max -V min1800，θz 为加速度计Z 计算出的车模倾角，V out 为Z 轴的输出电压，V offest 为Z 轴平衡点的电压，V max 、V min 为相对于平衡点±900处的电压.同时陀螺仪的电压也会随车模角速度变化而变化，将陀螺仪的量化电压值减去静态电压然后除以陀螺仪的灵敏度以及放大系数，便可得到车模晃动的角速度ω.将ω和θz 送入卡尔曼滤波器进行角度融合.3.3.1卡尔曼滤波器卡尔曼滤波的程序实现是先利用angle+=（gyro m-q bias ）dt 计算出陀螺仪积分出的角度，其中q bias 是陀螺仪偏差.此时利用陀螺仪积分求出的angle 作为系统的估计值，得到系统的观测方程；而加速度计检测的角度angle m 当作于系统中的测量值，得到系统状态方程.在程序中Q angle 和Q gyro 分别表示系统对加速度计及陀螺仪的信任度.根据Pdot=AP +PA ′+Q angle 计算出先验估计协方差的微分，用于将当前估计值进行线性化处理.其中A 为雅克比矩阵[5-7].本系统中雅克比矩阵为A =éëùû0-100随后计算系统预测角度的协方差矩阵P .计算估计值angle m 与预测值angle 间的误差angle err.计算卡尔曼增益K_0，K_1，K_0用于最优估计值，K_1用于计算最优估计值的偏差并更新协方差矩阵P .通过卡尔曼增益计算出最优估计值angle 及预测值偏差q bias ，此时得到最优角度值angle 及角速度值.3.3.2姿态检测的参数整定姿态调试采用串口波形显示的方式进行.XS128通过串口通讯向PC 机发送滤波前与滤波后的姿态倾角数据，通过上位机波形显示软件（数据被放大了十倍），将离散的数据点描绘为动态波形，参数的不同可以通过前后对比直观地看出.图7和图8截取了两张相对良好且不同运动状态的调试效果图.图7车体平滑振动图图8车体剧烈振荡图由以上两幅图可以直观地看出：加速度计传感器计算出的角度一定情况下可以反应出车体的倾角，但是毛刺太多、震荡太大不适合直接利用.卡尔曼滤波器不仅抑制了陀螺仪的零点漂移问题，还使计算的最优角度平滑、稳定，并能反映出车体的状态，满足设计需求.73常熟理工学院学报（自然科学）2012年3.4直流电机PID 控制分析系统要求可知，保持小车直立和运动的动力都来自于小车的两个车轮，车轮由两个直流电机驱动.因此，从控制角度来看，可以将小车作为一个控制对象，控制输入量是两个车轮的驱动电压.整个控制系统可以分为三个子系统.（1）小车平衡控制：以小车倾角为输入量，通过控制两个电机的正反转保持小车平衡.PID 控制器的程序实现为：OUT Motor=Kp *Angle+Kd *Angle dot ；式中OUT Motor 为平衡控制输出量，Angle 为卡尔曼滤波后的倾角偏差值，Angle dot 为卡尔曼滤波后的角速度值，Kp 和Kd 分别为比例系数及微分系数.在次采用是试凑法来整定Kp 和Kd 的，并用满占空比的百分比进行描述.先设定Kd =0，根据上述小车的数学模型可知，Kp 需要大于重力加速度所产生的效果才能使车模保持直立.将Kp 从零逐渐增大到5%左右时，车模开始有直立的趋势.进一步加大Kp 到11%左右，使得车模保持直立并开始来回摆动.说明此时车模比例参数已经可以克服重力的影响，然后逐步增加微分参数使车模摆动减小.适当微调Kp 、Kd 参数使车模可以保持稳定直立，车模保持上述情况的比例和微分参数有一个相当大的空间.由于受车模重心尺寸的影响，Kp 、Kd 的参数不唯一，此处Kp 为12.4%，Kd 为0.3%.实际处理时在车模动力传动系统中，各个环节都存在着静摩擦力，它会降低车模静态稳定性.所以程序中增加一个死区电压（满占空比的4%）.（2）小车速度控制：在保持平衡的基础上，通过调节小车倾角实现对速度的控制，实际上还是演变为对电机的控制实现小车的速度控制.本系统以编码器所测得脉冲数作为速度反馈环节，形成了速度的闭环控制.速度控制的函数参数为车速的偏差，返回值为倾角，通过平衡控制达到控制车速的目的.在实际速度控制中，采用了带死区的PID 算法，防止调节过于频繁，导致系统振荡.设定系统平衡控制周期为10ms ，为减少干扰并且提高速度控制的力度，系统速度控制周期设定为50ms.速度程序计算代码为：angle speed=Speed Kp *Speed error+Speed Ki*Speed diff ；式中angle speed 为速度控制输出量，Speed erro 为速度脉冲的偏差，Speed Kp 、Speed Ki 为比例、积分项的系数.设定速度值为0，调整这些系数并观察车模在受到外力作用时能否迅速恢复直立静止状态.逐步增大比例参数会发现车模能在平衡点附近来回运动，然后增加微分参数使车模能够很快趋于静止，微调参数后使车模抵抗外部干扰冲击的能力逐步增强，此处取Speed Kp 为满占空比的1.22%，Speed Ki 为满占空比的0.23%.（3）小车方向控制：通过控制两个电机间的电压不同实现转向.本系统是在速度控制周期内，对两个电机实现差速输出，则可在保持平衡的基础上实现转弯.系统调试时，由其自动采集道路中的电磁场信息（由通有100mA 交变电流的导线所产生），并对路径参数进行识别与路径优化后，完成前进、转弯等动作.4总结本设计主要研究两轮自平衡小车的设计与实现.通过相应硬件与软件的设计，实现了两轮自平衡小车的动态平衡与运动控制.系统实物如图9所示.系统硬件以飞思卡尔公司16位单片机MC9S12XS128为控制核心，采用压电陀螺仪ENC-03及MEMS 加速度计MMA7260构成了惯性姿态检测系统，通过H 桥电机驱动及旋转编码器实现了直流电机的闭环调速.本系统结合硬件构架设计了以卡尔曼滤波器为核心的数据融合算法.通过卡尔曼滤波器将陀螺仪与加速度计的输出融合为准确的倾角与角速度输出，为系统的控制提供了有力保障.本设计的控制策略采用PID 控制算法.通过对小车的运动建模，构建了小车运动控制的PD 控制算法，并对PID 参数进行了整定.本设计最终实现了两轮自平衡小车的平衡控制及运动控制.小车在保持两轮平衡的基础上可以实现前进、后退、转弯等基本动作.图9系统实物7410李荣伟，李鑫，孙传开，等：两轮自平衡小车的设计75参考文献：[1]He Kezhong,Sun Haihang,Guo Mtthe.Research of intelligent mobile robot key techniques[J].IEEE International Conference on In-dusIrial Technology,1996(7):503-507.[2]屠运武，徐俊艳，张培仁.自平衡控制系统的建模与仿真[J].系统仿真学报，2004（04）.[3]霍亮.两轮自平衡电动车的关键技术研究[D].哈尔滨：哈尔滨工程大学，2010.[4]邹学锋，卢新艳.基于Allan方差的MEMS陀螺仪性能评价方法[J].微纳电子技术，2010，47（8）：490-493.[5]赵杰，王晓宇，秦勇，等.基于UKF的两轮自平衡机器人姿态最优估计研究[J].机器人，2006（11）：605-609.[6]王晓宇，闫继宏，臧希喆，等.两轮自平衡机器人多传感器数据融合方法研究[J].传感器技术学报，2007（3）：668-672[7]耿延睿，崔中兴.组合导航系统卡尔曼滤波衰减因子自适应估计算法研究[J].中国惯性技术学报，2001（04）.A Design of Two-Wheel Self-Balance VehicleLI Rong-wei,LI Xin,SUN Chuan-kai,ZHANG Dong-lin,JIANG Zhen-feng (School of Electrical and Automation Engineering,Changshu Institute of Technology,Changshu215500,China) Abstract:A two-wheel self-balancing electric vehicle is an important branch of moving robots,because of its dynamic system with multi-variables,nonlinear,strong coupling,time-varying,with parametric uncertainties, and other characteristics,and the complex research can make the experiment device of a control theory become an ideal platform.This paper summarizes the current situation of research on a two-wheel self-balancing robot. And gyroscope ENC-03,MMA7260for attitude sensing sensors and MEMS accelerometer system are designed in the paper.By means of Freescale16-bit single-chip Microcomputer Mc9s12xs128for the control of core pro⁃cessor,the paper completes the acquisition of sensor signal processing,body control,as well as features such as human-computer interaction.Besides,the paper realizes the independent control balance of the car features such as size,speed and steering angle.Experimental results show that performance meets the design require⁃ments of the system.Key words:two-wheel self-balance;gesture detection;Kalman filter;data fusion;PID controller。