admin管理员组文章数量:1632155
2024年6月12日发(作者:)
∗∗∗第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.
版权声明:本文标题:基于STM32 的云台稳定器设计 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://www.elefans.com/shuma/1718182541a651520.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论