一種面向無人水下航行器的多普勒自主導(dǎo)航方法

劉沛佳 ,秦麗萍 ,李廣華 ,侯冬冬 ,朱政宇

(1.鄭州大學(xué) 電氣與信息工程學(xué)院,河南 鄭州,450001;2.中國船舶集團(tuán)有限公司 第713 研究所,河南 鄭州,450015;3.河南省水下智能裝備重點實驗室,河南 鄭州,450015)

0 引言

隨著世界各國開始大規(guī)模地探索認(rèn)知海洋與開發(fā)利用海洋,無人水下航行器(unmanned undersea vehicle,UUV)得到快速發(fā)展和廣泛應(yīng)用[1-2]。在UUV 執(zhí)行任務(wù)的過程中,水下導(dǎo)航技術(shù)不可或缺[3]。導(dǎo)航系統(tǒng)為UUV 提供實時的姿態(tài)、速度和位置參數(shù),是UUV 順利完成任務(wù)的前提和保障[4]。現(xiàn)階段,全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system,GNSS)是實現(xiàn)高精度陸地導(dǎo)航和空中導(dǎo)航的核心系統(tǒng)[5]。然而,無線電信號在水中迅速衰減,導(dǎo)致GNSS 無法在水中應(yīng)用[6]。此外,相比陸地導(dǎo)航和空中導(dǎo)航中相同類型的傳感器,水下導(dǎo)航傳感器一般性能較低而價格卻較為昂貴。因此,水下環(huán)境的特殊性給水下導(dǎo)航帶來挑戰(zhàn)[7]。

目前,UUV 一般搭載慣性基組合導(dǎo)航系統(tǒng),利用超短基線(ultra short base-line,USBL)定位系統(tǒng)、多普勒計程儀(Doppler velocity log,DVL)等傳感器輔助慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)進(jìn)行組合導(dǎo)航。特別地,USBL 需要在水底或水面船只上布設(shè)輔助基站。由于基站覆蓋范圍有限,INS/USBL 組合導(dǎo)航系統(tǒng)在軍事作戰(zhàn)和遠(yuǎn)航科研任務(wù)中的應(yīng)用會受到限制。DVL 利用自身所發(fā)射聲信號的多普勒頻移解算UUV 速度,可用于自主導(dǎo)航[8-10]。INS/DVL 組合導(dǎo)航系統(tǒng)無需先驗地圖和外部輔助基站,具有隱蔽性和完全自主性,在UUV 特別是自主水下航行器(autonomous undersea vehicle,AUV)上得到廣泛應(yīng)用[11-13]。

然而,INS 和DVL 的價格均較為昂貴,中小型AUV 成本可能難以支撐INS/DVL 組合導(dǎo)航系統(tǒng)。鑒于此,文中設(shè)計由DVL、深度計(pressure sensor,PS)、電子羅盤(electronic compass,ECP)和微機(jī)電系統(tǒng)(micro-electro-mechanical system,MEMS)慣性量測單元(inertial measurement unit,IMU)組成的航位推算導(dǎo)航系統(tǒng),提出一種DVL 自主導(dǎo)航方法。相比于INS/DVL 組合導(dǎo)航,DVL 自主導(dǎo)航利用低成本姿態(tài)傳感器代替INS 以降低導(dǎo)航系統(tǒng)的成本。原理上,DVL 自主導(dǎo)航系統(tǒng)與INS/DVL 組合導(dǎo)航系統(tǒng)都缺少直接的位置量測,位置參數(shù)通過航位推算獲取,定位誤差隨航程增加而積累[14-15]。為實現(xiàn)高精度自主導(dǎo)航,需盡可能提高用于航位推算的速度和姿態(tài)精度,抑制定位誤差積累速度[16]。現(xiàn)有研究針對INS/DVL 組合導(dǎo)航已提出多種誤差修正方法,通過修正DVL 和INS 誤差提高INS/DVL 組合導(dǎo)航定位精度。文獻(xiàn)[17]將DVL 安裝誤差的標(biāo)定問題轉(zhuǎn)化為求解2 個三維點列之間變換矩陣的問題,所需點列為同一導(dǎo)航參數(shù)在2 個不同坐標(biāo)系內(nèi)的量測。例如,DVL 量測得到的AUV速度和GNSS/INS 組合導(dǎo)航輸出的AUV 速度即構(gòu)成2 個三維點列。文獻(xiàn)[18]將DVL 的安裝誤差、偏置誤差和刻度因數(shù)誤差建模為卡爾曼濾波器的狀態(tài)變量,通過狀態(tài)估計實現(xiàn)DVL 多種類型誤差的標(biāo)定。特別地,誤差標(biāo)定一般需要精確的外參考系統(tǒng),上述標(biāo)定方法通常利用GNSS/INS 組合導(dǎo)航系統(tǒng)作為外參考。文獻(xiàn)[19]～[20]建立了由AUV 姿態(tài)高動態(tài)變化引入的DVL 測速誤差模型,基于INS 高頻姿態(tài)參數(shù)提出針對性誤差修正方法。文獻(xiàn)[21]提出一種基于卡爾曼濾波器的新型INS/DVL 組合導(dǎo)航初始對準(zhǔn)方法,可以同步補(bǔ)償INS 偏置、DVL 桿臂和安裝誤差角。然而,由于低成本姿態(tài)傳感器性能不及INS,INS/DVL組合導(dǎo)航誤差修正方法難以直接應(yīng)用于DVL 自主導(dǎo)航。鑒于此,文中提出的DVL 自主導(dǎo)航方法通過傳感器一體化集成克服安裝誤差和時間異步問題,采用ECP/IMU 融合方案獲取高頻姿態(tài)參數(shù),在此基礎(chǔ)上,制定DVL 異常數(shù)據(jù)處理、高動態(tài)誤差修正以及GNSS 輔助策略。所提出的方法在自主研發(fā)的“海為一號”AUV 上得到應(yīng)用,下文結(jié)合“海為一號”AUV 導(dǎo)航系統(tǒng)的研究和應(yīng)用進(jìn)行介紹。

1 DVL 自主導(dǎo)航系統(tǒng)組成與工作原理

1.1 系統(tǒng)組成

圖1 所示為 “海為一號”AUV 導(dǎo)航系統(tǒng)組成,包含導(dǎo)航計算機(jī)、DVL、PS、ECP、MEMS IMU、GNSS,可以擴(kuò)展捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system,SINS)。在 無SINS 配 置的條件下,“海為一號”AUV 采用DVL 自主導(dǎo)航方法。PS 和ECP 集成于DVL 殼體中,GNSS 在AUV 上浮到水面時獲取衛(wèi)星信號為DVL 自主導(dǎo)航系統(tǒng)提供位置修正。表1 所示為“海為一號”AUV 搭載的傳感器型號和參數(shù)。

表1 傳感器型號和參數(shù)Table 1 Sensor types and parameters

圖1 “海為一號”AUV 導(dǎo)航系統(tǒng)Fig.1 Navigation system of Haiwei 1 AUV

1.2 DVL 自主導(dǎo)航原理

1.2.1 坐標(biāo)系定義

導(dǎo)航坐標(biāo)系n: 以AUV 浮心O為原點,X n、Y n和Z n分別指向東向(E)、北向(N)和天向(U);

載體坐標(biāo)系b: 以AUV 浮心O為原點,X b沿AUV 橫軸指向右,Y b沿AUV 艏向指向前,Z b垂直于AUV 橫截面指向上;

DVL 儀器坐標(biāo)系d: 以DVL 浮心O d為原點,X d沿DVL 橫軸指向右,Y d沿DVL 艏向指向前,Z d垂直于DVL 橫截面指向上。

1.2.2 航位推算原理

DVL 通常固定安裝在AUV 底部,安裝過程中使d系和b系的3 個軸對應(yīng)平行。因此,可以認(rèn)為DVL 的量測是AUV 在b系內(nèi)的速度V b。工程中,四波束DVL 最為常用,它通過4 個換能器向水底發(fā)射4 條聲波波束,通過4 條波束的多普勒頻移解算AUV 的三維速度。文獻(xiàn)[22]介紹了四波束DVL 的工作原理。

在DVL 自主導(dǎo)航中,利用DVL 輸出的速度和ECP 輸出的姿態(tài)進(jìn)行航位推算。在單個推算周期中,AUV 在n系內(nèi)的位移變化矢量dsk=[dsEdsNdsU]T為

式中:tk-1和tk分別為第k-1 和第k個DVL 采樣時刻;V n為AUV 在n系內(nèi)的速度;Cnb為b系到n系的變換矩陣。

對應(yīng)地,在單個推算周期中AUV 位置變化矢量dpk=[dL dλ dh]T通過下式求得

式中,RM和RN分別為地球子午圈半徑和卯酉圈半徑。

AUV 在k時刻的位置矢量pk=[Lk λk hk]T可通過下式推算得到

式中,p0為初始時刻AUV 的位置矢量,下潛之前可以通過GNSS 獲得。

2 關(guān)鍵技術(shù)問題及解決方案

為提高DVL 自主導(dǎo)航精度,以下針對多種關(guān)鍵技術(shù)問題給出解決方案。

2.1 傳感器安裝誤差和時間異步處理

DVL 自主導(dǎo)航利用多傳感器量測進(jìn)行航位推算,傳感器的分立不可避免會帶來安裝誤差與時間異步的問題。針對該問題,采用傳感器一體化集成方案,如圖2 所示。DVL、ECP 和PS 集成于DVL 殼體中,固定安裝后利用原位標(biāo)定方法對安裝誤差進(jìn)行補(bǔ)償。此外,采用同一個數(shù)據(jù)處理器,不僅可以同步采集和輸出各自量測的導(dǎo)航參數(shù),還可以進(jìn)行前級數(shù)據(jù)融合。例如,將DVL 輸出的V b變換為V n。基于上述方案,通過一次原位標(biāo)定即可消除DVL 和ECP 間的安裝誤差,無需每次導(dǎo)航都進(jìn)行標(biāo)定,同時克服了時間異步問題。

圖2 傳感器一體化集成方案Fig.2 Integration scheme of the sensor

2.2 高頻姿態(tài)參數(shù)獲取

高動態(tài)運動控制和導(dǎo)航解算需要高頻姿態(tài)參數(shù)。ECP 與DVL 一體化集成后同步輸出數(shù)據(jù),其數(shù)據(jù)輸出頻率受DVL 解算周期的約束。具體地,聲信號在水中的傳播速度較低,它從被發(fā)射到被接收需要一定時間。圖3 所示為聲信號傳播過程示意圖。因此,DVL 解算周期應(yīng)大于聲信號在水中的傳播時間,可通過下式約束DVL 輸出頻率。

圖3 DVL 聲信號傳播過程Fig.3 DVL acoustic signal propagation process

式中:c為聲信號在水中的速度;D1和D2分別為聲信號發(fā)射和接收過程的傳播距離。

因此,DVL 難以實現(xiàn)高頻數(shù)據(jù)輸出。“海為一號”AUV 搭載的DVL 輸出頻率可設(shè)定為1～10 Hz(典型值1 Hz)。為了獲取高頻姿態(tài)參數(shù),在“海為一號”AUV 導(dǎo)航系統(tǒng)中增加一個低成本MEMS IMU(100 Hz)作為輔助姿態(tài)傳感器,提出一種ECP/IMU 姿態(tài)融合方案,技術(shù)路線如圖4 所示。以ECP(1 Hz)作為主姿態(tài)傳感器,其輸出作為基準(zhǔn)姿態(tài)參數(shù)。由于IMU 與DVL 不可避免存在安裝誤差,因此不直接使用IMU 輸出的姿態(tài)參數(shù),而是在ECP 輸出的姿態(tài)參數(shù)上疊加IMU 輸出的高頻姿態(tài)變化量,從而得到高頻姿態(tài)輸出

2.3 DVL 異常數(shù)據(jù)處理

2.3.1 DVL 盲區(qū)規(guī)避和流速補(bǔ)償

DVL 存在2 個異常的量測區(qū)域: 上層盲區(qū)和下層底跟蹤失鎖區(qū),如圖5 所示。DVL 盲區(qū)為換能器表面至其下方某一深度閾值的帶狀區(qū)域。根據(jù)DVL 產(chǎn)品型號、發(fā)射聲信號頻率以及軟件配置的不同,該閾值范圍一般為0.5～2.5 m。如果換能器到水底距離小于該閾值,會極大程度降低DVL的信噪比,導(dǎo)致DVL 測速誤差增大甚至輸出無效數(shù)據(jù),因此,應(yīng)避免DVL 在盲區(qū)工作。DVL 底跟蹤失鎖區(qū)是由于換能器到水底距離大于DVL 量程造成的。DVL 作為一種中層水域傳感器,存在量程約束。根據(jù)發(fā)射聲信號頻率的不同,DVL 的最大量程一般在20～300 m。當(dāng)換能器到水底距離大于DVL 量程時,DVL 波束無法到達(dá)水底,DVL的量測將不再是對地速度,而是對流速度。在這種情況下,應(yīng)對DVL 的量測進(jìn)行流速補(bǔ)償。

圖5 DVL 異常量測區(qū)域Fig.5 The DVL abnormal measurement zone

式中:e為地球坐標(biāo)系,與地球固聯(lián)并隨地球轉(zhuǎn)動;w為水流坐標(biāo)系,與水流固聯(lián)并隨水流運動;為n系到b系的變換矩陣。

式(6)經(jīng)變換可得到

2.3.2 DVL 野值剔除

DVL 受盲區(qū)和外部環(huán)境影響可能產(chǎn)生野值。野值與真實速度存在較大偏差,進(jìn)而在導(dǎo)航結(jié)果中引入定位誤差。由于航位推算工作機(jī)理,該誤差會存在于后續(xù)的DVL 自主導(dǎo)航結(jié)果中,因此,需要進(jìn)行野值剔除。RTI SeaPILOT 系列DVL 對于盲區(qū)內(nèi)的量測輸出無效數(shù)據(jù)“-999 99”，此類野值容易識別和剔除。然而,DVL 受環(huán)境因素影響產(chǎn)生的野值值域范圍大,具有隨機(jī)性和偶發(fā)性,此類野值難以直接識別和剔除。針對上述問題,采用基于趨勢預(yù)測的野值剔除策略,技術(shù)路線如圖6所示。

首先,建立滑動窗口,利用窗口時間內(nèi)的DVL量測進(jìn)行趨勢預(yù)測,閾值計算如下

式中:Vk為k時刻DVL 量測;N為滑動窗口寬度;ai為加權(quán)系數(shù);VTH為趨勢預(yù)測k+1 時刻的速度閾值。

進(jìn)而,利用VTH進(jìn)行野值判斷和剔除,有

具體地,如果|Vk+1|＞|VTH|,則Vk+1為野值,利用前N個時刻的速度量測加權(quán)平滑得到替代Vk+1進(jìn)行導(dǎo)航解算,di為 加權(quán)系數(shù)。

2.4 DVL 高動態(tài)誤差修正

DVL 從發(fā)射到接收聲信號有一定的時間間隔ΔT。DVL 在速度解算時一般假設(shè)AUV 速度在ΔT內(nèi)保持不變。然而,如果AUV 存在高動態(tài)姿態(tài)變化,會在DVL 速度解算中引入誤差。此外,DVL多數(shù)情況下并非安裝在AUV 的浮心,AUV 的高動態(tài)姿態(tài)變化會引起桿臂效應(yīng)。桿臂效應(yīng)產(chǎn)生的線速度并非AUV 的線運動引起的,但卻會疊加到DVL 的量測中引入測速誤差。針對INS/DVL 組合導(dǎo)航,將文獻(xiàn)[19]中已建立AUV 高動態(tài)姿態(tài)變化引入DVL 測速誤差模型并提出匹配的誤差修正方法。文獻(xiàn)[20]進(jìn)一步簡化了該方法的約束條件。在通過ECP/IMU 融合獲取高頻姿態(tài)參數(shù)的基礎(chǔ)上,將該方法拓展應(yīng)用于DVL 自主導(dǎo)航,技術(shù)路線如圖7 所示。

圖7 DVL 高動態(tài)誤差修正Fig.7 The correction of DVL high dynamic error

分析 ΔT內(nèi)姿態(tài)變化角對DVL 速度解算的影響,綜合考慮桿臂效應(yīng),推導(dǎo)AUV 姿態(tài)角高動態(tài)變化引入的DVL 誤差修正方程,推導(dǎo)過程參見文獻(xiàn)[20]。隨后,基于DVL 誤差修正方程和ECP/IMU 高頻姿態(tài)參數(shù)構(gòu)建DVL 高動態(tài)誤差修正模型。最后,利用修正后的DVL 速度和ECP/IMU 姿態(tài)進(jìn)行航位推算導(dǎo)航。

2.5 GNSS 輔助導(dǎo)航策略

當(dāng)AUV 上浮到水面或在近水面航行時,可以利用GNSS 位置對DVL 自主導(dǎo)航位置進(jìn)行修正。然而,如果在AUV 機(jī)動過程中進(jìn)行修正,出現(xiàn)的位置跳變可能給運動控制系統(tǒng)帶來極大負(fù)擔(dān),進(jìn)而帶來安全問題。針對此問題,提出基于推進(jìn)器轉(zhuǎn)速反饋的GNSS 輔助導(dǎo)航機(jī)制,如圖8 所示,利用推進(jìn)器轉(zhuǎn)速反饋判斷AUV 是否處于機(jī)動狀態(tài),確認(rèn)AUV 處于靜止?fàn)顟B(tài)時才對航位推算位置進(jìn)行修正。

圖8 基于推進(jìn)器轉(zhuǎn)速反饋的GNSS 輔助導(dǎo)航策略Fig.8 GNSS-assisted navigation strategy based on propeller speed feedback

3 DVL 自主導(dǎo)航試驗

結(jié)合“海為一號”AUV 試驗驗證所提出的DVL 自主導(dǎo)航方法。試驗在某水庫開展,如圖9所示,水域深度范圍約為2～25 m。“海為一號”AUV 在近水面航行,采用DVL 自主導(dǎo)航方法,利用GNSS 作為參考系統(tǒng)評估DVL 自主導(dǎo)航精度。在DVL 自主導(dǎo)航過程中,導(dǎo)航計算機(jī)同步采集GNSS 位置參數(shù),進(jìn)而計算DVL 自主導(dǎo)航定位誤差和航程(distance traveled,D.T.)的百分比。

圖9 試驗場景Fig.9 The scene of filed experiments

在湖面設(shè)置路徑點1～5,AUV 首先抵達(dá)路徑點1,隨后按路徑點的順序航行,最后再返回路徑點1,各路徑點的經(jīng)緯度如下,導(dǎo)航軌跡對比見圖10。

圖10 導(dǎo)航軌跡對比Fig.10 The comparison of navigation trajectories

路徑點1: 113.521 016 67°,34.539 611 11°;

路徑點2: 113.520 744 44°,34.540 111 11°;

路徑點3: 113.520 455 56°,34.539 794 44°;

路徑點4: 113.521 102 78°,34.539 761 11°;

路徑點5: 113.520 502 78°,34.540 050 00°。

試驗持續(xù)時間為881 s,航行距離為362.1 m。圖10 為DVL 自主導(dǎo)航和GNSS 軌跡對比圖,可以看出AUV 完成了路徑點跟蹤,DVL 自主導(dǎo)航軌跡和GNSS 軌跡有較好的一致性。圖11 所示為AUV 載體坐標(biāo)系內(nèi)速度,從圖中可知,AUV 啟動后的航行速度為0.2～0.6 m/s,每次接近路徑點時會減速調(diào)整航向,隨后加速向下一個路徑點航行。圖12 所示為DVL 自主導(dǎo)航定位誤差,可以看出誤差范圍為0～5.5 m,計算得到均方根誤差為2.5 m。針對圖中定位誤差的變化趨勢分析如下: DVL 自主導(dǎo)航定位誤差在直線軌跡下積累,在往返對稱的軌跡下會得到不同程度的抵消。圖13 所示為DVL 自主導(dǎo)航定位誤差和航程的百分比,該值在初始時刻值較大是由于航程較短,隨后由定位誤差和航程的積累速度決定,該試驗結(jié)束時刻該值為1.4% D.T.。需要特別說明的是,DVL 自主導(dǎo)航定位誤差由DVL 和姿態(tài)傳感器各種類型的誤差在航位推算過程中耦合后積分產(chǎn)生,在不同航行軌跡下起主導(dǎo)作用的誤差耦合項存在差異。此外,在不同航行軌跡下DVL 和姿態(tài)傳感器的量測誤差隨AUV 速度和姿態(tài)變化,所以單次試驗難以充分評估DVL 自主導(dǎo)航精度。鑒于此,開展了14 次不同航行軌跡下的DVL 自主導(dǎo)航試驗,表2 為試驗結(jié)果統(tǒng)計。在此基礎(chǔ)上,計算圓概率誤差(circular error probable,CEP)如下: 由于7 次試驗的定位精度優(yōu)于1.5% D.T.,所以DVL 自主導(dǎo)航定位精度CEP 約為1.5% D.T.。

表2 試驗結(jié)果統(tǒng)計Table 2 The statistics table of experiment results

圖11 AUV 速度Fig.11 Velocities of the AUV

圖12 定位誤差Fig.12 Positioning errors

圖13 定位誤差和航程百分比Fig.13 The percentage of positioning error and distance traveled

4 結(jié)束語

針對成本較低的中小型AUV,提出一種DVL自主導(dǎo)航方法,該方法能夠以較低成本的系統(tǒng)實現(xiàn)較高精度的自主導(dǎo)航,具有較強(qiáng)的實用性,可以為相關(guān)研究和應(yīng)用提供參考。隨著AUV 執(zhí)行任務(wù)日趨多樣,面臨日益復(fù)雜的水下環(huán)境,將對自主導(dǎo)航系統(tǒng)的精度和可靠性提出更高要求。因此,后續(xù)研究將圍繞DVL 自主導(dǎo)航誤差修正和容錯設(shè)計展開。