劉 昕,秦 超,2

(1. 中國(guó)電子科技集團(tuán)公司 第五十四研究所,河北 石家莊 050081;2. 哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院,黑龍江 哈爾濱 150001)

0 引言

近年來(lái)國(guó)內(nèi)四旋翼飛行器產(chǎn)業(yè)的發(fā)展迅猛,與傳統(tǒng)的直升機(jī)和固定翼相比,旋翼無(wú)人機(jī)的易用性(運(yùn)動(dòng)相互解耦)、可靠性(機(jī)械磨損小)和維護(hù)性(結(jié)構(gòu)簡(jiǎn)單、模塊化)等剛性體驗(yàn)讓人們的選擇成為必然[1]。旋翼無(wú)人機(jī)實(shí)現(xiàn)精準(zhǔn)姿態(tài)控制與位置控制的前提是導(dǎo)航模塊提供穩(wěn)定可靠的姿態(tài)、位置數(shù)據(jù),目前多采取組合導(dǎo)航技術(shù),將多種傳感器對(duì)姿態(tài)、位置等數(shù)據(jù)進(jìn)行信息融合[2]。文獻(xiàn)[3]介紹了過(guò)去10年中提出的解決方案。文獻(xiàn)[4-5]在旋翼無(wú)人機(jī)控制中應(yīng)用了傳統(tǒng)的導(dǎo)航算法。針對(duì)如何確保無(wú)人機(jī)控制平穩(wěn)性的問(wèn)題,本文使用微機(jī)電系統(tǒng)(MEMS)振動(dòng)陀螺提出了一種基于擴(kuò)展卡爾曼濾波(EKF)的導(dǎo)航算法,融合全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)觀測(cè)和自身加速度計(jì)輸出觀測(cè),克服GNSS數(shù)據(jù)失效時(shí)的姿態(tài)穩(wěn)定問(wèn)題。同時(shí)引入自適應(yīng)平滑采樣方法,降低了角速度輸出噪聲。

1 系統(tǒng)模型

組合導(dǎo)航算法基于擴(kuò)展卡爾曼濾波模型,以全球?qū)Ш叫l(wèi)星系統(tǒng)/慣性導(dǎo)航系統(tǒng)(GNSS/INS)組合導(dǎo)航的框架為基礎(chǔ),結(jié)合了無(wú)地磁傳感器的航姿參考系統(tǒng)算法[6]。算法以加速度計(jì)的輸出值和GNSS測(cè)量的位置Pm、速度Vm為觀測(cè)量,估計(jì)出四元數(shù)偏差和陀螺零偏bω。組合導(dǎo)航系統(tǒng)框架如圖1所示,圖中ωm、am分別為三軸MEMS陀螺儀和加速度的輸出,P、V和q分別為慣性解算得到的位置、速度和四元數(shù),φ、θ和γ為組合導(dǎo)航系統(tǒng)輸出的航向、俯仰和橫滾。

圖1 組合導(dǎo)航系統(tǒng)框架

1.1 慣性測(cè)量單元(IMU)誤差模型

IMU的誤差包括系統(tǒng)誤差和隨機(jī)誤差。系統(tǒng)誤差可通過(guò)器件常規(guī)標(biāo)定方法進(jìn)行消除或降低,包括零偏、刻度系數(shù)誤差及非正交性誤差等。本文選用的高解耦微型陀螺如圖2所示。微型陀螺敏感芯片采用多質(zhì)量塊解耦設(shè)計(jì)方案,驅(qū)動(dòng)與檢測(cè)音叉分離,通過(guò)撓性支撐結(jié)構(gòu)連接,避免了各模態(tài)的機(jī)械耦合誤差,達(dá)到解耦效果,隔離框的設(shè)計(jì)可有效地提升敏感芯片的環(huán)境適應(yīng)性[7]。

圖2 高解耦微型陀螺總體結(jié)構(gòu)示意圖

隨機(jī)誤差包括高斯白噪聲及隨機(jī)游走噪聲,其中白噪聲在概率上服從高斯分布,其功率譜密度為均勻分布。偏置隨機(jī)游走被認(rèn)為是離散化的維納過(guò)程,由于IMU每次更新位置時(shí)都會(huì)疊加一個(gè)新的白噪聲,故將其偏置噪聲建模為隨機(jī)游走噪聲[8]。IMU誤差模型為

(1)

1.2 航姿表示形式

(2)

航向角φ、俯仰角θ、橫滾角γ為

(3)

2 基于EKF的組合導(dǎo)航算法設(shè)計(jì)

2.1 狀態(tài)方程

(4)

式中:bω為陀螺零偏矢量;wω為噪聲矢量;Ω為簡(jiǎn)化的四元數(shù)乘法矩陣,且:

(5)

2.2 量測(cè)方程

GNSS系統(tǒng)是輔助INS進(jìn)行姿態(tài)保持的良好互補(bǔ)系統(tǒng),此處以其輸出位置、速度為觀測(cè)量建立輸出方程1:

(6)

由于無(wú)人機(jī)的運(yùn)動(dòng)形式有別于地面車輛,當(dāng)GNSS系統(tǒng)失效時(shí),無(wú)法引入車輛的運(yùn)動(dòng)學(xué)約束來(lái)抑制導(dǎo)航系統(tǒng)的誤差[9],因此,參照常規(guī)的航姿參考系統(tǒng)算法,即以陀螺儀和加速度計(jì)輸出、航姿變化量等參數(shù)為判斷條件,當(dāng)判定無(wú)人機(jī)處于靜態(tài)、準(zhǔn)靜態(tài)和低動(dòng)態(tài)條件下,以加速度計(jì)輸出為觀測(cè)量建立輸出方程2:

(7)

2.3 濾波算法

依照EKF的工作流程,將連續(xù)時(shí)間模型進(jìn)行離散化,得到:

(8)

式中:wk為過(guò)程噪聲矢量;vk為量測(cè)噪聲。假定wk,vk均為白噪聲,各自的協(xié)方差矩陣分別為Q、R。

離散化計(jì)算采用一階近似:

(9)

式中:T為濾波周期。

(10)

中間過(guò)程矩陣計(jì)算如下:

(11)

FVq0=2(q0amx-q3amy+q2amz)

(12)

FVq1=2(q1amx+q2amy+q3amz)

(13)

FVq2=2(-q2amx+q1amy+q0amz)

(14)

FVq3=2(-q3amx-q0amy+q1amz)

(15)

Fqq=

(16)

量測(cè)矩陣H為

(17)

(18)

以GNSS系統(tǒng)的位置、速度為觀測(cè)量時(shí),使用此矩陣的前6行;以航姿參考系統(tǒng)算法中的加速度計(jì)輸出為觀測(cè)量時(shí),使用此矩陣的后3行。

3 陀螺數(shù)據(jù)的自適應(yīng)平滑采樣

旋翼無(wú)人機(jī)控制要求導(dǎo)航系統(tǒng)輸出的姿態(tài)和角速度信息平穩(wěn),數(shù)據(jù)噪聲會(huì)影響控制的穩(wěn)定性。在盡量降低數(shù)據(jù)處理復(fù)雜度的前提下,將組合導(dǎo)航算法劃分為機(jī)械編排、卡爾曼濾波預(yù)測(cè)過(guò)程和卡爾曼濾波更新過(guò)程3個(gè)主要耗時(shí)模塊。分別在機(jī)械編排前、機(jī)械編排與卡爾曼濾波預(yù)測(cè)過(guò)程之間、卡爾曼濾波預(yù)測(cè)與更新之間進(jìn)行采樣,并依次將陀螺輸出分別記為ω1、ω2和ω3,在下次機(jī)械編排前進(jìn)行陀螺數(shù)據(jù)的自適應(yīng)平滑,算法流程圖如圖3所示。

圖3 基于自適應(yīng)平滑采樣的濾波流程

自適應(yīng)平滑采樣器基于Sigmoid函數(shù)[10],考慮無(wú)人機(jī)動(dòng)態(tài)性和陀螺的噪聲水平,依據(jù)無(wú)人機(jī)控制角速度輸入ωc及其變化率δωc構(gòu)造權(quán)重函數(shù):

(19)

則下一周期機(jī)械編排時(shí)角速度輸入可表示為

(20)

4 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證該算法的正確性,在某型搭載了SBG Ellipse-N型導(dǎo)航模塊的旋翼無(wú)人機(jī)上進(jìn)行了對(duì)比測(cè)試,該模塊的姿態(tài)精度可達(dá)0.05°,航向精度可達(dá)0.2°。分別在導(dǎo)航計(jì)算機(jī)中使用EKF和本文方法進(jìn)行了導(dǎo)航解算,并與安裝在無(wú)人機(jī)上的法國(guó)SBG導(dǎo)航模塊的輸出進(jìn)行對(duì)比,陀螺和姿態(tài)數(shù)據(jù)對(duì)比如圖4～8和表 1、2所示。結(jié)果表明,最大角速度誤差和角誤差標(biāo)準(zhǔn)差有明顯提升。

表1 EKF與本文方法最大角速度誤差對(duì)比

表2 EKF與本文方法角誤差標(biāo)準(zhǔn)差對(duì)比

圖4 SBG、EKF以及本文方法陀螺x軸輸出對(duì)比

圖5 SBG、EKF以及本文方法陀螺y軸輸出對(duì)比

圖6 SBG、EKF及本文方法陀螺z軸輸出對(duì)比

圖7 SBG、EKF及本文方法橫滾角對(duì)比

圖8 SBG、EKF及本文方法俯仰角對(duì)比

圖9為在旋翼無(wú)人機(jī)平臺(tái)上采用本文方法后的無(wú)人機(jī)飛行軌跡圖。由圖可看出軌跡重復(fù)性高,控制平穩(wěn)。

圖9 旋翼無(wú)人機(jī)飛行軌跡

5 結(jié)束語(yǔ)

本文提出了一種面向旋翼無(wú)人機(jī)的組合導(dǎo)航算法。算法基于擴(kuò)展卡爾曼濾波,結(jié)合了GNSS/INS組合導(dǎo)航算法和無(wú)地磁傳感器的航姿參考系統(tǒng)算法,在一定程度上解決了GNSS失效情況下的姿態(tài)穩(wěn)定問(wèn)題。針對(duì)旋翼無(wú)人機(jī)對(duì)導(dǎo)航系統(tǒng)輸出信息平穩(wěn)性的要求,采用自適應(yīng)陀螺采樣平滑方法,在少量增加數(shù)據(jù)處理復(fù)雜度的前提下,降低了航姿和角速度信息的噪聲,在無(wú)人機(jī)時(shí)實(shí)現(xiàn)了良好的飛行軌跡重復(fù)性和控制平穩(wěn)性。