基于位置觀測信息的Davenport四元數(shù)DVL標(biāo)定方法

唐洪瓊, 許江寧, 史文策,2, 何泓洋, 李方能,*

(1. 海軍工程大學(xué)電氣工程學(xué)院, 湖北 武漢 430033; 2. 中國人民解放軍91321部隊, 浙江 金華 322000)

0 引 言

自主式水下航行器(autonomous underwater vehicle, AUV)廣泛應(yīng)用于國家海防建設(shè)、海洋資源勘探等軍民領(lǐng)域,高精度、高可靠性的自主導(dǎo)航能力是AUV順利完成任務(wù)的前提[1-3]。捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system,SINS)由于結(jié)構(gòu)簡單、體積小、隱蔽性強(qiáng)、便于與其他設(shè)備集成化設(shè)計等優(yōu)點(diǎn),在水下定位導(dǎo)航與授時(position, navigation and timing, PNT)領(lǐng)域受到廣泛關(guān)注,成為AUV自主導(dǎo)航定位的重要手段[4-6]。然而,SINS本質(zhì)上是以牛頓第二定律為基礎(chǔ)的積分推算系統(tǒng),需要對慣性測量單元(inertial measurement unit,IMU)的輸出進(jìn)行積分運(yùn)算,導(dǎo)致其導(dǎo)航誤差會隨著時間的推移而不斷積累[7-9]。水下環(huán)境對全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system, GNSS)信號具有屏蔽作用,而多普勒計程儀(Doppler velocity log,DVL) 測速精度穩(wěn)定,可為SINS提供實時外部速度輔助融合信息,SINS/DVL組合導(dǎo)航已成為當(dāng)前解決水下導(dǎo)航問題的主流方案[10-12]。

DVL通過聲學(xué)多普勒效應(yīng)來測量AUV相對于水底或者水流的速度,在工程實際中,由于制作工藝、外界環(huán)境、安裝偏差等條件限制,導(dǎo)致DVL的測速誤差難以避免[13-14]。因此,在預(yù)先標(biāo)定DVL誤差參數(shù)的基礎(chǔ)上,通過標(biāo)定結(jié)果對DVL原始輸出進(jìn)行實時補(bǔ)償以確保速度量測信息的準(zhǔn)確性對提高SINS/DVL組合導(dǎo)航系統(tǒng)的精度具有重要意義[15-16]。

近年來,為實現(xiàn)DVL誤差參數(shù)的準(zhǔn)確標(biāo)定,眾多學(xué)者對此展開了研究。劉德鑄等[17]將最小二乘法應(yīng)用于標(biāo)定DVL安裝誤差角,并通過淺海試驗驗證了所提方法的有效性,但此方法無法實現(xiàn)對刻度因子誤差的標(biāo)定,一定程度上影響了DVL的量測精度。Li等[18]將DVL誤差標(biāo)定問題轉(zhuǎn)化為求取兩組矢量間的旋轉(zhuǎn)參數(shù)問題并通過基于奇異值分解(singular value decomposition, SVD)的最小二乘估計法實現(xiàn)了對其的求解,但當(dāng)AUV處于非勻速直線運(yùn)動時,該標(biāo)定方法的效果會變差。Lyu等[19]將DVL誤差作為系統(tǒng)狀態(tài)參數(shù)利用卡爾曼濾波(Kalman filter,KF)進(jìn)行實時估計實現(xiàn)了DVL的在線自標(biāo)定,但當(dāng)AUV處于不同的機(jī)動狀態(tài)時,狀態(tài)量的可觀測度難以得到保證。徐曉蘇等[20]構(gòu)建了GNSS/SINS/DVL組合導(dǎo)航系統(tǒng),提出了一種基于梯度下降(gradient descent, GD)四元數(shù)理論的標(biāo)定方法,仿真和車載試驗結(jié)果都驗證了方法的有效性。Wang等[21]在GD法的基礎(chǔ)上,利用二階擬牛頓(quasi-Newton, QN)法替代一階GD法以加快誤差收斂速度,并分別基于速度和位置觀測信息提出了兩種標(biāo)定方法,車載和船載試驗結(jié)果表明基于位置觀測信息的標(biāo)定方法效果更佳,但方法中的權(quán)值系數(shù)通常被設(shè)定為經(jīng)驗值,導(dǎo)致該方法在不同條件下的普適性收到了制約。隨著機(jī)器學(xué)習(xí)的快速發(fā)展,基于智能算法的標(biāo)定方法開始出現(xiàn)。Wang等[22]利用遺傳算法(genetic algorithm, GA)和支持向量回歸(support vector regression, SVR)搭建回歸預(yù)測器來標(biāo)定DVL,該方法無需預(yù)先建立模型且適用于標(biāo)定大安裝誤差角。Li等[23]利用改進(jìn)的粒子群優(yōu)化(particle swarm optimization, PSO)算法實現(xiàn)了對DVL刻度因子誤差和安裝誤差角的估計,并設(shè)計仿真和船載試驗驗證了標(biāo)定方法的有效性。但是,Wang和Li等的標(biāo)定方法參數(shù)設(shè)置復(fù)雜且計算量龐大,導(dǎo)致實時性效果不佳。

基于上述研究,本文提出了一種基于位置觀測信息的Davenport四元數(shù)方法以實現(xiàn)對DVL刻度因子誤差和安裝誤差角的標(biāo)定工作,首先,通過基于多普勒測速原理的位置觀測推算來標(biāo)定刻度因子誤差。而后,利用基于Davenport四元數(shù)方法的位置觀測矢量方程解算來標(biāo)定安裝角誤差。所提方法相較于現(xiàn)有方法,無需設(shè)置初始值,不依賴于特定軌跡和準(zhǔn)確的先驗知識,更具普適性。船載湖試對比分析了該標(biāo)定方法與其他方法的標(biāo)定效果,結(jié)果表明,在簡單和復(fù)雜不同的機(jī)動情況下,該標(biāo)定方法得到的速度精度更高,同時,利用標(biāo)定補(bǔ)償后的DVL輸出進(jìn)行SINS/DVL組合導(dǎo)航,該方法得到的位置誤差更小。

1 DVL測速誤差分析與建模

定義坐標(biāo)系:選取“東—北—天(E-N-U)”地理坐標(biāo)系為導(dǎo)航坐標(biāo)系,記為n系;選取“右—前—上”坐標(biāo)系為載體坐標(biāo)系,記為b系;DVL安裝坐標(biāo)系記為d系;地心慣性坐標(biāo)系記為i系;地球坐標(biāo)系記為e系;計算導(dǎo)航坐標(biāo)系記為n′系。

在理想的安裝情況下,DVL安裝坐標(biāo)系d系與載體坐標(biāo)系b系的坐標(biāo)軸應(yīng)該是互相一致的,即安裝矩陣為單位陣I3×3,但在工程實踐中,由于技術(shù)工藝限制導(dǎo)致安裝誤差角難以避免,載體上DVL安裝誤差示意圖如圖1所示,其中,xb-yb-zb表示b系,xd-yd-zd表示d系。

圖1 DVL安裝誤差示意圖

根據(jù)DVL的工作原理,其測速誤差模型[24]可表示為

(1)

在實際工程應(yīng)用中,由于SINS和DVL之間通常緊密安裝且桿臂矢量可直接通過測量獲得并予以補(bǔ)償[25],因此本文將忽略桿臂誤差的影響,式(1)可簡化為

(2)

(3)

(4)

2 標(biāo)定方法基本原理

在本文所提的DVL誤差標(biāo)定系統(tǒng)中,當(dāng)AUV處于水面航行狀態(tài)可接收GNSS信號時,利用KF最優(yōu)估計方法進(jìn)行SINS/GNSS組合導(dǎo)航,得到載體高精度的姿態(tài)和速度信息作為參考值,然后綜合DVL測量的d系下的速度值,通過標(biāo)定算法估計出刻度因子誤差和安裝誤差角并對DVL輸出進(jìn)行補(bǔ)償。DVL誤差標(biāo)定系統(tǒng)結(jié)構(gòu)如圖2所示。

圖2 DVL標(biāo)定系統(tǒng)結(jié)構(gòu)圖

2.1 刻度因子誤差的標(biāo)定

由于姿態(tài)旋轉(zhuǎn)矩陣的模值為1,即姿態(tài)旋轉(zhuǎn)矩陣與向量相乘時,只會改變向量的方向但不改變向量的大小。因此,同時對式(4)兩側(cè)進(jìn)行取模可得

(5)

此時,在DVL測量得到載體d系速度的基礎(chǔ)上,可以得出:

(6)

但需要指出的是,在利用式(6)計算刻度因子誤差時,由于直接使用DVL輸出的量測值會引入量測噪聲誤差,進(jìn)而降低了刻度因子誤差的標(biāo)定精度。因此,為進(jìn)一步消除DVL量測噪聲的影響,同時對式(4)兩側(cè)進(jìn)行位置運(yùn)算,此時,式(4)的左側(cè)可表示為

(7)

式中:k表示某一離散時刻;Δt表示采樣時間間隔。

設(shè)在DVL每次采樣時間間隔Δt內(nèi),SINS/GNSS組合導(dǎo)航系統(tǒng)解算更新n次,此時,對位置信息進(jìn)行離散化處理可得式(4)的右側(cè)為

(8)

因此,可以得出基于位置信息的刻度因子誤差s表達(dá)式為

(9)

2.2 安裝誤差角的標(biāo)定

利用式(4)和式(9),可以得到基于位置信息的安裝誤差角計算表達(dá)式為

(10)

令:

(11)

此時,式(10)將變換為

(12)

(13)

式中:ωk為量測向量對應(yīng)的權(quán)值,在本文標(biāo)定問題中,ωk=1。

為快速準(zhǔn)確地完成DVL安裝誤差角標(biāo)定,采用Davenport四元數(shù)方法[28-29]對旋轉(zhuǎn)矩陣R進(jìn)行求解。

在式(13)中,由于常數(shù)對目標(biāo)函數(shù)的最小化不產(chǎn)生任何影響,對其進(jìn)行線性變換后可以得到:

(14)

(15)

忽略常數(shù)項,目標(biāo)函數(shù)f可以進(jìn)一步化簡為

(16)

由于矩陣的跡運(yùn)算對目標(biāo)函數(shù)不產(chǎn)生影響,根據(jù)矩陣跡的性質(zhì)可以得到:

(17)

旋轉(zhuǎn)矩陣R可用四元數(shù)形式表示為

(18)

式中:q0和q分別為四元數(shù)的標(biāo)量部分和矢量部分。此時,目標(biāo)函數(shù)可改寫為

(19)

根據(jù)矩陣跡的性質(zhì),對式(19)作進(jìn)一步運(yùn)算:

(20)

對于tr([q×]AT)而言,將其展開可得

tr([q×]AT)=q(1)(A(3,2)-A(2,3))+q(2)(A(1,3)-
A(3,1))+q(3)(A(2,1)-A(1,2))

(21)

令:

(22)

則式(21)可化簡為

tr([q×]AT)=-aTq

(23)

將式(23)代入式(20)可得

(24)

設(shè)tr(A)=ρ且A+AT=B,式(24)可變換為

(25)

根據(jù)矩陣運(yùn)算法則,式(25)可改寫為線性形式:

(26)

式中:Q為矢量部分在前的四元數(shù)形式的姿態(tài)矩陣;D為Davenport矩陣。

基于上述推導(dǎo),目標(biāo)函數(shù)f化簡為minf(Q)=-QTDQ且QTQ=1,采用拉格朗日乘數(shù)法對目標(biāo)函數(shù)f最小化問題進(jìn)行求解,則

minf(Q,λ)=-QTDQ+λ(QTQ-1)

(27)

對方程進(jìn)行求導(dǎo)得

DQ=λQ

(28)

因此,Q為D的對應(yīng)于特征值λ的特征向量。此時,姿態(tài)四元數(shù)目標(biāo)函數(shù)f最小化問題轉(zhuǎn)化為求解矩陣D的最大特征值對應(yīng)的特征向量的問題,在求出Q的基礎(chǔ)上,將其轉(zhuǎn)換為歐拉角即為所標(biāo)定出的DVL安裝誤差角。

3 試驗驗證

為驗證本文所提DVL標(biāo)定方法的可行性和有效性,位湖北省武漢市木蘭湖開展了船載湖試試驗。船載導(dǎo)航設(shè)備包括CY-JG90J型捷聯(lián)慣性導(dǎo)航系統(tǒng)、PA600型DVL傳感器和GNSS接收機(jī),設(shè)備的參數(shù)如表1和表2所示。試驗過程中以高精度實時動態(tài)定位(real-time kinematic,RTK)-GNSS與SINS進(jìn)行組合后得到的結(jié)果作為參考基準(zhǔn)。主要試驗設(shè)備實物圖以及試驗平臺安裝概況如圖3所示。

表1 CY-JG90J型SINS的性能參數(shù)

表2 PA600型DVL的性能參數(shù)

圖3 試驗平臺搭建全景

此次船載試驗共采集12 000 s實測數(shù)據(jù),其中,0～1 200 s為SINS初始對準(zhǔn)階段,以滿足后期高精度組合導(dǎo)航的基本需求。選取初始對準(zhǔn)階段后的兩段1 200 s試驗數(shù)據(jù)對不同機(jī)動條件下DVL標(biāo)定效果進(jìn)行驗證,所選兩段時間內(nèi)DVL的原始輸出數(shù)據(jù)和對應(yīng)的試驗船行進(jìn)軌跡如圖4和圖5所示。其中,黃色圖標(biāo)表示軌跡起點(diǎn),綠色圖標(biāo)表示軌跡終點(diǎn)。

圖4 簡單機(jī)動情況下的DVL原始輸出及對應(yīng)軌跡

圖5 復(fù)雜機(jī)動情況下的DVL輸出及對應(yīng)軌跡

3.1 簡單機(jī)動情況下的性能驗證

利用圖4所示軌跡驗證載體在簡單機(jī)動情況下的DVL標(biāo)定效果。為比較方法性能,分別利用SVD標(biāo)定方法、KF標(biāo)定方法、基于位置觀測信息的擬牛頓標(biāo)定方法和本文所提的標(biāo)定方法進(jìn)行1 200 s的標(biāo)定試驗,利用標(biāo)定結(jié)果對DVL測量輸出進(jìn)行補(bǔ)償,并將補(bǔ)償后的結(jié)果與GNSS/SINS組合導(dǎo)航得到的DVL參考速度進(jìn)行對比,得到3個方向的速度誤差結(jié)果如圖6～圖8所示。為方便表述,上述4種標(biāo)定方法分別記為SVD、KF、擬牛頓四元數(shù)位置規(guī)則(qusai-Newton quaternions position, QNQ-P)和Davenport位置(Davenport position, DP-P)。

圖6 簡單機(jī)動時右向速度標(biāo)定誤差

圖7 簡單機(jī)動時前向速度標(biāo)定誤差

圖8 簡單機(jī)動時上向速度標(biāo)定誤差

從圖6～圖8可以看出,對右向速度而言,4種標(biāo)定方法補(bǔ)償后的誤差相對于未標(biāo)定數(shù)據(jù)均有了提升,SVD和KF方法誤差基本穩(wěn)定在0.1 m/s以內(nèi),QNQ-P和DP-P方法誤差基本穩(wěn)定在0.08 m/s以內(nèi),但DP-P方法標(biāo)定誤差更加平滑。對前向速度而言,由于試驗載體行進(jìn)過程中主要為前向速度且速度量測值不可避免地受外界噪聲影響,所以誤差結(jié)果波動較大,但相比于未標(biāo)定和KF方法標(biāo)定誤差,另外3種標(biāo)定方法所得誤差更小。對上向速度而言,由于KF標(biāo)定方法的天向通道發(fā)散,導(dǎo)致最終標(biāo)定結(jié)果不可信,而SVD、QNQ-P和DP-P方法標(biāo)定誤差效果基本相當(dāng)。總的來說,由于簡單機(jī)動情況下的試驗載體近似作勻速直線運(yùn)動,KF方法中部分狀態(tài)量的可觀測度較弱[30],因此導(dǎo)致KF方法的標(biāo)定效果大幅下降,而其他3種標(biāo)定方法的速度誤差均能基本穩(wěn)定在0.1 m/s以內(nèi),能夠滿足后續(xù)組合導(dǎo)航的要求。

為量化比較方法效果,選取1 200 s內(nèi)速度的最大絕對誤差MAX和平均絕對誤差(mean absolute error, MAE)作為性能評判指標(biāo),MAE定義如下所示:

(29)

最終計算結(jié)果如表3所示,可以看出,當(dāng)試驗載體進(jìn)行簡單機(jī)動時,KF方法的標(biāo)定結(jié)果不夠理想,僅有右向速度誤差優(yōu)于未標(biāo)定值,而SVD、QNQ-P和DP-P方法的速度標(biāo)定誤差在各個方向上均優(yōu)于未標(biāo)定值,達(dá)到了速度標(biāo)定的目的。

表3 簡單機(jī)動時不同標(biāo)定方法速度誤差標(biāo)定結(jié)果

為進(jìn)一步比較方法效果,利用SVD、QNQ-P、DP-P方法標(biāo)定補(bǔ)償后的DVL量測輸出與SINS通過標(biāo)準(zhǔn)KF算法進(jìn)行組合導(dǎo)航得到位置結(jié)果,并與KF標(biāo)定方法進(jìn)行對比以間接反映標(biāo)定效果的優(yōu)劣,試驗得到的軌跡結(jié)果以及位置誤差如圖9、圖10和表4所示。

表4 簡單機(jī)動時位置誤差的最大值與平均值

圖9 簡單機(jī)動時試驗載體軌跡對比圖

圖10 簡單機(jī)動時位置誤差對比圖

其中,位置誤差[31]的計算公式如下:

(30)

式中:ΔP表示位置誤差;L表示緯度;λ表示經(jīng)度。

由圖9可知,使用未標(biāo)定的速度進(jìn)行組合導(dǎo)航得到的軌跡發(fā)散嚴(yán)重,而通過標(biāo)定技術(shù)對DVL輸出進(jìn)行補(bǔ)償后再進(jìn)行組合導(dǎo)航的軌跡均能較好地跟蹤參考軌跡。需要指出的是,使用KF方法得到的位置信息準(zhǔn)確度較高,說明當(dāng)試驗載體近似作勻速直線運(yùn)動時,位置狀態(tài)量不同于安裝誤差角狀態(tài)量,其可觀測度并沒有受到影響。

由圖10可知,使用QNQ-P和DP-P方法標(biāo)定后的速度進(jìn)行組合導(dǎo)航得到的位置誤差精度相當(dāng)且明顯優(yōu)于其他方法,但DP-P方法的誤差曲線更加平滑。

表4可以更加直觀地反映位置誤差結(jié)果,從表中可以看出,相比于其他方法,QNQ-P和DP-P方法的標(biāo)定效果有了大幅提高且幅度均達(dá)到80%以上,而DP-P方法的導(dǎo)航精度更高,以位置誤差最大值為例,比QNQ-P方法進(jìn)一步提高了27.94%。試驗結(jié)果驗證了所提DP-P標(biāo)定方法在試驗載體簡單機(jī)動情況下的有效性。

3.2 復(fù)雜機(jī)動情況下的性能驗證

利用圖5所示軌跡驗證載體在復(fù)雜機(jī)動情況下的DVL標(biāo)定效果。利用不同標(biāo)定方法得到的DVL測量輸出與DVL參考速度進(jìn)行比較,得到3個方向的速度誤差結(jié)果,如圖11～圖13和表5所示。

表5 復(fù)雜機(jī)動時不同標(biāo)定方法速度誤差標(biāo)定結(jié)果

圖11 復(fù)雜機(jī)動時右向速度標(biāo)定誤差

圖12 復(fù)雜機(jī)動時前向速度標(biāo)定誤差

圖13 復(fù)雜機(jī)動時上向速度標(biāo)定誤差

從圖11～圖13可以看出,當(dāng)試驗載體作連續(xù)“S”形的復(fù)雜機(jī)動時,SVD方法的標(biāo)定效果出現(xiàn)了下滑,速度誤差曲線波動劇烈;KF方法中安裝誤差角狀態(tài)量的可觀測度增強(qiáng),右向和前向速度的標(biāo)定誤差結(jié)果明顯優(yōu)于簡單機(jī)動情況,上向速度由于天向通道發(fā)散的原因,標(biāo)定結(jié)果仍然不可信;QNQ-P和DP-P方法標(biāo)定精度基本相當(dāng)且優(yōu)于其他方法,但DP-P方法的速度誤差曲線波動范圍更小。從表5可以看出,當(dāng)試驗載體進(jìn)行復(fù)雜機(jī)動時,與未標(biāo)定的速度誤差相比,使用4種標(biāo)定方法后的速度誤差均有不同程度的降低,但就整體而言,本文所提DP-P方法的標(biāo)定效果最佳。

利用不同方法標(biāo)定后的速度進(jìn)行組合導(dǎo)航,得到的軌跡結(jié)果以及位置誤差,如圖14、圖15和表6所示。

表6 復(fù)雜機(jī)動時位置誤差的最大值與平均值

圖14 復(fù)雜機(jī)動時試驗載體軌跡對比圖

圖15 復(fù)雜機(jī)動時位置誤差對比圖

由圖14和圖15可知,使用未標(biāo)定的速度進(jìn)行組合導(dǎo)航得到的軌跡與參考軌跡存在較大偏差;使用SVD方法標(biāo)定補(bǔ)償后的速度進(jìn)行組合導(dǎo)航,位置誤差隨著時間的推移而逐漸增大;使用KF、QNQ-P和DP-P方法標(biāo)定補(bǔ)償后的速度進(jìn)行組合導(dǎo)航,1 200 s內(nèi)的位置誤差基本均能控制在20 m以內(nèi)。表6進(jìn)一步說明了KF、QNQ-P和DP-P方法在導(dǎo)航精度方面所存在的優(yōu)勢,相比而言, DP-P方法的精度更高,以位置誤差平均值為例,比KF方法提高了62.74%,比QNQ-P方法提高了10.94%。雖然DP-P方法的速度標(biāo)定精度與QNQ-P方法相當(dāng),但導(dǎo)航定位精度更高,進(jìn)而表明該方法標(biāo)定的DVL誤差參數(shù)準(zhǔn)確度更高。試驗結(jié)果驗證了所提DP-P標(biāo)定方法在試驗載體復(fù)雜機(jī)動情況下的有效性。

4 結(jié) 論

本文針對DVL刻度因子和安裝誤差角嚴(yán)重影響SINS/DVL組合系統(tǒng)導(dǎo)航精度的問題,提出了一種基于位置觀測信息的Davenport四元數(shù)方法,并將其應(yīng)用于DVL預(yù)標(biāo)定系統(tǒng)。通過船載湖試試驗驗證了方法的有效性,簡單和復(fù)雜不同的機(jī)動條件下,所提標(biāo)定方法在準(zhǔn)確度和穩(wěn)定性方面均具有優(yōu)越性,具有良好的工程應(yīng)用價值。