简易旋转倒立摆及控制装置

1、简易旋转倒立摆及控制装置(参赛队员姓名：指导教师姓名 参赛 队 编号：参赛学校：简易旋转倒立摆及控制装置（ C 题 ）摘 要：简易旋转倒立摆及控制装置是复杂的高阶闭环控制系统，控制复杂度 较高。系统以飞思卡尔 MK10DN512ZVLL10 单片机为核心，以 Mini1024j 编码 器为角度传感器，配合直流电机组成旋转倒立摆系统，经过充分的系统建模， 并 考虑单片机运算速度，最终确定采用改进的“模糊 PID”控制算法，通过软件控 制，可以满足基本部分要求和发挥部分要求。系统的突出特点在于充分的力学理论分析，通过力学建模和控制系统仿真， 获得了大量的定性分析结果，为系统的建立提供了很好的理论依

2、据。关键字 ：倒立摆 模糊 PID 力学建模 状态机一、系统方案1. 系统方案论证与选择 倒立摆系统是一个复杂的快速、非线性、多变量、强耦合、自然不稳定的系 统。对于该控制系统而言，合适的控制算法、精确的反馈信号、适合的电机驱动 等都对系统的稳定性、控制精度及抗干扰性起重要作用。针对上述问题， 分别设 计多种不同的解决方案，并进行选择论证。（ 1）控制算法选择 方案一：采用传统 PID 控制算法。传统 PID 控制算法是运用反馈求和后的误差信号的比例 （0 阶位置项 ）、积分 （误差累积项 ）、微分（1 阶速度项 ）进行系统校正的一种控制算法。 可用于被控对象 的结构和参数不能完全掌握， 或得

3、不到的精确数学模型的情况， 控制器的结构和 参数必须依靠经验和反复调试来确定。方案二：采用模糊 PID 控制算法模糊 PID 控制算法根据 PID 控制器的三个参数与偏差 e 和偏差的变化 ec 之 间的模糊关系， 在运行时不断检测 e及 ec，通过事先确定的关系， 利用模糊推理 的方法，在线修改 PID 控制器的三个参数，让 PID 参数可自整定。将模糊控制 算法与传统 PID 控制算法巧妙结合，不但具有 PID 控制算法精度高等优点，又 兼有模糊控制灵活、适应性强的优点。综合考虑选择方案二的模糊 PID 控制算法。（ 2）电动机选型 方案一：选择步进电动机 步进电动机是将电脉冲激励信号转换

4、成相应的角位移或线位移的离散值控 制电动机， 这种电动机每当输入一个电脉冲就动一步。 虽然控制时序和驱动电路 相对复杂，但步进距离很小，保持力矩大，制动能力强。但步进电机速度只在一 定范围可调，并且一般步进电机在不旋转时仍有若干相通电，功耗太大。方案二：选择直流电动机 直流电动机控制简单，利用双极性 PWM 即可实现调速和正、反转，功率调 节范围广、适应性好。直流电机的起动、制动转矩大，易于快速起动、停车，易 于控制，且直流电机的调速性能好，调速范围广，易于平滑调节。综上考虑选择方案二的直流电动机。（3）传感器的选择 方案一：使用角位移传感器 角位移传感器是一个高精度的电位器， 它输出为模拟量

5、。 但是在使用角位移 传感器时，为得到其与竖直方向 （即重力方向 ）的夹角，要使用重摆，且在角度变 化小时，由于传感器自身扭矩，将不会发生角位移，从而得不到采样数据。方案二：使用主轴编码器 主轴编码器采用与主轴同步的光电脉冲发生器， 通过中间轴上的齿轮 1:1 地 同步传动。一般是发光二极管发出红外光束，通过动、静两片光栅后，到达光电 二极管，接收到脉冲信号，变换成数字量输出。按编码方式不同，分为增量式编 码器和绝对编码器。前者输出脉冲，后者输出 8421 码。绝对值编码器减轻了电 子接收设备的计算任务，从而省去了复杂的和昂贵的输入装置，而且， 当机器合 上电源或电源故障后再接通电源， 不需要

6、回到位置参考点， 就可利用当前的位置 值，得到了广泛的应用。综上考虑选择方案二的绝对值式主轴编码器。2. 系统结构基于题目要求及以上分析，本系统以飞思卡尔 MK10DN512ZVLL10 单片机 作为核心处理芯片，包括摆杆状态检测、电机驱动、液晶显示等模块。系统框图 如图 1 所示。图1 系统结构框图系统各部分功能如下。中央处理模块：该模块采用飞思卡尔 MK10DN512ZVLL10 单片机作为主控 制器，完成系统的控制功能。摆杆检测模块： 该模块由主轴编码器构成， 用于检测摆杆的角位移及角速度， 并将信息反馈给 MCU 。电机驱动模块：该模块由步进电动机实现，用于驱动旋转臂做往复运动。液晶显

7、示模块：该模块由 1.8寸 SPI TFT全彩屏构成，用于实时动态显示摆 杆角度，以及摆杆保持平衡状态后维持的时间。二、理论分析与计算1. 系统模型的建立一级旋转式倒立摆系统由一个水平旋臂和一级摆杆组成， 旋臂由电机驱动在 水平面内作圆周运动，通过耦合作用带动摆杆转动。如图 2 所示，旋臂和摆杆可 以抽象为两个匀质杆，其中旋臂长度为 R，相对 y轴的角位移为 ；摆杆长度为L ，摆杆质心到连接点的距离为 L L 2，相对 z 轴的角位移为z图2 系统建模示意图由动力学理论，摆杆质心在 x和 z方向的速度分量为：Vx R Lcos(1) Vy Lsin 系统总动能系统的动能由 4 部分构成， 包括

8、：旋臂在水平面上的转动， 摆杆在铅直平面内的转动，摆杆质心沿 x轴、z轴方向的运动。对应的动能分量分别用 T1，T2 ，T3，T4 表示，因此系统动能 T 为这四个动能分量的和，系统动能如式 (2) ：T J1 2 2 J2 2 2 m(R Lcos ( )2 2 m( Lsin ( )2 2 (2) 其中， J2 mL2 3 。 系统总势能 以旋臂所在水平面为零势能面，则系统的势能为摆杆的重力势能：V mgL cos(3) 拉格朗日方程 由拉格朗日算子 H T V 可推导出拉格朗日函数：H J1 2 2 J2 2 2mR22 2 mL Rcos ( )( )mgLcos(4)已知系统的广义坐

9、标 q,，则由拉格朗日方程HHfi ，t qiqii(i 1,2) 可得方程组：HHMtH H (5) t0其中， fi为系统广义坐标上非有势力对应的广义外力， M 为电动机输出转 矩，M m gKiKg(Um KgKm ) Rm ， 为旋臂绕电机转轴转动的阻尼系数， U 为电机电枢电压。将式(5)中的方程在 ()T (0 0 0 0)T 处线性化，忽略高次项，最终可以得到系统的状态方程如式 (6)所示。004G4J1 mR23RG(4J1 mR2)L103R4J1 mR22 3(J1 mR2)g (4J1 mR2)L0100004 m gKi Kgm g i g(4J1 mR2)Rm3R m

10、 g KiKg(4J1 mR2 )L RmUm(6)其中， G ( m gKi KmKg2Rm) Rm将系统的机械参数值带入式 所示。(6)，可得倒立摆系统的线性化数学模型如式(5)y1001000046.8151.511011.878.9501000055.1583.14Um(7)上述建模过程所需各机械参数如表 1 所示表1 系统物理参数表物理参数物理意义参数值L摆杆长度0.199 mm摆杆质量0.005 kgR旋臂长度0.2 mM旋臂质量0.01 kgKi电机力矩系数-3 -13.8710-3 N m A-1Km反向电势系数4 10-3 V srad -1Kg变速器齿轮比4.2:1Rm直流

11、电机电枢电阻0.476m直流电机效率64.6 %g变速器效率95 %阻尼系数4.1610-3 N ms rad -12. 控制器算法的设计自适应模糊 PID 控制器以误差 e和误差变化 ec 作为输入，可以满足不同时 刻的 e和ec对 PID参数自整定的要求。利用模糊控制规则在线对 PID参数进行 修改，便构成了自适应模糊 PID 控制器。但是这里有一个问题：通过上述建模， 若将 作为模糊控制器的 4个输入，每个输入又选定 7 个词集的话， 那么 规则将有 74=2401 条，考虑到单片机的运算速度，这里考虑设计两个控制器，分 别控制 、 和 、 ，由一个单片机并行运算处理。然后，将它们的输出

12、决策相加作为电动机的控制信号，来控制倒立摆的平衡。其结构如图 3 所示图 3 模糊 PID 控制器结构 隶属度函数对 于 旋 臂 的 控 制 ， 取 输 入 , 和 输 出 Kp,Ti,Td 模 糊 子 集 为 NB,NM,NS,Z,PS,PM,PB ，分别代表负大， 负中，负小，零，正小，正中，正大。 论域为-3,3 ，量化等级为 -3,-2,-1,0,1,2,3 。输入输出隶属度函数均采用三角形， 如图 4 所示。图 4 输入输出量的隶属度函数对 于 摆 杆 的 控 制 ， 取 输 入 , 和 输 出 Kp,Ti,Td 模 糊 子 集 为 NB,NM,NS,Z,PS,PM,PB ，同上述对

13、于摆杆的控制， 输入输出隶属度函数也均采 用三角形， 但考虑倒立摆系统的控制以摆杆控制为主， 要求摆杆在角度为零时能 够平衡倒立故在零点附近分档较细，如图 5 所示。图 5 输入输出量的隶属度函数 模糊控制规则根据 Kp,Ti,Td对输出特性的影响，可以归纳出在不同输入下， K p ,Ti , Td 的自整定要求，根据控制经验，可得模糊规则，如表 2 所示。对应模糊规则，可以完 成模糊推理，决策出模糊输出量。表 2 Kp,Ti ,Td模糊控制规则e ecNBNMNSZOPSPMPBNBPB/NB/PSPB/NB/PSPM/NB/ZOPM/NM/ZOPS/NM/ZOPS/ZO/PBZO/ZO/P

14、BNMPB/NB/NSPB/NB/NSPM/NM/NSPM/NM/NSPS/NS/ZOZO/ZO/PSNM/PS/PMNSPM/NM/NBPM/NM/NBPM/NM/NSPS/NS/NSZO/ZO/ZONS/PS/PSNM/PM/PMZOPM/NM/NBPS/NS/NMPS/NS/NMZO/ZO/NSNS/PS/ZONM/PS/PSNM/PM/PSPSPS/NS/NBPS/NS/NMZO/ZO/NSNS/PS/NSNS/PS/ZONM/PM/PSNB/PB/PSPMZO/ZO/NMZO/ZO/NSNS/PS/NSNM/PM/NSNM/PM/ZONM/PB/PSNB/PB/PSPBZO/ZO/P

15、SNS/ZO/ZONS/PS/ZONM/PM/ZSNM/PB/ZONB/PB/PBNB/PB/PB最后，进行反模糊判决，利用重心法去模糊化，将模糊输出分解成实际作用 于电机的物理量。三、电路与程序设计1. 硬件电路设计（1）主控电路设计主控电路设计如图 6 所示，系统采用飞思卡尔 MK10DN512ZVLL10作为主控 芯片，采集编码器的位置信息， 并根据倒立摆的角度和角加速度等数据计算摇臂 运动速度和方向，从而实现倒立摆的平衡。图 6 主控电路图（ 2）电机驱动电路 系统中的电机采用 MOS管组成 H 桥驱动电路，该驱动就有电路可靠，驱动 能力强，成本低等优点，完全满足本系统的要求。电路如图

16、 7 所示。图 7 直流电机驱动图2. 系统软件设计（ 1） 软件设计为了支撑上述题目要求和控制算法， 并且降低模块间的耦合度从而为测试工 作提供方便，我们设计了如下的软件构架：设备驱动层 设备驱动层是针对单片机外设的驱动控制信号和反馈信号进行初步的封装， 从而实现高层逻辑不必关心时序或电压等数字、模拟信号层数据。I/O 数据配接层I/O 数据配接层实现输入输出数据与控制逻辑数据的匹配，包括数据类型转 换（ floatunsigned int）和数据缓冲维护。控制决策层 控制决策层是控制系统的核心，其中包括系统状态控制和运动决策控制。图 8 控制决策层框图由图 8 所示，控制决策层首先从数据配

17、接层的输入缓冲读入控制外设和传感 器外设的反馈数据， 首先根据数据进行状态控制， 在确定系统状态之后再配合状 态数据进行运动决策， 将决策结果发送至 I/O 输出数据配接模块从而实现对设备 的驱动。图 8 比较直观地说明了决策层在控制系统中所处的位置。 根据上述分析， 软件设计流程如图 9 所示。开始发挥部分基本部分是否为本步启动倒立摆启动倒立摆N是否达60YY结束360N是否达到启动倒立摆YY测是否撤N掉外力N测是否到达165附近启动平衡程序NYN10sYYN等待扰动启动平衡程序N检测是否倒立Y启动平衡程序NYYN测时间是否大于5启动旋转平衡程序Y是否旋360结束图 9 软件流程图四、系统测

18、试及结果分析 针对竞赛最终测试项目及计划演示的扩展表演项目内容， 制订并进行了如下 测试，测试结果如表 3 所示。表3 竞赛测试数据表测试项目测试次 数成功（准确）次 数成功（准确）率备注传感器部分静态角度测量2020100%误差0.1度动态角度测量2015（ 误差小于 0.5 度次数）75%已通过算法 优化基本部分自启动超 60 度2020100%自启动超 360 度2020100%165 度自平衡201995%165 自平衡旋转角度20125（平均最大 转角）138%（超过 90 度）统计规律发挥部分自启动倒立2020100%5s 内稳定施加扰动倒立2020100%倒立转圈2020100%扩展液晶显示角度2020100%大扰动演示2017 次85%20g砝码， 90度干扰结果分析测试结果显示， 本设计已基本完成设计要求中所规定