王 超,周 軍,黃浩乾,沈寒伊,唐家成
(河海大學(xué)能源與電氣學(xué)院,江蘇 南京 211100)
在捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)中慣性測(cè)量元件(陀螺和加速度計(jì))與運(yùn)載體固聯(lián),通過(guò)計(jì)算機(jī)采集慣性元件的輸出信息并進(jìn)行數(shù)值積分,求解運(yùn)載體的姿態(tài)、速度和位置等導(dǎo)航參數(shù)。高精度SINS 對(duì)慣性器件性能和導(dǎo)航算法精度的要求非常苛刻,但由于高精度慣性器件價(jià)格昂貴且提升其精度非常困難,所以在影響SINS 的誤差源中要求導(dǎo)航算法誤差比重必須很小,一般需小于5%[1]。由于載體姿態(tài)可能劇烈變化(如戰(zhàn)斗機(jī)的翻滾運(yùn)動(dòng)),姿態(tài)更新算法便成了SINS 算法的核心,對(duì)解算精度影響最為突出,具有重要的研究和應(yīng)用價(jià)值。
相對(duì)單一的慣導(dǎo)解算,SINS/GPS 組合導(dǎo)航利用SINS 解算得到的速度、位置和GPS 輸出的速度、位置的差值構(gòu)建觀測(cè)量,通過(guò)Kalman 濾波器對(duì)慣性解算誤差進(jìn)行最優(yōu)估計(jì),利用估計(jì)出來(lái)的誤差修正慣性解算結(jié)果,從而大大提高導(dǎo)航解算精度。但實(shí)際上,GPS 信號(hào)受建筑遮擋、載體機(jī)動(dòng)等因素影響可能失鎖,Kalman 濾波器失去了一部分觀測(cè)量,會(huì)導(dǎo)致導(dǎo)航系統(tǒng)精度下降甚至發(fā)散。針對(duì)這些問(wèn)題,有學(xué)者提出了利用神經(jīng)網(wǎng)絡(luò)輔助的方法[2-6]。其基本思想是利用GPS 信號(hào)有效時(shí)的導(dǎo)航數(shù)據(jù)訓(xùn)練神經(jīng)網(wǎng)絡(luò),在GPS 失效時(shí)接入該訓(xùn)練好的網(wǎng)絡(luò),由其預(yù)測(cè)出組合導(dǎo)航數(shù)據(jù),使導(dǎo)航結(jié)果不發(fā)散。鮑泳林等[2]以慣性器件測(cè)量數(shù)據(jù)為輸入,Kalman 輸出的狀態(tài)估計(jì)作為標(biāo)簽對(duì)徑向基函數(shù)神經(jīng)網(wǎng)絡(luò)(Radial Basis Function Neural Network,RBFNN)進(jìn)行訓(xùn)練,并利用訓(xùn)練結(jié)果對(duì)濾波器進(jìn)行校正;陳鑫鑫等[3]以不同時(shí)刻純慣性解算的速度和位置差值為輸入,以組合導(dǎo)航結(jié)果為輸出,對(duì)多個(gè)BPNN 并行進(jìn)行訓(xùn)練;林雪源[4]、劉慶元[5]采用MEMS 傳感器測(cè)量數(shù)據(jù)為輸入,Kalman 濾波器輸出的水平速度和位置誤差作為標(biāo)簽對(duì)RBFNN 或前饋神經(jīng)網(wǎng)絡(luò)(Back Propagation Neural Network,BPNN)進(jìn)行了訓(xùn)練;李松等[6]對(duì)基于BPNN 神經(jīng)網(wǎng)絡(luò)輔助的導(dǎo)航濾波方法進(jìn)行了探索和論證,提出利用濾波殘差和增益進(jìn)行網(wǎng)絡(luò)訓(xùn)練的方法。但對(duì)這些方法進(jìn)行SINS/GPS 組合導(dǎo)航數(shù)值仿真時(shí),利用濾波殘差對(duì)神經(jīng)網(wǎng)絡(luò)的訓(xùn)練效果不明顯。采用濾波增益矩陣雖可取得較好效果,但涉及要素過(guò)多,以SINS/GPS 組合導(dǎo)航問(wèn)題為例,增益矩陣為15×6 維,則要求至少90 個(gè)輸入層神經(jīng)元,使得訓(xùn)練速度過(guò)慢,算法過(guò)度冗余。
結(jié)合目前SINS/GPS 組合導(dǎo)航的主要問(wèn)題[7]和近年來(lái)神經(jīng)網(wǎng)絡(luò)算法技術(shù)[8-9],研究目標(biāo)為:
(1)在SINS/GPS 組合導(dǎo)航方法上引入BP 神經(jīng)網(wǎng)絡(luò)輔助修正環(huán)節(jié),以提高組合導(dǎo)航姿態(tài)估計(jì)精度;
(2)完善并優(yōu)化神經(jīng)網(wǎng)絡(luò)訓(xùn)練環(huán)節(jié),構(gòu)建降維特征向量,減少網(wǎng)絡(luò)的計(jì)算冗余,提高訓(xùn)練效率。
SINS/GPS 組合導(dǎo)航系統(tǒng)通常選用捷聯(lián)慣導(dǎo)的誤差傳播方程作為其狀態(tài)方程,選擇“東-北-天”地理坐標(biāo)系為導(dǎo)航系,取姿態(tài)失準(zhǔn)角(φx,φy,φz)、速度誤差量、位置和陀螺常值漂移(εE,εN,εU)、加速度計(jì)常值偏置(?E,?N,?U),同時(shí)考慮桿臂誤差和時(shí)間不同步誤差(δt),可獲得以速度和位置誤差為狀態(tài)觀測(cè)量的SINS/GPS 組合導(dǎo)航狀態(tài)空間模型,如下
式中:X=[φT(δvn)T(δp)T(εb)T(?b)T(δlb)Tδt]T,而
更確切地,F為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣;G為系統(tǒng)噪聲驅(qū)動(dòng)矩陣;Wb為系統(tǒng)噪聲矢量,和分別為陀螺角速度測(cè)量白噪聲和加速度計(jì)比力測(cè)量白噪聲;V為測(cè)量噪聲矢量,Vv和Vp分別為GPS 接收機(jī)速度測(cè)量白噪聲和位置測(cè)量白噪聲;H為系統(tǒng)的量測(cè)矩陣,其他符號(hào)含義參見(jiàn)嚴(yán)恭敏等[1]的4.2.5 節(jié)。需要注意的是,為簡(jiǎn)化符號(hào)表示,式(1)中的時(shí)間變量t均省略不寫(xiě),也就是說(shuō),SINS/GPS 組合導(dǎo)航狀態(tài)空間模型(1)是線性時(shí)變的。
一般情況下,如果條件允許的話,應(yīng)當(dāng)對(duì)SINS和GPS 之間的桿臂(或時(shí)間不同步)誤差進(jìn)行精確測(cè)量并作相應(yīng)補(bǔ)償,不再將它們列入濾波器狀態(tài),這樣既有利減少濾波計(jì)算量,還能防止桿臂(或時(shí)間不同步)狀態(tài)估計(jì)不準(zhǔn)而影響其他狀態(tài)的估計(jì)效果。實(shí)際應(yīng)用中,如果桿臂(或時(shí)間不同步)誤差難以精確測(cè)量,或隨時(shí)間變化,才考慮其狀態(tài)建模和濾波估計(jì),并且只有在適當(dāng)機(jī)動(dòng)的情況下,這些狀態(tài)才是可觀測(cè)的。下文不考慮將它們列入濾波器狀態(tài),故X為15 維的矢量。
F(τ)、G(τ)是關(guān)于時(shí)間τ的確定性矩陣,w(τ)是零均值的高斯白噪聲,式(2)的詳細(xì)推導(dǎo)請(qǐng)參見(jiàn)文獻(xiàn)[1]。
記離散化時(shí)間間隔為T(mén)=tk-tk-1,當(dāng)F(t)在較短的積分區(qū)間[tk-1,tk]內(nèi)變化不太劇烈,并且滿(mǎn)足F(tk-1)T?I時(shí),則Φk/k-1可近似為:
忽略高階項(xiàng)后得到k時(shí)刻系統(tǒng)離散狀態(tài)方程為:
式中:Φk/k-1、Γk/k-1為離散化后的狀態(tài)轉(zhuǎn)移矩陣和噪聲驅(qū)動(dòng)矩陣,它們可以表示為:
式中:F(tk-1)表示上一時(shí)刻系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣,G(tk-1)表示上一時(shí)刻系統(tǒng)噪聲驅(qū)動(dòng)矩陣。
根據(jù)上述離散狀態(tài)方程和量測(cè)方程,通過(guò)Kalman 濾波的SINS/GPS 組合導(dǎo)航姿態(tài)估計(jì)如下:
BP 神經(jīng)網(wǎng)絡(luò)算法是目前應(yīng)用廣泛的函數(shù)逼近方法。按照誤差反向傳播方法訓(xùn)練后的BP 神經(jīng)網(wǎng)絡(luò)是一種多層前饋網(wǎng)絡(luò)模型,包括輸入層,一個(gè)或多個(gè)隱藏層和輸出層。采用包含一個(gè)隱藏層的網(wǎng)絡(luò)模型。研究表明,包含一個(gè)隱藏層的神經(jīng)網(wǎng)絡(luò)可以逼近任何函數(shù)模型[10],如圖1 所示。BP 神經(jīng)網(wǎng)絡(luò)的傳播過(guò)程包括正向和誤差反向傳播兩個(gè)過(guò)程,正向傳播指輸入信號(hào)通過(guò)隱藏層作用于輸出層,期間經(jīng)過(guò)激勵(lì)函數(shù)的非線性變換,產(chǎn)生輸出信號(hào)。若實(shí)際輸出與期望輸出不相符,則轉(zhuǎn)入誤差反向傳播過(guò)程。將輸出誤差通過(guò)隱藏層向輸入層逐層反傳,并將誤差分?jǐn)偨o各層所有單元,以從各層獲得的誤差信號(hào)作為調(diào)整各層權(quán)值的依據(jù)[11]。
圖1 BP 神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)圖
通過(guò)調(diào)整輸入層與隱藏層和隱藏層與輸出層的連接強(qiáng)度、閾值,使誤差沿著梯度方向下降,經(jīng)過(guò)反復(fù)迭代,即網(wǎng)絡(luò)訓(xùn)練過(guò)程,直到網(wǎng)絡(luò)輸出和期望輸出間誤差接近于零或預(yù)定閾值,停止訓(xùn)練[12]。
BP 神經(jīng)網(wǎng)絡(luò)在組合導(dǎo)航領(lǐng)域的應(yīng)用時(shí),問(wèn)題主要集中在特征向量的構(gòu)建和估計(jì)上。
通過(guò)文獻(xiàn)分析,我們可以看到神經(jīng)網(wǎng)絡(luò)輔助的組合導(dǎo)航系統(tǒng)主要采用濾波殘差、增益來(lái)構(gòu)建特征向量。濾波增益雖可以達(dá)到理想的效果,卻因組合導(dǎo)航系統(tǒng)狀態(tài)向量維數(shù)較大,訓(xùn)練速度十分緩慢。對(duì)此,考慮基于矩陣QR 分解構(gòu)造神經(jīng)網(wǎng)絡(luò)的輸入特征向量。QR 方法是一種等價(jià)變換方法,是計(jì)算一般矩陣特征值問(wèn)題的有效方法之一。
定義1 如果矩陣A可以化成適當(dāng)維數(shù)的正交矩陣Q與上三角矩陣R的乘積,即
則稱(chēng)式(7)是矩陣A的QR 分解。
需要注意的是,A矩陣可以是非方矩陣,而Q矩陣是與A的行數(shù)相同的方矩陣,R具有與A相同的行列數(shù),即R也可以是非方矩陣。
基于定義1,關(guān)于矩陣A的QR 分解的主要性質(zhì)與算法特征可歸納為如下的引理[13]。這些引理的結(jié)論對(duì)于理解本文的姿態(tài)估計(jì)算法的狀態(tài)特征向量的降維處理的有效性和可行性有關(guān)鍵作用。
引理1假設(shè)m≥n,設(shè)A為m×n矩陣,且n個(gè)列向量線性無(wú)關(guān),若A是列滿(mǎn)秩矩陣,即rank(A)=n,則存在矩陣分解式
式中:Q為m×m正交矩陣,R為m×n上三角矩陣,且除去相差一個(gè)對(duì)角元素的模全為1 的對(duì)角矩陣因子D外,分解式(8)是唯一的。其中
備注1引理1 的結(jié)論說(shuō)明矩陣A的QR 分解若存在的話具有唯一性,該唯一性正好對(duì)應(yīng)了增益矩陣特征(具有代表性)的唯一性?;谶@樣的理解,將(6)式定義的Kalman 濾波算法的增益矩陣Kk進(jìn)行QR 分解就可以有效地將增益矩陣的主要特征要素提取出來(lái),形成低維的參數(shù)向量,使得特征信息的提取可以在低維向量空間中完成,從而大大簡(jiǎn)化運(yùn)算關(guān)系,降低運(yùn)算量。這對(duì)于提高相應(yīng)的載體姿態(tài)估計(jì)算法的效率無(wú)疑是有用的。
更具體地,對(duì)式(6)中的濾波增益矩陣Kk進(jìn)行QR 分解,將Kk分解成Q15×15和R15×62 個(gè)矩陣的乘積,具體形式如式(9)所示:
這里,Q15×15和R15×6的定義方式是顯然的。
提取R15×6陣的上三角子矩陣中的所有非零元素行向量進(jìn)行列堆疊用于構(gòu)建神經(jīng)網(wǎng)絡(luò)的輸入特征向量Xin=[r1,1,r1,2,r1,3,…,r6,6]T,由式(9)可知,該特征向量?jī)H有21 個(gè)元素,而非原來(lái)的90 個(gè)。因此,利用QR 分解提取增益矩陣的主要特征,可以顯著減小神經(jīng)網(wǎng)絡(luò)輸入特征向量的維數(shù),在不影響算法性能的前提下,有效提高BP 神經(jīng)網(wǎng)絡(luò)的訓(xùn)練速度和組合導(dǎo)航算法的計(jì)算效率。
備注2由式(6)的第二式可知
為此,接下來(lái)證明Pk/k-1是可逆的。上文已經(jīng)說(shuō)明必正定,所以至少是半正定的;而Pk-1是我們通過(guò)先驗(yàn)給出初始值P1后逐次迭代產(chǎn)生的,所以,只要Pk-1是正定的(P1總可以給定為正定的),結(jié)合狀態(tài)轉(zhuǎn)移矩陣Φk/k-1的非奇異性,可以得出一定是正定的。最后,根據(jù)如下引理可知,Pk/k-1必為可逆矩陣。
引理2如果A和B是正定矩陣,則對(duì)任何不同時(shí)為零的向量x和y,則有xTAx+yTBy>0。
以上備注2 的討論表明,在一定條件下Kalman濾波的增益矩陣的QR 分解處理總是存在的。
采用基于QR 分解Kalman 濾波的增益矩陣構(gòu)建神經(jīng)網(wǎng)絡(luò)特征向量的方法,可有效地減小輸入特征向量的維數(shù),進(jìn)而提高網(wǎng)絡(luò)訓(xùn)練速度。BP 神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)圖如圖2 所示。
圖2 QR 分解后的BP 神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)
BP 網(wǎng)絡(luò)輸出量的物理含義為姿態(tài)估計(jì)誤差,即:
在完成網(wǎng)絡(luò)訓(xùn)練后,即可利用BP 神經(jīng)網(wǎng)絡(luò)的輸出信息來(lái)輔助修正組合導(dǎo)航的濾波估計(jì)結(jié)果,整個(gè)系統(tǒng)構(gòu)成如圖3 和圖4 所示。
圖3 基于神經(jīng)網(wǎng)絡(luò)的Kalman 濾波算法訓(xùn)練流程圖
圖4 使用BP 神經(jīng)網(wǎng)絡(luò)修正濾波結(jié)果的流程圖
備注3SINS 可以輸出載體系統(tǒng)的位置、速率、姿態(tài),GPS 可以輸出系統(tǒng)的位置、速率,但無(wú)法得到姿態(tài)。Kalman 濾波器的觀測(cè)方程是依靠SINS 和GPS 的位置、速率差構(gòu)建的(見(jiàn)式(1)的Z),SINS 和GPS 信息就是由此來(lái)混合的。SINS 的姿態(tài)輸出信息則體現(xiàn)在式(1)的F陣?yán)锩?這些信息要一并輸入才能計(jì)算Kalman 濾波方程。濾波結(jié)果是SINS 導(dǎo)航的解算誤差,利用這些誤差就可以實(shí)現(xiàn)補(bǔ)償SINS解算的設(shè)計(jì)要求。
在常規(guī)Kalman 濾波算法的基礎(chǔ)上,加入BP 神經(jīng)網(wǎng)絡(luò)輔助修正環(huán)節(jié)具體就是,首先對(duì)濾波增益矩陣Kk進(jìn)行QR 分解,然后提取R陣中非零元素構(gòu)建BP 網(wǎng)絡(luò)的輸入特征向量。最后利用網(wǎng)絡(luò)的輸出結(jié)果輔助修正濾波估計(jì)值,即可以進(jìn)一步提高組合導(dǎo)航系統(tǒng)的姿態(tài)估計(jì)精度。
為驗(yàn)證BP 神經(jīng)網(wǎng)絡(luò)輔助的SINS/GPS 組合導(dǎo)航算法的有效性,通過(guò)2 組仿真實(shí)驗(yàn)進(jìn)行驗(yàn)算。對(duì)比不同工況下QR 分解構(gòu)建特征向量的BP 神經(jīng)網(wǎng)絡(luò)輔助的Kalman 濾波算法(Back Propagation based on QR Kalman Filtering,BP-QRKF)與常規(guī)Kalman濾波算法(Kalman Filtering,KF)的估計(jì)精度。
具體地,輸入特征向量為21 維,對(duì)應(yīng)(9)式中R陣的21 個(gè)非零元素。設(shè)置隱含層神經(jīng)元個(gè)數(shù)為24個(gè),采用Sigmoid 激活函數(shù);輸出層神經(jīng)元個(gè)數(shù)為3個(gè),采用Purelin 函數(shù)線性輸出。
實(shí)驗(yàn)1 主要驗(yàn)證QR 分解的神經(jīng)網(wǎng)絡(luò)特征向量構(gòu)建方法的可行性。驗(yàn)證QR 分解法能否在不影響導(dǎo)航精度的情況下有效縮減輸入特征向量的維度,提高算法的效率。仿真時(shí)間設(shè)置2 500 s,載體運(yùn)動(dòng)狀態(tài)為靜止,SINS 和GPS 精度如表1 所示。
表1 仿真器件參數(shù)
首先直接利用濾波增益矩陣Kk(不采用QR 分解法)構(gòu)建特征向量,此時(shí)網(wǎng)絡(luò)輸入為90 維,仿真結(jié)果如圖5 所示。
圖5 BP-QRKF 算法(未采用QR 分解)與常規(guī)KF 算法濾波精度對(duì)比
為了直觀起見(jiàn),將方位角誤差曲線進(jìn)行放大,結(jié)果如圖6 所示。
圖6 方位角誤差放大圖
雖然此時(shí)BP-QRKF 算法精度優(yōu)于常規(guī)KF 算法,但由于未對(duì)Kk精簡(jiǎn),導(dǎo)致BP 網(wǎng)絡(luò)的輸入特征向量維度過(guò)高,計(jì)算耗時(shí)過(guò)大。BP 神經(jīng)網(wǎng)絡(luò)單次訓(xùn)練平均耗時(shí)及組合導(dǎo)航濾波估計(jì)精度如表2 所示。
表2 未采用QR 分解法仿真結(jié)果統(tǒng)計(jì)
若利用QR 分解Kk,提取R陣中非零元素作為輸入特征向量,仿真結(jié)果如圖7 所示。
圖7 BP-QRKF 算法(采用QR 分解)與常規(guī)KF 算法濾波精度對(duì)比
BP 神經(jīng)網(wǎng)絡(luò)單次訓(xùn)練平均耗時(shí)及組合導(dǎo)航濾波估計(jì)精度如表3 所示。
表3 采用QR 分解法仿真結(jié)果統(tǒng)計(jì)
對(duì)比圖5 與圖7 和表2 與表3 可得出以下結(jié)論:
(1)無(wú)論是否采用QR 分解,采用神經(jīng)網(wǎng)絡(luò)補(bǔ)償后的KF 算法的姿態(tài)估計(jì)精度均優(yōu)于常規(guī)KF 算法。
(2)采用QR 分解后,將BP 網(wǎng)絡(luò)輸入向量由90維減少到21 維,網(wǎng)絡(luò)規(guī)模顯著減小,單次訓(xùn)練時(shí)間從20 s 縮短到0.67 s,提高了2 個(gè)量級(jí)。
(3)采用QR 分解對(duì)Kk進(jìn)行特征變量的精簡(jiǎn),濾波估計(jì)精度并沒(méi)有下降,這說(shuō)明QR 分解法可有效保留濾波增益矩陣的特征。
用3.1 訓(xùn)練完成的神經(jīng)網(wǎng)絡(luò)進(jìn)行實(shí)際靜態(tài)姿態(tài)估計(jì),時(shí)長(zhǎng)設(shè)置為150 s,用于測(cè)試算法的魯棒性。
圖8 BP-QRKF 算法與常規(guī)KF 算法濾波精度對(duì)比
為了直觀起見(jiàn),對(duì)方位角誤差圖進(jìn)行放大,結(jié)果如圖9 所示。
圖9 方位角誤差放大圖
實(shí)際靜態(tài)組合導(dǎo)航濾波估計(jì)精度如表4 所示。
表4 仿真結(jié)果統(tǒng)計(jì)
可以看出,靜態(tài)姿態(tài)估計(jì)精度提高了2 個(gè)數(shù)量級(jí)(3.1 節(jié)中的算例實(shí)際為4 個(gè)數(shù)量級(jí))。這是由于GPS 信號(hào)是每隔一段時(shí)間才會(huì)發(fā)送,算法一開(kāi)始接收不到量測(cè)信息,導(dǎo)致無(wú)法計(jì)算濾波增益矩陣Kk,所以BP-QRKF 算法一開(kāi)始無(wú)法對(duì)姿態(tài)估計(jì)進(jìn)行補(bǔ)償。當(dāng)有了GPS 信號(hào)后,提案方法就可以很好地補(bǔ)償KF 的姿態(tài)估計(jì)結(jié)果。
BP 神經(jīng)網(wǎng)絡(luò)輔助的SINS/GPS 組合導(dǎo)航方法,利用QR 分解法構(gòu)建神經(jīng)網(wǎng)絡(luò)特征向量,輔助修正Kalman 濾波的姿態(tài)估計(jì)結(jié)果。經(jīng)過(guò)仿真驗(yàn)證,相較于傳統(tǒng)方法,神經(jīng)網(wǎng)絡(luò)輔助的組合導(dǎo)航系統(tǒng)可以達(dá)到更高精度,對(duì)未來(lái)組合導(dǎo)航技術(shù)發(fā)展具有明確的參考價(jià)值。
需要指出的是,濾波增益矩陣Kk的更新依賴(lài)于GPS 的信息,如今GPS 可以以較高頻率輸出信息,然而算法由于沒(méi)有考慮動(dòng)態(tài)參數(shù),故而對(duì)動(dòng)態(tài)實(shí)時(shí)姿態(tài)估計(jì)的補(bǔ)償效果較差。今后的研究工作可以考慮把更新頻率較快的慣性測(cè)量元件(陀螺和加速度計(jì))的輸出信息加入輸入特征向量,探討動(dòng)態(tài)姿態(tài)估計(jì)的高速補(bǔ)償算法的開(kāi)發(fā)問(wèn)題。