两轮自平衡小车论文.

1、2013年全国大学生电子设计竞赛两轮自平衡小车设计作者：杨魏，黄敏杰，夏俊逸2015.7.17ii本文采用自制的两轮简易小车作为试验平台，以 MEM传感MPU605（为传感器的姿态 感知系统，通过离散卡尔曼滤波器对两种传感器的数据进行滤波融合，选用32位单片机STM32F103R为控制核心处理器，完成对数据的采集处理和车身控制，采用 PID控制算法 实现小车两轮自平衡。用蓝牙控制前后运动。实验结果验证了该系统的性能满足设计要求。关键词：两轮自平衡；姿态感知；STM32F103RB卡尔曼滤波；PID控制41系统方案11.1 姿态检测模块的论证与选择 11.2 电机驱动模块的论证与选择 12系统硬

2、件设计12.1 STM32F103RB 单片机系统22.1.1 STM32F103RB 单片机介绍22.1.2单片机最小系统设计 32.1.3电源管理模块设计 42.2姿态检测模块 MPU-605052.2.1 MPU-6050 简介52.2.3数字运动处理器（DMP 62.3速度检测模块设计 72.3.1编码器介绍72.3.2 编码器电路设计82.4 电机驱动模块82.4.1 L298N 简介82.4.2 L298N 特点93理论分析与计算93.1两轮平衡小车数学模型 93.2 PID 控制器设计 103.2.1 PID 控制器原理103.2.2 PID 控制器设计 113.2.3 PID

3、程序123.3基于卡尔曼滤波的数据融合 134系统软件设计 154.1系统软件设计框架 154.2 资源模块初始化 154.3两轮小车姿态信息检测 165测试方案与测试结果 165.1测试方案165.1.1硬件连接检测 165.1.2小车功能检测165.2 测试分析与结论 161系统方案本系统主要由姿态检测模块、电机驱动模块、蓝牙模块、红外对管模块、电源模块组成, 下面分别论证这几个模块的选择。1.1姿态检测模块的论证与选择方案一：ENC-03 种普通陀螺仪。方案二：MPU-6050 MPU-60X0是全球首例9轴运动处理传感器。它集成了 3轴MEM陀螺 仪，3轴MEMS卩速度计，以及一个可扩

4、展的数字运动处理器DMP（ Digital MotionProcessor），可用I2C接口连接一个第三方的数字传感器，比如磁力计。扩展之后就可以通过其I2C或SPI接口输出一个9轴的信号（SPI接口仅在MPU-600C可用）。MPU-60X0 也可以通过其I2C接口连接非惯性的数字传感器，比如压力传感器。综合以上两种种方案，选择方案二。1.2电机驱动模块的论证与选择方案一：TB6612FNG体积小，但容易坏。方案二：L298N具有信号指示，转数可调，抗干扰能力强，具有过电压和过电流保护，可单独控制两台直流电机，可单独控制一台步进电机。综合以上三种方案，选择方案二。2系统硬件设计本文设计的两轮

5、自平衡小车采用姿态传感器（MPU605）监测车身所处的俯仰状态和状态变化率，通过高速微控制器STM32F103R完成数据融合处理，得到平滑而稳定车体姿 态信息，然后驱动电动机产生前进或后退的加速度来控制车体保持平衡，红外线检测障碍 物，蓝牙控制前进后退。结构框图如图。20主控制器STM32F103RBL29SX电机顋动蓝牙红外线图1系统设计总体结构框图系统硬件主要由STM32F103R单片机系统、电源模块、姿态检测模块、串口通讯模块、 电机驱动模块等组成。2.1 STM32F103RB单片机系统本设计中控制平台采用 ST意法半导体的STM32F103R微控制器，STM32系列基于专为要 求高性

6、能、低成本、低功耗的嵌入式应用专门设计的ARM CORTEX-M内核，其最高主频72MHz在满足平衡控制的调节频率的同时有更多的CPU资源，工作电压为5 V。其丰富的内置模块资源可以为姿态检测系统提供 12位精度的A/D转换，同时使用定时器可以产 生PWM&出控制电机转速和转向。2.1.1 STM32F103RB 单片机介绍STM32F103xB增强型系列使用高性能的 ARM? Cortex?-M3 32位的RISC内核，工作频 率为72MHz内置高速存储器（高达128K字节的闪存和20K字节的SRAM）丰富的增强I/O 端口和联接到两条APB总线的外设。所有型号的器件都包含2个12位的ADC

7、 3个通用16 位定时器和1个PWME时器，还包含标准和先进的通信接口 ：多达 2个I2C接口和SPI接 口、3 个 USART接 口、一个 USB接 口和一个 CANS 口。STM32F103xx中等容量增强型系列产品供电电压为 2.0V至3.6V，包含-40 C至+85 C温度范围和-40 C至+105 C的扩展温度范围。一系列的省电模式保证低功耗应用的要 求oSTM32F103x中等容量增强型系列产品提供包括从 36脚至100脚的6种不同封装形式； 根据不同的封装形式，器件中的外设配置不尽相同。下面给出了该系列产品中所有外设的 基本介绍。这些丰富的外设配置，使得 STM32F103X）产

8、品容量增强型系列微控制器适合于 多种应用场合：电机驱动和应用控制医疗和手持设备 PC游戏外设和GPS平台工业应用：可编程控制器（PLC）、变频器、打印机和扫描仪警报系统、视频对讲、和暖气通风空调系统等2.1.2单片机最小系统设计本设计采用STM32F103R单片机为控制器，最小系统原理图如图 3-3所示，主要包括 单片机供电、复位电路、时钟电路以及 BDM接 口电路。系统时钟的选择是在启动时进行，复位时内部 8MHZ的RC振荡器被选为默认的CPU时 钟，随后可以选择外部的、具失效监控的 416MHZ寸钟；当检测到外部时钟失效时，它将 被隔离，系统将自动地切换到内部的 RC振荡器，如果使能了中断

9、，软件可以接收到相应 的中断。同样，在需要时可以采取对 PLL时钟完全的中断管理(如当一个间接使用的外部 振荡器失效时)。多个预分频器用于配置 AHB的频率、高速APB(APB2和低速APB(APB1区 域。AHB和高速APB的最高频率是72MHz低速APB的最高频率为36MHzSBfJJM PA；C4 匹pay1亡2丁M1PA42CarPACPAtPA2PA3PA4 PA! PASPATM PAS 41初札2 贮M PA1CI C：jT3 yiz :J3 30044 jfPCUSTM3：FISJMT6图3-3单片机最小系统及时钟电路原理图单片机的外部复位电路可以使用按钮和电容构成，也可以使用

10、专门的复位芯片。考虑到产品经济型，本设计中采用了由电阻电容构成的简易复位电路，如图3-5所示。加电后，由于电容的充电时间，RESET保持低电平，单片机复位；一段时间后，电容电量充满，RESET端输出高电平，此时单片机运行。手动复位时，按下手动复位按钮，RESETS保持低电平,单片机复位；释放手动复位按钮后，RESETS输出高电平，单片机工作。图3-5复位及按键接口电路2.1.3电源管理模块设计可靠的电源方案是整个硬件电路稳定可靠运行的基础。电源模块由若干相互独立的稳压电路模块组成。这样做可以减少各模块之间的相互干扰。整个系统需要3种电源： VDD = 2.03.6V: VDD引脚为I/O引脚和

11、内部调压器供电。 VSSA VDDA = 2.03.6V:为ADC复位模块、RC振荡器和PLL的模拟部分提供供电。 使用ADC时，VDDAF得小于2.4V。 VDDA和VSSA必须分别连接到 VDDffi VSS VBAT = 1.83.6V:当关闭VDD寸，（通过内部电源切换器）为RTC外部32kHz振荡器 和后备寄存器供电。电源SL-5V1jJOCJtST SPST* Cl图3-6电源模块电路图2.2姿态检测模块MPU-6050姿态检测模块能够精确并稳定地检测出当前的车身姿态，这是本设计两轮自平衡小车实现有效控制的关键。本设计选用MEMS专感MPU6050作为姿态检测传感器MPU-605C

12、实物连接图2.2.1 MPU-6050 简介MPU-60X0是全球首例9轴运动处理传感器。它集成了 3轴MEM陀螺仪，3轴MEMS 加速度计，以及一个可扩展的数字运动处理器DMP（ Digital Motion Processor ），可用I2C接口连接一个第三方的数字传感器，比如磁力计。扩展之后就可以通过其I2C或SPI接口输出一个9轴的信号（SPI接口仅在 MPU-600C可用）。MPU-60XC也可以通过 其I2C接口连接非惯性的数字传感器，比如压力传感器。MPU-60X0对陀螺仪和加速度计分别用了三个 16位的ADC将其测量的模拟量 转化为可输出的数字量。为了精确跟踪快速和慢速的运动，

13、传感器的测量范围都是用户 可控的，陀螺仪可测范围为土 250， 500， 1000， 2000 /秒（dps），加速度计可 测范围为土 2， 4， 8， 16g。一个片上1024字节的FIFO,有助于降低系统功耗。和所有设备寄存器之间的通信 采用400kHz的I2C接口或1MHz的SPI接口（ SPI仅MPU-6000可用）。对于需要高 速传输的应用，对寄存器的读取和中断可用 20MHz的SPI。另外，片上还内嵌了一个温度传感器和在工作环境下仅有土 1%变动的振荡器。芯片尺寸 4X 4X 0.9mm采用QFN封装（无引线方形封装），可承受最大 10000g的冲击，并有可 编程的低通滤波器。关于

14、电源，MPU-60X0可支持VDD范围2.5V 5% 3.0V 5% 或3.3V 5% 另外 MPU-6050还有一个VLOGIC引脚，用来为I2C输出提供逻辑电平。VLOGIC电压可取 1.8 5%或者 VDDMPU6050电路图2.2.3数字运动处理器（DMPDMP就是指MPU605C内部集成的处理单元，可以直接运算出四元数和姿态，而不再需要另 外进行数学运算。DM啲使用大大简化了四轴的代码设计。DMP是数字运动处理器的缩写，顾名思义mpu6050并不单单是一款传感器，其内部还包含了可以独立完成姿态解算算法的 处理单元。如在设计中使用 DMPin ve nse nse官方提供的姿态解算算法

15、应该比像楼主这样的小白要可靠的多。其次，由DMP实现姿态解算算法将单片机从算法处理的压力中解放出来，单片机所要做的是等待DMP解算完成后产生的外部中断，在外部中断里去读取姿态解算的结果。这样单片机有大量的时间来处理诸 如电机调速等其他任务，提高了系统的实时性。DMP从陀螺仪、加速度计以及外接的传感器接收并处理数据，处理结果可以从DMP寄存器读出，或通过FIFO缓冲。DMPt权使用MPU勺一个外部引脚产生中断。2.3速度检测模块设计两轮自平衡小车的原理是利用地面对车轮的摩擦力抵消车受到的重力，在本系统的控制环节中有两路闭环控制，即倾角闭环控制以及速度闭环控制。为实现速度的闭环控制，必须加入速度检

16、测装置实现速度闭环控制中的反馈环节。本系统测速模块采用OMRO欧姆龙）公司500线增量式旋转编码器如图3-112.3.1编码器介绍编码器内部为一个中心有轴的光电码盘，其上有环形通、暗的刻线，有光电发射和接 收器件读取,获得四组正弦波信号组合成 A B C D,每个正弦波相差90度相位差（相对 于一个周波为360度），将C、D信号反向，叠加在A、B两相上，可增强稳定信号；另每 转输出一个Z相脉冲以代表零位参考位。由于A B两相相差90度，可通过比较A相在前还是B相在前，以判别编码器的正转 与反转，通过零位脉冲，可获得编码器的零位参考位如图3-12。旋转方向：CCW （从轴侧看：左旋转）A柑日相O

17、N： H；*OFFfLJ ONlHJOFFCL)rrLjr-L一H-1 ,;4T1011945图3-12编码器A、B相输出波形本系统用OMRC旋转编码器相关参数如表3-1所示：表3-1编码器参数表项目参数电源电压DC 5V12V纹波的峰峰值小于5%消耗电流30mA以 下分辨率500脉冲/转输出相A B、Z三相输出状态集电极开路输出最高输出响应42kHz输出上升或下降时间1us以下起动转矩1mN- ml以下轴允许力径向10N、轴向5N允许最咼转速5000r/mi n质量35g2.3.2编码器电路设计由于编码器采用集电极开路输出，输出波形为矩形波，因此编码器外围电路较为简单。 需要在信号输出端接入

18、一个上拉电阻，即可将信号提供给单片机采集数据。如图3-13所示，PULSE引脚为编码器A相，接单片机的脉冲计数口，通过单片机的PACN模块对输入脉冲进行计数从而获取电机转速。DIR为编码器B相输出，接单片机I/O口，通过A、B相位差进行软件鉴相，从而判断电机转动方向。图3-13编码器接口电路2.4电机驱动模块电机的驱动芯片选用L298N作为驱动芯片。2.4.1 L298N 简介L298是SGS公司的产品，内部包含4通道逻辑驱动电路，是一种两相和四相电机的专用驱动器，即内含两个H桥的高电压大电流双全桥式驱动器，接收标准的TTL逻辑电平信号，可驱动46V、2A以下的电机。本设计中直流电机的驱动信号

19、由STM32F103R提供，信图6理论分析与计算2.4.2 L298N 特点基本特点如下：（1）具有信号指示。（2）转数可调。（3）抗干扰能力强。（4）具有过电压和过电流保护（5）可单独控制两台直流电机（6）可单独控制一台步进电机（7）PWM脉宽平滑调速。（8）可实现正反转。（9）米用光电隔离。3理论分析与计算3.1两轮平衡小车数学模型已知自平衡车高度为I，质量为m,将其抽象为一级倒立摆，并将倒立摆至于可水平移动的小车上。假设其受外力干扰引起的车体角加速度为x（t），沿垂直于车体方向进行受力分析如图2-7，可以得到自平衡车倾角与车轮移动加速度为a（t）以及外力干扰带来的加速度x(t)之间的运动

20、方程图2-7外力干扰条件下的小车受力分析小车运动微分方程表达式如式2-8 :tdt2= gsinfita t cosFtlx t（式 2-8）当倾角二很小的时候，可以进行线性化处理：cos(、1、sin(R ：宀，运动微分方程可简化为:d勺tdt2=g t -at lx t（式 2-9）自平衡车静止时a t =0,其运动微分方程为:d2, t dt2（式 2-10）3.2 PID控制器设计3.2.1 PID控制器原理当今的自动控制技术都是基于反馈的概念。反馈理论的要素包括三个部分：测量、比 较和执行。测量系统需要控制的变量，与期望值相比较，用这个误差纠正调节控制系统的 响应。在工程实际中，应用

21、最为广泛的调节器控制规律为比例、积分、微分控制，简称PID控制，又称PID调节。PID控制器问世至今已有近70年历史，以其结构简单、稳定性好、 工作可靠、调整方便而成为工业控制的主要技术之一。PID控制器由比例单元（P）、积分单元（I ）和微分单元（D）组成。其输入e （t） 与输出u （t ）的关系为：一1 tde（t ）1u（t ）=Kp t-iQt）dtTD（式-Ti 0dt 一2-11）其中Kp为比例系数；Ti为积分时间常数；Td为微分时间常数。PID控制器具有原理简单、使用方便、适应性强、鲁棒性强、对模型依赖少等特点， 因此使用PID控制器实现两轮自平衡车的控制是完全可行的。3.2.

22、2 PID 控制器设计由小车静止时其运动方程可得到系统输入输出传递函数:H（s）=妙=匕（式 212）X（s）二 g（式2-12）s _l此时系统具有两个极点：Sp = , g。其中一个极点位于s平面的右半平面。根据奈 奎斯特稳定判据可知系统不稳定，因此小车在静止状态不能保持平衡0由小车受力分析可知小车平衡的条件是提供额外的回复力及阻尼，其来源为车轮与地 面的摩擦力。由式2-7可知，车轮提供的加速度的大小是根据角度 二及角速度J的反馈得 出,因此需要在控制系统中引入角度 二及角速度丁构成比例（P）微分（D）反馈环节，如图2-8 所示。图2-8加入比例微分环节后的控制系统结构图加入比例微分反馈后

23、的系统传递函数为：。（S）1H s（式X（s）孑丄匕才匕謳（式I I2-13）此时，系统的两个极点为s = 一k2*Jk；4|（kig）。根据奈奎斯特稳定判据可知，p2I系统稳定需要两个极点都位于 s平面的左半平面。要满足这一点，需要 &g, k20。由此 可得出结论，但ki g, k20时，小车可以保持平衡，这也与上文中小车受力分析的结果相 符。在反馈环节中，与角度二成比例的控制量称为比例控制；与角速度成比例的控制量称 为微分控制（角速度是角度的微分）。因此上面系数ki，k2分别称为比例和微分控制参数。 其中微分参数相当于阻尼力，可以有效抑制自平衡车振荡。控制系统的输出量为电机控制量，因而小

24、车平衡控制的PID控制器的输出方程可写为：OUT_Motor=Kp*Angle+Kd*Angle_dot（式 2-14）式2-14中，OUT_Motor为PID控制输出量，Angle为反馈倾角值，Angle_dot为反馈 角速度值，Kp和Kd分别为比例系数及微分系数。3.2.3 PID 程序#in clude #in clude math.h#i nclude vpwm.hfloat PID_realize（）;void pwm（void）;exter n float Pitch;float SetSpeed=-1.04;定义目标值float ActualSpeed=0.0;反馈值float

25、err=0.0;偏差值float err_last=0.0;上次偏差值float Kp=135;float Ki=2.7;float Kd=135;float output=0.0;电压值控制执行器的变量float in tegral=0.0; 积分量float PID_realize()ActualSpeed=Pitch;err=SetSpeed-ActualSpeed;in tegral+=err;output=Kp*err+Kd*(err-err_last)+Ki* in tegral; err_last=err;return output;3.3基于卡尔曼滤波的数据融合卡尔曼滤波器是一

26、种高效率的递归滤波器（自回归滤波器），能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波器不仅能估计信号的过去和当前状态，甚至能估计将来的状态。卡尔曼滤波器解决离散时间控制过程的一般方法，首先定义模型线性随机微分方程。假设卡尔曼滤波模型k时刻真实状态是从（k-1 ）时刻推算出来，如下式人二 AXk Buk Wk（式2-15）式2-15中，xk是k时刻状态；A是k-1时刻状态变换模型；B是作用在控制器向量 山上 的输入控制模型；Wk是过程噪声，假设其均值为零，协方差矩阵 Qk符合多元正态分布： Wk N（0,Qk）（式2-16）k时刻对应真实状态Xk的测量Zk满足下式：Zk

27、= HkXk Vk（式2-17）式2-17中Hk是观测模型，将真实控制映射为观测空间；Vk为观测噪声，其均值为零,式 2-18)协方差矩阵Rk符合正态分布：VkN(O,R)初始状态以及每一时刻的噪声都认为是互相独立的卡尔曼滤波器的操作主要包括两个阶段：预估与更新。在预估阶段，滤波器根据上 时刻状态，估算出当前时刻状态；在更新阶段，滤波器利用当前时刻观测值优化在预估阶段获得的测量值，以获得一个更准确的新估计值冋1213 卡尔曼滤波器迭代过程如下：1. 先验状态估计：?k|kAXk_i BUk_i2. 先验估计误差协方差Pk |k_1 二 AkPk_lAQ3.卡尔曼增益KPkk-iHTHPk k_iHT R2-21)4.后验状态估计?k吆|k心夂沖?十)(2-22)5.后验误差协方差Pk=(1 - H)Pk k-1(2-23)在上面各式中：A:作用在上的n阶矩阵;B:作用在控制向量 山4上的nx 1输入控制矩阵；H: mx n观测模型矩阵，将真实状态空间映射为观测空间;Pk|k4 : nx n先验估计误差协方差矩阵；R : n x n后验估计误差协方差矩阵;Q: nx n过程噪声协方差矩阵；R: mx m过程噪声协方差矩阵；I : n阶单位矩阵；Kk : nxm矩阵，称之为卡尔曼增益4系统软件设计4.1系统软件设计框架系统软件设