共軸無人機(jī)擴(kuò)展卡爾曼姿態(tài)控制研究*

□ 有德義 □ 郝永平

1.沈陽理工大學(xué) 研究生院 沈陽 110159 2.德州職業(yè)技術(shù)學(xué)院 電子與新能源技術(shù)工程系 山東德州 253034

1 研究背景

共軸雙旋翼飛行器是一種上下槳同軸反向旋轉(zhuǎn)且能夠自主懸停的無人機(jī),是近年來無人機(jī)研究的熱點(diǎn)機(jī)型之一。共軸雙旋翼飛行器結(jié)構(gòu)與雙旋翼直升飛機(jī)結(jié)構(gòu)的不同之處在于沒有尾旋翼平衡扭矩,具有外形尺寸小、氣動(dòng)效率高等優(yōu)勢,只需要通過上下兩對螺旋槳產(chǎn)生的推力來保持自身平衡,飛行姿態(tài)通過舵機(jī)連接傾斜盤舵平面與下旋翼旋轉(zhuǎn)平面來調(diào)整。無人機(jī)采用齒輪傳動(dòng)方式,將動(dòng)力從電機(jī)傳送到旋翼,并且上下槳葉之間存在氣流擾動(dòng),加之操縱機(jī)構(gòu)的復(fù)雜性和控制的非線性等原因,會導(dǎo)致共軸雙旋翼飛行器有較明顯的機(jī)械振動(dòng)和噪聲。

近年來,世界各國都對多傳感器數(shù)據(jù)融合、濾波算法、姿態(tài)估計(jì)的問題更加關(guān)注,許多科研單位和院校對無人機(jī)姿態(tài)控制和導(dǎo)航做了較為深入的研究。我國對無人機(jī)姿態(tài)控制的研究起步較晚,共軸無人機(jī)姿態(tài)控制的研究還有待發(fā)展。國防科技大學(xué)、北京理工大學(xué)、沈陽理工大學(xué)等高校開展了對共軸無人機(jī)自主系統(tǒng)的研究,并且取得了較好的效果。吳和龍[1]在多旋翼無人機(jī)的低成本組合導(dǎo)航關(guān)鍵技術(shù)研究文章中提出基于多維狀態(tài)量的高維擴(kuò)展卡爾曼濾波數(shù)據(jù)融合算法,在此基礎(chǔ)上,建立了一種基于22維狀態(tài)量的高維擴(kuò)展卡爾曼濾波數(shù)據(jù)融合方法,進(jìn)行完整的數(shù)學(xué)模型模擬仿真試驗(yàn),結(jié)果表明具有較高的準(zhǔn)確率。沈躍等[2]在植保無人機(jī)航姿估計(jì)與分析中,采用磁力計(jì)實(shí)時(shí)校準(zhǔn)的無人機(jī)航姿兩級解算方法,這一方法考慮外界環(huán)境磁場干擾,采用無跡卡爾曼濾波器融合算法,通過四元數(shù)較好地解析出橫滾角和俯仰角姿態(tài)信息。劉旭航等[3]通過卡爾曼濾波建立加速度誤差模型,結(jié)合外部傳感器信息,得到陀螺儀校正量,并通過計(jì)算得到的增益信息對陀螺的初始值進(jìn)行校正,設(shè)計(jì)互補(bǔ)濾波器,濾去來自加速度計(jì)、磁強(qiáng)計(jì)、陀螺儀的低頻噪聲,從而消除傳感器噪聲信號對姿態(tài)估計(jì)的影響。蘇黎世聯(lián)邦理工大學(xué)的研究學(xué)者建立完整擴(kuò)展卡爾曼濾波算法的多傳感器數(shù)據(jù)融合開源框架,集成視覺傳感器、全球?qū)Ш叫l(wèi)星系統(tǒng)定位模塊、激光雷達(dá)、超聲波雷達(dá)等,達(dá)到精準(zhǔn)的姿態(tài)定位效果[4]。無人機(jī)常見的導(dǎo)航估計(jì)算法有互補(bǔ)濾波、卡爾曼濾波、粒子濾波、神經(jīng)網(wǎng)絡(luò)濾波[5]。吳東旭[6]為了提高姿態(tài)角解算的精度,利用多傳感器數(shù)據(jù)融合,分別設(shè)計(jì)互補(bǔ)濾波算法和卡爾曼濾波算法,并且串級比例積分微分姿態(tài)控制算法,寫入嵌入式飛行控制系統(tǒng)中,進(jìn)行飛行試驗(yàn),但是在研究中并沒有考慮共軸無人機(jī)自身的主要誤差來源。筆者以小型折疊共軸雙旋翼飛行器為研究對象,分析誤差、振動(dòng)的主要來源,建立姿態(tài)四元數(shù)模型,提出基于改進(jìn)的擴(kuò)展卡爾曼濾波的數(shù)據(jù)融合算法,并在此基礎(chǔ)上進(jìn)行載體姿態(tài)靜態(tài)和動(dòng)態(tài)試驗(yàn)對比。

2 姿態(tài)運(yùn)動(dòng)模型

2.1 共軸無人機(jī)結(jié)構(gòu)

共軸無人機(jī)包括雙旋翼、共軸機(jī)體、傾斜裝置、電機(jī)、舵機(jī)、控制核心板等結(jié)構(gòu)。控制核心板在共軸機(jī)體內(nèi)部提供所有的控制信號。電機(jī)位于共軸機(jī)體上方,通過齒輪傳動(dòng)提供旋轉(zhuǎn)動(dòng)力,依靠兩個(gè)旋翼反向轉(zhuǎn)動(dòng)的升力和自身重力產(chǎn)生合力,實(shí)現(xiàn)上下運(yùn)動(dòng)。舵機(jī)位于共軸機(jī)體的上部,采用連桿控制操控盤傾斜,進(jìn)而帶動(dòng)下旋翼產(chǎn)生俯仰和橫滾等姿態(tài)運(yùn)動(dòng)。共軸無人機(jī)簡化模型如圖1所示。

圖1 共軸無人機(jī)簡化模型

2.2 參考坐標(biāo)系

在忽略地球自傳和地球質(zhì)心曲線運(yùn)動(dòng)的情況下,建立共軸無人機(jī)數(shù)學(xué)模型。為更好地確定姿態(tài)、速度、加速度和外力力矩的分量,需要引入多種坐標(biāo)系。機(jī)體坐標(biāo)系是固連在無人機(jī)上并隨無人機(jī)運(yùn)動(dòng)的一種動(dòng)坐標(biāo)系,地面坐標(biāo)系轉(zhuǎn)換到機(jī)體坐標(biāo)系,需要經(jīng)過X軸、Y軸、Z軸旋轉(zhuǎn)。定義φ為橫滾角,θ為俯仰角,ψ為航向角,無人機(jī)繞X軸轉(zhuǎn)動(dòng)后的旋轉(zhuǎn)矩陣Rx(φ)為:

(1)

無人機(jī)繞Y軸轉(zhuǎn)動(dòng)后的旋轉(zhuǎn)矩陣Ry(θ)為:

(2)

無人機(jī)繞Z軸旋轉(zhuǎn)后的旋轉(zhuǎn)矩陣Rz(ψ)為:

(3)

(4)

(5)

2.3 四元數(shù)姿態(tài)解算

四元數(shù)表示三維空間的旋轉(zhuǎn)信息,通過構(gòu)造四元數(shù)得到變換矩陣。四元數(shù)滿足:

(6)

四元數(shù)的模為:

(7)

q0=cos(α/2)

(8)

q1=lsin(α/2)

(9)

q2=msin(α/2)

(10)

q3=nsin(α/2)

(11)

式中:α為旋轉(zhuǎn)矢量繞旋轉(zhuǎn)軸的轉(zhuǎn)角;l、m、n為旋轉(zhuǎn)矢量在i、j、k虛軸上的投影。

用四元數(shù)表示姿態(tài)的優(yōu)勢為在任意姿態(tài)下都不會出現(xiàn)奇異性[7]。

(12)

用四元數(shù)來更新橫滾角、俯仰角、航向角,有:

θ=arcsin[2(q2q3+q0q1)]

(13)

(14)

(15)

載體上的三軸陀螺儀可以測得共軸無人機(jī)在導(dǎo)航坐標(biāo)系中的角速度在機(jī)體坐標(biāo)系中的分量。ωx、ωy、ωz為機(jī)體坐標(biāo)系下的旋轉(zhuǎn)角速度,通過四元數(shù)微分方程建立的狀態(tài)方程Ωb為:

(16)

采用一階龍格庫塔法求解四元數(shù)微分方程,可以得到離散時(shí)間模型q(k)為:

q(k)=q(k-1)

+Tq(k-1)[Ωb(k-1)]/2

(17)

式中:T為系統(tǒng)采樣時(shí)間間隔;q(k-1)為k-1時(shí)刻姿態(tài)四元數(shù)。

設(shè)置初始四元數(shù)q(0),利用式(17)可以計(jì)算遞推出k時(shí)刻四元數(shù)q(k)的取值,進(jìn)而能夠更新姿態(tài)矩陣[8]。

3 誤差分析

陀螺儀測量角速度具有高動(dòng)態(tài)性,但測量的是角度的導(dǎo)數(shù),再對時(shí)間積分才能得到角度。陀螺儀內(nèi)部運(yùn)動(dòng)一段時(shí)間,噪聲和偏置會導(dǎo)致數(shù)據(jù)偏差,需要借助其它傳感器進(jìn)行校正。建立多維狀態(tài)量,包含姿態(tài)四元數(shù)(q0,q1,q2,q3)、導(dǎo)航系三維速度信息(vN,vE,vD)、導(dǎo)航系三維位置信息(PN,PE,PD)、三維陀螺儀角增量誤差(δgx,δgy,δgz)、三維加速度計(jì)增量誤差(δax,δay,δaz)、三維地磁信息(mN,mE,mD)、三維地磁誤差(δmx,δmy,δmz)、二維水平風(fēng)速信息(vwindN,vwindE)。初始時(shí)刻四元數(shù)為:

q0=cos(ψ/2)cos(θ/2)cos(φ/2)

+sin(ψ/2)sin(θ/2)sin(φ/2)

(18)

q1=sin(ψ/2)cos(θ/2)cos(φ/2)

-cos(ψ/2)sin(θ/2)sin(φ/2)

(19)

q2=cos(ψ/2)sin(θ/2)cos(φ/2)

+sin(ψ/2)cos(θ/2)sin(φ/2)

(20)

q3=cos(ψ/2)cos(θ/2)sin(φ/2)

+sin(ψ/2)sin(θ/2)cos(φ/2)

(21)

共軸無人機(jī)的振動(dòng)源主要有七個(gè)方面。第一,無刷電機(jī)通過齒輪傳動(dòng)裝置帶動(dòng)上下旋翼,構(gòu)成一個(gè)由電機(jī)運(yùn)轉(zhuǎn)、齒輪嚙合等組成的機(jī)械扭轉(zhuǎn)系統(tǒng),進(jìn)而產(chǎn)生機(jī)械振動(dòng)。第二,上下旋翼在轉(zhuǎn)動(dòng)時(shí)會產(chǎn)生周期的空氣動(dòng)力,通過槳轂和旋翼軸向機(jī)體傳動(dòng)。其中,旋翼振動(dòng)是主要振源,原因是機(jī)械不均衡和周期的空氣動(dòng)力相互作用。第三,微機(jī)電系統(tǒng)傳感器安裝,以及機(jī)體坐標(biāo)系不對稱和標(biāo)度帶來的誤差。第四,初始條件校準(zhǔn)時(shí),參數(shù)和當(dāng)前姿態(tài)角誤差。第五,四元數(shù)計(jì)算誤差、濾波算法誤差等。第六,微機(jī)電系統(tǒng)陀螺儀因自身設(shè)計(jì)缺陷而由高動(dòng)態(tài)特性產(chǎn)生的漂移誤差等。第七,機(jī)體運(yùn)動(dòng)和姿態(tài)估計(jì)延時(shí)產(chǎn)生的動(dòng)態(tài)誤差。

為減小機(jī)體振動(dòng)對微機(jī)電系統(tǒng)傳感器的影響,應(yīng)用補(bǔ)償算法,并且在姿態(tài)解算中進(jìn)行慣性測量單元傳感器權(quán)重調(diào)整,同時(shí)還應(yīng)該避開結(jié)構(gòu)的自振頻率與轉(zhuǎn)子所引起的激振力頻率。

4 卡爾曼濾波姿態(tài)估計(jì)

利用線性卡爾曼濾波對非線性系統(tǒng)的狀態(tài)進(jìn)行估計(jì),采用泰勒展開式,使非線性系統(tǒng)在局部線性化,這樣高斯分布的參數(shù)在線性變換后依然保持高斯分布,就可以使用標(biāo)準(zhǔn)卡爾曼濾波器的卡爾曼濾波結(jié)構(gòu)。在非線性不強(qiáng)烈的情況下,擴(kuò)展卡爾曼濾波器可以有效過濾掉非線性。在非線性較強(qiáng)烈的情況下,濾波效果會降低。建立狀態(tài)方程為:

x(k)=f(x(k-1),k-1)+w(k-1)

(22)

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

(23)

式中:x(k)為k時(shí)刻系統(tǒng)狀態(tài)矩陣;z(k)為k時(shí)刻系統(tǒng)觀測矩陣;f(x(k-1),k-1)、h(x(k),k)分別為非線性離散系統(tǒng)k-1時(shí)刻預(yù)測狀態(tài)矢量函數(shù)和k時(shí)刻測量狀態(tài)函數(shù);w(k-1)為k-1時(shí)刻系統(tǒng)狀態(tài)噪聲矩陣;v(k)為k時(shí)刻測量噪聲矩陣。

此處假設(shè)系統(tǒng)的狀態(tài)量x(k)為:

x(k)=

(q0(k)q1(k)q2(k)q3(k)bwx(k)bwy(k)bwz(k))

(24)

x(k)中包括姿態(tài)四元數(shù)和三個(gè)轉(zhuǎn)動(dòng)軸陀螺儀隨機(jī)漂移量。

初始化狀態(tài),假設(shè)初值濾波器為:

(25)

(26)

進(jìn)行擴(kuò)展卡爾曼濾波,時(shí)間預(yù)測更新估計(jì)為:

(27)

(28)

測量更新為:

(29)

(30)

式中:Hk是k時(shí)刻測量雅可比矩陣;Rk為k時(shí)刻觀測噪聲協(xié)方差矩陣。

計(jì)算k時(shí)刻卡爾曼增益Kk為:

(31)

(32)

Pk|k=(I-KkHk)Pk∣k-1

(33)

(34)

更新誤差協(xié)方差時(shí),測量更新方程利用觀測變量校正狀態(tài)估計(jì)和協(xié)方差估計(jì),Fk為:

(35)

根據(jù)狀態(tài)噪聲和觀測噪聲的統(tǒng)計(jì)特性,只有系統(tǒng)的狀態(tài)方程和觀測方程都接近線性,擴(kuò)展卡爾曼濾波才可以更接近真實(shí)值。

5 測試平臺

共軸無人機(jī)飛行測試平臺如圖2所示,其中包含對共軸無人機(jī)升力、振動(dòng)、旋翼轉(zhuǎn)速等的測試。共軸無人機(jī)垂直飛行過程中,主要受到來自槳葉旋轉(zhuǎn)時(shí)產(chǎn)生的空氣動(dòng)力和自身重力的影響[9]。支撐結(jié)構(gòu)作為測試平臺中最主要的構(gòu)造部分,用于穩(wěn)固機(jī)身,槳葉產(chǎn)生的氣動(dòng)力及機(jī)身受到的氣動(dòng)力都會通過機(jī)身結(jié)構(gòu)間接或直接施加在支撐結(jié)構(gòu)上。機(jī)身、傳感器,以及其它構(gòu)件的重力,也都會直接施加在支撐結(jié)構(gòu)上[10]。共軸無人機(jī)在測試過程中,由機(jī)械結(jié)構(gòu)產(chǎn)生的振動(dòng)力同樣會施加在支撐結(jié)構(gòu)上[11]。測試平臺通過姿態(tài)測量傳感器進(jìn)行靜態(tài)實(shí)時(shí)穩(wěn)定的姿態(tài)數(shù)據(jù)采集和測量,通過實(shí)時(shí)測量的橫滾角、俯仰角、航向角信息,來分析主要誤差來源。

圖2 共軸無人機(jī)測試平臺

支撐結(jié)構(gòu)作為承受共軸無人機(jī)空氣動(dòng)力及自身重力的主體部件,在受力方面比較復(fù)雜。共軸無人機(jī)不僅產(chǎn)生維持飛行的氣動(dòng)升力,而且產(chǎn)生在發(fā)生橫滾、俯仰、航向運(yùn)動(dòng)時(shí)的力矩。由此,共軸無人機(jī)施加在支撐結(jié)構(gòu)上的力主要可以分為升力,以及橫滾運(yùn)動(dòng)力、俯仰運(yùn)動(dòng)力、航向運(yùn)動(dòng)力。測試平臺使用的測試設(shè)備包括扭矩傳感器、拉力傳感器、數(shù)據(jù)采集等[12-13]。

6 測試數(shù)據(jù)分析

6.1 靜態(tài)姿態(tài)角

姿態(tài)角包括橫滾角、俯仰角、航向角。忽略共軸無人機(jī)下方固連在萬向軸上,同時(shí)忽略轉(zhuǎn)臺繩索約束對姿態(tài)角的影響,共軸無人機(jī)處于準(zhǔn)靜態(tài),即輸入油門量為50%時(shí),姿態(tài)角隨時(shí)間變化曲線如圖3所示。測試選用擴(kuò)展卡爾曼濾波和Mahony互補(bǔ)濾波進(jìn)行對比。共軸無人機(jī)處于準(zhǔn)靜態(tài)時(shí),采用擴(kuò)展卡爾曼濾波,姿態(tài)角變化小于0.5°,而采用Mahony互補(bǔ)濾波,姿態(tài)角的變化接近1°。可以看出,擴(kuò)展卡爾曼濾波能夠很好地減弱共軸無人機(jī)的機(jī)體振動(dòng)。分析100 s內(nèi)采樣1 000次姿態(tài)角信息,顯示采用擴(kuò)展卡爾曼濾波對橫滾角、俯仰角、航向角都有較明顯的改善作用,為前期機(jī)械結(jié)構(gòu)減振濾波設(shè)計(jì)和控制算法優(yōu)化提供了參考。

圖3 姿態(tài)角隨時(shí)間變化曲線

6.2 飛行姿態(tài)角

共軸無人機(jī)飛行姿態(tài)角測試處于相對封閉的校園無風(fēng)環(huán)境中,如圖4所示。姿態(tài)角期望值和實(shí)際值變化曲線如圖5所示,二維飛行日志導(dǎo)出軌跡如圖6所示,實(shí)際飛行路徑如圖7所示。

圖4 飛行姿態(tài)角測試環(huán)境

圖5 姿態(tài)角期望值和實(shí)際值變化曲線

圖6 二維飛行軌跡

圖7 實(shí)際飛行路徑

對于采集的共軸無人機(jī)振動(dòng)數(shù)據(jù),利用軟件進(jìn)行數(shù)據(jù)分析,具體包括60 s內(nèi)的900個(gè)采樣點(diǎn)數(shù)據(jù)。各方向位置變化曲線如圖8所示。實(shí)際飛行時(shí)X方向期望值與實(shí)際值誤差小于0.1 m,Y方向期望值與實(shí)際值非常接近,Z方向,即高度方向期望值與實(shí)際值同樣誤差較小,可見共軸無人機(jī)姿態(tài)估計(jì)跟隨性較好,在一定程度上解決了誤差累積和隨機(jī)噪聲漂移等問題。

圖8 各方向位置變化曲線

7 結(jié)束語

針對小型共軸雙旋翼飛行器誤差源和姿態(tài)測量的問題,分析了橫滾、俯仰、航向姿態(tài)特性,提出四元數(shù)擴(kuò)展卡爾曼濾波數(shù)據(jù)融合算法,在此基礎(chǔ)上進(jìn)行測試分析。

在靜態(tài)姿態(tài)角測試中,對共軸無人機(jī)振動(dòng)幅度和頻率進(jìn)行分析,數(shù)據(jù)表明,擴(kuò)展卡爾曼濾波在姿態(tài)估計(jì)中誤差小于Mahony互補(bǔ)濾波。飛行姿態(tài)角測試表明,擴(kuò)展卡爾曼濾波跟蹤效果較好,能夠減小高速飛行過程中產(chǎn)生的機(jī)械振動(dòng)噪聲對姿態(tài)角的影響,提高姿態(tài)角的穩(wěn)定性和控制魯棒性,并有效提高共軸姿態(tài)和位置估計(jì)的精度。通過研究可見,擴(kuò)展卡爾曼姿態(tài)控制在無人機(jī)姿態(tài)解算中具有一定的工程應(yīng)用價(jià)值。