毕业论文-基于PID控制算法的自平衡机器人.doc

中北大学2016届毕业设计说明书毕业设计说明书基于PID控制算法的自平衡机器人学生姓名： 学号： 学 院： 仪器与电子学院专 业： 微电子学 指导教师： 2016年6月PID控制算法的自平衡机器人摘 要 随着机器人研究的进一步深入，在工业生产、安防系统、智能家居、物流网等领域的应用更加广泛，在实际应用中，可能遇到复杂的任务环境。相比多轮的轮式机器人，两轮自平衡机器人体积小，运动灵活，能够在比较狭窄、需要大转角的场合中运动。这种机器人两轮共轴，可以通过运动保持自身平衡，能实现前进、后退、转向、原地静止等基本运动功能，由于其结构特殊，能适应不同的地形环境，研究两轮自平衡机器人，具有重要的意义。本设计以两轮车模为研究平台，以恩智浦公司32位微处理器MK60FX512VLQ15为控制核心。通过加速度传感器和陀螺仪，设计滤波算法，实现了倾角的测量；通过设计MOS电机驱动电路，使用单片机输出PWM，实现了电机速度控制；通过编写控制算法，实现了两轮车模的自平衡功能；通过编码器测量电机转速，对速度进行闭环控制，实现了两轮自平衡机器人的速度控制功能。最终，小车能够原地直立无漂移达十分钟以上，可实现原地零半径回转，并且能够控制移动速度，直线运动速度可达到2m/s。通过无线遥控器，使小车能够在自平衡的基础上，实现自由运动的任务。关键词： 单片机，PID，倾角测量，自平衡，MOS驱动Self-balancing robot based on PID algorithmAbstract Along with the further robotics research, and robots was widely used in the industrial production, security systems, intelligent household and IOT. Inapracticalapplication, the robots may encounter complex task environment. Compared with the Wheeled-robot which has several wheels, self-balancing two-wheel robot has the advantages of flexible movement because of its small size; its suitablein themorenarrowuseof theenvironment. This kind of robot has two wheels of coaxial, to keep balance by way of movement. The robot can achieve the basic movement function; it can move forward and backward, turn left and right, as well as still standing. Due to its special structure, it can adapt to different terrain environment. For the reason given above, researches on the Two-Wheeled Self-Balancing Robot would be significant.This design is based on a Two-Wheeled car, and uses the Microcontroller MK60FX512VLQ15 as the controllingkernel. Themainworksaccomplishedin the paper include:(1)Designing a filter algorithm,achieving Angle measurement based on Inertial Measurement Unit - accelerometer and gyroscope.(2)Designing a driving circuit of MOSFET, achieving motor speed control through PWM technology.(3)Designing control algorithm, achieving self-balancing.(4)Measuring motor speed, achieving speed control under the condition of self-balancing through Closed-Loop Control. Finally, the robot can stands upright without drift for ten minutes or more, and has a zero turning radius. In addition, the speed of the robot is controllable, speed up to 2m/s. By using a remote control, the robot can realize free movement.Key words: Microcontroller, PID, Measurement of angle，Self-BalancingDriving circuit of mosfet目录1 绪论11.1研究目的和意义11.2国内外发展现状11.3设计内容22 总体设计方案32.1 设计思路32.2 硬件设计方案42.3 机械结构设计方案53 硬件电路设计73.1 单片机最小系统电路73.2 加速度传感器模块83.3 陀螺仪模块93.4 电机驱动模块103.5 电源管理模块114 软件程序设计124.1 系统流程图124.2 程序初始化134.3 PID控制算法144.3.1 PID控制算法介绍144.3.2 PID控制算法参数整定174.4 倾角估计算法174.4.1 互补滤波法184.4.2 卡尔曼滤波法194.5 直立控制程序设计194.6 速度控制程序设计214.7 方向控制程序设计225 开发工具及调试结果245.1 开发工具介绍245.1.1 MDK介绍245.1.2 Serial Digital Scope介绍245.2 调试结果245.2.1 PID算法控制电机调试结果245.2.2 互补滤波法调试结果265.2.3 卡尔曼滤波法调试结果266 总结与展望28附录A 主控板电路图29附录B 电机驱动电路图30附录C 主要源程序31参考文献37致谢40II1 绪论1.1 研究目的和意义 随着机器人研究的进一步深入，在工业生产、安防系统、智能家居、物流网等领域的应用更加广泛，在实际应用中，可能遇到复杂的任务环境。相比多轮的轮式机器人，两轮自平衡机器人具有体积小，运动灵活等特点，能够在比较狭窄、需要大转角的场合中运动1。这种机器人两轮共轴，可以通过运动保持自身平衡，能实现前进、后退、转向、原地静止等基本运动功能，由于其结构特殊，能适应不同的地形环境，研究两轮自平衡机器人，具有重要的意义2。其次，两轮自平衡车还可以作为代步交通工具,相比传统的代步工具如自行车、电动车等，两轮自平衡车体积大幅减小，重力大幅减,运动灵活，绿色环保3。常见的轮式机器人主要是以四轮或三轮为主的多轮式结构，与之相比，两轮式自平衡机器人主要有如下优点：（1）转向灵活，能够实现原地任意半径和任意角度转向，弥补了多轮机器人的缺点。（2）体积小，对于在环境狭窄的空间中尤为适用；（3）重量轻，大幅度减小了车体结构；（4）绿色环保，功率小，适合于电池供电的系统，在同等能量密度电池的情况下，可以大幅度延长续航时间。1.2 国内外发展现状 两轮自平衡机器人最早是由日本电气通信大学的喀左教授于1986年提出的，但是由于当时的控制原理和策略还不成熟，其设计的机器人只能在单一方向运动，无法实现转弯4。 2002 年，瑞士联邦工业大学的Felix Grasser等人设计出可遥控的自平衡机器人，这个机器人两轮共轴，每个轮子有独立的驱动，能实现速度控制、转弯等运动功能，运动速度可达到1.5m/s。相比之前的作品，该机器人已趋于完善。 2002 年，美国的赛格威公司设计了第一辆载人电动平衡车Segway，设计时速可达20km/h，并且成功推向市场，但是价格昂贵，高达5000美元5。 近几年，国内也有很多高校和企业开始研究自平衡机器人。2004 年，中国科学技术大学成功设计了一款载人两轮自平衡车，可实现零半径转弯。其使用简单，用户经过10分钟简单的练习就可以驾驶，正常工作时，使用者通过身体前倾和后仰来控制加速和减速，通过操作手柄，可控制平衡车转弯，行驶速度达到 10 km/h,充满电可行驶约30 km。根据自平衡机器人质心的位置是否可变，可将自平衡机器人分为变质心自平衡机器人和定质心自平衡机器人两大类，由于变质心机器人结构复杂，而定质心机器人结构简单，容易实现，可控性高，目前国内外的研究主要是对定质心自平衡机器人的研究6。1.3 设计内容 使用ARM Cortex-M4内核的Kinetis K60单片机为主控制器，对加速度计和陀螺仪的数据进行融合，得到车身倾角最优估计，设计MOS电机驱动电路，编写PID控制算法控制车轮，达到自主直立的目的。利用编码器构成速度反馈回路，使用PID控制算法进行速度和转向的控制，最终使其在自平衡的基础上，完成期望的运动。本文将就硬件设计、滤波算法设计、控制算法设计等内容展开研究，具体内容如下： （1）硬件电路设计：设计制作主控制器电路板、电机驱动控制板，（2）传感器数据融合：编写算法，将陀螺仪所测数据和加速度计所测数据进行融合，得到最优倾角值7。（3）控制程序设计：通过对状态反馈控制、PID算法等方法和理论的研究，设计控制程序，使小车能够完成直立、前进、后退、转向等运动功能。（4）单片机程序设计：设计单片机程序，使其具备人机交互、控制等功能。2 总体设计方案2.1 设计思路 从控制的角度来看，两个直流电机是该机器人系统唯一的控制对象，机器人的运动控制任务可以分解为以下三个独立控制任务8： （1） 直立控制任务：检测机器人的倾角，并设定目标倾角值，使用PD算法进行闭环控制，使控制车模稳定在平衡位置。 （2） 速度控制任务：直立自平衡车的速度控制与普通的多轮车速度控制不同，在直立系统中，速度控制是通过改变车身倾角来实现的9。具体实施思路是，对电机转速加入干扰，使车身偏离平衡位置，以此刺激直立控制任务，从而达到控制速度的目的，速度控制使用PI算法10。（3） 方向控制任务：通过控制两个电机的差速来达到转向的目的，方向控制使用PD算法，使用X轴的角速度作为微分项的因子，可以极大改善转向的动态性能，避免振荡。程序设计中，三个控制任务独立进行计算和控制，但是每一个任务的控制对象都是电机，因此它们直接也存在着干扰与耦合。在设计每一个控制任务时，为了便于分析，都假设其他两个任务是稳定的。例如，在进行速度控制程序设计时，假设机器人是能够稳定直立的；在进行方向控制时，假设机器人的直立控制和速度控制都是稳定的；在进行直立控制时，假设机器人的速度控制和方向控制都是稳定的11。这三个任务中保持机器人平衡是最首要的任务，由于机器人同时受到三种控制的影响，从直立控制的角度来看，速度、转向两个控制任务会干扰直立控制的稳定性12。因此，虽然三个控制任务独立运行，但是它们之间有优先级，即控制应该最优先满足直立的要求，其次是方向控制的要求，最后才是速度控制的要求。图2.1 系统结构框图2.2 硬件设计方案 本系统采用一个主控电路板+一个电机驱动电路板的结构，将两者分开设计，一是可以避免电机驱动对主控的影响及电磁干扰，二是出于经济的考虑。 主控制电路板主要包括以下部分：单片机核心电路、电源管理电路、单片机接口、按键电路、蜂鸣器电路等。其中，电源管理电路分为3.3V稳压电路和5V稳压路，5V稳压电路使用LM2940三端线性稳压器设计，该稳压器输入7.2V电池电压，输出5V电压；3.3V稳压电路使用LM1117三端线性稳压器设计，该稳压器输入接LM2940的5V电压，输出3.3V电压。考虑到本系统中器件、传感器较多，因此5V管理电路和3.3V管理电路均使用两个。微控制器接口主要包括：OLED接口、蓝牙接口、MMA7361传感器接口、L3G4200D传感器接口、编码器正交解码接口2个、四通道PWM接口、遥控器解码接口，以及预留IO，方便调试使用。图2.2 主控电路板结构图2.3 机械结构设计方案 车模的整体机械结构对整个系统有直接影响，因为系统是在机械结构上实现的。车模的机械参数如车体重心、车身高度、传感器排布与布局、齿轮松紧度等，都对整个系统起着至关重要的作用，良好的机械结构可以使系统更稳定。经过多次测试与调试，我们发现重心越低越好，越集中越好，重心低会使控制更顺滑，直立更稳定，抗干扰能力更强，而重心集中则会让车模转向更加灵活。因此，尽量降低重心，使结够更紧凑，是机械调整中最重要的部分。整个系统中，重量最重的当属7.2V镍镉电池，电池的安放高度，大大影响整个系统的重新高度，本系统中，将电池座通过螺丝和热熔胶固定在较低的位置，电池则通过扎带捆绑在电池座上。系统中的两块电路板体积较大，重力则仅次于电池，并且还有导线、传感器等连接在电路板上，因此应该将电路板尽量放低，对于降低重心也有极大帮助。加速度计易受电机振动的影响，车模上最受震动影响最小的位置在轮轴中心附近。由于加速度计Z轴在垂直于水平面时灵敏度最高。因此将加速度计以垂直于水平面的角度安装在轮轴中央，使用热熔胶加以固定。图2.3 机械结构图3 硬件电路设计3.1 单片机最小系统电路恩智浦K60(原飞思卡尔)系列单片机，是基于ARM Cortex-M4处理器内核的超低功耗、混合信号处理单片机，Cortex-M4内核带DSP指令，性能可达1.25 DMIPS/MHz (部分型号集成浮点运算单元)。 本系统使用MK60FX512VLQ15单片机为控制核心，该单片机主频高达150MHz，内部集成512KB Flash、144KB SRAM，可以轻松应对大多数的嵌入式应用场合。片内集成了4个可配置分辨率的高速16位模数转换器（ADC）、两个12位数模转换器（ADC）、4个可输出PWM的高速比较器、6个支持IrDA的UART、3个硬件IIC总线接口和3个硬件SPI总线接口、32通道DMA控制器以及大量的多功能定时器，丰富的片内外设资源，不仅大大减轻了系统设计的困难，还有效降低了成本。此外，该单片机提供十种低功耗模式，运行功耗低至350uA/MHz,静态功耗低至5.6uA，在高性能应用场合也可有效降低系统功耗，降低发热量13。该单片机采用LQFP144封装，性能优良，能够胜任本设计要求。图3.1主控芯片原理图3.2 加速度传感器模块 MMA7361是恩智浦公司（原飞思卡尔）的微型电容式加速度传感器，其功耗低，体积小，价格便宜。内部集成一级低通滤波器，具有温度补偿、自我测试，自由落体检测等功能。运行电压2.2-3.6V，模拟量输出，具有两种不同的灵敏度可选择。正常工作状态下电流消耗400uA，在休眠模式下仅需3uA电流。MMA7361三轴模拟加速度计的引脚排列如图3.2所示，引脚功能如表3.1所示：图3.2 MMA7361引角排列图表3.1 MMA7361引角功能引脚名称描述参数1N/C空脚/2XOUTX轴加速度输出0-3.3V3YOUTY轴加速度输出0-3.3V4ZOUTZ轴加速度输出0-3.3V5VSS地GND6VDD电源3-5V7Sleep芯片休眠控制引脚0:休眠；1：工作8N/C空脚/90g-Detect自由落体检测0:正常；1：自由落体10g-Select量程选择0：2g;1:4g12N/C空脚/13Self Test芯片自我测试与初始化/14N/C空脚/图3.3 MMA7361原理图3.3 陀螺仪模块 L3G4200D是意法半导体公司推出的16位三轴数字陀螺仪，该传感器功耗低，温漂小。具有三种可选分辨率（250/500/2000 dps），集成高通、低通滤波器，用户可自由选择带宽，工作电压2.4-3.6V，通过IIC/SPI接口输出数据。图3.4 L3G4200D原理图3.4 电机驱动模块 单片机要通过PWM控制直流电机转动，就需要电机驱动电路。电机驱动电路的性能直接关系到直流电机的性能，进而关系到整个系统的稳定性。常见的电机驱动方案有种：（1）采用集成电机驱动芯片；使用集成电机驱动芯片设计（常用的电机驱动芯片有BTS7970、L298N等），外围电路简单、可靠性高，但是驱动的性能有限。由于本系统车模的电机的内阻小于10毫欧，而集成电机驱动芯片内部的每个场效应管导通电阻在 120 毫欧以上，这样，电机驱动回路总电阻就太大了，驱动电路的输出功率和发热量都很大，电机性能得不到充分发挥。 （2）采用N沟道场效应管和专用栅极驱动芯片设计。 分立的 N 沟道场效应管的导通电阻很低，一般小于10毫欧，大大减小了电机驱动回路总电阻。另外，专用的栅极驱动芯片可以提高场效应管的开关速度，使PWM控制方式的调制频率提高，从而减少电枢电流脉动14。 经过多次试验与调试，我们最终选用半桥电机驱动芯片IR2184，它可以驱动高端和低端两个N沟道场效应管，能提供较大的栅极驱动电流，并具有硬件死区、硬件防同臂导通等功能。使用两片 IR2184半桥驱动芯片可以组成完整的直流电机 全桥式驱动电路。其性能优异、价格便宜，所以我们选择它进行设计15。图3.5电机驱动原理图3.5 电源管理模块电路设计中电源管理电路可分为线性稳压电源（7805、LM2940、LM1117等）和开关型稳压电源（LM2596、LM2941等）两大类。线性稳压电压纹波小、电路简单，但是效率较低，功耗较大。对于单片机和传感器等器件，对电源的纹波有较高要求，一般不需要较高的电流，因此应该选择线性稳压电源；开关稳压电源功耗小，效率高，但电路却比较复杂，电路的纹波大，对于舵机等器件电源的功率输出有较高要求，而对于纹波要求较低，因此应该选择开关型稳压电源16。LM2940:LM2940-5.0一款低压差三端稳压器，固定输出电压5V，最大输出电流1A，当输出电流为1A时，输入输出电压差为0.8V，最大输入电压为26V。它具有纹波小、转换效率高、电路结构简单、价格便宜的优点。本系统用其为加速度传感器、陀螺仪、编码器供电。 LM1117：LM1117是一个低压差线性三端稳压器，有5个固定电压输出的型号：1.8V、2.5V、2.85V、3.3V和5V，电压输出精度高达1%，当其在输出电压3.3V，输出电流为800mA时输入输出电压差为1.2V。本系统使用固定3.3V输出的LM1117为单片机、OLED等模块供电。 三端稳压器电路设计中滤波电容：利用三端稳压器设计稳压电路，输入端通常接一个电容值较大的电解电容，可以起到滤波作用，提高稳压器工作的稳定性和瞬态响应17。图3.6电源管理电路4 软件程序设计4.1 系统流程图图4.1 程序流程图4.2 程序初始化 程序运行，需要对单片机寄存器和传感器进行初始化。初始化主要包含以下内容：（1）发光二极管初始化程序在运行过程中，发光二极管按照一定的频率亮灭，作为程序运行的标志，方便观察程序运行状态。发光二极管初始化即通用I/O口初始化，将I/O口方向配置为推挽输出模式。（2）ADC及MMA7361传感器初始化本系统中，加速度计输出模拟量，因此需要通过模数转换器进行读取，MK60单片机内部集成4个16位ADC转换器，通道多达数十个，分辨率高，转换速度快，可以省去片外模数转换器。此部分程序，将ADC配置为10位单端输入模式，并且初始化MMA7361相应的控制引角。（3）OLED初始化：本系统使用OLED+按键组合构成基本的人机交互系统，具有便于调试参数、观察系统运行情况等功能。此OLED模块使用SPI接口，分辨率达到128*64。（4）IIC接口及L3G4220D传感器初始化：L3G4200D传感器通过IIC/SPI接口输出16位数字量，本系统使用硬件IIC接口对L3G4200D传感器进行读取，时钟频率400Khz。初始化程序中，需要完成对传感器的唤醒，将采样频率配置为1000Hz，量程为2000dps。（5）PWM初始化：本系统通过PWM控制电机转动，两个电机总共需要四个PWM输出通道，PWM频率配置为10Khz，并且设置初始占空比为0。（6）正交解码接口初始化：本系统使用AB相编码器测量电机转速，AB相输出方波频率相同，相位不同，根据相位差来判断转向。通过定时器的正交解码功能，可以方便获得电机转速。本系统有两个轮子，使用两个编码器分别测速，因此需要两个正交解码通道。（7）UART及其DMA初始化：UART即通用异步收发器（Universal Asynchronous Receiver/Transmitter），本系统使用UART借口的蓝牙模块同上位机进行通信，方便进行调试和观测数据。DMA指直接存储器访问技术（DirectMemoryAccess），利用该技术，外部设备不通过CPU而直接与系统内存交换数据，从而解放CPU，大大提高CPU工作效率。（8）按键及其中断初始化：本系统使用按键作为人机交互的唯一输入接口，共使用了五个独立按键。中断是指当发生特定事件时，CPU从当前程序转向特定程序段（中断服务程序）执行的功能。本系统中，将按键一端接地，IO口接上拉电阻，当按键按下的时候，会在对应IO上产生下降沿，因此我们将按键对应的IO口配置为下降沿触发中断。（9）周期性定时器初始化：为了固定的控制周期，尤其是要确保陀螺仪的积分频率稳定，本系统使用PIT周期性定时器产生控制终端，中断周期3ms。（10）NVIC嵌套向量中断控制器初始化：Cortex-M4内核为处理器提供了强大的中断管理功能，NVIC即Nested Vectored Interrupt Controller的缩写。通过NVIC控制器，可以灵活地配置中断优先级，可确保高优先级程序优先执行，提高实时性 18。4.3 PID控制算法4.3.1 PID控制算法介绍在自动控制中，按照偏差（目标值与反馈值之差）的比例(Proportion)、积分（Integration）、微分（Differential）的组合进行控制的方法称为PID控制算法。由于其原理简单、稳定性好、可靠性高、易于调整等优点，在自动控制领域应用最为广泛，已有近70年历史，现有的很多控制方法都是基于PID控制算法发展演变而来19。当我们对目标系统和控制对象的模型不够了解，或者不能得到控制系统的参数时，PID控制算法尤为适用20。图4.2 PID算法结构（1）比例控制部分：比例控制的计算因子是目标值与测量值之间的偏差，当偏差产生时，比例控制部分立即产生控制作用，以消除偏差，偏差越大，比例控制作用越明显，反之，控制作用越弱，比例控制在很大程度上决定PID控制器的控制趋势，控制作用最为明显21。（2）积分控制部分：积分控制的计算因子是偏差的和，可以消除静差，提高系统的无差度，其控制作用会有明显的滞后现象，如果使用不当，容易产生低频振荡，积分控制作用的强弱主要取决于积分时间常数Ti，Ti越小，积分作用越强，反之，则越弱。（3）微分控制部分：微分控制的计算因子是偏差的变化率，能够抑制偏差的变化，微分控制可以预测偏差的变化趋势，因此可以在偏差变大之前，产生提前的控制效果，从而加快系统调节速度，微分控制可以提高系统的动态性能，提高反应速度，克服振荡。 PID控制算法的计算结果是偏差信号的比例、积分、微分三部分的线性叠加，偏差值e(t)即目标值r(t)与反馈值c(t)的差，属于闭环控制系统，因此需要使用传感器构成反馈回路。PID控制器的微分方程： （式4.1）其中，。PID控制器的传输参数 （式4.2）PID控制器的差分方程： （式4.3）其中， 称为比例项 称为积分项 称为微分项 工程实际中，针对不同的控制对象和应用场合，常常不需要比例、积分、微分三部分同时作用，而是只使用三项中的一项或者二项，其中，比例控制是必须的22。常用的控制方式：（1） P控制（2） PI控制（3） PD控制（4） PID控制 在实际应用中，通常依据控制器输出与执行的对应关系，又可将基本的PID算法分为以下两种类型：（1）位置式PID算法 （式4.4）（2）增量式PID算法 =（式4.5）4.3.2 PID控制算法参数整定PID控制算法调试过程中，最重要的就是PID算法的参数Kp、Ti、Td的确定，参数的合理性，很大程度上决定了系统控制的稳定性。PID的参数整定方法有很多种，概括起来有主要两大类23：（1）理论计算整定法：主要是根据系统的数学模型，经过理论计算得到大致参数。然而在实际应用中，被控系统的数学模型往往很难建立，并且在不同的外加条件下，系统模型大不相同，因此理论计算得到的参数通常不能直接使用，还需要在调试过程中调整和修改24。（2）工程整定法：主要根据工程调试经验，在试验中根据系统的响应对参赛进行调整。这种方法简单可靠，易于掌握，在工程应用中往往使用这种方法。 4.4 倾角估计算法要控制车模平衡，首先就需要测量车身倾角。测量倾角主要有两种方法:（1）利用加速度传感器：加速度计静止放置的时候，因为受到重力加速度的影响，三个轴感应到的加速度跟传感器与地平面之间的倾角有关，具体关系如下图所示。图4.3 三轴加速度计与重力加速度的关系加速度传感器的值与地面之间的倾角呈三角函数关系，在加速度计仅受重力加速度作用时，倾角可通过以下几种方法计算：然而小车在运动过程中，加速度计收到多个方向加速度的干扰，同静态时不同，取加速度传感器瞬时值计算倾角误差太大。难以获得稳定的倾角数据。（2） 利用陀螺仪测量的角速度进行积分：角速度积分可以得到变化的角度。但是传感器都有误差，陀螺仪也不例外，而且陀螺仪还存在温漂，因此长时间的积分过程，很难保证得到正确的角度25。因此需要一种算法，能够结合陀螺仪的动态性能和加速度传感器的静态精确度，从而得到可靠的倾角数据26。4.4.1 互补滤波法互补滤波算法，通过加速度计对陀螺仪积分所得角度进行校正，以避免陀螺仪产生较大积分误差和漂移，使得系统既保持了陀螺仪的动态性能，又具有加速度计的静态精确度 27。互补滤波器参数少，程序简单，效率高，在低成本的惯性导航系统中应用广泛。互补滤波算法： 其中，K代表互补滤波中的权值，K越大，对加速度传感器的信任程度就越大，dt代表采样时间。在使用中，根据实际情况和需要调节K和dt，直到满意为止。 4.4.2 卡尔曼滤波法卡尔曼滤波法源于匈牙利数学家Rudolf Emil Kalman的论文A New Approach to Linear Filtering and Prediction Problems（线性滤波与预测问题的新方法）。卡尔曼滤波算法是一个最优化自回归数据处理算法28，对于很多工程问题，它的效率很高，可靠性很高。卡尔曼滤波法广泛应用已经超过了30年，包括控制、制导、导航、通讯等现代工程等，近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等29。4.5 直立控制程序设计 车模直立控制需要进行闭环控制，因此就需要能够测量车体的倾角，构成反馈回路。在能够测量倾角的情况下，设定目标平衡角度，使用PID算法进行控制。因为车体只会在一个的方向上运动，只存在一个维度的倾斜，因此只需要测量一个维度的倾角，然后控制轮子转动，抵消车体在该方向上的倾斜，就可以完成平衡控制30。如图2.2.1所示。 图4.4 直立原理我们对系统进行简单建模，首先，两轮自平衡车可以简化为一个一阶倒立摆模型，假设车体的高度为 L，质量为 m ，在外力作用下，车模产生角加速度为x(t)。如图4.5所示，沿着垂直于底盘的方向进行受力分析，可以得到运动加速度a(t)以及外力干扰加速度x(t)之间的运动方程。图4.5 系统模型车模运动方程： (式4.6)(式4.7) (式4.8)模引入比例、微分控制之后的系统框图如图2.2.3所示：图4.6 引入比例、微分控制之后的控制系统框图其中k1,k2分别为比例与微分控制参数。其中微分参数相当于阻尼力，可以有效抑制车模荡31。因此，车模的平衡控制可以简化为测量车模的倾角和倾角速度控制车模车轮的加速度来消除车模的倾角32。图4.7 直立控制算法流程图4.6 速度控制程序设计 直立自平衡车的速度控制方法与普通多轮车的速度控制方法不同，因为普通车模的电机的转速与速度成正比，而自平衡车的电机转速是平衡控制、速度控制、方向控制三部分的叠加9。因为直立系统的首要任务是保持直立，当车模倾角发生改变，车模就会在直立控制的作用下向倾斜方向运动。速度控制就是利用了直立系统的这种特性34。改变车模倾角有两种方法：（1） 改变车模直立控制的目标角度，直立控制PD算法中，有一个目标倾角，当目标倾角不是真正的平衡位置倾角时，车模会一直运动。通过编码器测量转速，形成速度反馈回路，就可以使用PID算法进行速度调节。（2）给电机施加干扰输出。在直立过程中，如果增加或者减少电机输出，车模就很难维持平衡，在直立控制的作用下，车模就会运行。通过编码器测量转速，形成速度反馈回路，就可以使用PID算法进行速度调节。本系统中，使用第二种方法对速度进行控制，因为第二种方法对直立的影响较小，可以提高系统的稳定性，但是速度控制的灵敏度没有第一种高。图4.8 速度控制算法流程图4.7 方向控制程序设计 直立车模的转向是通过两轮的差速来完成的，转向控制使用PD算法，本系统不具备自主识别路径的功能，因此PD算法的输入参数使用遥控器完成。为了改善转向的动态特性，避免振荡，对PD算法做了修改，微分项的因子不再使用传统的e(t)-e(t-1)，而是使用陀螺仪的X轴，因为陀螺仪X轴对应着转弯的角速度，有着更高的灵敏度和精度。图4.9 方向控制算法流程图5 开发工具及调试结果5.1 开发工具介绍 5.1.1 MDK介绍MDK集成嵌入式开发系统源自德国Keil公司，支持ARM Cortex-M、Cortex-R4、ARM7、ARM9等内核,专为微控制器应用而设计，简单易学、功能强大，提供大量方便易用的插件，支持仿真、在线调试，支持C/C+语言和汇编语言，并且可以联合编写，编译器效率高，具有多种优化等级，用于开发大多数嵌入式软件非常方便。MDK功能特点：l 拥有高效率的ARM C/C+编译工具链l 支持Cortex-M0/M3/M4、Cortex-R4、ARM7和ARM9等系列微控制器。l 支持调试和仿真l 提供Keil RTX嵌入式实时操作系统l 提供TCP/IP协议栈l 提供USB设备和USB主机协议栈l 提供嵌入式GUI程序库l 执行分析工具和性能分析器可优化程序l 大量的项目例程，可以帮助用户熟悉MDK使用方法l 符合CMSIS (Cortex微控制器软件接口标准)标准5.1.2 Serial Digital Scope介绍 Serial Digital Scope是一款基于串口通信的虚拟示波器软件，具有四个通道的数据分析功能，支持最大波特率115200。协议简单可靠，具备CRC校验功能，可以很方便地移植到嵌入式设备中，在调试过程中，该软件带来了极大便利。5.2 调试结果 5.2.1 PID算法控制电机调试结果 为了学习和验证PID算法，设计了位置式PID算法在电机空载情况下进行测试，经过大量的参数整定和观察，掌握了PID算法的参数整定方法，以下是调试结果。图5.1 PID算法方波跟踪效果图5.2 PID算法锯齿波跟踪效果图5.3 PID算法正弦波跟踪效果如图所示，黄色代表速度的目标，红色是测量的实际速度。5.2.2 互补滤波法调试结果 互补滤波主要调节的是对于加速度计的比重K,K越大，则跟踪速度越快，但滤波效果越差。根据需要，选择适合系统的K系数。下面两幅图分别为K=0.01,和K=0.05时的滤波效果。红色代表加速度计数据，黄色代表陀螺仪数据，蓝色代表最终融合数据。当K=0.01时，滤波效果更明显，平滑度很高。当K=0.05时，跟踪效果好，平滑度差。图5.4 互补滤波效果（K=0.01时）图5.5 互补滤波效果（K=0.05）5.2.3 卡尔曼滤波法调试结果 以下是卡尔曼滤波法的滤波效果，滤波效果明显，收敛速度快，本系统使用卡尔曼滤波法作为倾角估计的方法。图5.6 卡尔曼滤波效果6 总结与展望随着机器人研究的进一步深入，在工业生产、安防系统、智能家居、物流网等领域的应用更加广泛，在实际应用中，可能遇到复杂的任务环境。相比多轮的轮式机器人，两轮自平衡机器人体积小，运动灵活，能够在比较狭窄、需要大转角的场合中运动。这种机器人两轮共轴，可以通过运动保持自身平衡，能实现前进、后退、转向、原地静止等基本运动功能，由于其结构特殊，能适应不同的地形环境，研究两轮自平衡机器人，具有重要的意义。本设计以32位微控制器MK60FX512VLQ15为核心，自行设计了主控电路板和电机驱动电路板，以两轮车模为研究平台，编写C语言程序，并进行了大量的实验与验证。分别研究了互补滤波和卡尔曼滤波，最终选择卡尔曼滤波算法作为倾角融合的算法。分别编写了不同的PID算法，实现了小车自平衡，以及在平衡状态下的速度控制和转向控制。最终，小车可以自平衡地实现各种运动的任务。系统具备OLED显示屏和独立按键，可以实现基本的参数设置和人机交互，通过无线蓝牙，可以将系统同外部设备如手机、电脑等连接起来，可进行无线控制和调试，通过遥控器，可以控制自平衡机器人运动，如果加入环境识别系统，如摄像头、CCD、红外对管传感器等，可以实现自主运动。长时间的调试与制作过程，已使本系统趋于完善。但是，目前主要还存在以下问题：1）倾角测量精确度问题：由于传感器存在误差，还存在温度漂移等现象，导致倾角测量的静态精确度难以达到较高水平；由于小车在运动中，陀螺仪和加速度计的噪声都很大，因此利用这这两种数据进行倾角估计，动态精确度很难达到较高水平。2）PID算法的参数整定问题：本系统中，在直立、速度、角度控制中均使用了PID算法，然而PID参数的确定主要依靠观察，针对于不同的情况，PID参数均一样，因此控制的稳定性、稳定速度都不够好。在接下来的学习中，我将继续努力。附录A 主控板电路图图A.1 主控板原理图图A.2 主控板PCB图附录B 电机驱动电路图图B.1 电机驱动原理图图B.2 电机驱动PCB图附录C 主要源程序/main.cint main()uint8_tdirswitchtemp,spswitchtemp; SmartCar_Init(); while(1) VisualScope_Out(); while(DMA_IsMajorLoopComplete(HW_DMA_CH2);if(StandUp_Flag=1&IS_RUNNING=0)dirswitchtemp=DirectionControlSwitch;spswitchtemp =SpeedControlSwitch;DirectionControlSwitch=0; SpeedControlSwitch=0;ZL.P*=1.5f;ZL.D*=1.5f;DelayMs(500);Motor_Enable();IS_RUNNING=1;/将运行标志置位DelayMs(500);StandUp_Flag=0;DelayMs(1000);ZL.P/=1.5f;ZL.D/=1.5f;SpeedControlSwitch=dirswitchtemp;DirectionControlSwitch=dirswitchtemp; /it.cvoid PIT0_ISR() static uint16_t FindZeroIndex=0;systime_speed+;/速度控制节拍+1systime_direction+;/方向控制节拍+1ADC_GetDataAndFilter();Angle_Calculate();/Yaw_Calculate();if(systime_direction=5)/HMC_angle=Get_Angle();systime_direction=0;Dr_Smooth=0.2;Direction_Calculate(t2-t2_mid); if(systime_speed=20) systime_speed=0 ;GPIO_ToggleBit(HW_GPIOE,26);/闪烁 Sp_Smooth=0.05;/重置平滑系数 Get_Speed(); Speed_Calculate(); if(FindZeroFlag) FindZeroIndex+; if(FindZeroIndex=400) GYROY_SUM+=T_Y; GYROX_SUM+=T_X; if(FindZeroIndex=499) FindZeroFlag=0;/清标志位 FindZeroIndex=0; TY_OFFSET=GYROY_SUM*0.01f; TX_OFFSET=GYROX_SUM*0.01f; GYROX_SUM=0; GYROY_SUM=0; Motor_Output();/init.cvoid SmartCar_Init() DelayInit();UART_QuickInit(UART3_RX_PE05_TX_PE04,115200);UART_SelectDebugInstance(HW_UART3); /*LED初始化，用作系统运行指示*/ GPIO_QuickInit(HW_GPIOE,26,kGPIO_Mode_OPP); GPIO_SetBit(HW_GPIOE,26); /*OLED初始化