程伟届毕业论文精选

1、机械工程学院毕业设计（论文）题 目： 基于视觉导航两轮平衡车的设计 专 业： 机械电子工程 班 级： 11机械电子工程113班 姓 名： 程 伟 学 号： 2206110103 指导教师： 姚 智 华 日 期： 2014.11.26 基于视觉导航两轮平衡车的设计摘 要：本论文旨在研制一种单轴双轮平衡车并实现视觉导航辅助驾驶。该系统的研究分为两部分，第一部分就是通过姿态传感器和姿态算法，补偿陀螺仪漂移误差和加速度计动态噪声误差得到系统角度角速度以达到平衡直立的目的；第二部分就是关于机器视觉的研究和视觉导航和辅助驾驶。第一部分：双轮平衡车实现原理就是通过系统的姿态传感器（陀螺仪和加速度计）监测车体

2、当前的俯仰状态和俯仰变化率状态（即为角度和角度度），由MCU计算处理驱动电动机向前或者向后的加速度实现车体平衡的效果。对于姿态监测，单独使用角速度传感器（陀螺仪）或者加速度传感器都是不能提供有效的信息保持车体直立，本论文基于互补滤波通过角速度传感器和加速度传感器进行角度融合，补偿陀螺仪漂移和加速度传感器动态噪声，估计最优俯仰状态。对于车体的直立控制、速度控制和方向控制，我们都要求准确性、稳定性、强劲性等特点，应用最普遍的就是PID算法。本论文通过车体当前运行状态下的直立偏差、速度偏差、方向偏差的比例、积分和微分（即为PID调节器），配合控制理论线性系统叠加原理最终对车体的左右电机进行控制。第二

3、部分：本论文通过CMOS摄像头采集路况实时信息，并对实时路况信息进行运算处理，识别出道路元素最终以车体转向的方式达到视觉导航。关键字： 平衡车 陀螺仪 加速度计 互补滤波 PID CMOS目录一、绪论：41.1研究背景：41.2研究意义：41.3现状分析：41.3.1国外现状：41.3.2国内现状：61.4本论文研究的内容：8二、整体方案设计：82.1整体概述：92.2硬件设计：102.2.1电源设计：102.2.2复位电路设计：112.2.3MCU电源设计：122.2.4调试接口JTAG电路设计：132.2.5电机驱动模块电路设计：142.2.6陀螺仪加速度计电路设计：172.2.7CMOS

4、摄像头电路设计：202.3软件设计：222.3.1MCU需要使用的资源配置：222.3.2PWM（Pulse Width Modulation）输出模块：232.3.3陀螺仪加速度计采集模块：252.3.4周期中断定时器模块：272.3.5互补滤波算法及平衡控制模块：282.3.6速度采集及速度控制模块：392.3.7CMOS摄像头采集处理算法及转向模块：492.4人机接口及调试接口调试软件设计：54三、总结与致谢：58参考文献59一、绪论：随着国民生活质量的提高、国内高新技术的发展，特别是嵌入式技术和机器人技术的发展，人民迫切希望更加智能更加方便的交通工具得到发明。为此，本论文研究一种单轴双

5、轮平衡代步车以及视觉导航技术实现辅助驾驶。1.1研究背景：近年来，国内机器人技术空前发展，机器人应用的领域也在不断扩大，当然机器人所面临的环境和所需要执行的任务也是越来越复杂。特别是在一些危险地带或者空间狭小人类无法触及的地方，机器人是一个很好的选择。无论是因为何种目的而设计的机器人都是朝着人民生活水平不对安提高的层次进行，为此一种两轮平衡代步车的概念被提出。1.2研究意义：两轮代步车是一个多变量、非线性的复杂不稳定系统。其运动环境复杂、运动学方程非完整约束，控制任务具有一定的复杂性。但是双轮代步车具有结构简单、运动灵活、适合在小空间内运动等特点，具有一定的应用前景。为了让代步车朝着智能化发展

6、，迫切需求其可以辅助驾驶，甚至自动驾驶，为此我们需要研究机器视觉及图像处理方面的理论。1.3现状分析：1.3.1国外现状：2002年，没过Lego公司设计了Legway。引入了电机差动驱动，可以再斜面或者不规则表边上运动。可以遥控操作，通过对电机的遥控可以再运动的时候保持平衡并可以实现原地转弯。2007年，日本早稻田大学研制出了NXTway。2002年，美国Segway公司开发了世界上第一部能够自平衡的两轮电动车。该电动车精心设计了机械机构并通过检测车体的角度和角速度，通过CPU处理运算控制电机前后加速运动的回复力保持车体平衡。Segway使用的是航空级陀螺仪、一组倾斜传感器、一套复杂的“直觉

7、软件”、一个加速度计、十个微处理器、两个镍氢电池组、一台电动机和每秒检测一百多次驾驶者重心的传感器。Scgway共有五个陀螺仪传感器，其中三个来检测前、后和左右方向的倾斜，另外两个传感器作为冗余，以增加安全性。传感器收集的信息被传送到主控芯片，它由两个电子控制器电路板构成。电路板包括一个微处理器的集群(德州仪器的DSP芯片)。这种集群的运算效率是普通PC的三倍，以保证车身能得到及时而高度精确的微调来防止倾倒。如果一个电路板出了毛病，另一个会立即接管所有的功能，以便让驾驶者能够安全地停下。在最高层次，主控芯片运行“直觉软件”控制Segway，软件会实时分析所有信息，调整电动机的速度。1.3.2国

8、内现状：中国科学技术大学研究出了自平衡两轮代步电动车，它是一种两轮式左右并行布置结构的具有自平衡系统的电动车。在车体内嵌入式CPU的控制下，采集平衡传感器以及速度、加速度传感器的数据，通过一定的控制算法，计算输出PWM信号控制两个伺服电机的转矩，使车体保持平衡并能够根据人体重心的偏移，自动前进、后退及转弯。 哈尔滨工程大学也有类似的双轮直立自平衡机器人，该系统采用两块Cygnal公司推出的C8051单片机和人机交互的上位机作为控制核心。车体倾斜角度检测采用AD公司推出的双轴加速度传感器ADXL202及反射式红外线距离传感器。利用PWM技术动态控制两台直流电机的转速。上位机与机器人间的数据通信采

9、用迅通生产的PTR2000超小型超低功耗高速无线收发MODEM。人机交互界面采用240*128图形液晶点阵、方向摇杆及按键。基于这些完备而可靠的硬件设计，使用了一套独特的软件算法，实现了该系统的平衡控制。同时，关于辅助驾驶技术，国内也有很好的发展。远光灯辅助系统：当摄像系统发现前方800m内没有其他车辆时，则自动地转换到远光灯行驶。若再次有车辆进入这一范围时，则远光灯的照射距离又会自动缩小。车道偏离报警和车速限制识别系统：在车速监控中，计算机根据摄像数据提供路边限速标志的图像。它检测树立在路边的所有圆形标牌，而把不是圆形的标牌图像过滤掉，然后对剩下的图像数据与保存的限速标志图像进行对比，最后只

10、留下带有车速限制的交通标志。辅助系统马上把车速限制的图像显示出来，而且显示的时间长短根据限速标志的有效区间而定。在卫星导航系统的电子地图上，将对各个居民点进行测定，当检测到进入居民点后即自动地显示出允许的车速，如50km/h即使是没有明显的限速标志时。车道偏离报警系统用摄像机不断地采集着道路标线。车载计算机计算着车辆的位置，并判断车辆是否会偏离车道。当驾驶员感觉到手中的方向盘在振动时，表示车道偏离系统在报警。1.4本论文研究的内容：1、 系统硬件选择电路原理图搭建。2、 电机控制技术PWM和算法PID的研究和在平衡车系统中的应用。3、 姿态传感器的使用，包括陀螺仪加速度计的介绍和使用方法的研究

11、。4、 姿态算法的研究，包括互补滤波将传感器数据融合得到可靠的倾角估计的算法研究。5、 CMOS摄像头的介绍和使用以及算法的研究。6、 CMOS摄像头在视觉导航中的使用及研究。7、 系统整体的软件设计平衡系统的控制软件设计。二、整体方案设计：在整体方案设计中，分为这么几个模块：1、 硬件方案设计：硬件方案包括电源设计、MCU最小系统及外围电路设计、电机驱动设计、陀螺仪加速度计传感器外围电路设计、CMOS摄像头设计、人机接口电路设计（包括输入设备和输出设备）。2、 软件方案设计：软件方案设计包括MCU的配置、传感器陀螺仪加速度计的信号采集、数据融合算法设计、PWM技术的软件实现、PID算法实现、

12、CMOS摄像头软件设计、视觉导航算法设计。3、 人机接口及调试接口调试软件设计：包括输入输出设备（按键、TFT液晶等）和PC上位机软件设计。2.1整体概述：对于基于视觉导航的两轮平衡车，我们可以对整个系统进行研究将系统分为几个模块实现模块化研究，使系统的稳定性得到大幅提高。我们可以将车体分为平衡系统、速度系统和转向系统。对于平衡系统，我们可以控制左右电机的正反向加速运动使系统保持平衡。对于速度系统，我们需要对系统的速度进行测量并精确控制系统的速度，实际上是调节系统的姿态实现速度控制，最后还是演变为通过控制电机实现控制速度。对于转向系统，通过控制左右电机的差速，控制系统的转向。2.2硬件设计：2

13、.2.1电源设计：电源模块是整个系统比较重要的单元，如果电源不稳定就可能导致系统死机、复位等症状。根据此系统的需要，系统由24V电源供电，本系统需要三种电压，分别是电机供电24V、MCU供电陀螺仪加速度计供电3.3V、编码器等供电5V。主要设计如下：1、 采用稳压芯片LM2940将24V电源电压稳压至5V，给摄像头、编码器等外围设备供电。2、 采用稳压芯片LM1117将5V电源稳压至3.3V，给MCU、陀螺仪加速度计等供电。3、 本供电系统同时采用了大量电容滤波，保证电源电压稳定，防止系统崩溃。MCU最小系统及外围电路设计：MK60FX512VLQ15 MCU简介：MK60FX512VLQ15

14、 是以ARM Cortex-M4F内核设计，外围设备丰富，具有High speed analog-to-digital converter、FlexTimers、Low power timer、Periodic interrupt timer、CAN、Serial peripheral interface、Inter-integrated circuit (I2C)、UART等设备。是飞思卡尔公司专业汽车电子嵌入式MCU。ARM Cortex-M4F Core是ARM公司基于ARMv7构架的内核，精简指令集（RISC）具有DSP内核和FPU硬件浮点单元。对于复杂的控制算法具有很好的运算性能。2

15、.2.2复位电路设计：MK60FX512VLQ15 MCU是低电平复位，因此复位电路设计如下：本系统复位电路采用简单的阻容复位电路，应用简单。阻容复位可以再系统上电的时候自动复位MCU，同时在系统死机或者想要系统重新启动的时候可以按下复位按键使系统复位。具体原理如下：由于阻容串连电路中电容C46两端电压不能突变，因此在上电时，RST端会维持一段时间的低电平起到低电平复位信号的作用，随着v3.3电源通过电阻R23向电容C46充电，C46两端的电压差逐渐增大，经过一段时间后变为高电平，上电复位信号结束。当需要手动复位时按下按键S1，电容C46两端被短路放电，按键松开后RST端仍会维持一段时间的低电

16、平起到低电平复位信号的作用，随着Vcc电源通过电阻R23向电容C46充电，C46两端的电压差逐渐增大，经过一段时间后变为高电平，手动复位信号结束。2.2.3MCU电源设计：MK60FX512VLQ15 MCU是3.3V供电，因此电源设计如下：通过稳压芯片LM2940将电源电压稳压至5V再通过LM1117将5V稳压至3.3V给MCU供电。晶体电路设计：MK60FX512VLQ15 MCU时钟电路包括两部分，一个是芯片的主晶振给PLL提供时钟，用于产生芯片和外设所需要的工作时钟；另外一个是实时时钟RTC的时钟电路，实时时钟（RTC-Real Time Clock）提供一套计数器在系统上电和关闭操作

17、时对时间进行测量需要主晶体，因此电路设计如下：MK60FX512VLQ15 MCU时钟系统如下：2.2.4调试接口JTAG电路设计：MK60FX512VLQ15 MCU使用的是ARM Cortex-M4F内核，该内核内部集成了联合测试行为组织JTAG接口，通过JTAG接口可以实现程序下载和调试功能。JTAG(Joint Test Action Group；联合测试工作组)是一种国际标准测试协议（IEEE 1149.1兼容），主要用于芯片内部测试。现在多数的高级器件都支持JTAG协议，如DSP、FPGA器件等。标准的JTAG接口是4线：TMS、TCK、TDI、TDO，分别为模式选择、时钟、数据输

18、入和数据输出线。JTAG标准定义了一个串行的移位寄存器。寄存器的每一个单元分配给IC芯片的相应引脚，每一个独立的单元称为BSC（Boundary-Scan Cell）边界扫描单元。这个串联的BSC在IC内部构成JTAG回路，所有的BSR（Boundary-Scan Register）边界扫描寄存器通过JTAG测试激活，平时这些引脚保持正常的IC功能。JTAG引脚定义：TCK测试时钟输入；TDI测试数据输入，数据通过TDI输入JTAG口；TDO测试数据输出，数据通过TDO从JTAG口输出；TMS测试模式选择，TMS用来设置JTAG口处于某种特定的测试模式。TRST测试复位，输入引脚，低电平有效。

19、2.2.5电机驱动模块电路设计：此视觉导航平衡车系统采用24V直流电机，为此设计了一块基于BTS7970B的电机驱动。电路设计如下图：此电机驱动采用了英飞凌公司的BTS7970B电机驱动芯片。该芯片具有PWM频率高，输出方波稳定，具有自动保护功能。同时内部使用MOS管设计，导通电阻小，导通电流大。2.2.6陀螺仪加速度计电路设计：此视觉导航平衡车系统采用MPU6050陀螺仪加速度计集成芯片。MPU-6050整合了3轴陀螺仪、3轴加速器，并含可藉由第二个I2C端口连接其他厂牌之加速器、磁力传感器、或其他传感器的数位运动处理(DMP: Digital Motion Processor)硬件加速引擎

20、，由主要I2C端口以单一数据流的形式，向应用端输出完整的9轴融合演算技术InvenSense的运动处理资料库，可处理运动感测的复杂数据，降低了运动处理运算对操作系统的负荷，并为应用开发提供架构化的API。MPU-6050的角速度全格感测范围为±250、±500、±1000与±2000°/sec (dps)，可准确追踪快速与慢速动作，并且，用户可程式控制的加速器全格感测范围为±2g、±4g±8g与±16g。产品传输可透过最高至400kHz的IC或最高达20MHz的SPI（MPU-6050没有SPI）。MPU

21、-6000可在不同电压下工作，VDD供电电压介为2.5V±5%、3.0V±5%或3.3V±5%，逻辑接口VVDIO供电为1.8V± 5%（MPU6000仅用VDD）。MPU-6000的包装尺寸4x4x0.9mm(QFN)，在业界是革命性的尺寸。其他的特征包含内建的温度感测器、包含在运作环境中仅有±1%变动的振荡器。电路设计如下图：2.2.7CMOS摄像头电路设计：摄像头一般具有视频摄像/传播和静态图像捕捉等基本功能，它是借由镜头采集图像后，由摄像头内的感光组件电路及控制组件对图像进行处理并转换成电脑所能识别的数字信号，然后借由并行端口或USB连

22、接输入到电脑后由软件再进行图像还原。摄像头的工作原理：景物通过镜头(LENS)生成的光学图像投射到图像传感器表面上，然后转为电信号，经过A/D (模数转换)转换后变为数字图像信号，再送到数字信号处理芯片(DSP)中加工处理，再通过USB接口传输到电脑中处理，通过显示器就可以看到图像了。此视觉导航平衡车系统采用比亚迪公司的CMOS摄像头BF3003U，比亚迪公司提供各类CMOS，CCD图像处理解决方案，产品涉及手机摄像头模组，笔记本摄像头模组，安防摄像头，车载摄像头及光电导航模块等。BF3003U是一款车载摄像头。电路设计如下图：2.3软件设计：硬件电路设计是系统功能实现的保证，软件设计是系统控

23、制理论的实现。本系统采用IAR Systems公司的IAR Enbedded Workbench for arm集成开发环境。IAR Enbedded Workbench for arm适用于嵌入式系统的设计、开发和测试的每一个阶段，IAR Enbedded Workbench for arm含有很多模块，包括：带有C/C+编译器和调试器的集成开发环境(IDE)、实时操作系统和中间件、开发套件、硬件仿真器以及状态机建模工具。2.3.1MCU需要使用的资源配置：此视觉导航平衡车系统采用飞思卡尔MK60FX512VLQ15 MCU，MK60FX512VLQ15 是以ARM Cortex-M4F内核

24、设计，具有IEEE 1558以太网，全速和高速USB 2.0 On-The-Go带设备充电探测、硬件加密以及防篡改探测能力，具有丰富的模拟、通信、定时和控制外设、单精度浮点单元、NAND闪存控制器和DRAM控制器，如High speed analog-to-digital converter、FlexTimers、Low power timer、Periodic interrupt timer、CAN、Serial peripheral interface、Inter-integrated circuit (I2C)、UART等设备，从144LQFP封装256KB闪存开始可扩展到256MAPB

25、GA 1MB内存。是飞思卡尔公司专业汽车电子嵌入式MCU。ARM Cortex-M4F Core是ARM公司基于ARMv7构架的内核，精简指令集（RISC）具有DSP内核和FPU硬件浮点运算单元。对于复杂的控制算法具有很好的运算性能。软件设计分为PWM输出模块、陀螺仪加速度计采集模块、周期中断定时器模块、互补滤波算法及平衡控制模块、速度采集及速度控制模块、CMOS摄像头采集处理算法及转向模块。2.3.2PWM（Pulse Width Modulation）输出模块：脉宽调制（PWM）基本原理：控制方式就是对逆变电路开关器件的通断进行控制，使输出端得到一系列幅值相等的脉冲，用这些脉冲来代替正弦波

26、或所需要的波形。也就是在输出波形的半个周期中产生多个脉冲，使各脉冲的等值电压为正弦波形，所获得的输出平滑且低次谐波少。按一定的规则对各脉冲的宽度进行调制，即可改变逆变电路输出电压的大小，也可改变输出频率。PWM控制电机原理：利用MCU的PWM端口，固定PWM的周期，通过改变PWM的占空比，从而控制输出的电压。以达到控制速度的目的。通过MK60FX512VLQ15 MCU内部的FTM模块可以实现脉宽调制波形（PWM）输出。具体的配置如下：FTM0->MODE |= FTM_MODE_WPDIS_MASK; FTM0->MODE &= FTM_MODE_FTMEN_MASK;

27、FTM0->QDCTRL &= FTM_QDCTRL_QUADEN_MASK; FTM0->SC &= FTM_SC_CPWMS_MASK; FTM0->CONTROLS0.CnSC = 0; FTM0->CONTROLS0.CnSC |= (FTM_CnSC_MSB_MASK|FTM_CnSC_ELSB_MASK); FTM0->CONTROLS1.CnSC = 0; FTM0->CONTROLS1.CnSC |= (FTM_CnSC_MSB_MASK|FTM_CnSC_ELSB_MASK); FTM0->COMBINE &=

28、 FTM_COMBINE_DECAPEN0_MASK; FTM0->COMBINE &= FTM_COMBINE_COMBINE0_MASK; FTM0->CONTROLS2.CnSC = 0; FTM0->CONTROLS2.CnSC |= (FTM_CnSC_MSB_MASK|FTM_CnSC_ELSB_MASK); FTM0->CONTROLS3.CnSC = 0; FTM0->CONTROLS3.CnSC |= (FTM_CnSC_MSB_MASK|FTM_CnSC_ELSB_MASK); FTM0->COMBINE &= FTM_C

29、OMBINE_DECAPEN1_MASK; FTM0->COMBINE &= FTM_COMBINE_COMBINE1_MASK; FTM0->CONTROLS0.CnV = (FTM0->MOD)*(PWMDuty)/1000); FTM0->CONTROLS1.CnV = (FTM0->MOD)*(PWMDuty)/1000); FTM0->CONTROLS2.CnV = (FTM0->MOD)*(PWMDuty)/1000); FTM0->CONTROLS3.CnV = (FTM0->MOD)*(PWMDuty)/1000);

30、 2.3.3陀螺仪加速度计采集模块：此视觉导航平衡车系统采用MPU6050陀螺仪加速度计集成芯片，MPU-6050整合了3轴陀螺仪、3轴加速器，并含可藉由第二个I2C端口连接其他厂牌之加速器、磁力传感器、或其他传感器的数位运动处理(DMP: Digital Motion Processor)硬件加速引擎，由主要I2C端口以单一数据流的形式，向应用端输出完整的9轴融合演算技术InvenSense的运动处理资料库，可处理运动感测的复杂数据，降低了运动处理运算对操作系统的负荷，并为应用开发提供架构化的API。I2C（InterIntegrated Circuit）总线是由PHILIPS公司开发的两线

31、式串行总线，用于连接微控制器及其外围设备。是微电子通信控制领域广泛采用的一种总线标准。它是同步通信的一种特殊形式，具有接口线少，控制方式简单，器件封装形式小，通信速率较高等优点。I2C 总线支持任何IC 生产过程(CMOS、双极性）。通过串行数据（SDA）线和串行时钟 （SCL）线在连接到总线的器件间传递信息。通过MK60FX512VLQ15 MCU内部的I2C模块可以实现对MPU6050的采集。具体的配置如下：配置I2C：SIM_SCGC4 |= SIM_SCGC4_I2C0_MASK; PORTB_PCR2 = PORT_PCR_MUX(2);PORTB_PCR3 = PORT_PCR_M

32、UX(2);I2C_F_REG(I2Cxi2cn) = I2C_F_MULT(mult) | I2C_F_ICR(scl_dev) ;I2C0_C1 = I2C_C1_IICEN_MASK | I2C_C1_TXAK_MASK;配置MPU6050：i2c_writeaddr(I2C_Moudle, ADDRESS, MPU6050_PWR_MGMT_1, 0x00);i2c_writeaddr(I2C_Moudle, ADDRESS, MPU6050_PWR_MGMT_2, 0x00);i2c_writeaddr(I2C_Moudle, ADDRESS, MPU6050_CONFIG, 0x0

33、6);i2c_writeaddr(I2C_Moudle, ADDRESS, MPU6050_GYRO_CONFIG, 0x08);i2c_writeaddr(I2C_Moudle, ADDRESS, MPU6050_ACCEL_CONFIG, 0x08);读取姿态数据：ACCEL_Z = (int16)i2c_readaddr(I2C_Moudle ,ADDRESS, ACCEL_ZOUT) << 8)|(int16)i2c_readaddr(I2C_Moudle, ADDRESS, ACCEL_ZOUT + 1);GYRO_X = (int16)i2c_readaddr(I2C_

34、Moudle, ADDRESS, GYRO_XOUT) << 8)|(int16)i2c_readaddr(I2C_Moudle, ADDRESS, GYRO_XOUT + 1);2.3.4周期中断定时器模块：此视觉导航平衡车系统需要运行多任务，比如速度控制、姿态算法、平衡控制、转向控制、等。因此我们需要简要的实现任务切换算法，所以我们需要使用周期中断定时器实现任务切换算法。周期中断定时器配置如下：SIM->SCGC6 |= SIM_SCGC6_PIT_MASK; PIT->MCR &= PIT_MCR_MDIS_MASK; PIT->MCR |= PIT

35、_MCR_FRZ_MASK; PIT->CHANNEL1.LDVAL = (uint32_t)time; PIT->CHANNEL1.TFLG |= PIT_TFLG_TIF_MASK; PIT->CHANNEL1.TCTRL |= PIT_TCTRL_TEN_MASK; PIT->CHANNEL1.TCTRL |= PIT_TCTRL_TIE_MASK; NVIC_EnableIRQ(PIT1_IRQn); NVIC_Init(PIT1_IRQn, NVIC_PriorityGroup_3, 1, 0);简单的任务切换程序如下：void PIT0_IRQHandler

36、(void) PIT->CHANNEL0.TFLG |= PIT_TFLG_TIF_MASK;Stitic unsigned char sys_cnt = 0;switch(sys_cnt +) case 0: break;case 1: break;case 2: break; case 3: sys_cnt = 0;break; 2.3.5互补滤波算法及平衡控制模块：平衡理论分析：对于基于视觉两轮平衡车，可以分析为一个倒立摆系统，如果车体有向前倒的趋势，那么车体需要向前加速运动保持平衡，如果车体有向后倒的趋势，那么车体需要向后加速运动保持平衡。对此系统建立动力学数学模型，可以看出此系

37、统是一个倒立的重力场系统。在重力场中：回复力与角度成正比，同时空气中具有阻尼力，因此单摆系统可以保持稳定。是一个负反馈系统。F=-mgsin -mg在倒立摆系统中：偏离一定的位置之后回复力与位移方向相同，导致继续偏离平衡位置，最终倒下。是一个正反馈系统。因此我们必须额外提供其他的力（即车轮的加速度）以提供回复力，同时我们还需要提供阻尼力。F=mgsin -macos mg -mk1因此总结出，在整个系统中，角度即为系统增益，角速度即为系统阻尼，通过对角度和角速度的偏差的比例和微分控制既可以达到平衡。在这个系统中，我们需要两个条件，第一个是我们可以控制车体前后加速运动，这个我们可以控制电机运动达

38、到这个目的，第二个条件是我们可以监测车体此时此刻的姿态（包括角度和角速度），这个我们可以通过角速度传感器陀螺仪和加速度传感器加速度计来采集此时此刻的车体姿态。对于基于视觉两轮平衡车而言，我们通过加速度计和陀螺仪MPU6050采集到原始姿态值，但是加速度计对车体的加速度比较敏感，取瞬时值计算倾角误差比较大；而陀螺仪积分得到的角度不受小车加速度的影响，但是随着时间的增加积分漂移和温度漂移带来的误差比较大。所以这两个传感器正好可以弥补相互的缺点。互补滤波就是在短时间内采用陀螺仪得到的角度做为最优，定时对加速度采样来的角度进行取平均值来校正陀螺仪的得到的角度。就是，短时间内用陀螺仪比较准确，以它为主；

39、长时间用加速度计比较准确，这时候加大它的比重，这就是互补；加速度计要滤掉高频信号，陀螺仪要滤掉低频信号，互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号，例如，加速度计测倾角，其动态响应较慢，在高频时信号不可用，所以可通过低通抑制高频；陀螺响应快，积分后可测倾角，不过由于零漂等，在低频段信号不好。通过高通滤波可抑制低频噪声。将两者结合，就将陀螺和加表的优点融合起来，得到在高频和低频都较好的信号，互补滤波需要选择切换的频率点，即高通和低通的频率，这就是滤波。低通滤波(Low-pass filter) 是一种过滤方式，规则为低频信号能正常通过，

40、而超过设定临界值的高频信号则被阻隔、减弱。但是阻隔、减弱的幅度则会依据不同的频率以及不同的滤波程序（目的）而改变。它有的时候也被叫做高频去除过滤（high-cut filter）或者最高去除过滤（treble-cut filter)。低通过滤是高通过滤的对立。低通滤波器的离散公式：Y(k) = (Tc/(Tc+Ts)*Y(k-1) + (Ts/(Tc+Ts)*X(k)其中Tc为滤波时间系数，Ts为采样时间，X(k)为当前采样值,Y(k)为滤波器输出值,Y(k1)为滤波器的上次输出。高通滤波(high-pass filter) 是一种过滤方式，规则为高频信号能正常通过，而低于设定临界值的低频信号

41、则被阻隔、减弱。但是阻隔、减弱的幅度则会依据不同的频率以及不同的滤波程序（目的）而改变。它有的时候也被叫做低频去除过滤（low-cut filter）。高通滤波是低通滤波的对立。互补滤波的Matlab仿真：% Complement filter Simulation System% angle= 0.998 * (angle + angle_dot * dt) + 0.002 * ACCEL_Z;clc;clear; dt=0.002;%2msangle = 0;angle_dot = 0; for i=0:0.001:50 ACCEL_Z = sin(i) + 5*(-1+(1-(-1)*r

42、and(1);GYRO_X = cos(i) + 5*(-1+(1-(-1)*rand(1);angle_dot = GYRO_X; angle= 0.98 * (angle + angle_dot * dt) + 0.02 * ACCEL_Z; % ACCEL_Z = m紫红*星号 angle_dot = r红：点线 angle = k黑实线plot (i,ACCEL_Z,'m',i,angle_dot,'r',i,angle,'k');hold on end在此仿真系统中陀螺仪给的值为余弦函数+ 5*-1 1之间的随机，加速度的值为正弦函数+

43、5*-1 1 之间的随机数ACCEL_Z = sin(i) + (-1+(1-(-1)*rand(1);GYRO_X = cos(i) + (-1+(1-(-1)*rand(1);angle= 0.998 * (angle + angle_dot * dt) + 0.002 * ACCEL_Z;ACCEL_Z = sin(i) + (-1+(1-(-1)*rand(1);GYRO_X = cos(i) + (-1+(1-(-1)*rand(1);angle= 0.98 * (angle + angle_dot * dt) + 0.02 * ACCEL_Z;ACCEL_Z = sin(i) +

44、5*(-1+(1-(-1)*rand(1);GYRO_X = cos(i) + 5*(-1+(1-(-1)*rand(1);angle= 0.998 * (angle + angle_dot * dt) + 0.002 * ACCEL_Z;ACCEL_Z = sin(i) + 5*(-1+(1-(-1)*rand(1);GYRO_X = cos(i) + 5*(-1+(1-(-1)*rand(1);angle= 0.98 * (angle + angle_dot * dt) + 0.02 * ACCEL_Z;ACCEL_Z = sin(i) + 10*(-1+(1-(-1)*rand(1);G

45、YRO_X = cos(i) + 10*(-1+(1-(-1)*rand(1);angle= 0.998 * (angle + angle_dot * dt) + 0.002 * ACCEL_Z;ACCEL_Z = sin(i) + 10*(-1+(1-(-1)*rand(1);GYRO_X = cos(i) + 10*(-1+(1-(-1)*rand(1);angle= 0.98 * (angle + angle_dot * dt) + 0.02 * ACCEL_Z;互补滤波在基于视觉两轮平衡车MK60FX512VLQ15 MCU的C语言实现：Float angle = 0;Float an

46、gle_dot = 0;float bias_cf=0;const float dt=0.002;void Complement_Filter(float angle_m_cf,float gyro_m_cf) bias_cf*=0.998;/陀螺仪温漂低通滤波500次 bias_cf+=gyro_m_cf*0.002; angle_dot=gyro_m_cf-bias_cf; angle=(angle+angle_dot*dt*1)*0.998+angle_m_cf*0.002;/加速度计低通滤波互补滤波在MK60FX512VLQ15 MCU中实现之后根据仿真系统及采样频率的要求，我们需要将

47、互补滤波函数再任务中精确的每2ms执行一次，得到相对稳定的姿态估计值。得到车体姿态稳定的估计值之后我们就可以用来控制电机以达到使车体平衡的目的，在通过姿态估计值控制电机的算法中我们可以使用经典的PID调节器，即通过角度与零角度的偏差的比例、积分和微分控制，以达到快速稳定强劲的平衡效果。在PID调节器的介绍我们将在速度控制算法中解释，这里只给出平衡算法的PID实现。void Upright_Control_calculate(void) int PWM_L = 0, PWM_R = 0; Upright_M = Kp_ang * angle;/ = 比例系数*角度 Upright_M += Kd

48、_ang * angle_dot;/ = 微分系数*角速度 speed_M_error = speed_M - speed_M_old; Direction_M_error = Direction_M - Direction_M_old; PWM_L = (int)(moto_start*Upright_M*moto_start); PWM_R = (int)(moto_start*Upright_M*moto_start); if(angle > 150 | angle < -150) car_stop_flag = 0; M_PWM_output(PWM_L*car_stop_

49、flag, PWM_R*car_stop_flag);同样，我们的平衡控制算法在姿态估计算法之后必须立即执行，并保证精确的执行频率。2.3.6速度采集及速度控制模块：对于基于视觉两轮平衡车系统而言，速度的控制相对于普通普通系统而言要复杂很多。平衡车系统需要保持平衡状态，因此我们不能控制电机的转速来控制速度。由于平衡车系统向前倾斜系统就会向前运动，向后倾斜就会向后运动，因此，控制速度其实就是控制车体的角度。速度控制实在两个条件下的，第一，可以精确采集到车体当前速度，我们可以使用车体编码器测量轮胎的转速和方向；第二，可以使用测量的速度和设定的速度偏差控制车体倾角（PID调节器）。速度采集：对于速度

50、控制系统，速度采集尤为重要，采集到准确实时的速度信息才是控制速度的关键。此视觉导航平衡车系统速度采集采用了旋转编码器，具体型号为欧姆龙编码器。通过MK60FX512VLQ15 MCU内部的FTM模块可以实现正交解码功能，可以采集到编码器的旋转速度和旋转方向。正交解码：正交解码的A相（QEA）和B相（QEB）这两个通道间的关系是惟一的。如果A相超前B相，那么电机的旋转方向被认为是正向的。如果A相落后B相，那么电机的旋转方向则被认为是反向的。第三个通道称为索引脉冲，每转一圈产生一个脉冲，作为基准用来确定绝对位置。编码器产生的正交信号可以有四种各不相同的状态（01，00，10，11）。当旋转的方向改

51、变时，这些状态的顺序与此相反（11，10，00，01）。正交解码器捕捉相位信号和索引脉冲，并将信息转换为位置脉冲的数字计数值。具体的配置如下：SIM->SCGC3 |= SIM_SCGC3_FTM2_MASK; SIM->SCGC6 |= SIM_SCGC6_FTM1_MASK; FTM1->MODE |= FTM_MODE_WPDIS_MASK; FTM2->MODE |= FTM_MODE_WPDIS_MASK; FTM1->CNTIN = 0; FTM2->CNTIN = 0; FTM1->MOD = 65535; FTM2->MOD =

52、65535; FTM1->MODE |= FTM_MODE_FTMEN_MASK; FTM2->MODE |= FTM_MODE_FTMEN_MASK; FTM1->QDCTRL |= FTM_QDCTRL_QUADMODE_MASK; FTM1->QDCTRL |= FTM_QDCTRL_QUADEN_MASK; FTM2->QDCTRL |= FTM_QDCTRL_QUADMODE_MASK; FTM2->QDCTRL |= FTM_QDCTRL_QUADEN_MASK; FTM1->CNT = 0; FTM2->CNT = 0; FTM1-

53、>SC |= FTM_SC_CLKS(3); FTM2->SC |= FTM_SC_CLKS(3);Pulse_L = FTM1->CNT; FTM1->CNT = 0; Pulse_R = FTM2->CNT; FTM2->CNT = 0;速度控制：关于速度测量与速度采集的速度偏差，我们需要用来控制车体倾角已达到控制车体速度的目的。因此我们的系统速度偏差和倾角设定有一定关系，我们就可以用倾角的设定值和倾角的测量值之间的偏差控制电机输出就可以达到控制车体速度的目的。对于速度控制我们同样要求其实时性、快速性、稳定性、强劲性等特点，因此我们也需要使用经典的PID

54、调节器。PID调节器：PID调节器是通过输入e(t)的比例（proportion）、积分（integration）、微分（differentiation）控制输出u(t)。ut=kpet+1TIetdt+TD*detdt式中积分的上下限分别是0和t;传递函数为：Gs=UsEs=kp1+1TI*s+TD*skp为比例系数； TI为积分时间常数； TD为微分时间常数.比例控制：比例控制是一种简单的负反馈调节系统，控制器输出与输入误差成比例关系，因此次比例控制系统的输出存在稳态误差。积分控制：积分控制不单独存在一般和积分控制配合使用。控制器的输出与输入误差的积分成正比关系。积分控制可以消除稳态误差，配合比例控制使用及PI控制器，可以使系统稳态无误差。微分控制：微分控制一般也是不单独使用，配合比例控制使用。控制器的输出与输入误差的微分很成正比。在一般的控制系统中有时调节过程会出现震荡，原因是存在较大的滞后性的惯性环节，微分控制具有超前性，可以抑制系统震荡。对于滞后性比较大的系统可以使用PD控制器，有利于改善系统的动态特性。同时速度采集和速度控制也是需要控制周期的，我们这里采用100ms控制