基于STM32 的云台稳定器设计

∗∗∗第40卷第5期

Vol.40No.5JournalofSuihuaUniversity

绥化学院学报2020年5月

May.2020

基于STM32的云台稳定器设计

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

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

摘要：

针对视频拍摄过程中容易出现不稳定的现象，提出以STM32为主控核心的防抖动云台的设计方案。文章介绍

了以MPU9250为姿态传感器获取原始姿态角，利用卡尔曼滤波融合算法计算出稳定可靠的姿态角，当检测到姿态角发生偏

移时，通过PID算法调节SPWM波，经DRV8313芯片放大后驱动三相无刷直流电机，将抖动消除，使云台保持稳定。通过理

论仿真与实际测试，实现了能够搭载小型运动相机的云台稳定器。

关键词：

STM32；MPU9250；姿态算法；云台稳定器

中图分类号：TP273.2文献标识码：A文章编号：2095-0438（2020）05-0149-05

随着数码产品的快速发展，视频拍摄已经成为大部分人

生活的一部分。目前，便携式拍摄设备主要以手机或小型摄

像机为主，由于部分手机或者小型摄像机没有防抖功能或者

防抖功能较弱，直接手持设备拍摄时容易发生抖动，导致拍

摄的视频出现模糊、视频画面质量不佳等现象。为了解决拍

摄抖动的问题，提出以STM32为控制核心的防抖动云台的

设计方法，通过MPU9250获取云台姿态角，采用卡尔曼滤波

融合算法计算出最优姿态角，使用PID算法调节三相无刷直

流电机，使云台保持平衡。

一、系统总体设计方案

云台控制系统由MPU9250姿态传感器、STM32主控芯

片、电机驱动模块、横滚轴电机和俯仰轴电机等部分组成，系

统总体框图如图1所示：

图1系统总体框图

MPU9250

传感器

STM32单片机

横滚轴电机

驱动模块

SPWM

输出

俯仰轴电机

驱动模块

横滚轴

电机

云台

俯仰轴

电机

卡尔漫滤波

数据融合

PID

算法

本系统使用MPU9250作为姿态传感器

[1]

，能实时检测当

前云台的角速度、加速度等信息，并传送到STM32单片机

中，采用卡尔曼滤波算法进行滤波融合，得到最优的云台偏

转角

[2]

。根据当前云台姿态角与水平姿态的偏差，使用PID

算法输出两组三相SPWM波形，其中一组驱动横滚轴电机，

另一组驱动俯仰轴电机；每组SPWM波由三路相位差各为

120

o

的SPWM波形组成。三相SPWM波信号经过DRV8313

收稿日期：2019-10-30

作者简介：赖义汉（1968-），男，福建龙岩人，龙岩学院物理与机电工程学院副教授，硕士，研究方向：嵌入式系统研究与应用。

基金项目：福建省大学生创新创业训练计划项目（2）。

149

Copyright©博看网 . All Rights Reserved.

芯片进行功率放大后驱动模滚轴及俯仰轴无刷直流电机，调

节云台的俯仰轴和横滚轴姿态角，从而形成一个闭环的控制

系统，保持云台稳定。

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

（一）卡尔曼滤波。MPU9250传感器中集成有陀螺仪和

加速度传感器，其主要用于检测俯仰轴和横滚轴偏转角。陀

螺仪可以检测当前的角速度，将输出的角速度积分就可以得

到姿态，其高频动态特性好，但具有低频噪声，计算机积分后

会产生误差，若不对其误差做修正，长期使用会导致姿态计

算错误，即存在漂移现象，而且随着时间的累积，该误差会被

一直放大。加速度计可以测量三个轴的重力加速度，其低频

特性好，可以比较准确测量低速的静态加速度，但是具有高

频噪声，轻微的抖动都会造成数据的波动，无法保证实时姿

态的正确性，需要进行低通滤波，实时解算姿态波动严重，滤

波后又难于保证姿态正确。因此，单独使用其中一种传感器

都无法进行正确的姿态解算，系统拟通过卡尔曼滤波融合算

法

[3]

，得到最优的姿态角。

卡尔曼滤波（Kalmanfiltering）是一种利用线性系统状

态方程，通过系统输入输出观测数据，对系统状态进行最优

估计的算法，通过递归处理，将两个正态分布的数据融合为

一个正态分布的结果。卡尔曼滤波的具体处理流程包括状

[4]

变为差分方程

[5]

，使用三次迭代即可算出最终效果。

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

Δu

(

k

)

=k

p

[

e

(

k

)

-e

(

k-1

)

]

+k

p

k

p

T

D

[e(k-2e

(

k-1

)

+e(k-2))]

T

I

T

e

(

k

)

+

T

I

（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的加速度值和角速度

值通过卡尔漫滤波数据融合处理后，得到最优的云台的偏转

3.3V

态预测、协方差预测、误差增益更新、协方差更新等五个步

骤，具体计算方法如式(1)-(5)所示。

k-1

+Bu

k-1

x

-

状态预测：

k

=Ax

（1）

-

k

T

-1

P

=AP

k-1

A

+Q

协方差预测：

-

k

T

-

k

T

-

k

K

k

=PH

(HP

H

+R)

误差增益更新：

x

k

=x+K

k

(z

k

-Hx)

状态更新：

P

k

=(I-K

k

H)P

协方差更新：

-

k

-

k

（2）

（3）

（4）

（5）

角度值。

该算法假设测量都存在噪声，结合多个包含噪声的数

据，计算出最优解，这个过程可以当成是滤波的过程，十分适

合多传感器的数据融合。

（二）PID算法。PID算法是目前应用最为广泛的控制算

法之一，该算法仅有三个参数，原理简单易于实现，效果良

好。PID控制律的表达式如式（6）所示：

1

u

(

t

)

=K

p

[e

(

t

)

+

T

I

2

3

4

5

6

14

15

16

17

19

*

NCRESV

1

NCVDDIO

8

NCVDD

13

NC

NCnCS

22

NCSDA/SDI

24

NCSCL/SCLK

23

NCAD0/SDO

9

NCINT

12

RESVFSYNC

11

10

REGOUT

7

RESV

20

21

AUX_CL

AUX_DAGND

18

MPU9250

R1R4R5

10 K10 K10 K

nCS

SDA

SCL

AD0/SDO

INT

FSYNC

R2R3

C3

10 K10 K

0.1 uF

C1C2

0.01 uF

0.1 uF

其中

K

p

为比例系数；

u(t)

T

I

为积分系数；

T

D

为微分系数；

为输出量；

e(t)

为偏差量。而这种的数学表达式在计算机中

无法直接进行运算，为了应用该算法，使用离散化将控制式

∫

de(t)

e

(

t

)

dt+T

D

]

0

dt

t

（6）

图2MPU9250接口原理图

（二）三相直流电机驱动电路。三相无刷电机驱动信号

采用SPWM波控制

[7]

，即脉冲波的幅度不变，占空比大小呈

正弦变换的一序列脉冲波。系统由STM32单片机产生三相

的PWM波来模拟正弦波信号，且输出三个波形相位差各为

150

Copyright©博看网 . All Rights Reserved.

120

o

，用于驱动三相无刷电机。

三相无刷电机驱动采用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输出驱动横滚轴电机，从而调整

云台的横滚轴角度，俯仰轴的控制方式与横滚轴类似。

VCC

C4

1

0.1 uF

2

C53

0.1 uF4

ROLL_A5

6

7

ROLL_B8

ROLL_C9

10

11

12

13

14

*

CP1GND

28

CP2IN1

27

VCPEN1

26

VMIN2

25

OUT1EN2

24

PGND1IN3

23

PGND2EN3

22

OUT2NC

21

OUT3GND

20

PGDN3COMP0

19

VMFAULT

18

COMPPSLEEP

17

COMPNRESET

16

GNDV3P3OUT

15

DRV8313

ROLL_IN_A

ROLL_EN

ROLL_IN_B

ROLL_EN

ROLL_IN_C

ROLL_EN

驱动俯仰轴电机

获取当前系统

俯仰轴姿态角

迭代PID结构体,

计算增量

是

开始

是否有标准

姿态角?

否

获取标准比对姿态

获取当前系统

横滚轴姿态角

迭代PID结构体,

计算增量

调控俯仰轴PWM波调控横滚轴PWM波

驱动横滚轴电机

图5PID任务流程图

C6

10 uF

五、系统仿真与测试

首先从MCU中获取三轴角速度与三轴加速度数据进行

测试，选取其中25s的时间产生的数据，由MATLAB进行仿

真，如图6-7所示，原始数据中可以看到角速度高频性能良

好，而加速度则高频都是毛刺。

图3三机无刷电机驱动电路

四、系统软件设计

本系统搭载UCOS-II嵌入式操作系统，使得可以进行分

任务多线程操作，简化了开发过程。使用IIC协议与OLED

显示屏和MPU9250进行通信，使用卡尔曼滤波融合算法进

行姿态解析，使用PID算法调控SPWM波，从而控制电机达

到稳定姿态的效果。系统整体分为三个任务：姿态融合任

务、PID任务、错误扫描任务。

在主函数中，进行所有功能的初始化，包括使用的引脚、

SPWM时钟、IIC接口时钟、驱动电路等。姿态融合任务主要

包含姿态原始数据回传、姿态融合算法实现和最后计算所得

姿态输出，其流程如图4所示，PID任务用于计算SPWM波

的调控量，具体流程如图5所示。

开始

否

是否初始

上电？

是

校准陀螺仪数据和

加速度计数据

（需保持水平不动）

图6原始角速度图

获取三轴角速度与三

轴重力加速度

设定状态与

协方差初值

两个预测方程求解

更新滤波增益

输出融合值，更新滤

波状态和协方差

图4姿态融合任务流程图图7原始加速度

151

Copyright©博看网 . All Rights Reserved.

积分在计算机中无法直接进行，使用龙格库塔法可以在

计算机中实现高精度的积分

[8]

，由于比较复杂，这里只引入最

终结果，使用龙格库塔法直接对两轴的角速度进行积分得角

度波形如图8所示，而通过对两轴的加速度做正交，得到角

度波形如图9所示。

图11卡尔曼滤波算法融合结果

在实际调试中，给定不同的初始参数会导致融合结果的

不一致，实际上真正解算出的角度会因为每个芯片的不同，

图8原始角速度积分计算角度结果图

得到的偏移值也有所不同，这里只能尽量保证其性能平稳，

解析姿态不会大幅度漂移。

本系统搭载三相无刷直流电机，选用DRV8313作为功

率放大芯片，并对PID参数进行调节，当PID参数不合适的

时候，云台无法平稳运行，经过测试，本平台P参数小于10时

抖动严重，大于10时轻微抖动，而I参数影响不大，0.05-0.08

都可以平稳运行，D参数随P参数的变化而不同，经过测试能

够稳定运行的参数为P:10、I:0.08、D:5，在平稳运行时，通过

记录滤波融合后随机抽查传出的横滚轴和俯仰轴角度数据

如表1所示，从表中可以看出，角度的仅在±1°范围内抖动，

云台总体运行比较平稳。

表1横滚轴和俯仰轴角度数据

横滚轴角度(°)

0.6267

-0.6157

0.5938

0.8773

俯仰轴角度(°)

0.2063

0.2338

0.1898

-0.1678

0.8908

图9加速度正交计算角度结果

次数

1

2

3

4

5

为使结果更直观，将两者进行叠加显示，其结果如图10

所示，图中虚线为加速度正交角度，实线为陀螺仪积分角度，

可以看到在该情况下，加速度直接计算的角度高频噪声严

重，而陀螺仪计算的角度存在漂移现象，两者都无法直接使

用。而采用卡尔曼滤波算法来进行滤波融合，其结果如图11

所示，可以看到由算法进行融合后，能产生较为平滑的波形，

能够很好消除角度偏移和抖动，达到解算较为稳定的姿态

效果。

-0.75992

六、结语

系统采用姿态传感器MPU9250采集云台的姿态，并通

过卡尔漫滤波融合算法解析姿态，配合PID算法控制SPWM

波形控制三相无刷直流电机，从而调整云台的姿态，通过系

统仿真与实验测试，系统运行平稳，能够有效减小了图像的

抖动，画面稳定，其可以应用在各种小型的视频拍摄领域，对

于一些较大拍摄设备，只需要更换电机及支架，重新调整

图10直接计算角度叠加图

PID参数即可，而姿态的解算则是可以通用的。

152

Copyright©博看网 . All Rights Reserved.