风力摆控制系统初试报告

风力摆控制系统（B题）

摘要

本系统由STM32F103ZET6单片机、MPU6050陀螺仪模块、电机驱动模块、直流风机控制模块和电源模块组成。MPU6050陀螺仪检测到摆杆的水平面上的角速度和速度以及位置反馈给单片机，单片机用PID控制算法对数据进行处理，输出PWM波驱动直流风机运动，实现风力摆迅速起摆、等幅振荡、定角度起摆、快速制动、精准快速画圆等功能，并且偏差非常小，所用时间也远远小于题目中所给定时间。另外，该系统采用液晶屏显示，利用按键对风力摆进行控制，实现了良好的人机交互功能，使得控制操作非常方便。

关键词：风力摆；STM32；陀螺仪；直流风机

1系统方案

1.1系统结构

本系统主要由STM32F103ZET6单片机最小系统、电机驱动模块、电源模块、显示模块、摆杆、直流风机组成。直流风机为系统的执行机构，由专门的电机驱动芯片TB6612FN 驱动。摆杆固定在悬挂的万向节上，直流电机带动摆杆进行运动，系统通过固定在摆杆底部的MPU6050陀螺仪反馈回来的角度值和加速度值对四个风机进行PID 算法调节，实现了对摆杆的精准控制，完成了题目中的所有基本要求和发挥要求。

本系统的结构框图如图1-1所示。

图1-1系统结构框图

1.2方案比较与选择

1.2.1 风力摆控制方案的论证与选择

方案一：采用2只直流风机作为动力系统。采用2只风机并排垂直而立，分别位于摆杆两侧，通过控制风机转速控制风力摆使激光笔画线画圆。此方案风力摆负载轻，能快速度起摆，但其抗干扰性差，不容易实现精准控制。

方案二：采用3只直流风机作为动力系统。三只风机为等边三角形三边，相背而立，互成120°夹角。此方案相对于方案一在控制风力摆转动过程中状态微调方面有提升，但自成三角形，控制难度大，不利于精确控制风力摆状态。

方案三：采用4只直流风机作为动力系统。四只风机取一边靠于摆杆，朝向成顺时针排列，通过控制四只风机转速控制风力摆当前状态。此方案风力摆负载最重，但对于控制风力摆状态最为精确，动力最足，且抗干扰能力最强。

综合以上三种方案，考虑到风力摆的控制精度、抗干扰能力、系统响应速度等方面，我们选择方案三，采用4只直流风机作为动力系统。

1.2.2直流风机摆放位置论证与选择

方案一：四个风扇呈十字形摆放。风扇呈十字摆放时，同一平面的两个风机可以朝相反的方向扇风，控制摆杆朝两个相反的方向摆动，两组这样正交的风扇，就可以实现

电源模块

单片机 最小系统 矩阵键盘 陀螺仪 显示模块 摆杆

电机驱动模块 直流风机

摆杆在水平面上的受力，从而可以对摆杆进行各个方向的摆动进行控制。但是，经过测试，有两组相邻的两个风扇同时工作的时候，所扇出的风会互相干扰，影响摆杆的摆动，控制不精确。

方案二：四个风扇呈正方形摆放。风扇呈正方形摆放的时候，四个风扇互相背对，风从上面和下面的进风口进入风扇，四个风扇朝四个不同的方向朝外扇风，互不影响，并且通过对四个风扇的转速调节，可以精准的实现对摆杆各个方向运动的控制。

综合以上两种方案，方案二的硬件设计更加简洁，且精度更高，因此选择方案二。

1.2.3系统主控芯片的论证与选择

方案一：采用STC89C52单片机。STC89C52单片机是单片机中的经典，是一个8位的单片机，具有32个I/O接口，具有价格低廉、性能稳定、使用方便、外围电路成熟的优点，但是其缺点是其内部资源比较少，运行速度慢，功能较为单一，ROM空间较小。

方案二：采用大规模可编程器件。FPGA具有独立的I/O接口和逻辑单元，使用灵活，适用性强，且对于单片机来说，还有速度快，集成度高的特点，因此特别适用于复杂逻辑电路设计。但是FPGA的成本过高，算数运算能力不强，而且由于本设计对输出速度要求不高，因此FPGA的优势得不到充分体现。

方案三：采用STM32F103ZET6单片机。STM32F103ZET6为cotex-M3内核的32位的单片机，处理数据的速度明显快于8位单片机，而且端口比较多，内部资源丰富，可以同时处理多个任务，性能稳定，且性价比也比较高，完全满足本题目的要求。

由于本系统要同时用到四个直流风机来控制，还要处理传感器所传来的数据并进行分析，对单片机的处理能力要求较高，综合以上三种方案，STM32F103ZET6处理速度快，内存大，内部硬件资源丰富，性价比高，更适合设计要求，因此选择方案三。

2.系统理论分析与计算

2.1风力摆起摆物理受力分析

本系统的机械结构如图2-1。风力

摆的摆动部分用碳纤维作为摆杆，质量

较轻，相对于电机的质量来说可以忽略

不计，因此，摆动部分的质心可以近似

认为和四个电机的总质心重合。然后，

摆动部分就可以用物理模型（一个长为

R的轻质杆的下端固定一个质量为m

的小球）来代替，如图2-2。在不施加

外力时，当杆的摆角非常小，杆和小球

就相当于一个单摆，有固定的运动周

期：

2 Tπ=

图2-1

系统机械结构示意图

m

R

图2-2

风力摆物理模型

考虑到小球有固定的运动周期，就可以把施加给小球的起振力调成周期性变化的，力的周期和小球本身的固有震动周期相等，就可以产生共振，这样就可以实现在最短的时间让小球起振。对于本系统来说，就是让摆杆利用共振快速摆起较大的角度。

2.1风力摆的运动姿态分析

本系统采用四个风机，两个一组的形式来提供动力。两组风互相正交，因此可以将风力摆的运动轨迹分解为两个互相正交的运动矢量来对其进行运动分析。以坐标纸上的原点为原点，两组风机单独运动（理想状态下两组风机运动轨迹互成90度角）时所在的直线为坐标轴，建立平面直角坐标系。在X 坐标轴上对风机的摆杆进行受力分析（如图2-3和图2-4），图2-3为单摆平面内的几何关系图，图2-4为风机 的受力分析图。设摆线与竖直线之间的夹角为θ，在正下方时θ=0，

顺时针为正，逆时针为负。摆的角速度为w ，角加速度为α,F 为风机提供的主动力，f 为风力摆受到的阻力的大小，风力摆自由摆的过程 中速度很小，因此可以当作恒定值处理为一个恒定值，由于正弦运动相对于复杂的非正弦运动来说是比较容易控制的，对摆进行受力分析如下。 sin mR mg F f αθ=-+- (式2-1)

由式2-1可知可以通过调整F 的值，可以抵消掉f 。 令F=f ，则：

sin mR mg αθ=- (式2-2) 0g

R

αθ+=

(式2-3)

αθ=''

(式2-4)

令2g w R

=

得: 20w θθ+'='

(式2-5)

从而进一步解出：

()sin A wt θ?≈+

(式2-6)

从上面的分析中可以得到X 坐标轴上的θx 的公式为：()sin x x x x w A t θ?≈+，同理，也可以求到Y 坐标轴上的θy 的公式：()sin y y y y A w t θ?≈+;并且实际向量为L=x+y; 由图2-3可知：X 轴上的坐标的公式为，即

()tan sin()x x x x x R A w t ?=+

(式2-7)

Y 轴上的坐标的公式为tan y R y θ=, 即

tan x R x θ=Y

图2-4风机受力分析图

X

f

Ft mg

F

sin(t n())a y y y y y R A t ω?=+

(公式2-8 )

因为L 为实际向量，为L=x+y, 所以L =即

L =

（公式2-9）

这是摆杆实际运动的轨迹方程。

由上式2-7、2-8、2-9可以得出结论：左右摆动幅值相等的情况下（即Ax=Ay ），即相位差0x y ??-=°或180°时，两个角同时增大和减小，摆杆运动轨迹的是一条直线，若90x y ??-=°；两个角的平方和为一个定值，摆杆运动轨迹是一个圆。因此，本系统采用控制两组相互正交的风机摆动的相位差的方法作为核心控制思想。

2.3控制算法的分析

经过测试，电机在满载时扇出的风速可达4m/s ，如果简单的采用比例PWM 输出来控制电机转速，那么系统虽然能够达到稳定起摆状态，但是精度不高，还会出现来回震荡的情况，且起摆速度非常慢，远远超过15s 的起摆时间，根本达不到题目基本要求。为了弥补单纯的比例控制算法的缺点，我们采用PID 算法来对电机进行控制。 PID 算法在比例控制的基础上加入了积分调节和微分调节过程，很好的弥补了纯比例控制算法的缺点。PID 算法调节规律是连续系统动态品质校正的一种有效方法，对于本系统来说非常适合。PID 的调节过程示意图如下。

图2-5PID 控制系统原理图

经过前面的理论计算与公式推导，结合本题目的实际要求，可以确定PID 算法对于本系统控制的原理如下：

（1）比例环节：系统通过检测陀螺仪所反馈的角度值，和所设定的角度值进行对比计算出偏差error(t)，偏差一旦产生，对应直流风机立即产生控制作用，以减少偏差。

（2）积分环节：主要用于消除静差，提高摆杆震荡的无差度。积分作用的强弱取决于积分时间常数T1， T1越大，积分作用越弱，反之则越强。经过反复测试，本系统采用10ms 为控制周期，即T1=10ms 。此时系统的各项参数都比较稳定。

（3）微分环节：反映摆杆摆幅的变化趋势，并能在偏差信号变得太大之前，在 系统中引入一个有效的早期修正信号，从而加快直流风机的动作速度，减少风机调节时间。

本装置属于多输入多输出系统，采用PID多闭环控制，选取摆杆角度和角速度作为反馈信号。此方案中，不仅对系统的摆杆角度进行闭环控制，而且对摆杆速度也进行闭环控制，达到了对摆杆运动的精准控制。本设计控制参数主要是在风力摆运动理论的基础上通过实验测试获得。

3.电路与程序设计

3.1系统主要电路设计

3.1.1摆杆角度和角速度测量电路

本装置采用串口三轴加速度计作为摆杆的检测装置。此陀螺仪模块采用高精度的加速度计MPU6050，通过模块自带的处理器读取的测量数据，然后采用先进的数字滤波技术，有效的降低了测量噪声。模块内部集成了姿态传感器，配合动态卡尔曼滤波算法，能够在动态环境下准确输出模块当前的姿态，其误差为0.01度，通过串口输出，使用起来简单方便，且精度高，非常适合用作本系统的检测电路。其电路示意图如下图3-1。

图3-1串口加速度仪电路示意图

3.1.2 电机驱动电路

本系统采用小型空心杯电机作为动力源，经测试，该电机的标准电压为3.7V，满载时电流可达1A。考虑到摆杆摆起过程中，风机转速度较快，所需供电电流大，对电机驱动能力要求较高，故本设计采用MOSFET-H 桥芯片TB6612FNG作为电机驱动芯片。此芯片以如下的电路图工作时，输出电流大，瞬间峰值电流可达 3.2A，持续工作电流为1.2A；内含两个H 桥的高电压大电流全桥式驱动器，且集成度高，功耗小，能很好的对这种空心杯电机进行驱动控制。电机驱动电路如图3-2所示。

图3-2电机驱动电路图

3.1.3 电源模块电路

本装置使用了4个功率为4W的直流电机，单个电机的瞬时电流可达到0.5A，对电源功率有较高的要求；另外，由于本装置采用了PID算法来控制风机的转速，这就要求电源的电压波动范围要足够小，因此我们选用了12V 10A的开关电源作为初级电源。初级电源经过两个LM2596降压模块后可以输出两路稳定的3.7V 3A的功率分别给两个电机驱动供电；另外，由于单片机需要单独供电，所以本系统又采用了LM7805稳压芯片来输出稳定的5V电压给单片机以及传感器供电。电源电路的示意图如下图

3-3所示。

图3-3系统电源电路示意图

3.2 程序结构与设计

3.2.1 程序流程图

本系统采用键盘输入各参数及测试模式。系统开机启动进入开机界面，按下键盘上的任意键后进入菜单选择界面。菜单选择界面有两个功能基本功能测试和发挥部分测试。通过按键可以选择进入相应的功能，对基本部分要求和发挥部分要求进行试，功能执行完毕后系统回到菜单选择界面，继续等待按键输入执行相应的功能。系统程序流程图如图3-4示。

图3-4 程序流程图

4测试方案与测试结果

4.1 测试工具

（1）秒表

（2）卷尺

（3）量角器

（4）数字风速仪

4.2 测试方案及结果

4.2.1 基本部分测试

（1）让摆杆从处于自然下垂状态（摆角0o）开始，驱动电机带动旋转臂作往复摆动画出长度大于50cm的线段，记录线性度偏差和线段长度达到50cm的时间；

（2）让摆杆处于自然下垂状态，输入设定长度值，驱动风机带动摆杆作往复运动，尽快增大摆杆摆动幅度，达到设定长度，同时记录完成等幅震荡所需时间和线性度偏差；

（3）让摆杆处于自然下垂状态，输入设定角度值，尽快增大摆杆摆动幅度，达到设定长度和设定角度，同时记录达到预定角度所需时间和线性度偏差；

（4）把摆杆拉起一定角度（30°~45°）放开，5s内使风力摆制动达到静止状态，记录摆杆释放角度和制动时间。

4.2.2 发挥部分测试

（1）让摆杆处于自然下垂状态，输入圆半径，尽快增大摆杆摆动幅度，达到设定半径，记录达到目标半径所用时间和线性度偏差；

（2）在摆杆保持圆周运动的情况下，施加干扰后，记录摆杆恢复圆周运动所用时间和风扇与圆心之间的距离；

（3）把摆杆拉起一定角度（大于90°）放开，3s内使风力摆制动达到静止状态，记

录摆杆释放角度和制动时间。

表2发挥部分测试数据

能达到其他的发挥部分指标。

4.3 总结

测试结果表明，风力摆能快速起摆并保持稳定状态；摆杆可以按照预定角度运动；画出预定长度的线段；在制动测试中能够做到迅速制动；按照指定半径画圆；受到外界干扰时，摆杆依然能恢复保持圆周运动状态，能够圆满完成甚至超越题目的基本和发挥部分要求。同时本系统采用的原件都是低功耗、高性能、高性价比的原件，仅仅采用一个陀螺仪和四个小功率直流电机，实现了摆杆起摆、制动、圆周运动等功能，经济简洁，实用性强。