龍 文,陳 洋*,吳懷宇,程 磊,陳鵬震,王正熙

(1.武漢科技大學信息科學與工程學院,武漢 430081;2.武漢科技大學教育部冶金自動化與檢測技術(shù)工程研究中心,武漢 430081)

?

變載荷下六旋翼無人機高度控制*

龍 文1,2,陳 洋1,2*,吳懷宇1,2,程 磊1,2,陳鵬震1,2,王正熙1,2

(1.武漢科技大學信息科學與工程學院,武漢 430081;2.武漢科技大學教育部冶金自動化與檢測技術(shù)工程研究中心,武漢 430081)

六旋翼無人機應用于植保領(lǐng)域時通常需要定高飛行,單一機載微傳感器無法長時間準確的跟蹤六旋翼無人機的高度,且當負載發(fā)生變化時無人機的高度控制會出現(xiàn)不穩(wěn)定現(xiàn)象。針對上述問題,本文采用了一種基于氣壓計和加速度計卡爾曼濾波融合的高度測量系統(tǒng),并設計了一個雙閉環(huán)PID控制加前饋控制的高度控制器,最終獲得了無人機飛行高度的精確估計與穩(wěn)定控制。實驗結(jié)果表明,經(jīng)過卡爾曼濾波可以得到準確、平穩(wěn)的高度值和速度值。提出的高度控制器在變載荷情況下也能很好的控制無人機的高度。

六旋翼無人機;變載荷;前饋控制;卡爾曼濾波

近年來,多旋翼無人機逐漸成為航空領(lǐng)域?qū)W者們的研究熱點,多旋翼無人機是一種具有垂直起降與空中懸停等特殊飛行能力的多旋翼無人飛行器。在飛行過程中,通過改變旋翼的轉(zhuǎn)速,可以改變各種飛行姿態(tài)。正因為多旋翼無人機具有垂直起降、空中定點懸停、可以實現(xiàn)六自由度的飛行等獨特的飛行能力,使其在軍事領(lǐng)域和民用領(lǐng)域得到了廣泛的應用[1],包括軍事偵察,農(nóng)業(yè)植保,電力巡檢,測繪,航拍等。

無人機在執(zhí)行某些特定的任務過程中質(zhì)量有可能會發(fā)生變化,如農(nóng)業(yè)植保無人機在工作過程中隨著農(nóng)藥的噴灑,質(zhì)量會不斷的減小,但是為了更好的保證農(nóng)藥的噴灑質(zhì)量,不發(fā)生漏噴重噴現(xiàn)象,必須保證無人機能與作物保持固定的高度。如果高度控制系統(tǒng)不能感知自身質(zhì)量的變化便會導致高度控制不穩(wěn)定,響應偏慢等現(xiàn)象。由此可見針對此類六旋翼無人機,設計控制系統(tǒng)過程中考慮到無人機的質(zhì)量變化顯得尤為重要。旋翼無人機的高度控制系統(tǒng)設計主要包括兩個方面,分別是高度測量系統(tǒng)設計和高度控制器設計。高度估計算法方面目前公開的算法主要有互補濾波[2],卡爾曼濾波[3-5]。文獻[2]中采用加速度計與氣壓計進行互補濾波融合來估計高度,具有迭代簡單、適合工程實踐等優(yōu)點。文獻[3]中采用卡爾曼濾波的方法,利用加速度計和氣壓計,超聲波3種傳感器來進行高度的估計,相比互補濾波,卡爾曼濾波具有收斂速度快,精度高等優(yōu)點。使用最普遍的高度測量傳感器是GPS測量系統(tǒng),目前有許多關(guān)于GPS數(shù)據(jù)定高的融合算法,比如融合GPS/INS數(shù)據(jù)實現(xiàn)無人機的精確導航[6]、組合氣壓高速傳感器、全球定位系統(tǒng)(GPS)和捷聯(lián)式線加速度計,建立一種具有較高精度的無人直升機高度測量系統(tǒng)[7]、根據(jù)飛行器運動方程,推導出了高度和飛行狀態(tài)之間的關(guān)系,并由所得的飛行狀態(tài),利用 Kalman 濾波方法得到高度[8]。這些方法測高效果很好,但是GPS的使用條件苛刻,通常室內(nèi)或者有高大建筑物遮擋時無法使用。氣壓計可以克服這一困難,但是單純的氣壓器測量的高度存在很大的噪聲,而且測量出來的高度很容易受到環(huán)境的影響。另外在高度和垂直速度的測量中,氣壓式高度表有較大的慣性,因而瞬時高度和垂直速度的精度會受到影響。而慣性系統(tǒng)測量高度的誤差是以指數(shù)形式增長的,因此,用氣壓式高度表的信息對慣性高度系統(tǒng)進行阻尼,可得到品質(zhì)較好又不隨時間發(fā)散的組合高度系統(tǒng)[9]。利用氣壓計與加速度計卡爾曼濾波得到高度信息,然后采用現(xiàn)代控制理論中的LQI控制對無人機的高度進行控制。這樣做在無風和有風的情況下都能獲得較理想的定高效果,但是在無人機質(zhì)量發(fā)生變化以及電池消耗較大的情況時,高度控制效果大大降低[10]。

綜上所述,基于組合高度測量系統(tǒng)的思想和針對質(zhì)量變化的六旋翼無人機高度控制響應速度偏慢,控制效果偏差的問題,本文采用一種利用加速度計和氣壓計進行卡爾曼濾波融合得到垂直方向高度值,速度值和加速度靜態(tài)偏差的高度測量系統(tǒng)以及雙閉環(huán)PID控制加前饋控制的高度控制器。實驗結(jié)果表明數(shù)據(jù)融合算法能輸出準確的高度和速度信息。在雙閉環(huán)PID控制器的基礎(chǔ)上加上前饋控制,利用卡爾曼濾波器估計機體質(zhì)量,采用牛頓第二定律計算一個自適應升力作為前饋控制量,能加快控制器響應速度,當無人機負載發(fā)生變化時也能達到很好的控制效果。

1 高度測量系統(tǒng)設計

定義導航坐標系(n系)和機體坐標系(b系),如圖1所示。

圖1 導航坐標系與機體坐標系的關(guān)系

圖1中,導航坐標系采用東北天(ENU)右手坐標系,其坐標原點O位于運載體的質(zhì)心;機體坐標系是固連于無人機機體的參考坐標系[11]。

加速度計可以測得無人機在機體坐標系的三軸加速度,但是高度測量是相對導航坐標系,所以需要利用方向余弦矩陣將機體坐標系下的加速度轉(zhuǎn)換到導航坐標系中,得到相對大地的垂直方向加速度。計算公式如式(1):

(1)

(2)

注:式中cθ,cφ,cγ,sθ,sφ,sγ分別表示cosθ,cosφ,cosγ,sinθ,sinφ,sinγ。

anz表示導航坐標系中機體垂直方向加速度;那么機體相對大地的加速度為a=anz-g。

用z(k)表示在k時刻氣壓計的觀測高度,則有觀測模型:

z(k)=Hx(k)+μ(k)

(3)

定義系統(tǒng)的狀態(tài)變量為x(k)=[h(k)v(k)bias(k)]T,分別表示無人機高度,垂直方向速度,以及加速度計Z軸上的偏差。由式(3)可得,觀測矩陣H=[1 0 0],又由勻加速運動方程可得系統(tǒng)的狀態(tài)方程為:

(4)

即系統(tǒng)的狀態(tài)空間模型為:

x(k+1)=Ax(k)+Bu(k)+w(k)

(5)

(6)

式中:P(k)表示k時刻的協(xié)方差,K(k)為系統(tǒng)k時刻的卡爾曼增益。

至此,通過時間更新過程和觀測更新過程進行不斷的預測-修正來遞推出一個最優(yōu)的高度值和速度值,每次更新得到的加速度計偏差實時修正加速度Z軸上的偏差。高度卡爾曼濾波原理框圖[12]如圖2所示。

圖2 卡爾曼濾波原理框圖

2 高度控制器設計

高度控制器由雙閉環(huán)串級PID反饋控制加前饋控制組成,雙閉環(huán)串級PID有兩個環(huán),外環(huán)為高度環(huán),內(nèi)環(huán)為垂直方向速度環(huán),前饋控制通過卡爾曼濾波器估算當前機體質(zhì)量,然后利用牛頓第二定律計算出一個自適應升力直接補償給控制器輸出,這樣做可以提高控制器的響應速度,同時能適應無人機負載的變化。控制系統(tǒng)結(jié)構(gòu)如圖3所示。

圖3 高度控制器結(jié)構(gòu)圖

2.1 反饋控制

反饋控制的外環(huán)表達式,如式(7)所示:

(7)

內(nèi)環(huán)的表達式,如式(8)所示:

(8)

2.2 前饋控制

前饋控制部分由卡爾曼濾波器估計出機體的質(zhì)量,然后利用牛頓第二定律計算出一個自適應的升力?？柭鼮V波器以無人機質(zhì)量的倒數(shù)為狀態(tài)變量,由牛頓第二定律F=ma可得1/m=a/F,故以a/F作為觀測變量,F為油門量即無人機升力,建立狀態(tài)空間模型如式(9)、式(10)所示。

x(k+1)=Ax(k)+w(k)

(9)

z(k)=x(k)+v(k)

(10)

由于狀態(tài)變量為一維變量,且認為A為1所以狀態(tài)估計方程認為無人機質(zhì)量一直保持不變,測量方程通過無人機在導航坐標系中的加速度除以油門量得到一個觀測的質(zhì)量倒數(shù),去修正預測方程預測的質(zhì)量倒數(shù)。具體卡爾曼濾波遞推過程如圖2所示。通過卡爾曼濾波器可以實時的估計無人機的質(zhì)量,然后利用式(11)可得無人機的升力,即為前饋控制輸出的控制量。

(11)

綜上所示,當無人機在tT時刻切換到定高模式時,系統(tǒng)默認以該時刻估算出來的高度為期望高度hr,當無人機受到擾動,高度有所上升或下降時,就會與期望高度產(chǎn)生一個高度差,對高度差做比例運算,輸出一個期望的垂直方向上的速度vr,對期望速度進行限幅,本文限制幅度為8 m/s,期望的垂直方向上的速度與估計的當前時刻速度產(chǎn)生一個速度差,對速度差做PID運算,積分運算限幅,微分運算需要經(jīng)過截止頻率為5 Hz的一階慣性低通濾波器(LPF),輸出值為反饋控制的控制量,同時前饋控制通過卡爾曼濾波估計機體質(zhì)量通過牛頓第二定律計算出一個自適應升力作為前饋控制量來提高無人機的響應速度,保證無人機在負載發(fā)生變化時也能快速的穩(wěn)定無人機高度。

考慮到實際情況能在定高模式下推動油門控制無人機上升下降,我們給油門值設定了一個死區(qū),在定高模式下遙控器油門的給定滿足一個分段函數(shù),如式(12)所示:

(12)

即在油門通道輸出PWM脈寬計數(shù)為1 400～1 600范圍內(nèi),且為定高模式時以當前高度作為外環(huán)期望高度。當定高模式下油門值輸出PWM值不在1 400～1 600范圍內(nèi)時,直接由遙控器的給定作為反饋控制內(nèi)環(huán)的速度給定,跳過外環(huán)部分,限制最大爬升速度為8m/s。低于1 400時,限制最大下降速度為4m/s。做這樣的處理是為了在定高模式下死區(qū)內(nèi)為高度控制將無人機穩(wěn)定在一個高度。需要改變高度時,當油門量超過死區(qū),油門量就會作為內(nèi)環(huán)給定速度,跳過外環(huán),讓無人機以一定速度爬升或者下降到期望的高度,油門回到死區(qū)繼續(xù)控制無人機穩(wěn)定在當前的高度。在許多開源飛控中沒有采用這種單環(huán)和雙環(huán)的切換,直接將遙控器輸出作為速度,然后積分出來高度,作為高度的給定,這樣做雖然也可以做到在定高模式中控制無人機高度,但是積分會導致延時產(chǎn)生,導致控制手感偏差。

3 實驗與結(jié)果

本文搭建了相應的實驗平臺,飛行控制單元主要由主控模塊、慣性測量及氣壓計模塊、GPS測量模塊、無線通訊模塊和電機驅(qū)動模塊構(gòu)成。其中,主控模塊用于協(xié)同整個系統(tǒng)的運行控制;慣性測量模塊用于測量飛行器三軸線速度和角速度,氣壓計與GPS模塊主要用于跟蹤三維空間位置信息;無線通訊模塊用于無人機與上位機軟件之間的通信;電機驅(qū)動模塊則利用PWM驅(qū)動直流電機。飛行控制單元的主控模塊采用Cortex-M3內(nèi)核的32 位單片機STM32F103。該芯片提供I2C接口、SPI接口和PWM輸出接口分別用于連接慣性測量模塊、無線通訊模塊和電機驅(qū)動組模塊。本文實驗在六旋翼植保無人機平臺中進行了實驗驗證,該實驗的實物效果圖,如圖4所示。

圖4 六旋翼植保無人機實物效果圖

圖5 靜態(tài)與動態(tài)高度測試圖

3.1 高度測量系統(tǒng)實驗

首先進行卡爾曼濾波狀態(tài)估計的測試實驗,將六旋翼無人機平放在地面上取一組數(shù)據(jù)測試靜態(tài)效果,然后將無人機起飛到一定高度下降,通過上位機分別讀取氣壓計和數(shù)據(jù)融合算法得出的高度值和垂直方向速度,從而測試卡爾曼濾波估計高度值和速度值得靜態(tài)跟蹤性能和動態(tài)跟蹤性能,如圖5、圖6所示。

圖6 靜態(tài)與動態(tài)速度測試圖

通過多次測試可以得出,單一的氣壓計測得的高度值噪聲方差約為0.04,而經(jīng)過本文數(shù)據(jù)融合算法得到的高度值噪聲的方差約為0.000 4。由圖5和圖6可以明顯看出卡爾曼濾波算法能有效融合加速度計與氣壓計的數(shù)據(jù)得到一個平穩(wěn)、準確的高度值和速度值,解決了單一加速度計計算高度容易產(chǎn)生累計誤差,單一氣壓計計算高度噪聲過大的問題,充分證明了本文采用的數(shù)據(jù)融合算法可靠性。

3.2 高度控制系統(tǒng)實驗

驗證完數(shù)據(jù)融合算法的可靠性,下面進行高度控制系統(tǒng)測試,首先驗證加入了前饋控制的高度控制器,安裝好六旋翼無人機后給水箱裝滿水起飛,當無人機飛到一定高度后切換到定高模式,開始通過水泵噴灑水箱里面的水并向前飛行,通過上位機觀察隨著機身質(zhì)量的減小,在定高狀態(tài)下高度的變化。然后相同的條件下測試只有雙閉環(huán)PID控制器時,定高模式下,隨著質(zhì)量的減小,高度的變化曲線,實驗結(jié)果如圖7所示曲線。

圖7 變載荷條件下定高測試圖

通過圖形可以看出,針對變載荷下多旋翼無人機,加入了前饋控制的高度控制器得到的高度保持的效果明顯比只有雙閉環(huán)PID的控制效果響應速度更快,定高效果更平穩(wěn)。通過卡爾曼濾波也能精準的跟蹤機體質(zhì)量的變化。經(jīng)過多組實驗結(jié)果證明,基于本文的高度控制算法,可以實現(xiàn)六旋翼植保無人機在質(zhì)量不斷變化的條件下保持高度方向上0.2m的控制精度。

4 結(jié)束語

六旋翼無人機進行高度控制的過程中,由于高度估計存在偏差以及負載質(zhì)量的變化,容易導致高度控制不穩(wěn)定的現(xiàn)象。針對上述問題,本文在狀態(tài)估計方面采用卡爾曼濾波估計無人機的高度值、垂直方向上的速度以及加速度Z軸上的偏差等參數(shù)。在高度控制器設計方面,引入卡爾曼濾波來實時估計無人機的質(zhì)量,進而根據(jù)牛頓第二定律計算自適應升力作為前饋控制量,疊加到雙閉環(huán)PID的反饋控制器中。最后,將所采用的方法應用于六旋翼植保無人機高度估計與高度控制中,經(jīng)過多次實際飛行測試結(jié)果表明:①利用卡爾曼濾波狀態(tài)估計方法,對加速度計與氣壓計數(shù)據(jù)進行融合,能有效的抑制氣壓計的隨機噪聲,使無人機能夠精準的跟蹤其高度和速度變化。實驗數(shù)據(jù)表明利用絕對精度為0.4m氣壓計與加速度計進行數(shù)據(jù)融合可以得到精度為0.03m的垂直方向上高度信息。②實現(xiàn)了六旋翼無人機在質(zhì)量發(fā)生變化時依然保證高度平穩(wěn),在傳統(tǒng)雙閉環(huán)PID控制的基礎(chǔ)上加入前饋控制使其控制效果得到明顯的改善,實驗數(shù)據(jù)表明采用本文的高度控制算法,能在變載荷的情況下得到0.2m的控制精度。

[1] Sa R C,Barreto G A,De Araujo A L C,et al. Design and Construction of a Quadrotor-Type Unmanned Aerial Vehicle:Preliminary Results[C]//2012 Workshop on Engineering Applications(WEA). IEEE,2012:1-6.

[2] 方家豪,葉欣,董偉,等. 基于加速度信息修正的四旋翼位置估計算法研究[J]. 傳感技術(shù)學報,2016,29(11):1684-1691.

[3] 趙世榮. 基于嵌入式的四旋翼飛行器姿態(tài)控制設計[D]. 廣州:中山大學,2016.

[4] Mammarella M,Campa G,Napolitano M R,et al. Machine Vision/GPS Integration Using EKF for the UAV Aerial Refueling Problem[J]. IEEE Transactions on Systems Man and Cybernetics Part C,2008,38(6):791-801.

[5] Oh S M,Johnson E. Development of UAV Navigation System Based on Unscented Kalman Filter[C]//AIAA Guidance,Navigation,and Control Conference and Exhibit. 2006:524-533.

[6] 李洪輝,裴海龍. 無人直升機高度測量的設計與實現(xiàn)[J]. 自動化與儀表,2012,27(7):11-15.

[7] 謝勇,陳照海,陳萬法. 某無人直升機高度測量系統(tǒng)融合方法[J]. 兵工自動化,2010(2):85-86.

[8] 胡永紅. 小型飛行器高度定位數(shù)據(jù)融合方法[J]. 傳感器與微系統(tǒng),2004,22(6):24-26.

[9] 王新龍. 慣性導航基礎(chǔ)[M]. 西北工業(yè)大學出版社,2013.

[10] 王偉,周勇,王峰,等. 基于氣壓高度計的多旋翼飛行器高度控制[J]. 控制工程,2011,18(4):614-617.

[11] 王立,章政,孫平. 一種自適應互補濾波姿態(tài)估計算法[J]. 控制工程,2015,22(5):881-886.

[12] Wang L,Zhang Z,Sun P. Quaternion-Based Kalman Filter for AHRS Using an Adaptive-Step Gradient Descent Algorithm[J]. International Journal of Advanced Robotic Systems,2015,12:131.

龍 文(1993-),男,湖北仙桃人,碩士研究生,研究方向為多旋翼無人機狀態(tài)估計與控制,963626536@qq.com;

陳 洋(1980-),男,博士,副教授,公開發(fā)表學術(shù)論文30余篇。主要研究方向為移動機器人建模,規(guī)劃與控制,人機交互與機器學習,chenyag@wust.edu.cn。

Altitude Control of Six Rotor UAVs with Variable Load*

LONG Wen1,2,CHEN Yang1,2*,WU Huaiyu1,2,CHENG Lei1,2,CHEN Pengzheng1,2,WANGZhengxi1,2

(1.School of Information Science and Engineering,Wuhan University of Science and Technology,Wuhan 430081,China;2.Engineering Research Center for Metallurgical Automation and Detecting Technology of Ministry of Education,Wuhan University of Science and Technology,Wuhan 430081,China)

Six-rotor UAVs usually require constant altitude flight when applied in plant protection,as we know the UAV altitude could not be tracked by a single airborne micro-sensor and that the altitude-hold would be unstable in the case of loads of the UAVs are changed. Aiming to solve these problems,a novel system based on barometer and accelerometer Kalman filtering fusion is proposed for altitude measuring,and also a controller based on double-closed-loop PID and feedforward controller is designed for altitude-hold. As a result,we obtain the accurate altitude-estimation and the stable control of the UAVs. Results of the experiment show thatthe value of altitude and velocity could be more accurate and steady by Kalman filtering. And the altitude-controller has a good performance under the condition of changing load.

six-rotor UAVs;Variable Load;feedforward;Kalman filter

項目來源:國家自然科學基金項目(61573263,61203331);湖北省科技支撐計劃項目(2015BAA018)

2016-11-24 修改日期:2017-03-10

TP242

A

1004-1699(2017)07-1125-06

C:7230

10.3969/j.issn.1004-1699.2017.07.026