摘要：针对视频拍摄过程中容易出现不稳定的现象，提出以STM32为主控核心的防抖动云台的设计方案。文章介绍
了以MPU9250为姿态传感器获取原始姿态角，利用卡尔曼滤波融合算法计算出稳定可靠的姿态角，当检测到姿态角发生偏移时，通过PID 算法调节SPWM 波，经DRV8313芯片放大后驱动三相无刷直流电机，将抖动消除，使云台保持稳定。通过理论仿真与实际测试，实现了能够搭载小型运动相机的云台稳定器。 关键词：STM32；MPU9250；姿态算法；云台稳定器中图分类号：TP273.2文献标识码：A 文章编号：2095-0438（2020）05-0149-05
（龙岩学院物理与机电工程学院
福建龙岩
364012）
赖义汉
龙忠华刘子铄李小强熊益康
∗∗∗第40卷第5期绥化学院学报2020年5月Vol.40
No.5
Journal of Suihua University
May .2020
收稿日期：2019-10-30
作者简介：赖义汉（1968-），男，福建龙岩人，龙岩学院物理与机电工程学院副教授，硕士，研究方向：嵌入式系统研究与应用。基金项目：福建省大学生创新创业训练计划项目（201811312055）。 随着数码产品的快速发展，视频拍摄已经成为大部分人生活的一部分。目前，便携式拍摄设备主要以手机或小型摄像机为主，由于部分手机或者小型摄像机没有防抖功能或者防抖功能较弱，直接手持设备拍摄时容易发生抖动，导致拍摄的视频出现模糊、视频画面质量不佳等现象。为了解决拍摄抖动的问题，提出以STM32为控制核心的防抖动云台的设计方法，通过MPU9250获取云台姿态角，采用卡尔曼滤波融合算法计算出最优姿态角，使用PID 算法调节三相无刷直流电机，使云台保持平衡。
一、系统总体设计方案
云台控制系统由MPU9250姿态传感器、STM32主控芯片、电机驱动模块、横滚轴电机和俯仰轴电机等部分组成，系统总体框图如图1所示：
图1系统总体框图
本系统使用MPU9250作为姿态传感器[1]，能实时检测当前云台的角速度、加速度等信息，并传送到STM32单片机中，采用卡尔曼滤波算法进行滤波融合，得到最优的云台偏转角[2]。根据当前云台姿态角与水平姿态的偏差，使用PID 算法输出两组三相SPWM 波形，其中一组驱动横滚轴电机，另一组驱动俯仰轴电机；每组SPWM 波由三路相位差各为120o 的SPWM 波形组成。三相SPWM 波信号经过DRV8313 149
芯片进行功率放大后驱动模滚轴及俯仰轴无刷直流电机，调节云台的俯仰轴和横滚轴姿态角，从而形成一个闭环的控制系统，保持云台稳定。
二、系统姿态解算及PID 算法
（一）卡尔曼滤波。MPU9250传感器中集成有陀螺仪和加速度传感器，其主要用于检测俯仰轴和横滚轴偏转角。陀螺仪可以检测当前的角速度，将输出的角速度积分就可以得到姿态，其高频动态特性好，但具有低频噪声，计算机积分后会产生误差，若不对其误差做修正，长期使用会导致姿态计算错误，即存在漂移现象，而且随着时间的累积，该误差会被一直放大。加速度计可以测量三个轴的重力加速度，其低频特性好，可以比较准确测量低速的静态加速度，但是具有高频噪声，轻微的抖动都会造成数据的波动，无法保证实时姿态的正确性，需要进行低通滤波，实时解算姿态波动严重，滤波后又难于保证姿态正确。因此，单独使用其中一种传感器都无法进行正确的姿态解算，系统拟通过卡尔曼滤波融合算法[3]，得到最优的姿态角。
卡尔曼滤波（Kalman filtering ）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，通过递归处理，将两个正态分布的数据融合为一个正态分布的结果[4]
。卡尔曼滤波的具体处理流程包括状
态预测、协方差预测、误差增益更新、协方差更新等五个步骤，具体计算方法如式(1)-(5)所示。
状态预测：x -k =Ax
k -1+Bu k -1（1）
协方差预测：
P -k
=AP k -1A T +Q （2）误差增益更新：
K k =P -k
H T
(HP -k
H T
+R )-1
（3）状态更新：
x k =x -k
+K k (z k -Hx -k
)（4）
协方差更新：
P k =(I -K k H )P -k （5）该算法假设测量都存在噪声，结合多个包含噪声的数据，计算出最优解，这个过程可以当成是滤波的过程，十分适合多传感器的数据融合。
（二）PID 算法。PID 算法是目前应用最为广泛的控制算法之一，该算法仅有三个参数，原理简单易于实现，效果良好。PID 控制律的表达式如式（6）所示：
郭超人u ()t =K p [e ()t +
1T I
∫
autoit0t e ()t dt +T D de (t )
dt
]（6）
其中K p 为比例系数；T I 为积分系数；T D 为微分系数；u(t)为输出量；e(t)为偏差量。而这种的数学表达式在计算机中无法直接进行运算，为了应用该算法，使用离散化将控制式
变为差分方程[5]，使用三次迭代即可算出最终效果。
其增量型控制算法，如式（7）所示：
Δu ()k =k p []e ()k -e ()k -1+k p
T
T I
e ()k +k p
T D
T I
[e (k -2e ()k -1+e (k -2))]（7）
本系统使用增量型PID 算法[6]，定义一个结构体，存储e
(k)，e(k -1)，e(k-2)，每次计算都是迭代更新，并且输出Δu ()k 。
三、系统硬件电路设计
本系统以STM32F103RCT6单片机作为主控芯片，其自带了通用异步串行通信接口（USART ）、两线式串行总线（IIC ）、四线同步串行接口总线（SPI ）等各种通信接口，工作频率最高达72MHz ，支持单周期乘法和硬件除法，使得运算速度大大提升。
（一）姿态传感器。姿态传感器MPU9250内部集成有三轴陀螺仪、三轴加速度计和三轴磁力计，其输出为16位的数字量；MPU9250模块通过IIC 总线接口（SDA 、SCL ）与STM32单片机进行通信，传输速率可达400kHz/s ，其电路如图2所示。
MPU9250作为从机，其地址为0x0D ，系统只读取MPU9250角速度与加速度值，每个传感器单轴数据分为高低8位，分别存储在两个寄存器中，以加速度数据为例，X 轴加速度高8位数据的地址为0x3B ，X 轴加速度低8位数据的地址为0x3C ，需要读取两个字节才能完成一次完整的采样。本系统调用了加速度传感器与角速度传感器的数据，共计6个寄存器的数据。
单片机读取姿态传感器MPU9250的加速度值和角速度值通过卡尔漫滤波数据融合处理后，得到最优的云台的偏转
角度值。
0.1 uF
图2MPU9250接口原理图
（二）三相直流电机驱动电路。三相无刷电机驱动信号采用SPWM 波控制[7]，即脉冲波的幅度不变，占空比大小呈正弦变换的一序列脉冲波。系统由STM32单片机产生三相的PWM 波来模拟正弦波信号，且输出三个波形相位差各为
150
120o ，用于驱动三相无刷电机。
三相无刷电机驱动采用DRV8313集成芯片，其内部集成有三个可独立控制的半H 桥驱动器，最大可驱动电流为2.5A 、电压为24V 的负载。系统采用两个三相无刷直流电机分别控制云台的横滚轴与俯仰轴。由于横滚轴与俯仰轴控制方式相同，以横滚轴为例，其电路如图3所示。由STM32产生的三路SPWM 波与DRV8313的ROLL_IN_A ，ROLL_IN_B ，ROLL_IN_C 相连，经DRV8313进行功率放大，由引脚ROLL_A 、ROLL_B 、ROLL_C 输出驱动横滚轴电机，从而调整云台的横滚轴角度，俯仰轴的控制方式与横滚轴类似。
图3三机无刷电机驱动电路
四、系统软件设计
本系统搭载UCOS-II 嵌入式操作系统，使得可以进行分任务多线程操作，简化了开发过程。使用IIC 协议与OLED 显示屏和MPU9250进行通信，使用卡尔曼滤波融合算法进行姿态解析，使用PID 算法调控SPWM 波，从而控制电机达到稳定姿态的效果。系统整体分为三个任务：姿态融合任务、PID 任务、错误扫描任务。
在主函数中，进行所有功能的初始化，包括使用的引脚、SPWM 时钟、IIC 接口时钟、驱动电路等。
姿态融合任务主要包含姿态原始数据回传、姿态融合算法实现和最后计算所得姿态输出，其流程如图4所示，PID 任务用于计算SPWM 波的调控量，具体流程如图5
所示。
图4姿态融合任务流程图图5PID 任务流程图
五、系统仿真与测试
首先从MCU 中获取三轴角速度与三轴加速度数据进行测试，选取其中25s 的时间产生的数据，由MATLAB 进行仿真，如图6-7所示，原始数据中可以看到角速度高频性能良好，而加速度则高频都是毛刺。
图6原始角速度图
图7原始加速度
151
积分在计算机中无法直接进行，使用龙格库塔法可以在计算机中实现高精度的积分[8]，由于比较复杂，这里只引入最终结果，使用龙格库塔法直接对两轴的角速度进行积分得角度波形如图8所示，而通过对两轴的加速度做正交，得到角度波形如图9所示。
图8原始角速度积分计算角度结果图
图9加速度正交计算角度结果
沈阳大学学报
为使结果更直观，将两者进行叠加显示，其结果如图10所示，图中虚线为加速度正交角度，实线为陀螺仪积分角度，可以看到在该情况下，加速度直接计算的角度高频噪声严重，而陀螺仪计算的角度存在漂移现象，两者都无法直接使用。而采用卡尔曼滤波算法来进行滤波融合，其结果如图11所示，可以看到由算法进行融合后，能产生较为平滑的波形，能够很好消除角度偏移和抖动，达到解算较为稳定的姿态效果。
三溴化吡啶图10直接计算角度叠加图
图11卡尔曼滤波算法融合结果
在实际调试中，给定不同的初始参数会导致融合结果的不一致，实际上真正解算出的角度会因为每个芯片的不同，得到的偏移值也有所不同，这里只能尽量保证其性能平稳，解析姿态不会大幅度漂移。
本系统搭载三相无刷直流电机，选用DRV8313作为功率放大芯片，并对PID参数进行调节，当PID参数不合适的时候，云台无法平稳运行，经过测试，本平台P参数小于10时抖动严重，大于10时轻微抖动，而I参数影响不大，0.05-0.08都可以平稳运行，D参数随P参数的变化而不同，经过测试能够稳定运行的参数为P:10、I:0.08、D:5，在平稳运行时，通过记录滤波融合后随机抽查传出的横滚轴和俯仰轴角度数据如表1所示，从表中可以看出，角度的仅在±1°范围内抖动，云台总体运行比较平稳。
表1横滚轴和俯仰轴角度数据
次数
1
2
3
山东省人口与计划生育条例修正案4
5
横滚轴角度(°)
神探夏洛克字幕0.6267
0.8773
-0.6157
-0.75992
0.5938
俯仰轴角度(°)
0.2063
0.2338
0.1898
0.8908
-0.1678
六、结语
系统采用姿态传感器MPU9250采集云台的姿态，并通过卡尔漫滤波融合算法解析姿态，配合PID算法控制SPWM 波形控制三相无刷直流电机，从而调整云台的姿态，通过系统仿真与实验测试，系统运行平稳，能够有效减小了图像的抖动，画面稳定，其可以应用在各种小型的视频拍摄领域，对于一些较大拍摄设备，只需要更换电机及支架，重新调整PID参数即可，
而姿态的解算则是可以通用的。152
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议