王康南 鄭州外國(guó)語(yǔ)學(xué)校(河南鄭州 450001)，王利霞 鄭州輕工業(yè)學(xué)院(河南 鄭州 450002)

　　近些年來(lái)，無(wú)人飛行器在航空攝影、高空遙感以及高空地形勘探等領(lǐng)域得到了廣泛發(fā)展，機(jī)載穩(wěn)定云臺(tái)的其應(yīng)用的關(guān)鍵所在[1-3]。但是在飛行過(guò)程中，云臺(tái)極易受到機(jī)體姿態(tài)變化、振動(dòng)以及氣流擾動(dòng)等因素的影響，從而造成機(jī)載圖像抖動(dòng)、模糊[4]。針對(duì)這一問(wèn)題，設(shè)計(jì)出一種適用于無(wú)人飛行器的機(jī)載穩(wěn)定云臺(tái)控制系統(tǒng)。該云臺(tái)系統(tǒng)采用了三軸穩(wěn)定結(jié)構(gòu)，控制部分由主控單元模塊、姿態(tài)檢測(cè)模塊、無(wú)刷電機(jī)驅(qū)動(dòng)模塊等等構(gòu)成。主控單元通過(guò)對(duì)姿態(tài)檢測(cè)模塊反饋的數(shù)據(jù)進(jìn)行互補(bǔ)濾波解算，驅(qū)動(dòng)無(wú)刷電機(jī)對(duì)云臺(tái)姿態(tài)進(jìn)行實(shí)時(shí)控制。通過(guò)對(duì)該云臺(tái)控制系統(tǒng)的測(cè)試與實(shí)驗(yàn)，無(wú)人飛行器搭載該云臺(tái)后，機(jī)載視頻圖像更加清晰、穩(wěn)定，滿(mǎn)足了系統(tǒng)設(shè)計(jì)要求。

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

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

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

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

　　整個(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所示。

　　圖2 系統(tǒng)工作流程圖

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

　　在本系統(tǒng)中，主控單元基于嵌入式微控制器STM32F103開(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)似。

　　圖3 電機(jī)驅(qū)動(dòng)單元電路原理圖

　　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所示。

　　圖4 MPU6050電路原理圖與電路板

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

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

　　主控單元軟件完成的主要功能有：對(duì)主控處理器I/O口、定時(shí)器、串行通信、IIC協(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所示。

　　圖5主控程序流程圖

　　在每個(gè)控制周期內(nèi)，外部中斷子程序需要完成以下工作：(1)對(duì)陀螺儀和加速度計(jì)輸出信號(hào)進(jìn)行采集;(2)根據(jù)RC指令，完成相應(yīng)位置環(huán)的校正運(yùn)算;(3)完成速度環(huán)和穩(wěn)定環(huán)的校正運(yùn)算;(4)執(zhí)行PID控制算法;(5)生成PWM信號(hào)驅(qū)動(dòng)直流電機(jī)。因此，外部中斷子程序流程如圖6所示。

　　圖6 外部中斷子程序流程圖

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

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

　　以俯仰通道為例，互補(bǔ)濾波的原理可描述為：

　　(1)

　　其中， f1(s) 為一階低通濾波器傳遞函數(shù)， f2(s)為一階高通濾波器傳遞函數(shù)，且f1(s)+ f2(s)=1。ωm為陀螺儀測(cè)得的旋轉(zhuǎn)角速度。為俯仰角預(yù)估值，可通過(guò)式(2)計(jì)算得到。

　　(2)

　　其中，分別為在一段時(shí)間內(nèi)在俯仰、滾轉(zhuǎn)和偏航通道上的加速度平均值。

　　從式(1)可以看出，濾波效果取決于參數(shù)K的選取。但是在噪聲較大時(shí)低通的阻帶衰減較慢，難以獲得較好的濾波效果。為了提高姿態(tài)角的解算精度，本文基于常規(guī)互補(bǔ)濾波原理，采用一種自適應(yīng)互補(bǔ)濾波(Adaptive Complementary Filtering, ACF)算法，其原理如圖7所示。

　　圖7 ACF原理圖

　　圖7中，Kp為比例系數(shù)，Ki為積分系數(shù)?？紤]實(shí)時(shí)性問(wèn)題，通常設(shè)定Ki保持不變，通過(guò)Kp的動(dòng)態(tài)調(diào)整實(shí)現(xiàn)俯仰角誤差的動(dòng)態(tài)補(bǔ)償。自適應(yīng)補(bǔ)償系數(shù)Kp的算法如下：

　　(3)

　　其中，Kp0為初始補(bǔ)償系數(shù);Kp1姿態(tài)臨界發(fā)散系數(shù)。ωmax為陀螺儀的最大量程，ωc為陀螺儀的截止角速度(ωc<ωmax)。

　　從圖7可以看出，加速度計(jì)的實(shí)時(shí)數(shù)據(jù)由式(2)解算得到俯仰角預(yù)估值θ，經(jīng)過(guò)低通環(huán)節(jié)增加的PI 控制器與陀螺儀的實(shí)時(shí)數(shù)據(jù)積分后得到的角度相融合，得到機(jī)載云臺(tái)當(dāng)前俯仰角 。同時(shí)， 作為負(fù)反饋，實(shí)時(shí)機(jī)載云臺(tái)的姿態(tài)角。姿態(tài)更新算法流程見(jiàn)圖8所示。

　　圖8 姿態(tài)更新算法流程圖

　　3 實(shí)驗(yàn)研究

　　本文搭建了以ST公司的STM32F103為主控制器，InvenSense公司的MPU6050芯片作為慣性測(cè)量單元的機(jī)載云臺(tái)控制器硬件平臺(tái)，并將其搭載于某多旋翼無(wú)人飛行器上進(jìn)行了機(jī)載測(cè)試實(shí)驗(yàn)，如圖9所示。

　　圖9 機(jī)載飛行測(cè)試實(shí)驗(yàn)

　　依據(jù)所選用的慣性測(cè)量元件參數(shù)可知，陀螺儀的最大量程ωmax =2000°/sec，截止角速度ωc=2πf，陀螺儀截止頻率f為100Hz，由PID參數(shù)工程整定法得到KP0、KP1分別為5和20。因此自適應(yīng)補(bǔ)償系數(shù)Kp為：

　　通過(guò)與常規(guī)互補(bǔ)濾波算法的對(duì)比試驗(yàn)，驗(yàn)證自適應(yīng)互補(bǔ)濾波算法的有效性。試驗(yàn)結(jié)果如圖10所示。

　　圖10 算法對(duì)比測(cè)試試驗(yàn)

　　其中，圖10(a)為采用互補(bǔ)濾波時(shí)解算得到的俯仰角和滾轉(zhuǎn)角，其解算誤差的均方值約為1.65°和0.39°。圖10(b)為采用自適應(yīng)互補(bǔ)濾波時(shí)解算得到的俯仰角和滾轉(zhuǎn)角，其解算誤差的均方值約為1.26°和0.15°。初始時(shí)刻的俯仰角和滾轉(zhuǎn)角均不處于0°是由于機(jī)載云臺(tái)并非絕對(duì)水平導(dǎo)致。

　　以俯仰通道為例，在機(jī)載云臺(tái)給定俯仰角為0時(shí)，通過(guò)機(jī)載云臺(tái)的視軸穩(wěn)定誤差實(shí)驗(yàn)來(lái)驗(yàn)證系統(tǒng)的穩(wěn)態(tài)性能。實(shí)驗(yàn)結(jié)果如圖11所示。從圖中可以看出，其穩(wěn)定誤差在±0.02°之間，穩(wěn)定精度約為0.26 mrad，具有較高的穩(wěn)定精度。

　　圖11視軸穩(wěn)定誤差曲線(xiàn)

　　4結(jié)論

　　為了實(shí)現(xiàn)多旋翼無(wú)人飛行器機(jī)載慣性云臺(tái)的穩(wěn)定控制，使得機(jī)載視頻圖像穩(wěn)定清晰，本文設(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)性能，完全滿(mǎn)足了多旋翼無(wú)人飛行器機(jī)載云臺(tái)的穩(wěn)定性要求。

　　參考文獻(xiàn)：

　　[1]申斌,吳一波,林冬生.旋翼機(jī)的發(fā)展與應(yīng)用[J].科技傳播,2013,(23):145-146.

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

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

　　[4]MILLER RICK, MOOTY GREG. HILKERT.J. M. Gimbal system configurations and line-of-sight control techniques for small UAV applications[C].Proc. SPIE 8713, Airborne Intelligence, Surveillance, Reconnaissance (ISR) Systems and Applications X, 871308. Baltimore,Maryland,USA,201

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