四旋翼飛行器的懸停控制研究

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

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

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

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

眾所周知，為了使四旋翼飛行器相比與固定翼飛行器發揮更大的優勢，懸停控制是非常必要的，而現階段大多數的懸停控制方案基于民用全球定位系統（Global Position System）或計算機視覺控制。但GPS對環境要求較大，在某些GPS信號不覆蓋的地方不能使用，而計算機視覺控制對微處理器的功能要求比較高。但氣壓計的使用相對簡單，只需要利用簡單地機械結構設計避免氣流造成的大噪聲的數據，非常適合用于懸停控制

本文將注重對傳感器數據的處理，從理論推導出發，并聯系實際應用，將低通濾波與卡爾曼濾波相結合，并通過實體飛行器測試算法，提出并證明一種比較穩定的四旋翼懸停控制方案。

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

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

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

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

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

2 四旋翼飛行器控制系統

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

2.1 控制系統的模塊化設計

通過AHRS（attitude heading reference system）可以獲得實時的歐拉角，通過氣壓計可以獲得實時高度數據，運用適當的微處理器（Microcontroller Unit）快速的控制，使得四旋翼飛行器可以快速的對誤差進行反應。因此，本文描述的四旋翼飛行器呈現如下結構。

2.2 控制系統懸停的目標介紹

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

3 基于AHRS與氣壓計的懸停控制系統的算法設計與實現

3.1 獲得理想的控制量

3.1.1 低通濾波

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

理想一階低通濾波器的傳遞函數為

Z變換：

式中T為采樣周期

Z反變換得到差分方程

稱α為濾波系數，1>α>0

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

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

經過分析，濾波系數

ω為截止頻率。

3.1.2 卡爾曼濾波

卡爾曼濾波系統中的信號和噪聲狀態方程如下：

其中，為系統狀態轉換矩陣，Qk-1為系統噪聲方差陣。Hk為觀測矩陣，Rk為觀測噪聲方差陣。移動物體的狀態方程和輸出方程，見式

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

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

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

3.1.3 高度數據的獲得

本系統的加速度計與氣壓計的組合懸停系統的狀態變量為

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

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

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

3.2 高度控制算法

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

采用位置式數字PID控制：

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

高度偏差量為：

Error=Current-Previous

其中，Current為當前高度，Previous期望角度。

Z軸速度為

則本系統控制算法可表示為

4 結束語

經過在實際四旋翼飛行器上的測試，在實際飛行中，使用AHRS與氣壓計的四旋翼飛行器，通過本文所述的方法，可以實現基本的懸停控制，如圖4所示。

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

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

整機質量：1500g

最大起飛重量：2kg

最大任務載荷：300g

電機：4208 直流無刷電機 kv980

槳：1047塑料正反槳

機架：DJIF450

腳架：塑料

電池： 5200mAh 鋰電池

控制器：stm32

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

參考文獻

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

作者單位

哈爾濱理工大學 黑龍江省哈爾濱市 150000