代懷宇++段崇陽++孫瑞達

摘 要旋翼式飛行器是近些年來無人機發(fā)展的重要方向，在多個領(lǐng)域發(fā)揮著重要的作用。本文介紹一種四旋翼飛行器定點懸停的控制策略，氣壓計與電子航向系統(tǒng)（AHRS）為獲取關(guān)鍵參數(shù)的傳感器，應(yīng)用低通濾波與卡爾曼濾波。在保證控制精度的同時，筆者盡量使算法簡單，不過高的依賴微處理器的運算速度。之后探討了數(shù)據(jù)處理方面存在的問題，希望可以對懸?？刂频难芯控暙I微薄之力。

【關(guān)鍵詞】四旋翼 定點懸停 氣壓 AHRS 低通濾波 卡爾曼濾波

四旋翼飛行器是一種結(jié)構(gòu)簡單的多旋翼形式的飛行器，其本身具有多重優(yōu)勢特征，主要表現(xiàn)為機體結(jié)構(gòu)簡單、造價低廉、陀螺效應(yīng)小等多種優(yōu)勢。最重要的是，四旋翼飛行器可以做到垂直起降，使得起飛與降落的成本大大減小。因此，這種飛行器廣泛的應(yīng)用與空中拍攝、監(jiān)視、偵查等方面。然而，四旋翼飛行器本身也存在一定的缺陷，飛行器本身在飛行的過程中容易受到噪聲甚至是氣流等多方面干擾，使控制變得困難。

眾所周知，為了使四旋翼飛行器相比與固定翼飛行器發(fā)揮更大的優(yōu)勢，懸?？刂剖欠浅１匾?，而現(xiàn)階段大多數(shù)的懸?？刂品桨富诿裼萌蚨ㄎ幌到y(tǒng)（Global Position System）或計算機視覺控制。但GPS對環(huán)境要求較大，在某些GPS信號不覆蓋的地方不能使用，而計算機視覺控制對微處理器的功能要求比較高。但氣壓計的使用相對簡單，只需要利用簡單地機械結(jié)構(gòu)設(shè)計避免氣流造成的大噪聲的數(shù)據(jù)，非常適合用于懸?？刂?/p>

本文將注重對傳感器數(shù)據(jù)的處理，從理論推導(dǎo)出發(fā)，并聯(lián)系實際應(yīng)用，將低通濾波與卡爾曼濾波相結(jié)合，并通過實體飛行器測試算法，提出并證明一種比較穩(wěn)定的四旋翼懸?？刂品桨?。

1 四旋翼飛行器懸停的意義與工作原理

四旋翼飛行器的懸停控制一直是國內(nèi)外學(xué)者關(guān)注的一個重點課題。懸停，即四旋翼飛行器在某一時刻相對于地面保持一種靜止的空間狀態(tài)。

為了方便敘述，定義機體坐標(biāo)系OXYZ：以飛行器重心為原點，縱軸為X軸，方向同機頭方向；在飛行器對稱面內(nèi)且垂直于X軸為Z軸，方向向下；與X，Z軸垂直且向右為正方向為Y軸。根據(jù)右手定則由飛機重心為原點定義地面慣性坐標(biāo)系OgXgYgZg。OX軸相對于OgXgYg平面的變化，即俯仰角（pitch）；OX軸在OgXgYg平面的投影與OgXg之間的夾角，即偏航角（yaw）；以及飛行器對稱面繞X軸轉(zhuǎn)動角度，即橫滾角（roll）。

由于四軸飛行器在空間中有6個自由度，即在地面慣性坐標(biāo)系中，沿三個坐標(biāo)軸做平移和旋轉(zhuǎn)運動。通過對四軸飛行器的運動分析，可以得出飛機的姿態(tài)變化可以分解成橫滾角，俯仰角，航向角的變化，而OgZg軸的平移運動可以看做高度的變化（altitude），如圖1所示。

可以看出四旋翼飛行器的運行方式主要包括四個方面的內(nèi)容，有垂直起降和懸停（即高度的變化）、偏航運行、俯仰運動以及滾轉(zhuǎn)運動四個，其中，基本的運動方式就是懸停運動以及垂直起降運動，但飛行器在空中飛行時會受到氣流、氣壓等多種噪聲干擾，飛行器的運動狀態(tài)會發(fā)生改變。所以控制控制四旋翼飛行器的懸停，即控制橫滾角，俯仰角，與航向角，以及高度。

2 四旋翼飛行器控制系統(tǒng)

經(jīng)過反復(fù)多種方案的反復(fù)比較分析，本文決定采用電子航向系統(tǒng)，高精度氣壓計作為數(shù)據(jù)的采集，通過I2C+SPI方式向主控芯片傳輸數(shù)據(jù)，并對氣壓計做隔離處理，降低氣流對氣壓計的干擾。為了方便采集飛行時的實時數(shù)據(jù)，利用串口方式+藍牙方式，使飛行器與上位機進行通訊。

2.1 控制系統(tǒng)的模塊化設(shè)計

通過AHRS（attitude heading reference system）可以獲得實時的歐拉角，通過氣壓計可以獲得實時高度數(shù)據(jù)，運用適當(dāng)?shù)奈⑻幚砥鳎∕icrocontroller Unit）快速的控制，使得四旋翼飛行器可以快速的對誤差進行反應(yīng)。因此，本文描述的四旋翼飛行器呈現(xiàn)如下結(jié)構(gòu)。

2.2 控制系統(tǒng)懸停的目標(biāo)介紹

由于篇幅有限，本文只研究在手動飛行中的懸停過程。當(dāng)飛行器在飛行時，當(dāng)橫滾角（pitch），俯仰角（roll）比較穩(wěn)定時，對飛行器發(fā)射懸停信號。此時飛行器將進行自穩(wěn)控制，并記錄飛行器穩(wěn)定后的姿態(tài)、航向、高度數(shù)據(jù)。在可以修正的范圍內(nèi)進行修正。由于文獻（1）將已經(jīng)將姿態(tài)的估計與控制做了詳細的描述，使飛行器姿態(tài)很好的保持穩(wěn)定，本文將不再描述姿態(tài)的控制，而將重點放在高度數(shù)據(jù)的處理與高度控制上，如圖2所示。

3 基于AHRS與氣壓計的懸?？刂葡到y(tǒng)的算法設(shè)計與實現(xiàn)

3.1 獲得理想的控制量

3.1.1 低通濾波

低通濾波分為很多種，對于從模擬信號發(fā)展而來的最簡單的低通濾波，分為一階阻容濾波，和二階阻容濾波，阻容濾波在頻域上有阻擋高頻信號的特點，在相位上存在滯后，阻容濾波也成為滯后濾波。而二階低通濾波較一階低通濾波

理想一階低通濾波器的傳遞函數(shù)為

Z變換：

式中T為采樣周期

Z反變換得到差分方程

稱α為濾波系數(shù)，1>α>0

Y（n）=αX（n）+（1-α）Y（n-1）

其中Y（n）為本次計算結(jié)果，X（n）為得到的新數(shù)據(jù)， Y（n-1）為前一次計算的結(jié)果。濾波參數(shù)與截止頻率和采樣時間有關(guān)。

經(jīng)過分析，濾波系數(shù)

ω為截止頻率。

3.1.2 卡爾曼濾波

卡爾曼濾波系統(tǒng)中的信號和噪聲狀態(tài)方程如下：

其中，為系統(tǒng)狀態(tài)轉(zhuǎn)換矩陣，Qk-1為系統(tǒng)噪聲方差陣。Hk為觀測矩陣，Rk為觀測噪聲方差陣。移動物體的狀態(tài)方程和輸出方程，見式

式中：Q表示系統(tǒng)動態(tài)狀態(tài)矩陣；R為噪聲到系統(tǒng)狀態(tài)的映射矩陣；O為系統(tǒng)輸出矩陣；ξ為系統(tǒng)噪聲；η為測量噪聲。

基于狀態(tài)觀測方程，可以獲得卡爾曼濾波器五大遞推公式。其中，為校正前預(yù)測值；為校正后估計值；為校正前的預(yù)測均方差；為校正后的誤差均方值，Kk為卡爾曼增益矩陣。

卡爾曼濾波的主要思想是“預(yù)測”與“修正”。在本飛行控制系統(tǒng)中，對于姿態(tài)結(jié)算和高度數(shù)據(jù)獲取，卡爾曼濾波都是最重要的算法，（在文獻中，）已經(jīng)介紹了卡爾曼濾波在姿態(tài)解算中發(fā)揮的作用，本文將詳細討論卡爾曼濾波在高度數(shù)據(jù)獲取中的應(yīng)用。

3.1.3 高度數(shù)據(jù)的獲得

本系統(tǒng)的加速度計與氣壓計的組合懸停系統(tǒng)的狀態(tài)變量為

式中Px代表四旋翼飛行器的高度，Vx代表Z周速度。

加速度計長期數(shù)據(jù)可靠度較大，而實時采樣數(shù)據(jù)受到機體震動等外界因素的干擾，導(dǎo)致毛刺較多。從頻域上講，加速度計的低頻信號可用而高頻信號大多為噪聲。Z軸加速度的二次積分為Z軸移動的距離，而加速度計若存在誤差，則誤差也會被二次積分，但若高度發(fā)生變化，加速度計的反映非常靈敏。

氣壓計的讀數(shù)受環(huán)境影響較大，漂移較為嚴(yán)重，但短期內(nèi)較為可信。因此，先對加速度計做低通濾波，經(jīng)測試研究，經(jīng)一階低通濾波處理過的數(shù)據(jù)在實時性與可靠性等方面都滿足四軸飛行器控制要求，在將得到的數(shù)據(jù)與氣壓計數(shù)據(jù)通過卡爾曼濾波器，用處理過的加速度計的數(shù)據(jù)去處理氣壓計的數(shù)據(jù)。最后輸出的高度數(shù)據(jù)我們認(rèn)為是理想的。

3.2 高度控制算法

對四旋翼飛行器的物理模型進行分析后，可以知道Z軸的速度可以更好的體現(xiàn)高度是否穩(wěn)定，因此，若能夠直接對Z軸速度進行很好的閉環(huán)控制，必然會改善懸停的動態(tài)特性及穩(wěn)定性。Z軸速度可以由Z軸高度的變化對時間做微分得到，本文將Z軸速度作為整個系統(tǒng)的增穩(wěn)環(huán)節(jié)，而僅僅對Z軸速度進行控制則會使四旋翼控制的實時性變差，因此將Z軸速度作為內(nèi)環(huán)控制，作為整個系統(tǒng)的增穩(wěn)環(huán)節(jié)。而高度作為外環(huán)，其作用體現(xiàn)在對四旋翼飛行器高度的精確控制。

采用位置式數(shù)字PID控制：

上式中，u（t）為PID輸出值，e（t）為期望值與實際值之差，Kp、Ki、Kd為比例、積分、微分系數(shù)，對應(yīng)項為比例、積分、微分項，再將積分項、微分項離散化得到PID公式。

高度偏差量為：

Error=Current-Previous

其中，Current為當(dāng)前高度，Previous期望角度。

Z軸速度為

則本系統(tǒng)控制算法可表示為

4 結(jié)束語

經(jīng)過在實際四旋翼飛行器上的測試，在實際飛行中，使用AHRS與氣壓計的四旋翼飛行器，通過本文所述的方法，可以實現(xiàn)基本的懸?？刂?，如圖4所示。

以下為試驗用四軸飛行器的各項參數(shù)。

機身尺寸：50cm*50cm*20cm

整機質(zhì)量：1500g

最大起飛重量：2kg

最大任務(wù)載荷：300g

電機：4208 直流無刷電機 kv980

槳：1047塑料正反槳

機架：DJIF450

腳架：塑料

電池： 5200mAh 鋰電池

控制器：stm32

從實際測試數(shù)據(jù)可知，利用本文所描述的算法，可以的到較理想的飛行器高度數(shù)據(jù)并加以控制，觀測到的結(jié)果與實驗數(shù)據(jù)吻合。

參考文獻

[1]汪紹華，楊瑩.基于卡爾曼濾波的四旋翼飛行器姿態(tài)估計和控制算法研究[J].控制理論與應(yīng)用，2013（10）：12.

作者單位

哈爾濱理工大學(xué) 黑龍江省哈爾濱市 150000