王秋瀅,尹 娟

(哈爾濱工程大學(xué) 信息與通信工程學(xué)院,黑龍江 哈爾濱,150001)

0 引言

協(xié)同導(dǎo)航是指通過(guò)集群航行器之間導(dǎo)航信息的共享,使單航行器的導(dǎo)航誤差得到校正的導(dǎo)航方式。在協(xié)同導(dǎo)航中,只需要給少量航行器配備高精度傳感器,即可達(dá)到較好的導(dǎo)航精度。因此,協(xié)同導(dǎo)航不僅可以提高系統(tǒng)整體的導(dǎo)航性能,并且大大節(jié)約了成本[1],已廣泛應(yīng)用于無(wú)人水下航行器(unmanned undersea vehicle,UUV)集群中。

由于在水下無(wú)法有效獲取高精度的全球定位系統(tǒng)(global positioning system,GPS)衛(wèi)星信號(hào)進(jìn)行導(dǎo)航定位,慣性導(dǎo)航、多普勒測(cè)速儀(Doppler velocity log,DVL)、長(zhǎng)基線導(dǎo)航與航位推算被廣泛使用于UUV的導(dǎo)航[2]。目前,航位推算是最常用的低成本導(dǎo)航解決方案,是指在已知初始位置的情況下,利用姿態(tài)傳感器提供的航向信息以及DVL提供的速度信息對(duì)UUV的位置信息進(jìn)行遞推計(jì)算[2-6]。三軸地磁傳感器與兩軸微機(jī)電(microelectromechanical system,MEMS)陀螺儀組合姿態(tài)傳感器具有功耗低、成本少、自主性強(qiáng)等優(yōu)點(diǎn),已經(jīng)越來(lái)越多地應(yīng)用于從UUV導(dǎo)航中。

由于航位推算的誤差受導(dǎo)航傳感器精度影響較大,當(dāng)導(dǎo)航傳感器精度不高時(shí),往往需要對(duì)航位推算的誤差及時(shí)校正。協(xié)同導(dǎo)航是最常使用的UUV航位推算誤差校正方法。目前最主要的UUV協(xié)同導(dǎo)航方式為單領(lǐng)航艇主從式協(xié)同導(dǎo)航,在此方式中存在單個(gè)配備高精度導(dǎo)航系統(tǒng)的主UUV,多個(gè)配備低精度導(dǎo)航系統(tǒng)的從 UUV。從UUV利用主UUV發(fā)送的自身高精度導(dǎo)航信息以及主從UUV的相對(duì)距離對(duì)從UUV自身導(dǎo)航誤差進(jìn)行校正。

單領(lǐng)航艇主從式 UUV協(xié)同導(dǎo)航主要依賴UUV間的水聲通信,如果水聲通信中斷,協(xié)同導(dǎo)航將不可用。由于MEMS陀螺儀精度較差,陀螺漂移大[7],嚴(yán)重制約了從UUV自身導(dǎo)航精度,此時(shí)若從 UUV上浮接受 GPS信號(hào)校正自身誤差,則增加了從UUV能量消耗,減少了從UUV工作時(shí)間。因此,必須對(duì)MEMS陀螺儀的陀螺漂移進(jìn)行辨識(shí)補(bǔ)償,進(jìn)而提高從UUV的導(dǎo)航精度,以達(dá)到在水聲通信短時(shí)失效時(shí),從 UUV導(dǎo)航精度仍能滿足自身需求的目的。

目前,對(duì)于協(xié)同標(biāo)定方面研究較少,文獻(xiàn)[8]對(duì)協(xié)同導(dǎo)航下的 MEMS陀螺儀漂移標(biāo)定進(jìn)行了研究,但文獻(xiàn)中僅對(duì)單軸陀螺進(jìn)行標(biāo)定,并且沒(méi)有對(duì)協(xié)同標(biāo)定路徑設(shè)計(jì)進(jìn)行分析。

針對(duì)上述問(wèn)題,文中利用基于擴(kuò)展卡爾曼濾波(extended Kalman filter,EKF)的協(xié)同導(dǎo)航結(jié)果作為標(biāo)定觀測(cè)量,在協(xié)同導(dǎo)航正常運(yùn)行時(shí)在線解算出兩軸 MEMS陀螺的陀螺漂移,提高從 UUV自身導(dǎo)航精度,以達(dá)到在協(xié)同導(dǎo)航短時(shí)失效時(shí),從 UUV導(dǎo)航精度能夠滿足自身需求的目的。通過(guò)可觀測(cè)性分析,確定協(xié)同標(biāo)定的路徑設(shè)計(jì)原則,并根據(jù)該原則設(shè)計(jì) 3組協(xié)同標(biāo)定路徑,通過(guò)濾波算法在線標(biāo)定 MEMS陀螺儀的陀螺漂移并補(bǔ)償。仿真結(jié)果驗(yàn)證了該方法可以快速有效地解算MEMS陀螺漂移,從而提高從UUV自身導(dǎo)航精度。

1 UUV集群協(xié)同標(biāo)定模型及算法

1.1 UUV集群協(xié)同導(dǎo)航模型及算法

1.1.1 協(xié)同導(dǎo)航模型

UUV的運(yùn)動(dòng)可分為相對(duì)獨(dú)立的水平面運(yùn)動(dòng)和垂直面運(yùn)動(dòng),因?yàn)樯疃瓤捎蓧毫鞲衅髦苯訙y(cè)得,因此 UUV導(dǎo)航定位問(wèn)題的研究可由三維空間轉(zhuǎn)化至二維平面內(nèi)。最常解決二維平面內(nèi)導(dǎo)航定位問(wèn)題的導(dǎo)航方式為航位推算[9]。在已知初始位置的情況下,利用三軸地磁傳感器與兩軸微機(jī)電 MEMS陀螺儀組合姿態(tài)傳感器提供的載體航向信息以及 DVL等速度傳感器提供的載體速度信息對(duì)載體的位置信息進(jìn)行遞推計(jì)算,進(jìn)而實(shí)現(xiàn)水下航行器的導(dǎo)航定位。

地磁傳感器三軸分別指向東北天,MEMS陀螺儀兩軸分別指向載體的ox、oz軸。地磁傳感器無(wú)法獨(dú)立解算3個(gè)姿態(tài)角,需要已知1個(gè)姿態(tài)角才能解算另外2個(gè)姿態(tài)角[10]。因此,文中由兩軸陀螺儀解算航向角提供給三軸地磁傳感器解算橫滾角和俯仰角,地磁傳感器解算的橫滾角與俯仰角再提供給陀螺儀解算下一時(shí)刻的航向角。

將 UUV的運(yùn)動(dòng)視為點(diǎn)運(yùn)動(dòng)體,即其運(yùn)動(dòng)軌跡可在東北向坐標(biāo)系下進(jìn)行描述。若已知 UUV初始時(shí)刻的位置 (px,1,py,1),則k時(shí)刻 UUV的位置可通過(guò)航位推算模型表示為

式中:Δt為采樣時(shí)間間隔;Vk-1為 DVL測(cè)量的k-1時(shí)刻的UUV速度信息;φk,φk-1分別為k,k- 1時(shí)刻 UUV 的航向角信息;γk-1為k-1時(shí)刻UUV 的橫滾角信息;θk-1為k-1時(shí)刻 UUV 的俯仰角信息;為z軸MEMS陀螺儀測(cè)量的k-1時(shí)刻航向角角速度信息;為z軸MEMS陀螺儀測(cè)量的k-1時(shí)刻航向角角速度信息。

航位推算模型可簡(jiǎn)單描述成

系統(tǒng)噪聲協(xié)方差矩陣

雖然航位推算應(yīng)用簡(jiǎn)單、成本低廉,但是航位推算誤差會(huì)隨時(shí)間累積,導(dǎo)航效果取決于傳感器的量測(cè)精度。配備低精度傳感器的從 UUV由于傳感器精度有限,導(dǎo)致航位推算精度較低,航位推算誤差的時(shí)間累積性比較明顯[11-12]。為抑制航位推算誤差的不斷積累,考慮以主、從UUV相對(duì)距離為觀測(cè)量,通過(guò)協(xié)同導(dǎo)航算法,修正從UUV的位置誤差和航向角誤差,再將修正后的航向角提供給地磁傳感器解算橫滾角和俯仰角。

根據(jù)k時(shí)刻接收到的主 UUV參考位置信息以及從 UUV自身位置狀態(tài)得到相應(yīng)的距離觀測(cè)方程。

1.1.2 協(xié)同導(dǎo)航算法

由于協(xié)同導(dǎo)航系統(tǒng)的非線性特性,采用 EKF進(jìn)行協(xié)同導(dǎo)航算法設(shè)計(jì)。根據(jù)上一節(jié)的協(xié)同導(dǎo)航模型,可得系統(tǒng)的濾波方程

從UUV在k時(shí)刻的狀態(tài)估計(jì)值

系統(tǒng)一步預(yù)測(cè)協(xié)方差為

式中,Φk-1為狀態(tài)轉(zhuǎn)移矩陣,且

濾波增益為

狀態(tài)更新為

協(xié)方差更新為

因此,只需給出從 UUV的初始狀態(tài)量和初始化濾波方差,就可根據(jù)式(6)～式(12)遞推估計(jì)從UUV每一時(shí)刻的位姿信息[13]。

1.2 在線標(biāo)定模型及算法

1.2.1 在線標(biāo)定模型

標(biāo)定是一種針對(duì) MEMS陀螺儀的有效誤差補(bǔ)償方法。傳統(tǒng)的離線標(biāo)定工作量太大并且需要巨大的成本投入,因此在線標(biāo)定是減小MEMS陀螺儀陀螺漂移的首要選擇。文中借助協(xié)同導(dǎo)航的結(jié)果,以協(xié)同導(dǎo)航校正后的航向角與未經(jīng)校正的航向角差值以及地磁傳感器輸出的俯仰角與陀螺解算的俯仰角差值為觀測(cè)量,通過(guò)濾波算法對(duì)兩軸MEMS陀螺儀的陀螺漂移進(jìn)行在線解算。

陀螺漂移是 MEMS陀螺儀最主要的確定性誤差,其是慣性傳感器輸入為零時(shí)的輸出。MEMS陀螺的陀螺漂移也稱作零位漂移,單位常用(°)/h表示,陀螺漂移常記作ε。在水聲通信短時(shí)失效時(shí),陀螺漂移可認(rèn)為是常值誤差。

通過(guò)兩軸 MEMS陀螺儀可解算航向角與俯仰角

MEMS陀螺儀在線標(biāo)定狀態(tài)模型為

在線標(biāo)定模型可簡(jiǎn)單描述成

在線標(biāo)定觀測(cè)方程為

式中,vk為觀測(cè)噪聲矩陣。

1.2.2 在線標(biāo)定算法

從UUV的MEMS陀螺儀在線標(biāo)定模型為線性模型,文中選擇卡爾曼濾波(Kalman filter,KF)算法進(jìn)行解算。根據(jù)上一節(jié)的在線標(biāo)定模型,可以得到系統(tǒng)的濾波方程為

狀態(tài)一步估計(jì)為

一步預(yù)測(cè)協(xié)方差陣為

濾波增益矩陣為

狀態(tài)更新為

協(xié)方差矩陣更新為

與EKF相同,只需給出初始狀態(tài)量和初始化濾波方差,就能夠遞推估計(jì)每一時(shí)刻的狀態(tài)量。

2 協(xié)同標(biāo)定可觀測(cè)性分析及路徑設(shè)計(jì)

卡爾曼最早提出可觀測(cè)性的概念:在一個(gè)線性系統(tǒng)中,若系統(tǒng)的觀測(cè)信息唯一地確定系統(tǒng)所有狀態(tài)量,則稱該系統(tǒng)是可觀測(cè)的。系統(tǒng)的可觀測(cè)性反映了觀測(cè)信息對(duì)于系統(tǒng)狀態(tài)信息的解算能力。對(duì)于UUV協(xié)同標(biāo)定來(lái)說(shuō),觀測(cè)信息有限、觀測(cè)能力較弱,因此,在協(xié)同標(biāo)定之前有必要對(duì)系統(tǒng)的可觀測(cè)性進(jìn)行分析,明確系統(tǒng)的可觀測(cè)條件,設(shè)計(jì)協(xié)同標(biāo)定路徑。

以如下所示的線性時(shí)變離散系統(tǒng)為例,說(shuō)明系統(tǒng)可觀測(cè)條件。

式中:x(k)為n維狀態(tài)向量;y(k)為m維輸出向量。

對(duì)于式(23)所示的線性時(shí)變離散系統(tǒng)完全可觀測(cè)的充分必要條件為

式中,N為系統(tǒng)的可觀測(cè)性矩陣。

協(xié)同導(dǎo)航觀測(cè)信息是一維距離量測(cè)量,因此至少需要進(jìn)行3次觀測(cè)才能獲得系統(tǒng)的三維狀態(tài)量唯一解。當(dāng)進(jìn)行3次距離觀測(cè)時(shí)的系統(tǒng)可觀測(cè)矩陣為

已知k、k+1、k+2時(shí)刻主UUV的位置為和以及主從UUV之間的距離觀測(cè)量和則系統(tǒng)可觀測(cè)矩陣進(jìn)一步表示為

由于第 3列的第 1個(gè)元素為0,故從可觀測(cè)矩陣可得:當(dāng)?shù)?1列或第 2列為 0時(shí),即相鄰 3個(gè)量測(cè)時(shí)刻主從 UUV的x軸或者y軸坐標(biāo)是相同的,系統(tǒng)不可觀測(cè);或當(dāng)?shù)?1列或第 2列線性相關(guān)時(shí),矩陣不滿秩,即

式中,θk為k時(shí)刻距離觀測(cè)方位角。

從UUV的MEMS陀螺儀在線標(biāo)定模型可以看出,當(dāng)UUV的俯仰角與橫滾角均為小量時(shí),航向角誤差可近似看做z軸MEMS陀螺儀漂移的累積,俯仰角誤差可近似看做x軸MEMS陀螺儀漂移的累積,即

此時(shí)的MEMS陀螺儀在線標(biāo)定可觀測(cè)矩陣

由于 UUV正常定深航行下,其俯仰角與橫滾角僅有小幅度擺動(dòng),因此最簡(jiǎn)單有效的標(biāo)定路徑為從UUV定深定向航行。

綜上所述,協(xié)同標(biāo)定路徑設(shè)計(jì)需要遵循以下原則:1)相鄰3個(gè)量測(cè)時(shí)間的主從UUV距離觀測(cè)方位角不同且從UUV定深定向航行。

3 仿真驗(yàn)證與分析

為了驗(yàn)證協(xié)同標(biāo)定的有效性,利用MATLAB進(jìn)行仿真驗(yàn)證。設(shè)定兩軸MEMS陀螺儀陀螺漂移為10°/h;角度隨機(jī)游走為 1°/h;從 UUV在載體系三軸上有搖擺的角速度,k時(shí)刻分別為

為了驗(yàn)證協(xié)同標(biāo)定路徑設(shè)計(jì)原則的有效性,設(shè)計(jì)3種路徑,對(duì)兩軸MEMES陀螺儀陀螺漂移進(jìn)行解算,路徑設(shè)計(jì)如下。

路徑 1:主 UUV 初始位置(0,125),以速度2m/s沿固定航向角0°定深航行,從UUV初始位置(0,0),以速度3 m/s沿固定航向角10°定深航行;

路徑 2:主 UUV 初始位置(0,125),以速度1m/s沿固定航向角0°定深航行,從UUV初始位置(0,0),以速度2 m/s沿固定航向角10°定深航行;

路徑3:主UUV初始位置為(0,90),以速度1 m/s沿固定航向角0°方向定深航行;從UUV初始位置(0,0),以速度2 m/s沿固定航向角-5°定深航行。

3.1 路徑1仿真

協(xié)同導(dǎo)航EKF濾波器初始設(shè)置如下[14-15]

陀螺零偏標(biāo)定KF濾波器初始設(shè)置如下

主、從UUV軌跡如圖1所示。協(xié)同標(biāo)定仿真結(jié)果如圖 2所示。由圖2可以看出,在路徑 1下,1 min內(nèi)濾波估計(jì)值趨于收斂,兩軸MEMS陀螺漂移標(biāo)定值為10°/h。

圖1 路徑1下主、從UUV軌跡Fig.1 Tracks of master and slave UUVs in path 1 test

圖2 路徑1下微機(jī)電陀螺儀陀螺漂移估計(jì)結(jié)果Fig.2 Estimation results of gyro drift of micro-electro mechanical system(MEMS)gyroscope in path 1 test

圖3為路徑1下從UUV航行10 min時(shí)協(xié)同標(biāo)定補(bǔ)償結(jié)果圖,可以看出沒(méi)有進(jìn)行標(biāo)定補(bǔ)償?shù)亩ㄎ徽`差在10 min后達(dá)30 m,而補(bǔ)償后的軌跡與真實(shí)軌跡幾乎完全重合。由局部放大圖知,從 UUV補(bǔ)償后定位誤差不到1 m,導(dǎo)航精度得到極大提高。

3.2 路徑2仿真

協(xié)同導(dǎo)航EKF濾波器初始設(shè)置如下

圖3 路徑1下協(xié)同標(biāo)定補(bǔ)償結(jié)果Fig.3 Results of collaborative calibration compensation in path 1 test

陀螺零偏標(biāo)定KF濾波器初始設(shè)置如下

主、從UUV軌跡如圖4所示。

圖4 路徑2下主、從UUV軌跡Fig.4 Tracks of master and slave UUVs in path 2 test

協(xié)同標(biāo)定仿真結(jié)果如圖5所示。路徑2下的協(xié)同標(biāo)定補(bǔ)償結(jié)果如圖6所示。

圖5 路徑2下MEMS陀螺儀陀螺漂移估計(jì)結(jié)果Fig.5 Estimation results of MEMS gyroscope gyro drift in path 2 test

圖6 路徑2下協(xié)同標(biāo)定補(bǔ)償結(jié)果Fig.6 Results of collaborative calibration compensation in path 2 test

由圖4～圖6可以看出,在路徑2下濾波估計(jì)值1 min內(nèi)趨于收斂,MEMS陀螺零偏誤差標(biāo)定值為10°/h,標(biāo)定補(bǔ)償后從UUV導(dǎo)航精度得到極大提高。

3.3 路徑3仿真

協(xié)同導(dǎo)航EKF濾波器初始設(shè)置如下

陀螺零偏標(biāo)定KF濾波器初始設(shè)置如下

主、從UUV軌跡如圖7所示。協(xié)同標(biāo)定結(jié)果如圖8所示。路徑3下的協(xié)同標(biāo)定補(bǔ)償結(jié)果如圖9所示。

圖7 路徑3下主、從UUV軌跡Fig.7 Tracks of master and slave UUVs in path 3 test

圖8 路徑3下MEMS陀螺陀螺儀漂移估計(jì)結(jié)果Fig.8 Estimation results of MEMS gyroscope gyro drift in path 3 test

圖9 路徑3下協(xié)同標(biāo)定補(bǔ)償結(jié)果Fig.9 Results of collaborative calibration compensation in path 3 test

由圖7～圖9可以看出,在路徑3下濾波估計(jì)值1 min內(nèi)趨于收斂,MEMS陀螺零偏誤差標(biāo)定值為10°/h,標(biāo)定補(bǔ)償后,從UUV導(dǎo)航精度得到極大提高。

綜上所述,在 3條標(biāo)定路徑下,在短時(shí)間內(nèi)完成對(duì)兩軸MEMS陀螺儀陀螺漂移的標(biāo)定,補(bǔ)償后的從 UUV導(dǎo)航誤差明顯減小。仿真結(jié)果驗(yàn)證了協(xié)同標(biāo)定的有效性以及協(xié)同標(biāo)定路徑設(shè)計(jì)原則的準(zhǔn)確性。

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

針對(duì) UUV協(xié)同導(dǎo)航水聲通信短時(shí)失效時(shí)協(xié)同導(dǎo)航不可用,且從 UUV配備的三軸地磁傳感器與兩軸微機(jī)電 MEMS陀螺儀組合姿態(tài)傳感器中MEMS陀螺的陀螺漂移大,導(dǎo)致UUV航位推算精度差的問(wèn)題,文中提出了 UUV協(xié)同導(dǎo)航下兩軸 MEMS陀螺儀協(xié)同標(biāo)定方法并推導(dǎo)了協(xié)同標(biāo)定路徑設(shè)計(jì)原則。利用基于EKF的協(xié)同導(dǎo)航結(jié)果作為標(biāo)定觀測(cè)量,在協(xié)同標(biāo)定路徑下在線解算MEMS陀螺儀漂移并補(bǔ)償,提高了從 UUV自身導(dǎo)航精度。當(dāng)主、從 UUV通信短時(shí)中斷時(shí),從UUV不需要上浮接受GPS信號(hào)校正自身導(dǎo)航誤差,降低了從UUV能量消耗,增長(zhǎng)了從UUV工作時(shí)間。仿真結(jié)果表明,文中提出的算法能夠快速有效估計(jì)出 MEMS陀螺儀漂移,極大提高從UUV導(dǎo)航精度?？紤]時(shí)間延遲等誤差的從UUV協(xié)同標(biāo)定將是下一步研究的重點(diǎn)。

相關(guān)文章導(dǎo)航

1.鐘宏偉,李國(guó)良,宋林樺,等.國(guó)外大型無(wú)人水下航行器發(fā)展綜述[J].水下無(wú)人系統(tǒng)學(xué)報(bào),2018,26(4).

2.嚴(yán)浙平,宋金雪.模型輔助和洋流估計(jì)下的UUV導(dǎo)航定位方法[J].水下無(wú)人系統(tǒng)學(xué)報(bào),2017,25(5).

3.鐘宏偉.國(guó)外無(wú)人水下航行器裝備與技術(shù)現(xiàn)狀及展望[J].水下無(wú)人系統(tǒng)學(xué)報(bào),2017,25(4).

4.錢東,趙江,楊蕓.軍用 UUV 發(fā)展方向與趨勢(shì)(上)——美軍用無(wú)人系統(tǒng)發(fā)展規(guī)劃分析解讀[J].水下無(wú)人系統(tǒng)學(xué)報(bào),2017,25(2).

5.劉明雍,沈俊元,張加全,等.一種基于無(wú)跡卡爾曼濾波的UUV協(xié)同定位方法[J].魚(yú)雷技術(shù),2011,19(3).