賴義漢 龍忠華 劉子鑠 李小強 熊益康

（龍巖學院物理與機電工程學院 福建龍巖 364012）

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

一、系統(tǒng)總體設計方案

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

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

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

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

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

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

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

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

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

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

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

三、系統(tǒng)硬件電路設計

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

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

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

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

圖2 MPU9250接口原理圖

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

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

圖3 三機無刷電機驅(qū)動電路

四、系統(tǒng)軟件設計

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

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

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

圖5 PID任務流程圖

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

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

圖6 原始角速度圖

圖7 原始加速度

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

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

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

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

圖10 直接計算角度疊加圖

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

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

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

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

六、結(jié)語

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