王日俊 曾志強(qiáng) 黨長(zhǎng)營(yíng) 段能全 杜文華 王俊元

摘 要： 為了使多旋翼無(wú)人飛行器在飛行過(guò)程中獲取的機(jī)載視頻圖像清晰且穩(wěn)定，設(shè)計(jì)一種基于STM32和自適應(yīng)互補(bǔ)濾波算法的機(jī)載慣性穩(wěn)定云臺(tái)。該云臺(tái)利用自適應(yīng)互補(bǔ)濾波對(duì)俯仰、滾轉(zhuǎn)和偏航三個(gè)通道上的姿態(tài)信息進(jìn)行融合，實(shí)時(shí)驅(qū)動(dòng)直流無(wú)刷電機(jī)進(jìn)行機(jī)載云臺(tái)的姿態(tài)調(diào)整，保持機(jī)載云臺(tái)在慣性空間中的穩(wěn)定性。實(shí)驗(yàn)結(jié)果表明，視軸穩(wěn)定精度達(dá)到0.26 mrad，滿足多旋翼無(wú)人飛行器的應(yīng)用需求。

關(guān)鍵詞： 多旋翼無(wú)人飛行器; STM32; 自適應(yīng)互補(bǔ)濾波; 機(jī)載云臺(tái); MEMS傳感器; 視軸穩(wěn)定

中圖分類(lèi)號(hào)： TN967?34; TP23 文獻(xiàn)標(biāo)識(shí)碼： A 文章編號(hào)： 1004?373X（2018）12?0145?04

Abstract： An airborne inertial stabilization cloud platform based on STM32 and the adaptive complementary filtering algorithm is designed to make the airborne video images obtained by the multi?rotor unmanned aircraft during its flight clear and stable. On this cloud platform， the attitude information from three channels of pitching， rolling and yaw is fused by means of adaptive complementary filtering to drive the DC brushless motor in real time for attitude adjustment of the airborne cloud platform and keep the stability of the airborne cloud platform in inertial space. The experimental results show that the visual axis′s stabilization precision is 0.26 mrad， which can meet the application requirements of the multi?rotor unmanned aircraft.

Keywords： multi?rotor unmanned aircraft; STM32; adaptive complementary filtering; airborne cloud platform; MEMS sensor; visual axis stabilization

多旋翼無(wú)人飛行器具有更好的環(huán)境適應(yīng)性，能夠?qū)崿F(xiàn)前飛、倒飛、懸停以及側(cè)飛等，具有自主巡航飛行能力和自主起降能力[1?2]。這些固定翼式飛行器所不具備的優(yōu)點(diǎn)，使得無(wú)論是在軍用領(lǐng)域還是民用領(lǐng)域，多旋翼無(wú)人飛行器都有著更為廣闊的應(yīng)用前景和發(fā)展前景[3?5]。因此多旋翼無(wú)人飛行器越來(lái)越多地受到公眾的關(guān)注[6]。在多旋翼無(wú)人飛行器上搭載機(jī)載慣性穩(wěn)定云臺(tái)，一方面可以提高飛行器的控制精度，更為重要的是擴(kuò)展飛行器的視覺(jué)能力，是軍事和民用領(lǐng)域應(yīng)用中的關(guān)鍵所在[7]。然而機(jī)載慣性穩(wěn)定云臺(tái)極易受到飛行器姿態(tài)變化、氣流擾動(dòng)以及機(jī)體振動(dòng)等因素的影響，尤其是當(dāng)飛行器自主飛行到一定高度之后的影響更大。由此造成了地面站監(jiān)控平臺(tái)獲得的機(jī)載視頻圖像抖動(dòng)、模糊[8]。為了解決這一問(wèn)題，本文設(shè)計(jì)并實(shí)現(xiàn)了一種基于STM32和MEMS傳感器的多旋翼飛行器機(jī)載三軸慣性穩(wěn)定云臺(tái)。

1 系統(tǒng)硬件方案設(shè)計(jì)

1.1 總體方案設(shè)計(jì)

機(jī)載云臺(tái)控制系統(tǒng)主要是通過(guò)主控制器對(duì)機(jī)體擾動(dòng)的隔離和對(duì)其他擾動(dòng)的補(bǔ)償或抑制來(lái)實(shí)現(xiàn)載荷在慣性空間上的穩(wěn)定，從而使得地面站獲取到的機(jī)載視頻圖像穩(wěn)定且清晰[9]。系統(tǒng)的結(jié)構(gòu)主要包括：主控單元、慣性測(cè)量單元、電機(jī)驅(qū)動(dòng)單元、圖像傳輸單元、遙控器控制單元以及人機(jī)界面。系統(tǒng)結(jié)構(gòu)框圖如圖1所示。

整個(gè)工作流程大致可以描述為機(jī)載云臺(tái)系統(tǒng)上電之后，對(duì)MEMS傳感器的初始姿態(tài)信息進(jìn)行自校準(zhǔn)，進(jìn)行初始姿態(tài)信息的解算，驅(qū)動(dòng)力矩電機(jī)對(duì)機(jī)載云臺(tái)調(diào)整到初始姿態(tài)位置，完成機(jī)載云臺(tái)的初始化過(guò)程。在飛行器的飛行過(guò)程中，根據(jù)傳感器的實(shí)時(shí)信息不斷對(duì)姿態(tài)信息進(jìn)行更新，主控單元依據(jù)更新后的姿態(tài)信息不斷地調(diào)整機(jī)載云臺(tái)在慣性控制中的位置，保持光學(xué)載荷視軸的穩(wěn)定。光學(xué)載荷通過(guò)圖像傳輸鏈路和視頻采集卡實(shí)時(shí)地將機(jī)載視頻圖像傳輸?shù)降孛姹O(jiān)控系統(tǒng)，并在監(jiān)視設(shè)備中顯示。RC遙控器依據(jù)設(shè)定的工作模式，通過(guò)操作手的實(shí)時(shí)操作和RC控制鏈路對(duì)機(jī)載云臺(tái)進(jìn)行控制，從不同角度對(duì)地面目標(biāo)進(jìn)行實(shí)時(shí)監(jiān)測(cè)。機(jī)載云臺(tái)系統(tǒng)工作的流程如圖2所示。

1.2 主控單元設(shè)計(jì)

在本系統(tǒng)中，主控單元基于嵌入式微控制器STM32

F103開(kāi)發(fā)。主要功能是實(shí)時(shí)完成對(duì)MEMS傳感器獲取的姿態(tài)信息的融合，同時(shí)接收上位機(jī)或RC發(fā)來(lái)的指令，對(duì)俯仰、滾轉(zhuǎn)和偏航3個(gè)自由度的電機(jī)運(yùn)動(dòng)控制單元發(fā)送驅(qū)動(dòng)指令，實(shí)現(xiàn)隔離機(jī)體姿態(tài)擾動(dòng)、保持機(jī)載云臺(tái)在慣性空間中的穩(wěn)定以及RC控制操作。此外，還應(yīng)具備與上位機(jī)進(jìn)行人機(jī)交互，實(shí)現(xiàn)控制參數(shù)、電機(jī)配置、MEMS傳感器校正等基本設(shè)置，實(shí)現(xiàn)陀螺儀和加速計(jì)的補(bǔ)償、RC遙控模式和范圍的設(shè)定等高級(jí)設(shè)置以及MEMS傳感器實(shí)時(shí)數(shù)據(jù)的顯示等。

1.3 電機(jī)驅(qū)動(dòng)單元設(shè)計(jì)

驅(qū)動(dòng)單元起到的是功率放大的作用，即將主控處理器的控制信號(hào)轉(zhuǎn)換成可以直接驅(qū)動(dòng)機(jī)載云臺(tái)直流力矩電機(jī)的功率信號(hào)。由于系統(tǒng)采用的直流力矩電機(jī)的堵轉(zhuǎn)電流為2 A，為了保證驅(qū)動(dòng)安全，所選驅(qū)動(dòng)芯片的最大輸出電流應(yīng)大于4 A。因此采用ST公司生產(chǎn)的集成三項(xiàng)半橋驅(qū)動(dòng)芯片L6234D，該芯片具有很強(qiáng)的驅(qū)動(dòng)能力，其驅(qū)動(dòng)電壓可達(dá)58 V，連續(xù)工作時(shí)的驅(qū)動(dòng)電流可以達(dá)到5 A，具有過(guò)流保護(hù)和低電壓鎖存功能。圖3是俯仰通道的電機(jī)驅(qū)動(dòng)單元的電路原理圖，滾轉(zhuǎn)與偏航通道的電機(jī)驅(qū)動(dòng)單元與此類(lèi)似。

1.4 慣性測(cè)量單元設(shè)計(jì)

整個(gè)系統(tǒng)中要求慣性測(cè)量單元（Inertial Measurement Unit，IMU）能夠提供控制算法中所需的角速度和加速度信號(hào)。此外，還考慮機(jī)載云臺(tái)結(jié)構(gòu)和安裝位置，要求角速度傳感器、加速度傳感器及其外圍電路的尺寸盡量小，便于整體結(jié)構(gòu)的緊湊。基于上述要求，采用6軸運(yùn)動(dòng)傳感器MPU6050作為慣性測(cè)量元件。IMU單元測(cè)量相機(jī)在三維空間中的角速度和加速度，并通過(guò)自適應(yīng)互補(bǔ)濾波算法解算出相機(jī)的姿態(tài)。慣性測(cè)量單元的電路原理圖及電路實(shí)現(xiàn)如圖4所示。

2 系統(tǒng)軟件方案設(shè)計(jì)

2.1 主控單元軟件設(shè)計(jì)

主控單元軟件完成的主要功能有：對(duì)主控處理器I/O口、定時(shí)器、串行通信、I2C協(xié)議、MPU6050內(nèi)存儲(chǔ)器、位置參數(shù)、速度參數(shù)、控制參數(shù)以及中斷向量和優(yōu)先級(jí)進(jìn)行初始化;確定機(jī)載云臺(tái)在慣性空間內(nèi)的坐標(biāo)位置，輸出PWM信號(hào)驅(qū)動(dòng)電機(jī)達(dá)到預(yù)先設(shè)定位置等。主控程序流程圖如圖5所示。在每個(gè)控制周期內(nèi)，外部中斷子程序需要完成以下工作：對(duì)陀螺儀和加速度計(jì)輸出信號(hào)進(jìn)行采集;根據(jù)RC指令，完成相應(yīng)位置環(huán)的校正運(yùn)算;完成速度環(huán)和穩(wěn)定環(huán)的校正運(yùn)算;執(zhí)行PID控制算法;生成PWM信號(hào)驅(qū)動(dòng)直流電機(jī)。因此，外部中斷子程序流程如圖6所示。

2.2 基于自適應(yīng)濾波的姿態(tài)解算設(shè)計(jì)

為了消除加速度計(jì)的噪聲干擾和陀螺儀的漂移，獲得準(zhǔn)確的姿態(tài)信息，綜合加速度計(jì)和陀螺儀各自的優(yōu)點(diǎn)，應(yīng)用互補(bǔ)濾波算法（CF），從頻域的角度分別加入低通和高通濾波器，將兩傳感器的姿態(tài)信息加以融合，可去除干擾，消除零位誤差，提高解算精度[10]。以俯仰通道為例，互補(bǔ)濾波的原理可描述為：

4 結(jié) 論

本文設(shè)計(jì)了一種基于STM32和自適應(yīng)互補(bǔ)濾波算法的機(jī)載慣性穩(wěn)定云臺(tái)。姿態(tài)解算對(duì)比試驗(yàn)結(jié)果表明，自適應(yīng)互補(bǔ)濾波算法有效地提高了機(jī)載云臺(tái)姿態(tài)的解算精度。視軸穩(wěn)態(tài)精度的實(shí)驗(yàn)表明，穩(wěn)定精度達(dá)到0.26 mrad，具有良好的穩(wěn)態(tài)性能，完全滿足多旋翼無(wú)人飛行器機(jī)載云臺(tái)的穩(wěn)定性要求。

參考文獻(xiàn)

[1] WANG Rijun， ZHANG Zhiwen. Fuzzy adaptive PID hybrid control for airborne platform mounted on mUAV [C]// Proceedings of IEEE Information Technology， Networking， Electronic and Automation Control Conference. Chongqing： IEEE， 2016： 737?741.

[2] CHEN W H， YANG J， GUO L， et al. Disturbance?observer based control and related methods： an overview [J]. IEEE transactions on industrial electronics， 2016， 63（2）： 1083?1095.

[3] 申斌，吳一波，林冬生.旋翼機(jī)的發(fā)展與應(yīng)用[J].科技傳播，2013（23）：145?146.

SHEN Bin， WU Yibo， LIN Dongsheng. Development and application of rotorcraft [J]. Public communication of science & technology， 2013（23）： 145?146.

[4] 王日俊，白越，續(xù)志軍，等.多旋翼無(wú)人飛行器機(jī)載云臺(tái)的復(fù)合穩(wěn)定控制方法[J].電光與控制，2016，23（4）：17?22.

WANG Rijun， BAI Yue， XU Zhijun， et al. Composite stability control of airborne platform mounted on multi?rotor UAV [J]. Electronics optics & control， 2016， 23（4）： 17?22.

[5] 王日俊.多旋翼無(wú)人飛行器載荷穩(wěn)像技術(shù)研究[D].北京：中國(guó)科學(xué)院大學(xué)，2015.

WANG Rijun. Study on image stabilization technology for the payload of mUAV [D]. Beijing： University of Chinese Academy of Sciences， 2015.

[6] 安鶴男，陳陽(yáng)，張軍，等.基于AVR單片機(jī)的機(jī)載慣性穩(wěn)定云臺(tái)設(shè)計(jì)[J].電子技術(shù)應(yīng)用，2015，41（11）：41?44.

AN Henan， CHEN Yang， ZHANG Jun， et al. Design of airborne inertial stabilization camera platform based on AVR microcontroller [J]. Application of electronic technique， 2015， 41（11）： 41?44.

[7] MILLER R， MOOTY G， HILKERT J M. Gimbal system configurations and line?of?sight control techniques for small UAV applications [C]// Proceedings of Airborne Intelligence， Surveillance， Reconnaissance Systems and Applications X. Baltimore： SPIE， 2013： 1?15.

[8] 王日俊，白越，續(xù)志軍，等.基于擾動(dòng)觀測(cè)器的多旋翼無(wú)人機(jī)機(jī)載云臺(tái)模糊自適應(yīng)跟蹤控制[J].浙江大學(xué)學(xué)報(bào)（工學(xué)版），2015，49（10）：2005?2012.

WANG Rijun， BAI Yue， XU Zhijun， et al. Fuzzy self?adjusting tracking control based on disturbance observer for airborne platform mounted on multi?rotor unmanned aerial vehicle [J]. Journal of Zhejiang University （Engineering science）， 2015， 49（10）： 2005?2012.

[9] MAI Y， ZHAO H Y， GUO S Y. The analysis of image stabilization technology based on small?UAV airborne video [C]// Proceedings of International Conference on Computer Science and Electronics Engineering. Hangzhou： IEEE， 2012： 586?589.

[10] 傅忠云，劉文波，孫金秋，等.自適應(yīng)混合濾波算法在微型飛行器姿態(tài)估計(jì)中的應(yīng)用[J].傳感技術(shù)學(xué)報(bào)，2014，27（5）：698?703.

FU Zhongyun， LIU Wenbo， SUN Jinqiu， et al. Application of adaptive hybrid filter algorithm in the estimation of the micro air vehicle attitude [J]. Chinese journal of sensors and actuators， 2014， 27（5）： 698?703.