电赛2015年B题：风力摆控制系统 - 嘉立创EDA开源硬件平台

# 电赛2015年B题：风力摆控制系统 演示视频已经上传B站 https://www.bilibili.com/video/BV11t4y1q72s ## 题目分析 风力摆控制系统近似看作是一个单摆控制系统 ***T=2Pi sqrt(L/g)*** 我们用的细杆长度 *L* 为 787.3 mm, 设重力加速度 *g=9.8 m/s2* , 则单摆周期： ***T=2Pi sqrt(0.787/9.8) = 1.7805 s*** 由于角度大于15°，公式计算并不准确，掐表计算十个周期，算得周期为1596ms. 频率 ***f=1/T=1/1.596=0.6265 Hz*** 可知我们需要设计出一个带宽大于 0.6265 Hz的控制系统。角度采样率根据奈奎斯特采样定理，理论上选取 ***fs>2f*** 即可，但是题目中要求了系统最大调节时间，为了使得控制效果更好，需要取 ***fs>10f*** 甚至更高，在本次设计中我们设置20602的采样率为1K，并开启了内部的DLPF滤波 ，截止频率为5Hz.，控制周期 T=1ms。 ## 系统框图  风力摆控制系统使用了STM32F072作为主控,并且板载 ICM20602姿态传感器 可以选贴6050作为风力摆 运动姿态检测. 同时 风力摆使用716电机作为运动执行系统, 通过STM32输出四路硬件PWM 通过MOS管驱动四个空心杯电机 空心杯电机实测电流可以达到1.5A,需要选用大功率MOS ## 选型说明 1. STM32F072 是 ST 推出的低功耗 Cortex-M0 处理器，48MHz 主频，16K SRAM。在风力摆应用中够用。 2. 本题并没有对性能有较高要求，刚好当时在看 F0 系列的处理器，就想着用 F0 做来试试看。 3. IMU 使用 ICM-20602，也是比较常用的一个 IMU。MPU-6050也能实现，二者的性能并无太大差异，可能20602性能稍好，只是我们想要验证一下ICM-20602 才采用的这个芯片。 4. 整体结构采用以 PCB 为主要结构件，一根碳纤维管（提高刚性）通过两端的法兰联轴器将风力摆本身固定在悬挂的木条上。IMU 则安装在 PCB 上随整个系统一起摆动，通过 IMU 输出的加速度和角速度确定系统的姿态，从而实现闭环反馈控制系统的状态。四个空心杯电机沿 PCB 对角线对称安装，以获得较好的机械对称性。 ## 原理说明 1. 我们尝试的第一个方案是使用类似飞控的算法：采集陀螺仪的数据后使用四元数解算出摆的姿态后再反解出欧拉角，采用 PID 控制器调节欧拉角跟随某条期望的摆动曲线。但是在摆动的过程中加速度计输出的数据是无意义的，并不能参与姿态解算，因此纯用角速度解算出的姿态会随着时间漂移。在一段时间后摆的静息位置甚至会由于姿态数据的0偏显著偏移铅垂线。同时由于这种姿态解算算法本身的缺陷，在实现题目的“画圆”要求后，解算出的方向角会出现显著漂移，导致摆动定角度直线的要求无法完成。最后我们放弃了这种控制方式。 2. 我们看了下其他人的实现方案，很多方案都在说这个核心是PID，但是困扰我们的是这个姿态的解算的事情，我们认为姿态解算才是这道题的核心，很多方案都是直接用6050 的 DMP 读出来的姿态，但是20602上面没DMP库.... 3. 随后我们在对风力摆的系统进行分析后，我们认为：风力摆的角度曲线由于姿态解算的问题难以使用 PID 控制器跟随，我们还可以通过跟随摆动时的角速度曲线或角加速度来实现。跟随角速度时控制器需要的反馈数据就是陀螺仪直出的实时角速度数据，规避了角速度再积分时产生的积分误差，能够获得较好的稳定性，不容易使系统出现整体漂移。跟随角加速度时输出量就是力矩，可以通过电机直接控制，也具有天然的控制优势。不过，由于我们采用的时空心杯电机，并不能很好的控制电机输出的力的大小，最后采用了跟随角速度曲线的方案。事实上，如果风机很猛，是完全可以控制摆恒定指向某一个角度定住， 实验结果证明，此方案的控制表现也是非常好的。我们核心控制算法就是这两行，控制公式如图：  `Pitch ` 和 `Row` 分别是 x 方向和 y 方向上的角度。对应的 `Omega`就是对应角度的角速度。