摘要：本文主要讲述利用Coretx-M4单片机控制风力摆运动轨迹的实现方法。系统根据当前加速度、角速度及图像信息，利用闭环控制算法调节电机，实现风力摆直线摆动、圆周摆动及定点静止等功能。系统响应速度快，控制精度高，交互操作界面简单易用，具有良好的交互性。

引言

风力摆系统的摆体由风扇构成，通过调整风扇的转速实现摆动位置及摆动路线的控制，由于摆动机构的滞后性，实现精确控制具有一定难度。通过合理的摆结构设计，并利用摆线理论建模，设计并优化控制策略，利用闭环结构提升摆的轨迹运动准确性。系统对风摆控制参数设定、抗扰因素测试等惯性滞后问题解决具有借鉴意义，同时也为分析该类问题建立了直观的测试平台。

1 系统结构

本系统硬件部分主要由电源模块、MCU模块、姿态采集模块、电机驱动模块、风力摆机械部分等组成。机械部分为万向节悬挂65cm硬质杆，底端连接4个风机构成风机组，中心固定姿态采集模块。MCU使用I2C协议采集姿态模块的数据，MCU根据设定值利用PID控制理论，通过控制PWM占空比来控制 4个风机的转速和方向，实现对风力摆的控制，整体系统结构如图1所示。

1.1 电源模块

7.2V /2000mAh镍镉电池为MCU模块、传感器模块和显示模块供电。

学生电源输出6V稳压，为四路电机驱动供电。

1.2 MCU模块

MCU模块是核心部分，负责数据处理。有以下功能：

负责读取风摆角度数据，将加速度、角速度信息进行互补滤波和四元数转化，计算当前风力摆角度信息;

负责读取摄像头图像信息，对数据进行去噪点化处理后提取标志物边缘，计算得出标志物中心点位置;

负责控制电机驱动模块，利用PID闭环控制算法调节电机转速和方向，实现对风力摆运动轨迹的控制。

1.3 姿态采集模块

姿态采集模块是整个控制系统的关键组成部分，本系统采用整合性六轴陀螺仪加速度计芯片，负责检测风力摆的加速度角速度信息^[1]。

1.4 电机驱动模块

电机驱动模块为双BTN7960组成的H桥电机驱动，根据MCU控制器输出的PWM信号和方向信息，控制空心杯电机的转速和方向。

1.5 摄像头模块

摄像头模块是系统的特殊功能部分，为实现摆头跟随目标物而设计。摄像头采集图像，并对目标物进行识别，进而实现对目标物的跟随。

2 系统理论分析与计算

2.1 风力摆状态的测量与计算

采用高精度的加速计和陀螺仪MPU6050，不断采集风力摆姿态数据。MPU6050对陀螺仪和加速度计分别用了三个16位的ADC，将其测量的模拟量转化为可输出的数字量，通过DMP处理器读取测量数据，然后通过I2C总线输出，得到风摆的姿态角度。

2.2 风力摆控制分析

风力摆通过4只空心杯电机提供驱动推力，姿态采集模块采集风力摆当前姿态角，单片机处理姿态角，调节4个电机PWM的比例，从而控制下一时刻风机工作状态，实现对于风力摆的控制^[3]。

在自由单摆模型中，单摆做简谐运动的周期跟摆长的平方根成正比，跟重力加速度的平方根成反比，跟振幅、摆球的质量无关^[6]。

(1)

风摆摆长确定后，周期也就确定，如图2所示，根据自由单摆简谐运动的特性，利用三角函数关系^[4~5]，风摆跟随自然周期做出单摆的运动，加上X、Y方向，两个方向运动相位差90度，如图3所示，风摆就能做出圆周运动。

在处理风力摆模型时，可认为是控制每一时刻风力摆的姿态角，从而控制类自由摆运动和圆周运动。当物体离开垂直的平衡位置之后，便会受到重力与悬线的作用合力，驱动重物回复平衡位置。这个力称之为回复力，公式为。

2.3 控制算法的分析

本系统采用PID算法来控制风机转动的速度^[2]。风机开始工作后，姿态采集模块不断采集当前风力摆姿态角状态，并与之前的状态比较，使得风力摆的运动状态逐渐趋向于平稳。PID算法控制器由角度比例P、角度误差积分I和角度微分D组成。

其输入e(t)与输出U(t)的关系为：

(2)

它的传递函数为：

(3)

3 电路与程序设计

3.1 电路设计

3.1.1 电源

7.2V蓄电池电源经LT1529-5稳压得到5V电源，再经过两片LT1085电源芯片稳压得到两路3.3V电源，一路单独供电MCU，一路供电其它外设。主控板电源原理如图4所示。