應(yīng)用于共軸雙旋翼飛行器的姿態(tài)控制系統(tǒng)設(shè)計(jì)*

李普森， 張 浩,2,3， 高 永， 王思孝

(1.中國海洋大學(xué) 信息科學(xué)與工程學(xué)院，山東 青島 266100；2. 海洋短波通信開放工作室，山東 青島 266200；3.青島海洋科學(xué)與技術(shù)試點(diǎn)國家實(shí)驗(yàn)室，山東 青島 266200)

0 引 言

隨著計(jì)算機(jī)技術(shù)以及飛行器設(shè)計(jì)行業(yè)的發(fā)展，近年來國際范圍內(nèi)掀起了無人飛行器的研制熱潮，目前小型無人飛行器廣泛應(yīng)用于各個(gè)領(lǐng)域。國內(nèi)外研制的旋翼式飛行器主要分為單旋翼飛行器，共軸雙旋翼飛行器、多旋翼飛行器，此類飛行器體積較小，靈活性較好，且能夠在比較狹小的空間內(nèi)長時(shí)間實(shí)現(xiàn)空中穩(wěn)定懸停、垂直起降和多方向自由飛行[1]。其中共軸雙旋翼無人飛行器和傳統(tǒng)多旋翼無人飛行器相比，機(jī)械結(jié)構(gòu)更加簡單緊湊，在對旋翼進(jìn)行折疊設(shè)計(jì)之后尺寸更小，更加便攜[2]；和單旋翼飛行器相比，懸停效率更高。綜合以上的特性，共軸雙旋翼無人飛行器是一種優(yōu)良的飛行器配置方案。

然而，與目前研究十分成熟的多旋翼飛行器相比，國內(nèi)外學(xué)者很少對共軸雙旋翼飛行器進(jìn)行研究。目前國內(nèi)外研究工作主要有：美國 Drexel大學(xué) Husnic Z對一種微型共軸雙旋翼直升機(jī)設(shè)計(jì)了控制器，并進(jìn)行了聯(lián)合仿真[3];Maryland大學(xué)Lee T E對雙旋翼涵道飛行器的懸停和前進(jìn)飛行性能進(jìn)行了研究[4]；沈陽理工大學(xué)暴慶攀對共軸雙旋翼飛行器在懸停狀態(tài)下的姿態(tài)控制進(jìn)行了研究[5];北京理工大學(xué)肖大華設(shè)計(jì)了一種微小型球形飛行器[6]；陳保國等人針對共軸雙旋翼飛行器設(shè)計(jì)了模糊比例—積分—微分(proportional integral differential,PID)姿態(tài)控制器[7]。

本文針對共軸雙旋翼無人機(jī)的研制設(shè)計(jì)了基于STM32微處理器和姿態(tài)傳感器的姿態(tài)控制系統(tǒng)，并針對經(jīng)典PID控制算法對飛行器控制效果不理想的問題，提出了一種帶積分自適應(yīng)因子的串級PID控制算法，提高了PID 控制的魯棒性和自適應(yīng)性，將其應(yīng)用于共軸雙旋翼飛行器具有很好的控制效果。

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

姿態(tài)控制系統(tǒng)硬件由JY901傳感器模塊、主控板卡、遙控通信模塊、電源模塊、電機(jī)舵機(jī)執(zhí)行機(jī)構(gòu)五部分組成?？刂葡到y(tǒng)選用STM32F405為主控芯片，通過Futaba遙控器和MFDLink 433 MHz增程模塊發(fā)出遙控指令，通過nRF521822無線模塊實(shí)現(xiàn)與地面站的數(shù)據(jù)回傳，獲得飛行信息，所搭載的JY901傳感器模塊集成了高精度的陀螺儀、加速度計(jì)和磁力計(jì)，采用高性能的微處理器和先進(jìn)的動(dòng)力學(xué)解算與卡爾曼動(dòng)態(tài)濾波算法，能夠快速求解出載體當(dāng)前的運(yùn)動(dòng)姿態(tài)，穩(wěn)定性很高[8]。

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

軟件采用C語言在MDK環(huán)境下使用FreeRTOS實(shí)時(shí)操作系統(tǒng)編寫主程序和子程序。主要程序包括系統(tǒng)初始化、任務(wù)創(chuàng)建、任務(wù)調(diào)度、遙控信號捕獲和處理、傳感器數(shù)據(jù)處理、飛行控制等。系統(tǒng)上電初始化之后創(chuàng)建任務(wù),然后實(shí)時(shí)操作系統(tǒng)根據(jù)所設(shè)定各個(gè)子任務(wù)的優(yōu)先級不同進(jìn)行任務(wù)調(diào)度，實(shí)現(xiàn)多任務(wù)的實(shí)時(shí)切換，保證程序的正常運(yùn)行。

2.1 遙控信號濾波算法

在對遙控信號的解析過程中發(fā)現(xiàn)遙控信號會受到外界電磁環(huán)境干擾或因?yàn)橥ㄐ挪粫吃斐墒д妫涮攸c(diǎn)是幅度大但并不頻發(fā)，主要體現(xiàn)在兩次相鄰的采樣之間偶爾會有大幅的數(shù)據(jù)變化。為了去除這種突變影響，采用限幅濾波算法進(jìn)行處理，其原理是先設(shè)定一個(gè)閾值，然后對每次的輸入值進(jìn)行一次保留，當(dāng)新數(shù)據(jù)輸入時(shí)，先計(jì)算與保存值的誤差，若誤差值(取絕對值)大于閾值，則不保存此次輸入的數(shù)據(jù)，相反，則將此次輸入的數(shù)據(jù)保存。

限幅濾波的優(yōu)點(diǎn)是運(yùn)算量小，需要注意的是,當(dāng)實(shí)際操控輸入值的采樣值差大于設(shè)定閾值時(shí)，輸入量會保持不變，所以限幅閾值也不易過低，以免影響正常操作。圖1為設(shè)置閾值為 200 時(shí)的效果對比，通過限幅濾波處理能在不影響原輸入信號情況下濾除突變干擾。

圖1 限幅濾波效果對比

2.2 傳感器數(shù)據(jù)讀取算法

JY901姿態(tài)傳感器的數(shù)據(jù)采用16 進(jìn)制的方式進(jìn)行傳輸，其通訊協(xié)議規(guī)定每個(gè)數(shù)據(jù)包以0x55開頭,SUM結(jié)尾,共11個(gè)字節(jié)，格式如表1所示。

表1 數(shù)據(jù)包格式

數(shù)據(jù)包為基本傳輸單位，將信息以數(shù)據(jù)包的形式進(jìn)行發(fā)送，以此保證數(shù)據(jù)傳輸?shù)母咝院驼_性。頭報(bào)文和校驗(yàn)和作為數(shù)據(jù)包的起止位用于判斷一個(gè)數(shù)據(jù)包是否接收完畢。數(shù)據(jù)包號用來判斷所傳輸數(shù)據(jù)的類型，極大程度地提高了信息的辨識度，方便用于對不同類型的數(shù)據(jù)進(jìn)行讀取，更有針對性地進(jìn)行處理。

數(shù)據(jù)內(nèi)容中每個(gè)數(shù)據(jù)分低字節(jié)和高字節(jié)依次傳送，二者組合成一個(gè)有符號的 short 類型的數(shù)據(jù)。假設(shè) Data 為實(shí)際的數(shù)據(jù)，DataH 為其高字節(jié)部分，DataL 為其低字節(jié)部分，那么Data=(short)(DataH?8|DataL)，2個(gè)字節(jié)的16進(jìn)制數(shù)的表示范圍在-32 767～32 768之間，根據(jù)不同類型數(shù)據(jù)的計(jì)算方法將數(shù)據(jù)映射到對應(yīng)的量程范圍就可以得到使用的數(shù)據(jù)。角度和角速度的計(jì)算方法如下：

1)角速度

Wx=((wxH?8)|wxH)/32 768×2 000((°)/s)

Wy=((wyH?8)|wyL)/32 768×2 000((°)/s)

Wz=((wzH?8)|wzL)/32 768×2 000((°)/s)

(1)

2)角度

Roll=((RollH?8)|RollL)/32 768×180(°)

Pitch=((PitchH?8)|PitchL)/32 768×180(°)

Yaw=((YawH?8)|YawL)/32 768×180(°)

(2)

3 帶積分自適應(yīng)因子的串級PID控制器

PID控制算法原理簡單，易于嵌入式系統(tǒng)實(shí)現(xiàn)，所以,目前大部分飛行器的姿態(tài)控制算法都是基于PID控制器設(shè)計(jì)的。由于共軸雙旋翼飛行器在懸停狀態(tài)下是不穩(wěn)定的，而且在飛行過程中，傳感器遇到磁場干擾會影響傳感器的數(shù)據(jù)采集，解算出的姿態(tài)角有誤差，只用經(jīng)典的單級PID 控制器很難使飛行器穩(wěn)定運(yùn)行，所以，本文提出了帶積分自適應(yīng)因子的串級PID控制器。

3.1 控制器設(shè)計(jì)

與經(jīng)典PID控制相比，帶積分自適應(yīng)因子的串級PID控制器(IAP-PID)的外環(huán)為角度環(huán)，內(nèi)環(huán)為角速度，雙環(huán)控制器加大了整個(gè)系統(tǒng)的阻尼，穩(wěn)定性更高，同時(shí)加入積分自適應(yīng)因子解決了PID中積分項(xiàng)容易引起的超調(diào)和過飽和現(xiàn)象，提高系統(tǒng)的自適應(yīng)能力。

控制器外環(huán)為角度環(huán)，以遙控信號rθ(k)作為期望值，機(jī)體搭載的JY901傳感器解算出的姿態(tài)角cθ(k)為反饋值，計(jì)算兩者偏差為

eθ(k)=rθ(k)-cθ(k)

(3)

其控制規(guī)律為

uθ(k)=kpθeθ(k)+kiθeiθ(k)+kdθedθ(k)

(4)

(5)

edθ(k)=eθ(k)-eθ(k-1)

(6)

式中kpθ為外環(huán)比例系數(shù)，kiθ為外環(huán)積分系數(shù)，kdθ為外環(huán)微分系數(shù)。

對于內(nèi)環(huán)PID來說，期望角速度為外環(huán)PID經(jīng)過處理后的輸出值uθ(k)，反饋角速度為傳感器模塊測得的當(dāng)前角速度cω(k)，兩者的偏差為

eω(k)=rω(k)-cω(k)

(7)

其控制規(guī)律為

uω(k)=kpωeω(k)+kiωeiω(k)+kdωedω(k)

(8)

(9)

edω(k)=eω(k)-eω(k-1)

(10)

式中kpω為內(nèi)環(huán)比例系數(shù)，kiω為內(nèi)環(huán)積分系數(shù)，kdω為內(nèi)環(huán)微分系數(shù)。

PID控制器中積分項(xiàng)的引入減小了系統(tǒng)的靜態(tài)誤差，但是在系統(tǒng)遇到較大波動(dòng)時(shí)，控制器由于調(diào)整誤差過大容易使系統(tǒng)產(chǎn)生很大的超調(diào)或者陷入過飽和狀態(tài)，影響系統(tǒng)穩(wěn)定性。針對這一問題提出以下解決方案：

1)防止積分過飽和。在計(jì)算系統(tǒng)輸出之前先對上一次輸出量進(jìn)行判斷，當(dāng)大于系統(tǒng)設(shè)定的最大值時(shí)，只對負(fù)誤差進(jìn)行累加；當(dāng)小于系統(tǒng)規(guī)定的最小值時(shí)，只對正誤差進(jìn)行累加，避免控制器在飽和狀態(tài)中的所處時(shí)間過長。此方案在程序編寫中實(shí)現(xiàn)。

2)改變積分項(xiàng)的累加速度。其基本思想是在積分系數(shù)之前乘以一個(gè)自適應(yīng)因子，當(dāng)誤差較大時(shí)，因子減小甚至為零；當(dāng)誤差較小時(shí)，因子適當(dāng)增大。積分自適應(yīng)因子的表達(dá)式如下所示

(11)

式中 Δωmax，Δωmin分別為期望值與反饋值的誤差的最大值和最小值，|e(k)|為誤差的絕對值。Δωmax，Δωmin的取值與系統(tǒng)有關(guān)，需要在調(diào)試的過程中確定。該式表示當(dāng)期望值與反饋值的誤差小于Δωmin時(shí)，積分項(xiàng)發(fā)揮正常作用；當(dāng)期望值與反饋值的誤差在Δωmin和Δωmax之間時(shí)，積分項(xiàng)根據(jù)誤差大小進(jìn)行自適應(yīng)變化；當(dāng)期望值與反饋值的誤差大于Δωmax時(shí)，積分項(xiàng)不發(fā)揮作用，變?yōu)镻D控制。

串級PID和積分自適應(yīng)相結(jié)合組成帶積分自適應(yīng)因子的P-PID控制器(IAP-PID)原理框圖如圖2所示。

圖2 IAP-PID控制器原理

3.2 系統(tǒng)仿真

為了驗(yàn)證IAP-PID控制算法的有效性，將共軸雙旋翼飛行器的模型導(dǎo)入到SIMULINK中作為模塊與搭建的控制器模型連接進(jìn)行聯(lián)合仿真。對目標(biāo)姿態(tài)角施加階躍激勵(lì)，然后,將IAP-PID控制算法與PID控制算法的響應(yīng)曲線進(jìn)行對比，仿真結(jié)果如圖3所示。

圖3 各姿態(tài)角階躍響應(yīng)對比

從圖3中的輸出響應(yīng)曲線來看，相比于經(jīng)典的PID控制算法，IAP-PID控制算法的系統(tǒng)穩(wěn)定時(shí)間更短，同時(shí)具有更小的超調(diào)量，具體的仿真結(jié)果對比如表2所示。

表2 階躍響應(yīng)仿真結(jié)果對比

由此可見， IAP-PID控制器對四旋翼飛行器姿態(tài)控制的穩(wěn)定性有明顯的提升，更能適應(yīng)共軸雙旋翼飛行器復(fù)雜飛行環(huán)境的要求。

4 飛行測試

將IAP-PID控制算法燒錄到硬件系統(tǒng)中，進(jìn)行大量的現(xiàn)場飛行試驗(yàn)和參數(shù)整定，最終得到的較為理想的PID控制參數(shù)如表3所示，此時(shí)飛行器具有穩(wěn)定的飛行效果。

表3 共軸雙旋翼無人機(jī)飛行PID參數(shù)

通過調(diào)用飛行日志，將飛行中的姿態(tài)角數(shù)據(jù)導(dǎo)入到MATLAB中描繪出飛行器在飛行過程中的姿態(tài)角變化，各姿態(tài)角的變化情況如圖4所示。

圖4 各姿態(tài)角飛行數(shù)據(jù)

從圖4中各姿態(tài)角的變化情況可以看出：在飛行過程中，姿態(tài)角存在小幅度的振蕩，這是因?yàn)榛赑ID的控制算法在計(jì)算姿態(tài)角時(shí)的動(dòng)態(tài)誤差較大；但是在IAP-PID 控制器的控制下，飛行器的姿態(tài)角變化幅度基本控制在1.5°以內(nèi)，在發(fā)生較大波動(dòng)時(shí)也能迅速調(diào)整回到0°左右，整體飛行過程比較平穩(wěn)?？梢宰C明IAP-PID控制器對共軸雙旋翼飛行器具有良好的控制效果。

5 結(jié) 論

本文針對共軸雙旋翼飛行器的研制，設(shè)計(jì)了基于STM32微處理器和姿態(tài)傳感器的姿態(tài)控制系統(tǒng)，同時(shí)提出了帶積分自適應(yīng)因子的串級PID控制算法(IAP-PID)。結(jié)果表明：本姿態(tài)控制系統(tǒng)可應(yīng)用于共軸雙旋翼飛行器，相比于傳統(tǒng) PID 控制算法，IAP-PID控制算法具更快的響應(yīng)時(shí)間和系統(tǒng)收斂速度，提高了系統(tǒng)的穩(wěn)定性和自適應(yīng)能力，能很好地控制飛行器的飛行姿態(tài)，保證飛行器穩(wěn)定飛行。