基于STM32的云台稳定器设计

赖义汉 龙忠华 刘子铄 李小强 熊益康

（龙岩学院物理与机电工程学院 福建龙岩 364012）

随着数码产品的快速发展，视频拍摄已经成为大部分人生活的一部分。目前，便携式拍摄设备主要以手机或小型摄像机为主，由于部分手机或者小型摄像机没有防抖功能或者防抖功能较弱，直接手持设备拍摄时容易发生抖动，导致拍摄的视频出现模糊、视频画面质量不佳等现象。为了解决拍摄抖动的问题，提出以STM32为控制核心的防抖动云台的设计方法，通过MPU9250获取云台姿态角，采用卡尔曼滤波融合算法计算出最优姿态角，使用PID算法调节三相无刷直流电机，使云台保持平衡。

一、系统总体设计方案

云台控制系统由MPU9250姿态传感器、STM32主控芯片、电机驱动模块、横滚轴电机和俯仰轴电机等部分组成，系统总体框图如图1所示：

图1 系统总体框图

本系统使用MPU9250作为姿态传感器[1]，能实时检测当前云台的角速度、加速度等信息，并传送到STM32单片机中，采用卡尔曼滤波算法进行滤波融合，得到最优的云台偏转角[2]。根据当前云台姿态角与水平姿态的偏差，使用PID算法输出两组三相SPWM波形，其中一组驱动横滚轴电机，另一组驱动俯仰轴电机；每组SPWM波由三路相位差各为120o的SPWM波形组成。三相SPWM波信号经过DRV8313芯片进行功率放大后驱动模滚轴及俯仰轴无刷直流电机，调节云台的俯仰轴和横滚轴姿态角，从而形成一个闭环的控制系统，保持云台稳定。

二、系统姿态解算及PID算法

（一）卡尔曼滤波。MPU9250传感器中集成有陀螺仪和加速度传感器，其主要用于检测俯仰轴和横滚轴偏转角。陀螺仪可以检测当前的角速度，将输出的角速度积分就可以得到姿态，其高频动态特性好，但具有低频噪声，计算机积分后会产生误差，若不对其误差做修正，长期使用会导致姿态计算错误，即存在漂移现象，而且随着时间的累积，该误差会被一直放大。加速度计可以测量三个轴的重力加速度，其低频特性好，可以比较准确测量低速的静态加速度，但是具有高频噪声，轻微的抖动都会造成数据的波动，无法保证实时姿态的正确性，需要进行低通滤波，实时解算姿态波动严重，滤波后又难于保证姿态正确。因此，单独使用其中一种传感器都无法进行正确的姿态解算，系统拟通过卡尔曼滤波融合算法[3]，得到最优的姿态角。

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，通过递归处理，将两个正态分布的数据融合为一个正态分布的结果[4]。卡尔曼滤波的具体处理流程包括状态预测、协方差预测、误差增益更新、协方差更新等五个步骤，具体计算方法如式(1)-(5)所示。

该算法假设测量都存在噪声，结合多个包含噪声的数据，计算出最优解，这个过程可以当成是滤波的过程，十分适合多传感器的数据融合。

（二）PID算法。PID算法是目前应用最为广泛的控制算法之一，该算法仅有三个参数，原理简单易于实现，效果良好。PID控制律的表达式如式（6）所示：

其中Kp为比例系数；TI为积分系数；TD为微分系数；u(t)为输出量；e(t)为偏差量。而这种的数学表达式在计算机中无法直接进行运算，为了应用该算法，使用离散化将控制式变为差分方程[5]，使用三次迭代即可算出最终效果。

其增量型控制算法，如式（7）所示：

本系统使用增量型PID算法[6]，定义一个结构体，存储e(k)，e(k-1)，e(k-2)，每次计算都是迭代更新，并且输出

三、系统硬件电路设计

本系统以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的加速度值和角速度值通过卡尔漫滤波数据融合处理后，得到最优的云台的偏转角度值。

图2 MPU9250接口原理图

（二）三相直流电机驱动电路。三相无刷电机驱动信号采用SPWM波控制[7]，即脉冲波的幅度不变，占空比大小呈正弦变换的一序列脉冲波。系统由STM32单片机产生三相的PWM波来模拟正弦波信号，且输出三个波形相位差各为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 姿态融合任务流程图

图5 PID任务流程图

五、系统仿真与测试

首先从MCU中获取三轴角速度与三轴加速度数据进行测试，选取其中25s的时间产生的数据，由MATLAB进行仿真，如图6-7所示，原始数据中可以看到角速度高频性能良好，而加速度则高频都是毛刺。

图6 原始角速度图

图7 原始加速度

积分在计算机中无法直接进行，使用龙格库塔法可以在计算机中实现高精度的积分[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 横滚轴和俯仰轴角度数据

六、结语

系统采用姿态传感器MPU9250采集云台的姿态，并通过卡尔漫滤波融合算法解析姿态，配合PID算法控制SPWM波形控制三相无刷直流电机，从而调整云台的姿态，通过系统仿真与实验测试，系统运行平稳，能够有效减小了图像的抖动，画面稳定，其可以应用在各种小型的视频拍摄领域，对于一些较大拍摄设备，只需要更换电机及支架，重新调整PID参数即可，而姿态的解算则是可以通用的。