风力摆控制系统设计.doc

第十二届智能控制设计大赛 题目风力摆设计 学 院 专 业 队 长 队 员 队 员 起始日期 摘 要 本风力摆系统主要包括单片机控制模块，开关电源（电源模块）激光笔及小型轴流风机，以及基于六轴倾角仪mpu6050的闭环控制系统。其中控制模块采用STM32为核心控制芯片，激光笔作为系统的执行机构，3.7V的小型轴流风机作为驱动风力摆的唯一动力，用3.7V的开关电源驱动轴流风机。轴流风机和摆杆通过万向节固定在支架上（用粗股导线将风力摆悬挂在支架上）。固定在支架上，测量得到的角度经过软件处理得到风力摆摆动所需要的PWM值。（本系统的PID算法算法是通过实际经验试验出风力摆的控制规律，稳定的完成风力摆运动过程中激光笔画轨迹。） 关键词风力摆; STM32; 轴流风机; PID算法; mpu6050 III 目 录 一、系统结构方案与设计1 1、机械结构设计1 2、主控芯片的论证与选择1 3、风力摆结构方案的论证和选择2 4、运动控制算法的论证和选择2 二、系统理论分析与计算2 1、摆杆位置检测2 2、风力摆运动控制分析2 3、 控制算法分析2 三、电路与程序设计3 1、电路的设计3 （1）STM32最小系统电路4 （2）稳压电源电路4 2、程序的设计4 （1）程序功能描述与设计思路4 （2）程序流程图4 四、测试方案与测试结果5 1、测试方案5 2、测试条件与仪器5 3、 测试结果及分析5 （1）测试结果数据5 （2）数据分析与结论7 五、结论与心得7 六、参考文献8 附录1电路原理图9 附录2源程序11 风力摆控制系统设计 一、系统结构方案与设计 1、机械结构设计 我们以轻质木条为材料加工成的十字作为风力摆支架的底盘，结构坚固克服摆动时的震动。以空心轻质塑料纤维棒作为摆杆减小了摆动时的自旋，用万向节将摆杆和支架的水平臂连接保证了摆动的灵活性以及达到了摆动角度和速度的精确控制。置于摆杆上的姿态采集模块mpu6050作为系统的闭环控制。四个轴流风机连接成方形作为系统的动力。这样我们就搭建了一个稳定可靠的机械系统，这使系统的各项参数处于比较稳定的状态，有利于后期控制系统的调节。 （本系统的PID算法是通过实际经验试验出风力摆的控制规律，稳定的完成风力摆运动过程中激光笔画轨迹。） 图1 风力摆结构示意图 2、主控芯片的论证和选择 方案一选择STC89C52单片机作为核心控制芯片，该单片机体积小操作简单，价格便宜。因为STC89C52单片机内部没有集成的函数库，且控制芯片外设模块较多，实际软件编写时复杂麻烦。 方案二选择stm32单片机进行系统的控制。STM32系列芯片时钟频率高达168MHz 具有512K字节SRAM，具有极强的处理计算能力。较为适合需要快速反应的倒立摆系统。 通过比较，我们选择方案二，采用STM32系列的F103C8T6单片机作为控制器。 3、风力摆结构方案的论证和选择 方案一采用3个风机连接背向摆成三角形作为摆的结构 方案二用4个风机连接摆成方形。 比较三角形较方形来说结构稳固，且风摆整体重量较轻。但在设置风力摆摆动的任意角度时方形结构容易控制，程序算法较为简单，故采用方形结构。 4、运动控制算法的论证和选择 方案一使用LQR控制器进行控制。LQR控制需要调整两个矩阵，对于LQR控制来说，需要求解Riccati方程和确定Q和R权矩阵，算法较为复杂，计算代价较高，相应时间较长，且不易被操作人员理解Q与R矩阵的物理意义。 方案二虽然PID算法调整参数较为麻烦，但PID可以实现单环稳定，对于PID控制来说，更容易被人理解，PID控制结构简单，调试方便，易于工程上实现。综上，我们采用PID控制算法对风力摆运动系统进行控制。 二、系统理论分析与计算 1、摆杆位置检测 为了检测摆杆的角度，从而判断摆杆的位置。我们采用高精度的串口6轴加速度计/陀螺仪MPU6050模块不断采集杆的位置信息。通过MPU6050模块与单片机的串口通信我们可以直接获取摆杆的三维角度pitch, roll, yaw。姿态采集模块得到的角用于闭环控制系统摆动的线路确保激光笔按照要求画出轨迹。 2、风力摆运动控制分析 风力摆采用4个0.1A的轴流风机作为动力系统。姿态采集模块mpu6050不断采集摆杆的角度，返给单片机处理调节单片机调节风机的PWM，从而是实现对风力摆的控制。 3、控制算法分析 本系统采用PID算法来控制风机转动的速度。风机开始工作后，姿态采集模块不断采集风力摆摆动的角度，并与之前的角度比较使得风力摆的摆动状态趋向平稳。PID分别表示摆杆的角度比例（P）、角度误差（I）、角度积分（D）。 P对风摆的当前角度偏差e（t进行调整，系数越大调节速度越快，减小误差，但是过大的比例，会造成风机速度状态的突变，从而导致摆杆状态不稳定 I加入积分调节，可以消除系统的稳态误差，提高无误差度。系统的稳定性下降，动态响应变慢。 D微分调节反应的是风摆的角速度，可以预见偏差变化的趋势具有可预见性因而可以产生超前调节，加入微分调节可以改善系统的动态性能。 比例（P） 执行器 积分（I） 微分（D） 传感器 图2 PID结构原理图 PID控制器由比例单元（P）、积分单元（I）和微分单元（D）组成。其输入e t与输出u t的关系为utkpet1/TI∫etdtTD*det/dt 式中积分的上下限分别是0和t。 因此它的传递函数为GsUs/Eskp11/TI*sTD*s 其中kp为比例系数； TI为积分时间常数； TD为微分时间常数 三、电路与程序设计 1、电路的设计 1电路模块 经过上述的分析和论证，决定了系统各模块采用的最终方案如下 控制模块采用STM32单片机； 姿态采集模块6轴加速度计/陀螺仪MPU6050模块 电源模块稳压5V给单片机风机供电 通信模块使用蓝牙模块实现上位机与单片机的无线通信； 显示模块 使用上位机实现对下位机的操作与接收来自下位机的信息 根据题目要求，系统主要实现风力摆的各项功能系统框图如图所示 激光笔 风力摆 轴流风机 姿态采集模块 上位机 蓝牙模块 Stm32控制系统 图3 系统框图 （2）STM32最小系统电路（见附录1） （3）稳压电源的电路设计（见附录1） （4）实物图与上位机界面图（见附录1） 2、程序的设计 在Keil uVision5开发环境下，采用C语言编程实现对对传感器输入信号的处理、对上位机指令的接收和对传感器信息的上传，以及对风力摆风速、方向的精确控制。 （1）程序功能描述与设计思路 程序功能描述 1）从静止开始，15 秒内完成幅度可控的摆动，画出长度在 3060 厘米间可设置，长度偏差不大于±2.5 厘米的直线段，并且具有较好的重复性；； 2）风力摆在一定角度放开后快速静止，风力摆拉开一定角度，10 秒内达到静止； 3）做可控半径的圆周运动，指定半径下，40 秒内重复三圈，轨迹落在指定半径±2.5 厘米的圆环内； 4）将当前角度和角速度上传到上位机，并画出动态曲线； （2）程序流程图 硬件初始化 接收来自上位机指令 模式4 模式5 模式1 模式2 模式3 模式6 X轴划线 快速定位 画方 画圆 快速停止 任意 划线 采集角度和加速度，发送给上位机 图4 程序流程图 四、测试方案和测试结果 1、测试方案 1、 硬件测试分别对每个模块做相应的测试。 2、 软件仿真测试使用Keil MDK软件进行仿真。 3、硬件软件联调在符合要求的环境中按照题目要求对风力摆的各项功能逐一进行测试 2、测试条件和仪器 测试条件上位机 测试仪器秒表，尺子，数字万用表 3、测试结果及数据分析 （1）测试结果数据 基本要求 1从静止开始，15s内风摆做类似自由摆运动，使激光笔稳定在地面画出一条不短于50cm的直线段并具有良好重复性。 表1 风力摆画长于50cm直线测试 测试次数 直线段长度 时间 1 49cm 3.5s 2 50.2cm 3.7s 3 50.1cm 3.6s （2）静止开始完成，15s内完成幅度可控的摆动，画出长度在30-60cm间可设置，长度偏差不大于-2.5cm，并且具有较好的重复性 长度设置 长度偏差 时间 30cm 1.1cm 2.6s 45cm 0.9cm 2.8s 60cm 1cm 3.9s 表2 风力摆画不同长度直线测试 3 从静止开始，15s内按照设置的方向画出不短于 20cm的直线段。 表3 设置方向不短于20cm线段测试 测试次数 线段长度 时间 1 20.9cm 2.3s 2 19.8cm 2.4s 3 20.5cm 2.4s 4 将风摆拉倒一定角度（30-45度）5s内使风摆达到静止状态。 风摆角度 是否达到静止 时间 30 是 8s 35 是 5.8s 40 是 6.1s 表4 风力摆静止测试 拓展部分 1以风力摆静止时的激光笔的光点为圆心，风力摆驱动激光笔在地面画圆30s内需重复3次,圆半径可在15-35cm范围内设置，激光笔画出的轨迹指定半径-2.5cm的圆环内。 表5 风力摆画圆测试 圆半径（r） 是否可以画圆 重复3次时间 15 是 20.8s 25 是 15.1s 30 是 14.4s 2 在发挥部分1后继续做圆周运动，在距离风力摆1-2m内用一台50-60w的台扇在水平方向吹向风力摆，台扇吹5s后停止，风力摆能够在5s内恢复发挥部分1规定的圆周运动，激光笔画出符合要求的轨迹。 实验次数 是否可以恢复 恢复时间 1 是 7s 2 是 6.2s 3 是 5.8s. 表6 风力摆抗干扰测试 （2）测试分析和结论 根据上述测试数据，可以得出以下结论 1、 风力摆能够很好的完成基本功能和部分拓展功能。 2、风力摆系统完成任务的时间较快。 3、风力摆系统保持了很好的稳定性。 综上所述，本设计基本达到设计要求。 五、结论与心得 进入大学后，我们被提供了更多的实践机会和平台，我们学习的不再仅仅局限于理论知识。此次智控竞赛题目的综合性很强，将我们之前所学的知识进行融合，这就要求我们在牢固掌握基本理论知识的同时，更需要灵活的运用。 高级组赛题无论哪个题目综合性都很强，解答的过程也很复杂，在如此短暂的时间内，要想独立将题目的基础部分和拓展部分完成是相当困难的，这就需要在电子设计方面有相当良好的基础以及对PID控制原理有相当程度的掌握。通过这次大赛，我们在各方面都获得了很大的进步，得到了一定的锻炼。 6、 参考文献 [1]谭浩强.C语言程序设计[M].北京清华大学出版社,2012 [2] 全国大学生电子设计竞赛组委会. 全国大学生电子设计竞赛获奖作品选编 （1994～1995）[M].北京北京理工大学出版社，1997年第1版. [3]高吉祥，黄智伟，陈和.高频电子线路[M]. 北京电子工业出版社，2003年第1版 [4]吴运昌.模拟集成电路原理与应用[M].广州华南理工大学出版社，2001年第1版 附录1电路原理图 图5 5V稳压电路 图6 Stm32最小系统原理图 图7 作品实物图 图8 上位机界面图 附录2源代码 include “delay.h“ include “MPU6050/mpu6050.h“ include “MPU6050/eMPL/inv_mpu.h“ include “usart.h“ include “math.h“ include “L298/l298.h“ include “Blt/bluetooth.h“ include “stdlib.h“ include “Led/led.h“ float pitch, roll, nyaw;//yaw没用到 short gx, gy, ngz;//角速度, ngz没用到 int main { s16 sumGx0, sumGy0; //角速度三次相加的值，用来算平均值 u8 mode 0;//模式 float setpitch0, setroll0, errpitch, errroll; //目标pitch、roll, 偏差pitch、roll s32 errpitchSum0, errrollSum0, errR2Sum100000; //偏差的累加,相当于积分项 float kp, ki, kd;//PID参数 s16 setR216, errR20, errR2last0;//画圆模式目标半径 s16 valx, valy, R2;//x,y方向pid算出的结果，根据这个结果确定PWM， R2是画圆模式当前半径 float kx, ky;//系数，用来校正 s8 kturn 1;//画圆模式下表示正反转 u8 cnt10;//count临时变量 extern u8 time;//时间，每10ms加1， 周期1460ms即加到146时清0（这个频率时画圆效果好） delay_init; whileMPU_Init //MPU6050初始化 {} whilempu_dmp_init //DMP库初始化 {} l298Init; //风机驱动初始化 uart_init115200; LedInit; bltTimerInit; //蓝牙驱动初始化 delay_ms1000; while1 { ifUSART_RX_STA mode USART_RX_BUF[0]; //接收到的第一个字节为模式位 switchmode { case 0//模式0清除偏差积分项，停止 errpitchSum errrollSum 0; errR2Sum100000; break; case 1 //模式1在任意方向画线段，线段长度由上位机设定的角度计算得到 errpitchSum1600; errrollSum2200; setroll USART_RX_BUF[1]0.1*USART_RX_BUF[2]; //上位机设定的横滚角值，第1个字节是整数部分，第2是小数部分（1位） ifUSART_RX_BUF[3] 90 { mode 6; //模式6在X轴方向画线段 setpitch USART_RX_BUF[1]0.1*USART_RX_BUF[2]; //上位机设定的俯仰角值 } else setpitch 57.3*atantansetroll/57.3*tanUSART_RX_BUF[3]/57.3;//第3个字节是设定划线的角度，以此算出目标pitch break; case 2 //模式2清除偏差积分项，停止 errpitchSum errrollSum 0; errR2Sum100000; break; case 3 //模式3画圆 setR2 s16USART_RX_BUF[1]*USART_RX_BUF[1]; //第1个字节是目标半径，setR2代表的是半径的平方 ifUSART_RX_BUF[2] 0//转向 kturn -1; else kturn 1; break; case 4 //模式4画方形 errpitchSum errrollSum 0; setpitch USART_RX_BUF[1]0.1*USART_RX_BUF[2];//目标pitch的整数部分和小数部分 setroll USART_RX_BUF[3]0.1*USART_RX_BUF[4]; break; case 5 //模式5快速定位 ifUSART_RX_BUF[1]7 0 setpitch 57.3 * atan2USART_RX_BUF[1], 70;//x方向上的长度 算出目标pitch else setpitch 57.3 * atan2USART_RX_BUF[1]-256, 70;//x方向上的长度为负 ifUSART_RX_BUF[2]7 0 setroll 57.3 * atan2USART_RX_BUF[2], 70; else setroll 57.3 * atan2USART_RX_BUF[2]-256, 70; break; } } switchmode { case 1 //划线模式，除了90以外， 90度另分为mode6 kp 2.8; ki 0.090; kd 3.0; //设定PID的比例系数，积分时间常数和微分时间常数，利用PID调节电机速度 ifrollsetroll-3 else valy s16kp*errroll ki*errrollSum - kd*gx; //利用PID控制Y轴方向的电机 ifsetpitch -1//0度 { kp 0; kd 4; setpitch 0; errpitch setpitch - pitch; valx s16kp*errpitch - kd*gy; } else { kp 2.8; ki 0.10; kd 2.8; ifpitchsetpitch-3 else valx s16kp*errpitch ki*errpitchSum - kd*gy; } break; case 6//划线模式 画90度 kp 2.8; ki 0.10; kd 2.8; ifpitchsetpitch-3 else valx s16kp*errpitch ki*errpitchSum - kd*gy; kp 0; kd 4; setroll 0; errroll setroll - roll; valy s16kp*errroll - kd*gx; break; case 2//快速停止模式 kp 0.2; kd 4; setpitch 0; errpitch setpitch - pitch; valx s16kp*errpitch - kd*gy; setroll 0; errroll setroll - roll; valy s16kp*errroll - kd*gx; break; case 3//画圆模式 delay_ms10; kp 20; ki 0.03; kd 0.13; errR2 s16setR2 - 4900*sinpitch/57.3*sinpitch/57.3sinroll/57.3*sinroll/57.3; errR2Sum errR2; R2 s16kp*errR2 ki*errR2Sum - kd*errR2-errR2last; errR2last errR2; ifkturn 1//正转（画的不圆，需要kx, ky 修正） { ky 0.0060*1-0.00040*setR2; kx 0.0059*10.00018*setR2; } else//反转 { ky 0.0059*1-0.00039*setR2; kx 0.0059*10.00017*setR2; } valy s16ky*kturn*R2*cos4.303*time*0.01; valx s16kx*R2*sin4.303*time*0.01; break; case 4 //画方模式 kp 0.2; ki 0.1; kd 4.0; ifpitchsetpitch-1 errrollSum 0; kturn -1; } else { setpitch -setpitch; errpitchSum 0; kturn 1; } } errroll setroll - roll; errrollSum errroll; valy s16kp*errroll ki*errrollSum - kd*gx; errpitch setpitch - pitch; errpitchSum errpitch; valx s16kp*errpitch ki*errpitchSum - kd*gy; break; case 5 //快速定位 kp 8.8; ki 0.2; kd 4.0; errroll setroll - roll; errrollSum errroll; valy s16kp*errroll ki*errrollSum - kd*gx; errpitch setpitch - pitch; errpitchSum errpitch; valx s16kp*errpitch ki*errpitchSum - kd*gy; break; default valxvaly0; break; } MoterMovevalx, valy; whilempu_dmp_get_data delay_ms1; ifcnt1 5 { cnt10; break; } } sumGy sumGx 0; MPU_Get_Gyroscope//取三次均值 sumGy gy; sumGx gx; MPU_Get_Gyroscope//取三次均值 sumGy gy; sumGx gx; MPU_Get_Gyroscope//取三次均值 sumGy gy; sumGx gx; gy sumGy/49; gx sumGx/49 3; } } 20