片单机控制单轴双轮自动平衡小车设计.doc

毕业设计（论文）

题 目：单片机控制单轴双轮自动平衡小车设计
系 别：电气工程系
专 业：电气工程及其自动化
班 级：电气121
学 号：
学生姓名：
指导教师：

2016年 月
摘要
两轮自平衡车由于其特有的灵活性以及便捷性越来越受到人们的关注，在人们的日常生活中也作为代步工具被越来越多的人群接受。本设计采用了基于ARM CORTEX-M3内核的STM32单片机，使平衡车姿态调整速度更快，数据处理更准确，运动性能更佳。并且放弃分别采用陀螺仪和加速度传感器来获取小车的姿态信息，而是直接采用了全球首例9轴运动处理传感器MPU-6050，通过DMP获取四元数，以算出角度，并通过PID控制实现小车的直立平衡行走。采用蓝牙模块实现和小车之间的通讯工作，设计最终实现了小车的平衡站立、前进、倒退功能。实现小车的平衡直立行走，其实就是要对电机进行适时的控制，电机作为机电转换装置，遍布于国民经济的各个领域以及人们日常生活的方方面面，所以对电机的完美控制更显得极其重要，也是本设计的主要目的之一。STM32F10x系列芯片是新型的32位嵌入式微处理器，具有优秀的数据处理能力，速度更快，在对本设计中的小车模型进行速度调整时，能展现出更灵敏的反应速度，更重要的是，它还具有较好的移植性，在很好的实现本设计任务的同时，利于将来更多功能的扩展。
关键词: 平衡车；MPU6050；STM32；PID控制

Abstract
More and more people have paid attention to The Two-wheeled Self-balancing Robot due to its unique flexibility and convenience. People had accept it as transport in people's daily life. This study is just based on the purpose of explore the The Two-wheeled Self-balancing Robot .This design bases on STM32 micro controller which based on the CORTEX-M3 ARM core.because of that , the robot adjust faster, data processing more accurate ,performance better . At the same time, we directly using the world's first 9 axis motion processing sensor MPU-6050 instead of the gyro and acceleration meter sensors to get the car's attitude information . to calculate the angle Through the DMP. And to achieve the car's upright balance walking through the PID control. Using Bluetooth module to achieve communication with the car. The Designed will achieve the balance of the car stand, forward, backward function. To achieve the balance of the car upright walking, in fact, is to timely control of the motor, electric motor is an electric machine conversion device which through all areas of the national economy and people's daily life, so the perfect control of the motor is more important and is one of the main objectives of this design.stm32f10x series chip is new 32-bit embedded microprocessor , it has excellent performance, good portability and improve the efficiency of the DC motor control, and we have modular the l system, which will be conducive to the balance of the car after the function expansion.
Key Word：Self-balancing Robot MPU6050 STM32 PID controller

目  录
1 绪论 1
2 平衡车总体设计方案 2
2.1 系统平衡原理分析 2
2.2 系统整体设计 3
3 系统硬件电路设计 5
3.1 系统硬件选型 5
3.2 最小系统设计 6
3.3 姿态检测电路设计 6
3.4 电源稳压模块 7
3.5 电机驱动模块设计 8
4 系统软件部分设计 10
4.1 软件系统总体结构  10
4.2 单片机初始化 10
4.3 姿态检测系统软件设计 11
4.3.1 MPU6050姿态获取方法 11
4.3.2 卡尔曼滤波算法 12
4.4 平衡PID控制软件实现  13
4.4.1 直立环PD控制 13
4.4.2 速度环PI控制 14
5  系统调试 16
5.1 角度矫正 16
5.2 卡尔曼滤波调试 16
5.3 PID参数的整定 16
5.3.1 PD控制调试 16
5.3.2 PI控制调试 16
6 结论 18
致谢 19
附录1 程序 20
附录2 电路图 25
参考文献 26

中原工学院信息商务学院毕业设计（论文）
1 绪论
近几年，不管是独轮车还是双轮平衡车都越来越受到人们的关注，随着时间发展，科技的进步，双轮平衡车也变得越来越智能化。
最开始，瑞士研制了用 DSP和FPGA 控制的基于倒立摆理论的双轮车，通过倾斜传感器和倾角传感器来检测车体，通过电机上的编码盘检测电机的速度。该设计采用了基于状态反馈的线性控制策略，车的运动被分解成直线和旋转运动，然后分析直线运动和旋转运动，得到电机需要的控制量，最终把控制量耦合叠加。其主要的设计思想是：使车子朝车体倾斜的方向运动来保持车身的平衡，该控制只能让小车平衡运动，而不能让小车自主直立。之后，赛格威公司研发了Segway平衡车，它拥有更多的姿态传感器，有5个陀螺仪传感器，然而事实是检测车身倾斜只需要3个传感器就够了，其他的两个传感器只是增加安全性。传感器采集到的数据发送给集成有许多处理器的电路板。这个集群是为了保证载人平衡车在其中任何一个处理器出现问题时能报告错误，保证了平衡车的安全性。它的设计是针对民用代步设计的，很具有代表性。国内方面，也已经有基于传感器 ADXL202 及反射式红外线距离传感器来获得姿态信息的设计[[] 王晓宇. 两轮自平衡机器人的研究[D]. 哈尔滨:哈尔滨工业大学,2007
]，该设计基于 PWM 动态来控制直流电机的速度，车与上位机间的数据通信显示使用人机交互界面使用图形液晶点阵、方向摇杆、按键。依靠这些可靠并且完备的硬件，使用独特的软件算法，最终实现Sway 的数据交换和平衡直立控制。
发展趋势：平衡车正在朝着越来越智能化[[]Googol  Technology LTD. Googol Technology Self-Balancing Robot GBOT1001 User Manual V1.0 [CD].2007
]的方向的发展，已经提出了众多的解决平衡控制的方案。未来平衡车必将越来越小巧轻便，配备更完善的人机交互介面。另外，两轮自平衡车的应用前景将会十分的广泛，未来的目标是制造出外形结构简洁，并且具有稳定自平衡性能的平衡车，还能有方便的驾驶模式，必将方便大家的出行。

2 平衡车总体设计方案
2.1 系统平衡原理分析
分析双轮平衡小车的平衡站立，我们自然联想到一根直立木棍在手指上保持直立，这正是人们在日常生活中对直立平衡控制的直观经验和感受。通过仔细分析不难发现，通过手指移动可以保持木棍的直立不倒，类比于单轴双轮平衡小车，我们在发现车身倾斜的时候，需要直接使车子向相反的方向运动，目的是抵消在这个维度上由于车身重力产生的倾斜，这样就能使车身保持直立平衡。导致车体倾斜的主要因素是θ角度[[]Tony, Liao, Deog.Variable Structure Control for Stabilizing Double Inverted Pendulum[C].2008 International Conference on Intelligent Computation Technology and Automation, California,US,2001
]的产生，θ角度是车身发生倾斜后车身和垂直于地面的直线的夹角，所以要想小车平衡，需要消除倾角，或将倾角控制在一个可控的范围之内，整个平衡控制流程如图2-1所示。

θ不为零车体倾斜
转动车轮
车体平衡
θ变小

图2-1 平衡控制框图
保持车身平衡的方法是转动电机来调整车身下部，以改变车身倾斜角度，来使车身保持在一水平垂直线上。然而这只是我们的直观感受，真正应用于理论设计的时候，就要写出小车的运动方程[[4] 陈国文.基于STM32单片机的双轮小车近静态动平衡控制统[D].闽南师范大学毕业论文设计.2014
]。车身受力分析如图2-2所示。
车模运动方程：
（2-1）
在角度θ很小时，运动方程简化为：
（2-2）
车模静止时:
（2-3）

图2-2 车模受力分析
2.2 系统整体设计
目前市场上已经开发出单轴双轮自动平衡小车，已经可以满足代步和娱乐的目的，本设计的要求是，要设计出这样一个系统，使它具有自动平衡的功能，并且能够前进和后退。
姿态传感器
蓝牙模块
主控芯片STM32
电机驱动电路
车速检测模块：编码器

电机
图2-3 硬件结构图
整个设计流程以STM32单片机为核心，选用了与该主控芯片相融和的其他几个必要的模块。姿态传感器模块电路测算小车车身姿态信息、并把信息提供给主控芯
片STM32。主控芯片STM32是整个小车模型的大脑，在接收到小车车身信息之后，与预先设定车身平衡数据进行比较，计算出小车需要调整的余量，转化成PWM[[] 阮毅,陈伯时.电力拖自动控制系统[M].清华大学出版社.2003
]控制信号，输出给电机驱动电路模块，电机驱动模块在接收到PWM信号之后再完成对电机的控制，电机上的编码器测速模块采集到电机速度信息，再把信息反馈给主控芯片STM32。重复这样一个过程，完成车模的直立平衡控制、直立运动控制[[] 尔桂花,窦曰轩.运动控制系统[M].清华大学出版社.2002 
]，依靠蓝牙模块发送对整个小车的运动控制命令。
整个小车被设计成如图2-3所示的几种模块。选用基于ARM的STM32F10X系列单片机，这是一系列功能更强大的数据处理能力更强的主控芯片[[] 刘金琨.智能控制[M].北京：电子工业出版社.2005 
]，必将会为之后平衡车的功能拓展提供方便。整个设计采用模块化的思想，这也正好切合了设计之初的想法，为实现小车更多功能的扩展提供方便。

3 系统硬件电路设计
3.1 系统硬件选型
姿态传感器选型：InvenSense公司的MPU-6050内置了一个数字可编程的低通滤波器、自带的可扩展的数字运动处理器DMP。并且，InvenSense 提供了一个 MPU6050 的嵌入式运动驱动库，结合 MPU6050的 DMP，可将原始数据直接换算成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，从而得到yaw，roll， pitch，他们分别是俯仰角、横滚角、航向角。使用内置的DMP，使主控芯片不用进行姿态解算过程。应用在平衡车上的MPU-6050有很多优点：体积小，功耗小，测量也很精确。MPU6050内部集成了三轴陀螺仪以及加速度传感器[[] 梅晓榕.自动控制元件及线路[M].哈尔滨工业大学出版社.2004 
]。如果选用多组方案，成本会升高。所以最终，直接采用集成了动态滤波功能的9轴姿态传感器，传感器可以把处理后的姿态角等信息用串口形式[[]  Jey, Tiga. Design of Fuzzy Logic Controller for Two-wheeled Self-balancing Robot[C].6th International Forum on Strategic Technology, IFOST 2011, Newyork, US, 2011:1266-1270.2008
]发送。我们想要得到的是小车的俯仰角。要想得到车体的俯仰角，只需利用原始数据，进行姿态融合解算即可。
电机选型：因为平衡车要进行速度控制的话还需要有编码器，为了减免不必要的工作量，并切保证质量，我们应尽量选取带有编码器的直流有刷电机。本设计采用的是型号是25GA20E260的带有带霍尔编码器的电机，工作电压在7～13V,越高的电压将会带来越高的性能。电机尾部带有13 线的霍尔编码器，电机的减速比是1:20，故车轮转一圈，电机可以输出260 个脉冲，倍频之后是1040。编码器的额定工作电压是5V,集成了上拉电阻和比较整形功能，可以直接输出方波。电机配有金属减速箱，额定电压下，减速后空载转速500rpm(转每分)。外形小巧，使用方便。
电机驱动选型：TB6612FNG芯片和L298N使用都是非常简单的。该芯片的数据输入只需要控制两条数据线的高低电频和一条线的PWM就能够控制一个电机，而该芯片正好能够控制两个电机，所以用6条线就够完成两个电机的驱动。TB6612FNG 是一个很优秀的双电机驱动板，它有两个接口去来驱动两个直流电机，如我们常用的微型金属减速马达控制器，当然也可用于控制一个双极步进电机。基于 MOSFET 场效应管的 H 桥驱动更有效率。两台电机的每个通道都有两个方向控制引脚和一个接受 PWM 输入频率达 100Khz 的速度控制引脚。TB6612相对于传统的L298N，效率提高很多,体积大幅减少，在额定范围内，芯片基本不发热。
TB6612FNG有两个Mosfet的H 桥电路。对比L298N，TB6612只需要接一个电容，发热也不严重，驱动功率也很高，将会为整个设计电路模块减小体积。
要想系统的电源模块满足要求，必然要按照适用于硬件系统中其他硬件的需求来确定。要满足姿态传感器芯片为mpu6050，电机25GA，驱动TB6612，STM32C8T6的具体要求，就需要提供: 12V、5V、3.3V的电压。德州仪器（TI）生产的稳压芯片LM2596提供的有：3.3V、5V、12V及可调（-ADJ）等多个电压档次产品。它的输出电压：3.3V、5V、12V及（ADJ）等，最大输出电压达到37V。用于提供1A输出电流且工作压差可低至1V的AMS1117系列稳压器设计。既使输出最大电流，可以保证AMS1117最大电压差也不会超过1.3V，随着负载电流的降低，电压差也慢慢变小。
设计最终使用STM32F103C8T6，读取mpu6050的数据时通过IIC接口，获得小车的姿态，接下来的过程是主控芯片通过PID控制给电机输出PWM控制信号，电机驱动模块芯片TB6612接受到PWM信号之后再完成对电机的控制[[] 高玉培.轮式移动机器人的结构设计[J].南昌航空大学学报.2014
]。另外，为了调试方便，除了设计了上述给模块外，还扩展了JLINK接口，使用的是SWD模式，用于仿真调试。因为需要记录数据和绘出数据波形，查看滤波和PID效果。所以扩展了串口电路。
3.2 最小系统设计
系统方框图如图5所示STM32系列是一款性价比极高的嵌入式应用专门设计的主频72MHz的ARM CORTEX-M3内核，方便之后功能的拓展，工作电压为3.3V。晶振电路为STM32单片机处理器[[11] 张培仁.基于16/32位处理器机器人控制系统设计与实现[M].北京:清华大学出版社, 2006
]提供8M频率的时钟脉冲，并串联两个10PF电容保
图3-1 最小系统图
证电路提供稳定的频率脉冲，设置倍频PLL为9,得到单片机的工作频率是72M，单片机最小系统图如图3-1所示。
3.3 姿态检测电路设计
本设计选用的MPU6050有以下几点理由，在获取角度和加速度的同时，因为MPU6050内置的DMP处理器，直接使发出数字信号，无疑使单片机的姿态处理时间缩短，姿态处理过程得到简化。获取姿态时没有分别采用传统的加速度计，陀螺仪[[] 屠运武,徐俊艳,张培仁.自平衡控制系统的建模与仿真[J].系统仿真学报.2004
]，而是采用集成了陀螺仪和加速器的MPU6050，为单片机直接提供向9轴融合演算技术。
图3-2 姿态检测电路
MPU6050电路如图3-2，Pin10引脚所接陶瓷电容C19，用于校准滤波；Pin13所接陶瓷电容C18是VD旁路电容； Pin20所接陶瓷电容C17是电荷泵电容； Pin8所接陶瓷电容C20是VLOGIC电容；SDA经一个10K的电阻连接到PB11上；SCL同样经一个10K的电连接到PB10上；INT直接连接到PB3上。
3.4 电源稳压模块
专门设计了电压模块[[13] 刘斌.两轮自平衡小车软硬件研发与基于模糊线性化模型的变结构控制研究.西安电子科技大学工学硕士学位论文.2009 
]为本系统所设计的各个模块提供电源。TB6612需要5V
图3-3 电源模块
电源来提供逻辑电平的输入，而12V的直流电源是提供给电机的电源，电机采用的是型号25GA20E260的12V直流减速电机，该电机自带霍尔编码器，而所用的单片机STM32使用3.3V直流电源，系统采用ASM1117-3.3V模块为单片机系统及其他外设提供稳定电压。同时系统采用LM2596稳压模块为姿态检测模块提供稳定工作电压，稳定性高。故系统同时采用LM2596稳压模块为驱动电路提供工作电压，并通过PWM实时控制电机的电压大小。具体电路设计见图3-3。
图中电解电容C11起到滤波稳压的作用，而瓷片电容C1、C2、C3、C21起到辅助滤波，去除电压中的波动毛刺电压的作用，二极管起稳压作用。
3.5 电机驱动模块设计
TB6612和传统的L298N驱动芯片的区别在于，TB6612体积更小，通过大电流MOSFET-H 桥，实现双通道电路输出，可以同步驱动2 个电机。TB6612FNG 输出的连续驱动电流达到最高1A。
AINl／AIN2、BIN1／BIN2是控制信号输入端，其中AINl／AIN2、BIN1／BIN2用于控制电机转动方向，其方式是控制电平高低；PWMA／PWMB用于控制两个电机转动速度，其方式是调节PWMA占空比；AIN1、AIN2、BIN1、BIN2分别连接STM32的PB13、PB12、PB14、PB15；PWMA和PWMB分别接STM32的PA11、PA8；AO1／A02、B01／B02 是2 路电机控制输出端， AO1／A02接在电机1两端、B01／B02接在电机2两端；STBY 作用是正常工作待机状态控制引脚；VM是电机的电源输入端引脚，在这里是12V,VCC引脚是逻辑电平输入端。如图3-4所示。
图3-4 驱动模块
TB6612芯片总共包含有4 种电机控制模式，它们分别是正转、反转、制动、停止。具体实现以上控制的各引脚的配制方法如表3-1所示。

INT1
INT2
PWM
DIRECTION
MOTOR
LOW
HIGH
0停止；255最高
前
MOTOR
HIGH
LOW
0停止；255最高
后
MOTOR
HIGH(LOW)
HIGH(LOW)
0停止；255最高
停止
表3-1 控制逻辑表
4 系统软件部分设计
4.1 软件系统总体结构 
开始
MPU6050传感器初始化、定时器初始化、外部中断串口初始化、PWM初始化
读取MPU6050角度与角速度，并用卡尔曼滤波进行滤波
采集小车速度和位置
对采集到的信息进行处理，并转换成PWM，对电机进行控制

图4-1 主程序流程图
前面的部分主要介绍了整个小车系统的设计思想，硬件电路原理，接下来介绍本设计的软件部分。根据设计要求，设计的系统软件流程如图4-1所示。系统软件流程是再采集完传感器数据后，再处理相关数据，并输出PWM信号控制电机的运行。
整个程序处理过程分为以下几步，第一步，主控芯片STM32首先采集姿态传感器MPU6050的数据，经过卡尔曼滤波，就得到了车身的姿态信息，第二步，根据此角度做PID调节，得到小车两个电机的PWM脉宽，再调整轮子速度，使它的俯仰角为零。之后就是重复采集、处理、调节、处理这一过程。然后，在这个基础上，给两电机加PWM值，就会实现小车的前进，倒退。软件设计流程是：首先单片机初始化，在获取车身姿态数据，经过卡尔曼[[] 付梦印,邓志红,张继伟.Kalman滤波理论及其在惯性导航系统中的应用[M].第二版.北京:科学出版社,2010
]融合算法，PID控制完成小车直立速度控制。
4.2 单片机初始化
开始
上电复位
中断系统初始化
I/O引脚配置
I2C接口配置
串口配置
间隔定制器配置
返回
初始化流程图如图4-2所示，系统使用外部8MHz晶振作为外部时钟源，通过STM32内部的相环使倍频达到72MHz。STM32的两条外设时钟总线分别是APB1和APB2，它们的时钟频率最高分别是36MHz、72MHz。整个初始化步骤是：首先软件设
图4-2 单片机初始化
置外设时钟到最高频率，开启ADC、定时器、DMA、串口和GPIO的时钟源。定时器用于产生控制电机的PWM信号。在程序中设置PWM信号的频率成20kHz和3600级占空比输出。 间隔定时器(Systick)产生8kHz信号用于产生ADC和Kalman采样频率。间隔定时器为一个24位递减定时器，仅有3个寄存器，使用固件库仅需一个语句即可完成配置。
4.3 姿态检测系统软件设计
4.3.1 MPU6050姿态获取方法
MPU6050加速度计部分：获得车身加速度的方法很简单，我们利用MPU6050自带的数字运动处理器DMP，就不再需要考虑复杂的滤波算法，我们就可以直接得到平衡小车的倾角。如图4-3.具体需要我们移植官方DMP 驱动库 ，移动成功之后，便可在主函数中调用库函数Read_DMP( )来读取角度，用来Accel 表示。
经由I2C读取到MPU6050的数字运动处理器DMP输出的四元数是 q30 格式的，要想把DMP输出的四元数换算成欧拉角，就要先变换成浮点数，然后再进行计算，公式如下：
（3-1）

（3-2）
式中quat[x]是四元数，x为0、1、2、3，Q[x]是转化成浮点数之后的四元数，x为0、1、2、3。式3-2是俯仰角的计算方法。平衡车只需要使用俯仰角 pitch ，横滚角Roll、航向角Yaw都用不到。
MPU6050中陀螺仪部分：陀螺仪模块可以实时记录小车摆动时的角速度Gyro 。通过卡尔曼滤波把加速度计测得的角度Accel 与陀螺仪获得的角速度Gyro 融合到一起，得到我们实际想要的更为准确的角度Angle ，角速度Angle_dot 。经卡尔曼滤波祛除了噪音影响的车身姿态信息将利于更好地实现PID调节。
返回
角度、角速度进行卡尔曼滤波
读取MPU6050陀螺仪输出值
开始
移植DMP驱动库，利用库函数Read_DMP（）读取车身角度
图4-3 姿态传感器初始化
4.3.2 卡尔曼滤波算法
我们知道仅仅依靠姿态传感器诸如陀螺仪和加速度传感器，我们就已经能获得校车的姿态信息，但是为什么还要卡尔曼滤波，其原因是在我们只是通过硬件提供的信息的话很难达到所需要的精度，硬件上的工艺水平是有限的，长时间下来，误差会不断地累加，小车也很难在一个较长的时间里保持直立平衡.为了解决这一问题，我们引入了卡尔曼滤波，从软件算法的角度来优化车子的准确性能。卡尔曼滤波器是一种高效率的递归滤波器，它可以从夹杂有噪声的测量中，估算动态系统的状态。包括过去、现在、甚至是下一步的状态信息，针对本设计而言，本设计所使用的惯性器件卡尔曼滤波将更合适。
简单来说，卡尔曼滤波器有预判与更新两个阶段。首先预判时，滤波器依靠前一刻的信息，估算出目前的状态；下一步，滤波器根据目前的值评估下一刻的角度信息。
卡尔曼滤波算法函数void Kalman_Filter(float Accel,float Gyro)的入口参数有两个分别是加速度和角速度，Accel 与Gyro 为测得的角度与角速度值，通过此函数计算滤波后，将得到我们想要的实际值Angle 与Angle_dot[[] 谭浩强.C程序设计[M].北京:清华大学出版社,2005
]。
Angle += K_0 * Angle_err; //这一步是后验估计
Angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
设置卡尔曼滤波参数如 Q_angle为0.01;Q_gyro为0.01;R_angle为0.003;Dt为0.005;char C_0 为 1;
Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
要想改变kalman 滤波器采样时间，改变上述程序中的dt即可。
4.4 平衡PID控制软件实现 
这里首先形象的分别说明一下P、I、D控制的区别，以利于我们建立起感性认识P为比例控制，使以最快的速度达到目标。I为积分控制，使误差为0而起调和作用。D为微分控制，可以加快调节进程。
4.4.1 直立环PD控制
平衡小车直立环使用PD控制，既是比例微分控制，其实像是一般的控制系统只需要P 控制或者PI 控制就已经能满足要求，但是如果需要我们干扰迅速响应的控制过程就需要D微分控制。
直立PD控制函数的入口参数：角度、角速度，返回值：直立控制PWM。
int balance(float Angle,float Gyro)
首先，要计算车身角度和预先设定需要小车保持平衡所需角度的偏差，用以下语句即可实现，其中Bias既是所要用到的偏差值。
Bias=Angle-ZHONGZHI;
求出平衡的角度中值和机械相关，本设计中，假设MPU6050已经完全垂直水平安装，那么中值既是0。
balance=kp*Bias+Gyro*kd;
计算平衡控制的电机的PWM ，其中kp是P系数 kd是D系数。
最后还要把pwm值返回给直立pwm：Balance。
到这里，就完成了对直立环的控制。
4.4.2 速度环PI控制
平衡小车速度环使用PI控制[[] 李春文，冯元坤，多变量非线性控制的你系统方法[M].北京：清华大学出版社，1991
]，既比例积分控制，速度控制使用频率最高的既是PI控制。PI 控制依照目标值和实际输出值的偏差，线性融合偏差的比例和积分，实现了小车模型的直立控制，下面是PI 控制函数。
函数的 velocity( )入口参数是小车左右电机的编码器提供的值 encoder_left,int encoder_right。
if(1==Flag_Qian) Movement=-90; //===如果前进标志位置1 位移为负
else if(1==Flag_Hou) Movement=90; //===如果后退标志位置1 位移为正
else Movement=0;
获取最新的速度偏差。
Encoder_Least =(Encoder_Left+Encoder_Right)-0;
对速度偏差进行低通滤波。
Encoder *= 0.7;
Encoder += Encoder_Least*0.3;
通过对偏差积分得到位移。
Encoder_Integral +=Encoder;
Encoder_Integral=Encoder_Integral-Movement; //接收遥控器数据，控制前进后退
if(Encoder_Integral>10000) Encoder_Integral=10000; //积分限幅
if(Encoder_Integral<-10000) Encoder_Integral=-10000; //限制遥控最大速度
使用速度PI 控制器计算速度控制的PWM。
Velocity=Encoder*kp+Encoder_Integral*ki;
到这里，就完成了对速度环的控制。
小车能够站立后，要实现对小车运动的控制，我们放弃了使用WIFI、红外线控制等，而是采用了目前较为热门的蓝牙(HC-06)来实现，借助于已有的安卓客户端APP，在硬件模块上搭载相应的蓝牙模块。
实验现象:蓝牙连接后，通过手机来控制小车的前进，后退，直立。主要蓝牙控制程序如下：
if(USART2_RX_STA&0X8000) //接收到一次数据了
LED =! LED;
得到蓝牙发送的数据。
reclen = USART2_RX_STA&0X7FFF;
加入使蓝牙结束的结束符。
USART2_RX_BUF[reclen] = 0;
判断接收的数据，并做出相应动作。
u2_printf("C%s\r\n",USART2_RX_BUF);

5  系统调试
5.1 角度矫正
本设计中姿态检测模块的调试是重点内容。首先，需要调试陀螺仪及加速度计，看是否正常，为了保证陀螺仪输出信号的准确性，需保持陀螺仪模块电路的偏置电压的稳定，才能使整个电路的温漂最小。将小车倾角分别置于0°、45°及90°，比上位机和实际倾角，及时调整。观察数据发现陀螺仪只能在短时间里显示较为准确的值，如果时间较长，陀螺仪计算出的角度会有误差。而对于加速度计，长时间测量会因震动而不准确。
5.2 卡尔曼滤波调试
通过以上分析说明了采用多传感器数据融合[[] 王晓宇,闫继宏,臧希喆等.两轮白平衡机器人多传感器数据融合方法研究[J].传感器技术学报．2007
]的必要性。姿态检测系统中的重点必然又是卡尔曼滤波。为了方便调试，小车经串口[[] 耿延睿,崔中兴.组合导航系统卡尔曼滤波衰减因子自适应估计算法研究[J].中国惯性技术学报.2001
]向PC机发滤波前后倾角数据，通离散的数据点描绘可以直接对比滤波效果。卡尔曼滤波整定参数有：卡尔曼增益、加速度计和陀螺仪权值。经过滤波之后，含有噪声的波形变得平稳。
5.3 PID参数的整定
5.3.1 PD控制调试
首先要确定平衡小车的机械中值，一般在0°附近。
其次是确定kp 值的极性（令kd=0）。如果给定值之后，小车往哪边倒，电机会往那边加速让小车到下，说明kp 极性反了。
再确定kp值的大小，让kp 一直增加，直至低频抖动。确定之后，就需要增加微分控制削弱来P控制。
确定kd 值的极性（令kp=0）当我们旋转小车的时候，车轮会同向以相同的速度跟随转动，这说明我们实现了角速度闭环，至此，我们可以确定kd 的极性是正的。确定参数的原则是：kd 一直增加，直到出现高频抖动。
完成以上步骤，就完成了PD的调试。
5.3.2 PI控制调试
首先计算速度偏差，偏差=测量值-目标值。
再确定kp 与ki 值的极性，根据经验，一般我们可以把ki 值设置为ki=kp/200;这样，只要我们可以得到kp 值的大小和极性，就可以完成速度控制部分的参数整定了。这样就可以缩短了PID 参数整定的时间。根据7200 代表占空比100%，我们可以大概估算kp 最大值=7200/(160*50%)=90。
当小车以一定的速度运行的时候，我们要让小车停下来，小车需要行驶更快的速度，这是典型的正反馈[[] 谢克明.自动控制原理[M].北京:电子工业出版社,2009
]，果，也是我们期望看到的。至此，我们可以确定kp,ki
的符号应该是正的，我们可以确定kp,ki的符号应该是正的。设kp=80,ki=kp/200 时，小车可以保持直立。
完成以上步骤，就完成了PI的调试。

6 结论
整个平衡车设计期间，总是认为算法最复杂，也是最为困难的部分，其实在真正进行设计时，恰恰是这个最复杂的部分，反而我们最容易解决最先的问题。设计过程有几个难点：一是传感器的数据处理。传感器容易受震动影响，易