基于STM32的云臺(tái)穩(wěn)定器設(shè)計(jì)

賴義漢 龍忠華 劉子鑠 李小強(qiáng) 熊益康

（龍巖學(xué)院物理與機(jī)電工程學(xué)院 福建龍巖 364012）

隨著數(shù)碼產(chǎn)品的快速發(fā)展，視頻拍攝已經(jīng)成為大部分人生活的一部分。目前，便攜式拍攝設(shè)備主要以手機(jī)或小型攝像機(jī)為主，由于部分手機(jī)或者小型攝像機(jī)沒(méi)有防抖功能或者防抖功能較弱，直接手持設(shè)備拍攝時(shí)容易發(fā)生抖動(dòng)，導(dǎo)致拍攝的視頻出現(xiàn)模糊、視頻畫面質(zhì)量不佳等現(xiàn)象。為了解決拍攝抖動(dòng)的問(wèn)題，提出以STM32為控制核心的防抖動(dòng)云臺(tái)的設(shè)計(jì)方法，通過(guò)MPU9250獲取云臺(tái)姿態(tài)角，采用卡爾曼濾波融合算法計(jì)算出最優(yōu)姿態(tài)角，使用PID算法調(diào)節(jié)三相無(wú)刷直流電機(jī)，使云臺(tái)保持平衡。

一、系統(tǒng)總體設(shè)計(jì)方案

云臺(tái)控制系統(tǒng)由MPU9250姿態(tài)傳感器、STM32主控芯片、電機(jī)驅(qū)動(dòng)模塊、橫滾軸電機(jī)和俯仰軸電機(jī)等部分組成，系統(tǒng)總體框圖如圖1所示：

圖1 系統(tǒng)總體框圖

本系統(tǒng)使用MPU9250作為姿態(tài)傳感器[1]，能實(shí)時(shí)檢測(cè)當(dāng)前云臺(tái)的角速度、加速度等信息，并傳送到STM32單片機(jī)中，采用卡爾曼濾波算法進(jìn)行濾波融合，得到最優(yōu)的云臺(tái)偏轉(zhuǎn)角[2]。根據(jù)當(dāng)前云臺(tái)姿態(tài)角與水平姿態(tài)的偏差，使用PID算法輸出兩組三相SPWM波形，其中一組驅(qū)動(dòng)橫滾軸電機(jī)，另一組驅(qū)動(dòng)俯仰軸電機(jī)；每組SPWM波由三路相位差各為120o的SPWM波形組成。三相SPWM波信號(hào)經(jīng)過(guò)DRV8313芯片進(jìn)行功率放大后驅(qū)動(dòng)模滾軸及俯仰軸無(wú)刷直流電機(jī)，調(diào)節(jié)云臺(tái)的俯仰軸和橫滾軸姿態(tài)角，從而形成一個(gè)閉環(huán)的控制系統(tǒng)，保持云臺(tái)穩(wěn)定。

二、系統(tǒng)姿態(tài)解算及PID算法

（一）卡爾曼濾波。MPU9250傳感器中集成有陀螺儀和加速度傳感器，其主要用于檢測(cè)俯仰軸和橫滾軸偏轉(zhuǎn)角。陀螺儀可以檢測(cè)當(dāng)前的角速度，將輸出的角速度積分就可以得到姿態(tài)，其高頻動(dòng)態(tài)特性好，但具有低頻噪聲，計(jì)算機(jī)積分后會(huì)產(chǎn)生誤差，若不對(duì)其誤差做修正，長(zhǎng)期使用會(huì)導(dǎo)致姿態(tài)計(jì)算錯(cuò)誤，即存在漂移現(xiàn)象，而且隨著時(shí)間的累積，該誤差會(huì)被一直放大。加速度計(jì)可以測(cè)量三個(gè)軸的重力加速度，其低頻特性好，可以比較準(zhǔn)確測(cè)量低速的靜態(tài)加速度，但是具有高頻噪聲，輕微的抖動(dòng)都會(huì)造成數(shù)據(jù)的波動(dòng)，無(wú)法保證實(shí)時(shí)姿態(tài)的正確性，需要進(jìn)行低通濾波，實(shí)時(shí)解算姿態(tài)波動(dòng)嚴(yán)重，濾波后又難于保證姿態(tài)正確。因此，單獨(dú)使用其中一種傳感器都無(wú)法進(jìn)行正確的姿態(tài)解算，系統(tǒng)擬通過(guò)卡爾曼濾波融合算法[3]，得到最優(yōu)的姿態(tài)角。

卡爾曼濾波（Kalman filtering）是一種利用線性系統(tǒng)狀態(tài)方程，通過(guò)系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法，通過(guò)遞歸處理，將兩個(gè)正態(tài)分布的數(shù)據(jù)融合為一個(gè)正態(tài)分布的結(jié)果[4]?？柭鼮V波的具體處理流程包括狀態(tài)預(yù)測(cè)、協(xié)方差預(yù)測(cè)、誤差增益更新、協(xié)方差更新等五個(gè)步驟，具體計(jì)算方法如式(1)-(5)所示。

該算法假設(shè)測(cè)量都存在噪聲，結(jié)合多個(gè)包含噪聲的數(shù)據(jù)，計(jì)算出最優(yōu)解，這個(gè)過(guò)程可以當(dāng)成是濾波的過(guò)程，十分適合多傳感器的數(shù)據(jù)融合。

（二）PID算法。PID算法是目前應(yīng)用最為廣泛的控制算法之一，該算法僅有三個(gè)參數(shù)，原理簡(jiǎn)單易于實(shí)現(xiàn)，效果良好。PID控制律的表達(dá)式如式（6）所示：

其中Kp為比例系數(shù)；TI為積分系數(shù)；TD為微分系數(shù)；u(t)為輸出量；e(t)為偏差量。而這種的數(shù)學(xué)表達(dá)式在計(jì)算機(jī)中無(wú)法直接進(jìn)行運(yùn)算，為了應(yīng)用該算法，使用離散化將控制式變?yōu)椴罘址匠蘙5]，使用三次迭代即可算出最終效果。

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

本系統(tǒng)使用增量型PID算法[6]，定義一個(gè)結(jié)構(gòu)體，存儲(chǔ)e(k)，e(k-1)，e(k-2)，每次計(jì)算都是迭代更新，并且輸出

三、系統(tǒng)硬件電路設(shè)計(jì)

本系統(tǒng)以STM32F103RCT6單片機(jī)作為主控芯片，其自帶了通用異步串行通信接口（USART）、兩線式串行總線（IIC）、四線同步串行接口總線（SPI）等各種通信接口，工作頻率最高達(dá)72MHz，支持單周期乘法和硬件除法，使得運(yùn)算速度大大提升。

（一）姿態(tài)傳感器。姿態(tài)傳感器MPU9250內(nèi)部集成有三軸陀螺儀、三軸加速度計(jì)和三軸磁力計(jì)，其輸出為16位的數(shù)字量；MPU9250模塊通過(guò)IIC總線接口（SDA、SCL）與STM32單片機(jī)進(jìn)行通信，傳輸速率可達(dá)400kHz/s，其電路如圖2所示。

MPU9250作為從機(jī)，其地址為0x0D，系統(tǒng)只讀取MPU9250角速度與加速度值，每個(gè)傳感器單軸數(shù)據(jù)分為高低8位，分別存儲(chǔ)在兩個(gè)寄存器中，以加速度數(shù)據(jù)為例，X軸加速度高8位數(shù)據(jù)的地址為0x3B，X軸加速度低8位數(shù)據(jù)的地址為0x3C，需要讀取兩個(gè)字節(jié)才能完成一次完整的采樣。本系統(tǒng)調(diào)用了加速度傳感器與角速度傳感器的數(shù)據(jù)，共計(jì)6個(gè)寄存器的數(shù)據(jù)。

單片機(jī)讀取姿態(tài)傳感器MPU9250的加速度值和角速度值通過(guò)卡爾漫濾波數(shù)據(jù)融合處理后，得到最優(yōu)的云臺(tái)的偏轉(zhuǎn)角度值。

圖2 MPU9250接口原理圖

（二）三相直流電機(jī)驅(qū)動(dòng)電路。三相無(wú)刷電機(jī)驅(qū)動(dòng)信號(hào)采用SPWM波控制[7]，即脈沖波的幅度不變，占空比大小呈正弦變換的一序列脈沖波。系統(tǒng)由STM32單片機(jī)產(chǎn)生三相的PWM波來(lái)模擬正弦波信號(hào)，且輸出三個(gè)波形相位差各為120o，用于驅(qū)動(dòng)三相無(wú)刷電機(jī)。

三相無(wú)刷電機(jī)驅(qū)動(dòng)采用DRV8313集成芯片，其內(nèi)部集成有三個(gè)可獨(dú)立控制的半H橋驅(qū)動(dòng)器，最大可驅(qū)動(dòng)電流為2.5A、電壓為24V的負(fù)載。系統(tǒng)采用兩個(gè)三相無(wú)刷直流電機(jī)分別控制云臺(tái)的橫滾軸與俯仰軸。由于橫滾軸與俯仰軸控制方式相同，以橫滾軸為例，其電路如圖3所示。由STM32產(chǎn)生的三路SPWM 波 與 DRV8313 的 ROLL_IN_A，ROLL_IN_B，ROLL_IN_C相連，經(jīng)DRV8313進(jìn)行功率放大，由引腳ROLL_A、ROLL_B、ROLL_C輸出驅(qū)動(dòng)橫滾軸電機(jī)，從而調(diào)整云臺(tái)的橫滾軸角度，俯仰軸的控制方式與橫滾軸類似。

圖3 三機(jī)無(wú)刷電機(jī)驅(qū)動(dòng)電路

四、系統(tǒng)軟件設(shè)計(jì)

本系統(tǒng)搭載UCOS-II嵌入式操作系統(tǒng)，使得可以進(jìn)行分任務(wù)多線程操作，簡(jiǎn)化了開(kāi)發(fā)過(guò)程。使用IIC協(xié)議與OLED顯示屏和MPU9250進(jìn)行通信，使用卡爾曼濾波融合算法進(jìn)行姿態(tài)解析，使用PID算法調(diào)控SPWM波，從而控制電機(jī)達(dá)到穩(wěn)定姿態(tài)的效果。系統(tǒng)整體分為三個(gè)任務(wù)：姿態(tài)融合任務(wù)、PID任務(wù)、錯(cuò)誤掃描任務(wù)。

在主函數(shù)中，進(jìn)行所有功能的初始化，包括使用的引腳、SPWM時(shí)鐘、IIC接口時(shí)鐘、驅(qū)動(dòng)電路等。姿態(tài)融合任務(wù)主要包含姿態(tài)原始數(shù)據(jù)回傳、姿態(tài)融合算法實(shí)現(xiàn)和最后計(jì)算所得姿態(tài)輸出，其流程如圖4所示，PID任務(wù)用于計(jì)算SPWM波的調(diào)控量，具體流程如圖5所示。

圖4 姿態(tài)融合任務(wù)流程圖

圖5 PID任務(wù)流程圖

五、系統(tǒng)仿真與測(cè)試

首先從MCU中獲取三軸角速度與三軸加速度數(shù)據(jù)進(jìn)行測(cè)試，選取其中25s的時(shí)間產(chǎn)生的數(shù)據(jù)，由MATLAB進(jìn)行仿真，如圖6-7所示，原始數(shù)據(jù)中可以看到角速度高頻性能良好，而加速度則高頻都是毛刺。

圖6 原始角速度圖

圖7 原始加速度

積分在計(jì)算機(jī)中無(wú)法直接進(jìn)行，使用龍格庫(kù)塔法可以在計(jì)算機(jī)中實(shí)現(xiàn)高精度的積分[8]，由于比較復(fù)雜，這里只引入最終結(jié)果，使用龍格庫(kù)塔法直接對(duì)兩軸的角速度進(jìn)行積分得角度波形如圖8所示，而通過(guò)對(duì)兩軸的加速度做正交，得到角度波形如圖9所示。

圖8 原始角速度積分計(jì)算角度結(jié)果圖

圖9 加速度正交計(jì)算角度結(jié)果

為使結(jié)果更直觀，將兩者進(jìn)行疊加顯示，其結(jié)果如圖10所示，圖中虛線為加速度正交角度，實(shí)線為陀螺儀積分角度，可以看到在該情況下，加速度直接計(jì)算的角度高頻噪聲嚴(yán)重，而陀螺儀計(jì)算的角度存在漂移現(xiàn)象，兩者都無(wú)法直接使用。而采用卡爾曼濾波算法來(lái)進(jìn)行濾波融合，其結(jié)果如圖11所示，可以看到由算法進(jìn)行融合后，能產(chǎn)生較為平滑的波形，能夠很好消除角度偏移和抖動(dòng)，達(dá)到解算較為穩(wěn)定的姿態(tài)效果。

圖10 直接計(jì)算角度疊加圖

圖11 卡爾曼濾波算法融合結(jié)果

在實(shí)際調(diào)試中，給定不同的初始參數(shù)會(huì)導(dǎo)致融合結(jié)果的不一致，實(shí)際上真正解算出的角度會(huì)因?yàn)槊總€(gè)芯片的不同，得到的偏移值也有所不同，這里只能盡量保證其性能平穩(wěn)，解析姿態(tài)不會(huì)大幅度漂移。

本系統(tǒng)搭載三相無(wú)刷直流電機(jī)，選用DRV8313作為功率放大芯片，并對(duì)PID參數(shù)進(jìn)行調(diào)節(jié)，當(dāng)PID參數(shù)不合適的時(shí)候，云臺(tái)無(wú)法平穩(wěn)運(yùn)行，經(jīng)過(guò)測(cè)試，本平臺(tái)P參數(shù)小于10時(shí)抖動(dòng)嚴(yán)重，大于10時(shí)輕微抖動(dòng)，而I參數(shù)影響不大，0.05-0.08都可以平穩(wěn)運(yùn)行，D參數(shù)隨P參數(shù)的變化而不同，經(jīng)過(guò)測(cè)試能夠穩(wěn)定運(yùn)行的參數(shù)為P:10、I:0.08、D:5，在平穩(wěn)運(yùn)行時(shí)，通過(guò)記錄濾波融合后隨機(jī)抽查傳出的橫滾軸和俯仰軸角度數(shù)據(jù)如表1所示，從表中可以看出，角度的僅在±1°范圍內(nèi)抖動(dòng)，云臺(tái)總體運(yùn)行比較平穩(wěn)。

表1 橫滾軸和俯仰軸角度數(shù)據(jù)

六、結(jié)語(yǔ)

系統(tǒng)采用姿態(tài)傳感器MPU9250采集云臺(tái)的姿態(tài)，并通過(guò)卡爾漫濾波融合算法解析姿態(tài)，配合PID算法控制SPWM波形控制三相無(wú)刷直流電機(jī)，從而調(diào)整云臺(tái)的姿態(tài)，通過(guò)系統(tǒng)仿真與實(shí)驗(yàn)測(cè)試，系統(tǒng)運(yùn)行平穩(wěn)，能夠有效減小了圖像的抖動(dòng)，畫面穩(wěn)定，其可以應(yīng)用在各種小型的視頻拍攝領(lǐng)域，對(duì)于一些較大拍攝設(shè)備，只需要更換電機(jī)及支架，重新調(diào)整PID參數(shù)即可，而姿態(tài)的解算則是可以通用的。