视觉导航智能车姿态控制系统设计与实现

1、 视觉导航智能车姿态控制系统设计与实现 题 目 视觉导航智能车姿态 控制系统设计与实现专 业 班 级 学 生 指导教师 年 视觉导航智能车姿态控制系统设计与实现 摘 要近年两轮自平衡电动车得到了很大的发展，因为其具有灵活、便利、节能等特点。全国大学生“飞思卡尔”杯智能汽车竞赛中的摄像头平衡组比赛要求是：让智能车模仿两轮电动平衡车的运行方式，让智能车仅用两个后轮驱动实现直立行走。 本课题以第九届全国大学生“飞思卡尔杯”智能汽车竞赛为背景，使用IAR编程软件对直立智能车进行软件开发。本文详细论述了智能车的各个模块的工作原理、电路图、机械安装以及控制算法。其中详细的开发过程，以及期间碰到的各种问题和

2、解决方法都在文中有详细的论述。本系统采用飞思卡尔公司K60单片机作为主控芯片，使用陀螺仪和加速度传感器采集角度信息，光电码盘采集速度信息，分别作为角度反馈和速度反馈，再结合PID控制算法，控制两个电机的转速，最终实现智能车能够直立并能够实现按预期的速度向前运行。关键字：智能车，直立控制，陀螺仪，加速度传感器，K60单片机AbstractIn recent years, two self-balancing electric vehicle has been great development, because it is flexible, convenient, energy-saving

3、features. National College Student "Freescale Cup" smart car race in the camera balance group competition requirements is: Let the smart car to imitate two electric counterbalance vehicle operating mode, make the smart car with only two rear-wheel drive to achieve walk upright.In this pape

4、r Ninth National University "Freescale Cup" Smart Car race background, using the IAR programming software for intelligent vehicle upright for software development. This paper discusses in detail the working principle of each module smart car, schematics, mechanical installation and control

5、 algorithms. Which details the development process, as well as a variety of problems and solutions encountered during are discussed in detail in the text.The system uses Freescale K60 MCU as the master chip, using a gyroscope and an accelerometer sensor angle information collection, information gath

6、ering speed optical encoder, respectively, as the angle feedback and velocity feedback, combined with PID control algorithm to control two motors speed, and ultimately achieve smart car can stand and be able to achieve speeds forward as expected.Keywords: smart car, upright control, gyroscope, accel

7、erometer, K60 MCU目 录第1章 绪论11.1 课题背景概述11.2 本文主要工作内容2第2章 视觉导航智能车姿态控制系统总体方案设计32.1 系统总体方案简介32.2 硬件方案32.3 软件方案42.4 本章小结5第3章 视觉导航智能车姿态控制系统硬件设计63.1 车模安装63.1.1 电池安装83.1.2 主控板安装83.1.3 陀螺仪及加速度传感器安装93.1.4 光电码盘的安装93.2系统电路原理图设计103.2.1 K60最小系统电路103.2.2 电源稳压电路133.2.3 电机驱动电路143.2.4 陀螺仪加速度传感器电路163.2.5 光电码盘测速电路163.3

8、系统电路PCB设计183.4 本章小结19第4章 视觉导航智能车姿态控制系统软件设计204.1 软件整体框架204.2 所用模块设计214.2.1 时钟模块224.2.2 FTM模块234.2.3 PIT模块234.2.4 AD模块244.3 控制算法设计254.3.1 直立控制254.3.2 速度控制314.4 本章小结32第5章 视觉导航智能车姿态控制系统调试335.1 调试工具简介335.2 调试方法与步骤355.2 调试结果37第6章 总结与展望396.1 总结396.2 展望39致 谢40参考文献41附录：部分程序代码43I第1章 绪论1.1 课题背景概述近年两轮自平衡电动车得到了很

9、大的发展，因为其具有灵活、便利、节能等特点。两轮自平衡电动车是一个高度不稳定的系统，它的动力学方程是时变、耦合、不稳定、多变量的非线性高阶方程，而且其运动方程非完整性约束，需要完成的控制任务具有多重性，因此，两轮自平衡电动车是一个复杂的控制系统，给经典控制理论带来了很大的挑战。可以检验各种控制方法的优劣。可以使用两轮自平衡电动车进行不确定性系统控制、非线性系统控制、自适应控制、智能控制等研究。全国大学生“飞思卡尔杯”智能汽车竞赛中的摄像头平衡组比赛是要让智能车模仿两轮电动平衡车的运行方式，让智能车仅用两个后轮驱动实现直立行走。飞思卡尔智能车大赛平衡组作为两轮电动平衡车的简化版应运而生，智能汽车

10、竞赛组委会在2012年电磁组直立行走的基础上规定2013年摄像头平衡组车模直立行走。这项新的改动带来挑战的同时也给参赛队伍带来了新的动力。 在车模直立行走比赛中，车模的倾角和角速度是控制车模直立控制的关键。一般测量角度信息采用两种传感器：加速度传感器或陀螺仪。加速度传感器可以用来测量车模的角度信息，但在实际车模运行过程中，车模会产生很大的摆动，对倾角信息测量带来干扰，它叠加在上述测量信号上使得输出信号无法准确反映车模的倾角。陀螺仪用来测量角速度，再将测到的角速度进行积分得到车模的倾角。由于陀螺仪输出的是车模的角速度，不会受到车体运动的影响，因此该信号中噪声很小。因此对于车模直立控制所需要的倾角

11、信息需要通过角速度传感器即陀螺仪来得到。因此，本次设计对基于陀螺仪的视觉导航智能车姿态控制系统进行研究。通过该课题本人学会了运用数学基础知识，建立系统模型，并进行分析，查阅相关中英文文献，了解本课题前沿发展动态，熟练的使用各种仪器(万用表、示波器)对实验结果进行分析，主动学习新的理论知识。 1.2 本文主要工作内容（1）明确视觉导航智能车姿态控制系统设计与实现设计要求，本次设计对基于加速度传感器-陀螺仪的视觉导航智能车姿态控制系统进行研究； （2）确定视觉导航智能车姿态控制系统的总体设计方案，本系统将以加速度传感器、陀螺仪作为检测元件，将其输出由AD输入到K60单片机中，由K60单片机处理并输

12、出控制信号，控制电机的驱动模块，进而控制电机的加速度，实现小车的直立行走； （3）设计视觉导航智能车姿态控制系统的硬件电路及机械结构，整个智能车硬件是由三部分组成的：K60最小系统板、主控板、电机驱动电路板； （4）设计并实现车模直立行走控制算法，本系统将采用PD角度闭环，PI速度闭环控制。要使智能车能够平稳快速地运行，需要有高效稳定的控制算法对车模速度进行闭环反馈控制，于是打算使用经典PI 控制算法，配合使用理论计算和实际参数补偿的办法即可消除外界各种因素的影响，使得车模运行的更稳定。 第2章 视觉导航智能车姿态控制系统总体方案设计智能车是一个软硬件结合的控制系统，只有软件和硬件完美结合的情

13、况下，才能达到预期的控制目标。2.1 系统总体方案简介本系统的总体方案为：陀螺仪和加速度传感器采集角度信息，使用K60单片机的ADC模块将采集到的数据进行数模转换。再使用K60单片机的FTM模块采集光电码盘的脉冲数，使用互补滤波算法，经加速度传感器和陀螺仪采集到的角度信息进行滤波融合，最终得到需要的倾角信息 ，将K60单片机采集到的脉冲数进行转换后变为“转/秒”作为速度反馈，通过角度PD控制算法和速度PI算法实现直立控制和速度控制。2.2 硬件方案系统在陀螺仪和加速度传感器采集车身姿态信息的基础上1，通过单片机处理信号，实现车体能够准确保持直立。系统硬件电路部分包括K60最小系统模块、电机驱动

14、模块、陀螺仪与加速度传感器模块、光电码盘测速模块等部分。综上所述，本智能车系统硬件部分包含了以下几个模块：（1） K60最小系统模块（2） 电源模块（3） 电机驱动模块（4） 陀螺仪与加速度传感器模块（5） 光电测速模块总体硬件方案框图如图2-1所示：图2-1总体硬件方案框图2.3 软件方案在硬件的基础上，通过软件的配合才能完成最终的控制目标，本系统的软件控制算法分为直立控制和速度控制，直立控制通过对角度偏差进行PD控制和速度偏差进行PI控制实现，速度控制通过对速度偏差进行PI控制实现2。整体软件框架如图2-2所示：图2-2 整体软件框架2.4 本章小结本章对智能车的整体方案，包括硬件方案和软

15、件方案做了详细的描述，对下面工作的展开有很大的指导意义。第3章 视觉导航智能车姿态控制系统硬件设计3.1 车模安装智能车硬件要求：（1） 使用D或E车模，动力轮着地，双向行驶，车模宽度不超过250mm。（2） 主控制处理器只能有一个，而且必须使用飞思卡尔公司的8 位、16 位、32 位处理器。（3） 传感器数量不超过16个，使用飞思卡尔系列加速度传感器，陀螺仪无 限制，禁止DC-DC升压作为电机动力，总电容不超过2000微法，电压不超过25V。（4） 自行设计PCB，并印制醒目LOGO。硬件方案选择：（1）车模选择 E车模两个车轮之间距离较大，有利于行驶时保持平稳，因此本系统采用E车模，E车模

16、实物图如图3-1所示：图3-1 E车模实物图（2）加速度传感器选择加速度传感器分为两类：数字式和模拟式。7260和7361利用单片机AD采集数据，7660和8451利用IIC和单片机进行通讯。智能车大赛官方规定：如果平衡组需要使用加速度传感器，则必须使用飞思卡尔公司的 MMA7260，MMA7660，MMA7361，MMA8450，MMA8451 等系列的加速度器。综合各方面的需求，本系统最终选择MMA7361加速度传感器。（3）陀螺仪选择：官方规定只能用村田公司的 ENC-03系列的陀螺仪，因此本系统使用ENC-03陀螺仪。ENC-03有MA、MB、RC三种类型。其中MA和MB的封装一样，性

17、能也一样，只是震动频率不同。RC则是尺寸比较小的。其他参数都一样。最终选择ENC-03RC#2陀螺仪，各型号陀螺仪性能对比见表3-1：表3-1 各型号陀螺仪性能对比型号时间10s时间1min时间10min时间1hENC-03MB0x1f40x1f20x1f10x1f2ENC-03RC#10x2a30x2ac0x2b50x2b2ENC-03RC#20x04f0x04f0x0580x05a一个ENC-03MB、两个ENC-03RC测试静态，AMS1117-3.3V供电，环境温度19°，测试长期加电的输出漂移，测试点为运放输出，为10位AD所采集的原始数据。（4）电机驱动芯片选择：电机驱动

18、芯片采用BTS7960，其驱动能力较强，BTS7960芯片用于电机驱动半桥高集成芯片3。 BTS7960通态电阻值为16mQ，输入电压为6V-24V，驱动电流可达43A。他的使用方法也很简单，只需要向芯片的第2个管脚输入PWM就可以控制它了。由于要实现智能车的直立，电机需要正反转，所以用四片BTS7960组成2个全桥驱动电路。3.1.1 电池安装电池安装在车模的电池支架上，由于电池比较重，所以尽量不要装的太高，负责中心太高，加大控制难度。电池安装实物图如图3-2所示：图3-2 电池安装实物图3.1.2 主控板安装主控板安装在电池的上面，在车模的底座上钻了两个孔，将主控板用铜柱固定在上面，主控板

19、安装实物图如图3-3所示：图3-3 主控板安装实物图3.1.3 陀螺仪及加速度传感器安装陀螺仪和加速度传感器的安装对智能车的直立控制很重要，陀螺仪是测量角速度的精密仪器，其灵敏度受安装位置和方式的影响，安装时要求稳固，且离车轴的位置要合适，以保证其灵敏度不受影响。加速度传感器噪声很大，易受车模震动以及摆动的影响，所以安装时离车轴不能太远。由于本系统采用了陀螺仪和加速度传感器集成模块，所以综合二者的安装要求，陀螺仪及加速度传感器安装实物图如图3-4所示：图3-4 陀螺仪及加速度传感器安装实物图3.1.4 光电码盘的安装本系统选用的是经济实惠的光电码盘测量两轮的转速，光电码盘安装实物图如图3-5所

20、示：图3-5 光电码盘安装实物图3.2系统电路原理图设计本系统以飞思卡尔公司MK60DN512VLL10芯片作为主控单元，使用的模块有K60最小系统模块，电源模块，电机驱动模块，加速度传感器及陀螺仪模块，光电码盘测速模块，下面将对每个模块的原理及电路图进行详细的论述。3.2.1 K60最小系统电路K60 微控制器系列具有IEEE 1588 以太网4，全速和高速 USB 2.0 On-The-Go 带设备充电探测，硬件加密和防窜改探测能力，具有丰富的模拟、通信、定时和控制外设，从100 LQFP 封装 256 KB 闪存开始可扩展到256 MAPBGA 1MB 闪存。 大闪存的 K60 系列器件

21、还可提供可选的单精度浮点单元、NAND 闪存控制器和DRAM 控制器。 图3-6给出了K60微控制器模块结构框图。图3-6 K60微控制器模块结构图K60性能指标如表3-2所示：表3-2 K60 性能指标工作特性 电压范围 1.71V - 3.6V 闪存编程电压最低至1.71V 温度范围(TA) -40 to 105°C 灵活的工作模式时钟 多用途时钟发生器 PLL 和FLL 内部参考时钟(32kHz 或 2MHz) 4MHz 到 32MHz 晶振 32kHz 到 40kHz 晶振 内部 1kHz 低功耗振荡器 DC 到 50MHz 外部方波输入时钟模拟 16 位 SAR ADC 可

22、编程的电压参考(VREF) 12 位 DAC 带6 位DAC 的高速模拟比较器 (CMP)定时器 1x8ch 电机控制/ 通用/PWM 定时器(FTM) 2x2ch 正交解码器/ 通用/PWM 定时器 (FTM) 载波调制定时器(CMT) 可编程延迟模块 (PDB) 1x4ch 可编程中断定时器(PIT) 低功耗定时器(LPT)通信 支持IEEE 1588 的以太网接口 USB 全速/ 低速 OTG/ 主机/ 从设备接 CAN SPI I2C，支持SMBUS UART ( 带ISO7816、IrDA 和硬件流控)人机接口 GPIO 支持引脚中断、DMA 请求、数字滤波和其他引脚控制选项 最大允

23、许5V 输入 电容式触摸传感输入K60单片机最小系统电路原理图如图3-7所示，外围电路有JINK下载端口，复位电路。图3-7 K60单片机最小系统电路原理图为了保持最小系统运行的稳定性，其电源需要加入滤波电路，并联小电容，串联电感，从而提高供电电路的兼容性，滤除高频噪声。K60最小系统电源滤波电路原理图如图3-8所示。图3-8 K60最小系统电源滤波电路原理图3.2.2 电源稳压电路整个系统涉及的电源有3.3V，5V。3.3V用来给K60最小系统和加速度传感器及陀螺仪模块供电，5V用来给光电码盘和BTS7960使能端供电。电池的电压为7.2V，5V电压由稳压芯片LM2940CT-5.0产生，3

24、.3V电压由稳压芯片REG1117-3.3产生。再加上电源开关电路共同组成电源稳压电路原理图如图3-9所示：图3-9 电源稳压电路原理图3.2.3 电机驱动电路本系统使用半桥驱动芯片BTS7960作为驱动模块的驱动芯片，此芯片驱动能力较强，输入电压范围为6V-24V，输出电流可达40A，使能端电压为5V。智能车的直立需要电机的正反转来实现，因此需要4片BTS7960来组成两个全桥驱动电路来控制两个电机，具体的电机驱动电路原理图如图3-10所示：图3-10 电机驱动电路原理图此外，考虑到电机驱动电流会对主控板造成影响，因此，本系统在K60最小系统和BTS7960之间加入看74LS244芯片隔离反

25、冲信号，以保护K60主控芯片的安全，隔离电路原理图如图3-11所示：图3-11 隔离电路原理图3.2.4 陀螺仪加速度传感器电路陀螺仪和加速度传感器使用了集成模块，具体的电路原理图如3-12所示：图3-12 陀螺仪及加速度模块电路原理图3.2.5 光电码盘测速电路为了进行精确的控制，必须及时的知道小车每个时刻的速度反馈，因此需要选用测速模块，本系统使用的是实用的光电码盘5，这类编码器对直立车模的控制已经足够，并有经济实惠的特点。光电码盘测速模块电路如图3-13所示：图3-13 光电码盘测速模块电路原理图视觉导航智能车姿态控制系统总体原理图如图3-14所示：图3-14 视觉导航智能车姿态控制系统

26、总体原理图3.3 系统电路PCB设计印刷电路板（Printed-Circuit Board，PCB）是由环氧树脂粘合而成的镀了铜的玻璃纤维板。腐蚀掉部分镀铜，只留下构成电路连接的铜层走线部分。在使用电子设计自动化软件（如DXP）设计PCB时需注意以下几个方面的问题：（1） 尽可能缩短高频元件之间的连线，想办法减少他们的之间的电磁干扰。容易受干扰的元件在放置时，原件彼此之间不能靠的太近，输入元件和输出元件之间应该有足够的距离。（2） 某些元件和导线之间电位差比较高，应该加大他们之间的距离，防止在放电时引起意外的短路。（3） 质量比较大的元件在放置时应该用支架固定之后再焊接。（4） 电位器、可调电

27、感线圈、可变电容器、微动开关等元件在布局时应该整个PCB板的结构要求。（5） 应该留出PCB板固定支架所占用的位置。本系统的主控板PCB 2D视图如图3-15所示，3D视图如图3-16所示：图3-15 主控板PCB 2D视图图3-16 主控板PCB 3D视图3.4 本章小结本章详细论述了本系统的硬件方案，即每个模块的原理及电路图，机械安装，只有在硬件系统完美再配合一定的软件算法，才能实现最终的控制目标。第4章 视觉导航智能车姿态控制系统软件设计4.1 软件整体框架软件功能主要包括：（1）加速度传感器及陀螺仪采集角度信息，光电码盘采集速度信息；（2）单片机将采集回来的数据进行PID运算；（3）单

28、片机输出PWM控制电机转速；（4）车模运行控制：直立控制、速度控制；（5）车模信息显示与参数调整：上位机监控，参数设定。本系统整个软件的流程为陀螺仪和加速度传感器先将角度信息由单片机AD模块读入单片机，在使用FTM模块读取光电码盘的脉冲数，然后将这些数据进行单位化，将角度信息进行互补滤波处理，得到准确的角度信息，再将角度偏差经过PD运算，速度偏差进行PI运算6，得到的数据通过PWM输出给电机，以改变电机的转速来实现直立控制与速度控制。软件算法整体分为直立控制和速度控制，首先实现直立控制，再实现速度控制，在进行速度控制的同时要注意不能影响到直立控制，通过协调直立控制和速度控制，最终实现智能车能够

29、直立并能稳定的向前运行。软件算法是本系统的核心内容，其中各个模块代码的编写，系统的调试都花了大量的时间，其中也碰到了很多问题，详细的程序代码在下文和附录中都给出了注释。调试方法及调试中遇到的问题，解决方法都在下文中进行了论述。下面将分模块对本系统的软件部分进行说明，整体软件流程图如图4-1所示：图4-1 整体软件流程图4.2 所用模块设计在整个系统中，用到了单片机中的以下模块：时钟模块、AD模块、PIT模块、FTM模块、以及GPIO模块。根据实际需要对各模块进行初始化和程序写入。4.2.1 时钟模块MCG 模块控制着器件系统时钟源的选择。时钟源被时钟发生逻辑器件分开作用于不同的地方。包括：主控

30、器件总线，从器件总线和闪存。他也控制着模块的时钟门控电路，来实现模块的单独关闭。系统主要的时钟来源于MCGOUTCLK 的输入。时钟产生逻辑器件可以产生多个独立的时钟信号以使得不同的外设工作在不同的频率。这能够更好在性能和功耗之间寻求平衡。不同的模块比如说USB OTG 控制器需要一个特定的时钟，这可以通过时钟逻辑发生器使用MCGPLLCLK 或者MCGFLLCLK 来产生。另外一些其他的模块也可以使用其他的时钟源来产生模块所需要的时钟。大多数模块的时钟源选择由SIM 模块的SOPT 寄存器决定7。本系统以外部晶振(50M)为时钟源，按照FEIFEBPBEPEE的顺序进行配置，代码如下:uns

31、igned char pll_init(void) unsigned char pll_freq; MCG_C2 = 0; SIM_SCGC4 |= SIM_SCGC4_LLWU_MASK;LLWU_CS |= LLWU_CS_ACKISO_MASK;MCG_C1 = MCG_C1_CLKS(2) | MCG_C1_FRDIV(3);while (MCG_S & MCG_S_IREFST_MASK); while (MCG_S & MCG_S_CLKST_MASK) >> MCG_S_CLKST_SHIFT) != 0x2); MCG_C5 = MCG_C5_PRD

32、IV(0x0e); MCG_C6 = 0x0; set_sys_dividers(0,2,2,7);MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV(30); /VDIV = 31 (x55) pll_freq = 180;while (!(MCG_S & MCG_S_PLLST_MASK); while (!(MCG_S & MCG_S_LOCK_MASK); MCG_C1 &= MCG_C1_CLKS_MASK;while (MCG_S & MCG_S_CLKST_MASK) >> MCG_S_CLKST_SHIFT

33、) != 0x3);return pll_freq;4.2.2 FTM模块本系统利用FTM模块产生脉宽调制波(PWM)来控制电机的转速，用FTM模块的正交解码功能读取光电码盘的脉冲数。FTM模块可以实现多功能定时，包括：PWM输出、输入捕捉、输出比较、定时中断、脉冲加减、脉冲周期脉宽测量。K60的FTM0有8个通道，可以实现电机或者舵机的PWM输出，但是没有正交解码功能，即对旋转编码器输入的正反向技术功能。K60的FTM1和FTM2可以实现正交解码功能，但是FTM1和FTM2都只有两个通道可以实现正交解码功能。用一个16位的计数器作为FTM模块的时间基准，可以读取该计数器的数值，读取的数值可作

34、为无符号数或者有符号数的补码对待。FTM模块的详细初始化程序见附录。4.2.3 PIT模块本系统使用PIT模块，产生以1ms为周期的中断。控制任务在中断服务程序中完成，具体过程为以下四步：（1）读取光电码盘脉冲数，累计电机转动圈数。（2）读取陀螺仪及加速度传感器的数值并进行角度融合。（3）车模直立控制8。（4）车模速度控制。PIT初始化程序代码如下：void pit_init(PITn_e pitn, uint32 cnt) ASSERT( cnt > 0 ); /用断言检测 时间必须不能为 0SIM_SCGC6 |= SIM_SCGC6_PIT_MASK; PIT_MCR = (0 /

35、| PIT_MCR_MDIS_MASK /禁用PIT定时器时钟选择（0表示使能PIT，1表示禁止PIT） /| PIT_MCR_FRZ_MASK /调试模式下停止运行（0表示继续运行，1表示停止运行） ); PIT_LDVAL(pitn) = cnt - 1 ; /设置溢出中断时间 PIT_Flag_Clear(pitn); /清中断标志位 PIT_TCTRL(pitn) &= PIT_TCTRL_TEN_MASK; /禁止PITn定时器（用于清空计数值） PIT_TCTRL(pitn) = ( 0 | PIT_TCTRL_TEN_MASK | PIT_TCTRL_TIE_MASK )

36、; /enable_irq(int)pitn + PIT0_IRQn); /开中断4.2.4 AD模块A/D转换即将连续的时间信号转换为相对应的数字信号，D/A转换则反之，A/D转换是数字信号处理中不可缺少的部分。一般在进行A/D转换之前要进行滤波处理。AD转换器的技术指标：（1） 分辨力 用输出的二进制数码的位移表示A/D转换的分辨率。转换的位 数越多，量化增量就越小，那么量化误差也就越小，随之分辨 率就变高了。常用的A/D有8位、10位、12位、16位、24位、 32位等。（2） 转换精度 A/D转换器的精度由最大化误差决定，大多转换器的末尾数字并 不可靠。A/D转换器包括模拟处理单元和数

37、字处理单元，所以转 换精度还应考虑模拟处理部分的误差。（3） 转换速度 完成一次A/D转换所用的时间即为转换速度，转换一次用的时 间越长，则转换精度越低。A/D转换的基本流程：初始化-开始转换操作-完成转换操作-获取AD值。使用A/D模块将陀螺仪，加速度传感器的输入信号进行模数转换，其精度为12位，再经过互补滤波转换为实际角度。详细的A/D采集程序代码见附录。4.3 控制算法设计4.3.1 直立控制车模的平衡控制经验来源于我们的生活经验。杂技演员可以让一根直木棒在手尖保持直立。要完成这个过程需要两个条件：一是表演者的手掌可以移动，二是可以通过眼睛观察到木棒的倾斜趋势和倾斜角度。然后通过手掌额移

38、动来消除木棒要倒得趋势，从而使木棒保持直立。这个简单的例子即为控制理论中的负反馈调节，两个条件缺一不可。智能车的平衡控制也是通过负反馈调节来实现的，与保持木棒的平衡相似。车模有两个车轮着地，车体会随着轮子向前或者向后转动而倾斜，我们通过控制车轮的转速来抵消车模倾斜的趋势来实现平衡。直立控制的目标是使智能车能够稳定的站在一点，同时和速度控制良好的配合。所以必须将二者的参数进行合理的调节才能达到最终的控制目标。在陀螺仪和和加速度传感器零点稳定的情况下，智能车可以静止在一点，对他施加干扰时，能快速恢复直立，轻推智能车，能够继续直立，但是不能停下来，会一直加速向前跑。加速度传感器和陀螺仪原理：我觉得很

39、多时候大家都被它的名字给误导了，准确的来说它测的不是加速度至少对于mma7260这类芯片它检测的是它受到的惯性力(包括重力！重力也是惯性力)。那又有人要问了F=ma惯性力不就是加速度么？加速度传感器实际上是用MEMS技术检测惯性力造成的微小形变，注意检测的是微小形变，所以，把加速度传感器水平静止放在桌子上它的Z轴输出的是1g的加速度，因为它Z轴方向被重力向下拉出了一个形变，可是它不是在以1g的加速度往下落。如果让它做自由落体运动，它的Z轴输出应该是0。给个形象的说法：可以把它看成是一块弹弹胶 它检测的就是自己在三个方向被外力作用造成的形变。从刚才的分析可以发现重力这个东西实际是个很神奇的东西。

40、它能隔空打牛，在不产生加速度的情况下对加速度传感器造成形变，在产生加速度的时候不造成形变，而其他力都做不到。可惜的是，加速度传感器不会区分重力加速度与外力加速度。所以，当系统在三维空间做变速运动时，它的输出就不正确了或者说，它的输出不能表明物体的姿态和运动状态。举个例子：当一个物体在空间做自由落体时，在X轴受到一个外力作用，产生g的加速度，这时候x y z轴的输出分别是g,0,0，如果这个物体被x轴朝下静止放在水平面上，它x y z轴的输出也分别是g,0,0所以说：只靠加速度传感器来估计自己的姿态是很危险而不可取的。加速度传感器的作用：加速度传感器,可以测量加速度,包括重力加速度,于是在静止或

41、匀速运动（匀速直线运动）的时候,加速度传感器仅仅测量的是重力加速度,而重力加速度与刚才所说的R坐标系（绝对坐标系）是固连的,通过这种关系,可以得到加速度计所在平面与地面的角度关系也就是横滚角和俯仰角计算公示如下 俯仰角  横滚角 陀螺仪的作用：陀螺仪可以测量角速度,具有高动态特性,但是它是一个间接测量器件,它测量的是角度的导数,角速度,显然要将角速度对时间积分才能得到角度看到积分，但是积分就会引起一个致命的问题，积分误差。积分误差的来源主要有两个：一个是积分时间积分时间Dt越小，输出角度越准一个是器件本身的误差假设陀螺仪固定不动,理想角速度值是0dps(degre

42、e per second)，但是有一个偏置0.1dps加在上面,于是测量出来是0.1dps,积分一秒之后,得到的角度是0.1度,1分钟之后是6度,还能忍受,一小时之后是360度,转了一圈所以说陀螺仪在短时间内有很大的参考价值，陀螺仪另外一个问题是它的测量基准是自身，并没有系统外的绝对参照物重力轴是个绝好的参照物 因此需要陀螺仪和加速度传感器的配合使用 如果要测偏航角YAW还需要电子罗盘感知地磁方向 给出水平方向的绝对参考（当然这个在智能车上不存在）。 陀螺仪和加速度传感器的融合：除了给出绝对参考系 陀螺仪和加速度传感器相互融合使用的最重要的原因是：综合考虑，加速度计是极易受外部干扰的传感器，但

43、是测量值随时间的变化相对较小。陀螺仪可以积分得到角度关系，动态性能好，受外部干扰小，但测量值随时间变化比较大。可以看出，它们优缺点互补，结合起来才能有好的效果，用通俗点的话来说，就是无论工作多久，加速度传感器如果没收到外部干扰它测的就一定是准的！ 陀螺仪虽不会受到外部干扰，可是时间长了，由于积分误差累计 它的值就全错了！所以两个数据融合的方法就是设计算法在短时间尺度内增加陀螺仪的权值，在更长时间尺度内增加加速度权值，这样系统输出角度就更真实了。再通俗点说就是：隔一段时间用加速度传感器的值修正一下陀螺仪的积分误差，然后在隔的这段时间内用陀螺仪本身的角度积分。所以将陀螺仪的检测值和加速度传感器的检

44、测值进行融合得到最终的倾角信息来控制车模直立。本系统的直立控制采用PD调节9，直立控制流程图如图4-1所示：图4-1 直立控制流程图直立控制程序代码如下：void AD_Calculate(void) Rd_Ad_Value(); Gyro_Now = -(float)(GYRO_VAL - ENC03 ) * Gyro_ratio;/将陀螺仪采集的数值进行归一化 angle_offset_vertical = (float)(MMA7361_vertical - MMA7361)*MMA7361_ratio ; /将加速度传感器采集到的角度归一化 if(angle_offset_vertic

45、al > 90) angle_offset_vertical = 90;/防止加速度传感器测量的角度数值溢出 if(angle_offset_vertical < -90) angle_offset_vertical = -90;/计算融合后的角度数值 /Kalman_Filter(angle_offset_vertical,Gyro_Now);QingHua_AngleCalaulate(angle_offset_vertical,Gyro_Now);/互补滤波void Speed_Calculate(float angle,float angle_dot) /速度计算 Spee

46、dControl(); speed_Start = angle * P_ANGLE + angle_dot * D_ANGLE ; /直立时所要的速度 Speed_L = speed_Start;/左轮总速度 Speed_R = speed_Start;/右轮总速度 Speed_L1 = g_fSpeedControlOutNew;/左轮总速度 Speed_R1 = g_fSpeedControlOutNew;/右轮总速度 /将最大PWM输出限制在 985 个 PWM 内 if(Speed_L > 985) Speed_L = 985; if(Speed_L < -985) Spe

47、ed_L = -985; if(Speed_R > 985) Speed_R = 985; if(Speed_R < -985) Speed_R = -985; if(Speed_L1 > 1000) Speed_L1 = 985; if(Speed_L1 < -1000) Speed_L1 = -985; if(Speed_R1 > 1000) Speed_R1 = 985; if(Speed_R1 < -1000) Speed_R1 = -985; Speed_L_Last = Speed_L; Speed_R_Last = Speed_R; /Spee

48、d_L_Last = Speed_L - Speed_L1; /Speed_R_Last = Speed_R - Speed_R1; if(Speed_L >= 0) /角度>0则车模向前倾，反之向后倾 ftm_pwm_duty(FTM0,FTM_CH4,0); ftm_pwm_duty(FTM0,FTM_CH5,(uint32)(Speed_L_Last - MOTOR_DEAD_VAL_L); /电机死区电压 else ftm_pwm_duty(FTM0,FTM_CH5,0); ftm_pwm_duty(FTM0,FTM_CH4,(uint32)(-Speed_L_Last -

49、 MOTOR_DEAD_VAL_L); /加入死区电压 if(Speed_R >= 0) /angle 大于 0，向前，小于 0，向后 ftm_pwm_duty(FTM0,FTM_CH7,0); ftm_pwm_duty(FTM0,FTM_CH6,(uint32)(Speed_R_Last - MOTOR_DEAD_VAL_R); /加入死区电压 else ftm_pwm_duty(FTM0,FTM_CH6,0); ftm_pwm_duty(FTM0,FTM_CH7,(uint32)(-Speed_R_Last - MOTOR_DEAD_VAL_R); /加入死区电压 4.3.2 速度控

50、制直立车模的速度控制较普通车模比较复杂，因为要同时保证车模的直立和稳定运行，直接通过改变电机的转速是不能实现速度控制目标的。速度控制的目的是能让智能车在短时间内能够达到预设的速度，同时对直立控制不造成影响。速度控制采用的是PI控制：为了使速度有一定的物理意义，对采集到的光电码盘的脉冲数要进行单位转换。根据单位的物理含义10，可以确定转换单位时的比例值。本算法中设定智能车的反向速度为0。速度控制流程图如图4-2所示：图4-2 速度控制流程图速度控制程序代码如下：void SpeedControl(void) float fDelta; float fP, fI; AD_Calculate();

51、DELAY_MS(500); val = ftm_quad_get(FTM1); ftm_quad_clean(FTM1); if (g_fCarAngle>=0) g_fCarSpeed = val; else g_fCarSpeed = -val; val = 0; g_fCarSpeed *= CAR_SPEED_CONSTANT; fDelta = CAR_SPEED_SETfDelta - g_fCarSpeed; /倾角偏差计算 fP = fDelta * SPEED_CONTROL_P; fI = fDelta * SPEED_CONTROL_I; g_fSpeedCont

52、rolIntegral += fI; /倾角积分数值 g_fSpeedControlOutOld = g_fSpeedControlOutNew; g_fSpeedControlOutNew = fP + g_fSpeedControlIntegral;4.4 本章小结本章详细论述了智能车的软件方案，其中包括总体软件流程图，及各个模块的初始化，具体的直立控制和速度控制怎么实现等内容，本章内容是本文的最核心内容，对整个智能车的制作有很大的指导意义。第5章 视觉导航智能车姿态控制系统调试5.1 调试工具简介本系统在开发过程中用到的调试工具有：（1 ）用IAR中自带的Live Watch来观察变量1

53、1，使用这个工具带来了很大的便利，可以实时的知道每个变量的动态值，为调试带来了很大的方便。调试界面如图5-1所示。图5-1调试界面（2） 单片机多功能调试助手，使用该软件可以通过串口通信和下位机通信。单片机多功能调试软件主界面如图5-2所示：图5-2 单片机多功能调试软件主界面（3） 使用Visual Scope虚拟示波器上位机观察加速度传感器的值和融合后的角度，此软件可以将变量的值绘制成曲线12，使调试更加准确方便。虚拟示波器显示界面如图5-3所示：图5-3 虚拟示波器显示界面5.2 调试方法与步骤调试方法为使用IAR的Live Watch观察变量的动态值是否正确合理，用串口助手将变量发送到上位机，观察变量值是否正确，使用Visual Scope13将加速度传感器归一化后的值和融合后的角度绘制成波形显示出来。具体的调试步骤如下：（1） 加速度传感器值归一化1）测量三种位置时加速度传感器值 加速度传感器零偏值MMA7361_vertical即将车模直立放置时，读取Z轴此时加速度传感器的AD值 正90°值即将车模向前偏置90°，读取此时Z轴加速度传感器的AD值 负9