一種基于梯度下降四元數(shù)理論的DVL誤差標(biāo)定方法

徐曉蘇，楊 陽，李 瑤

（1.微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點實驗室，南京 210096； 2.東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

水下自主航行器（AUV）無論在軍用還是民用領(lǐng)域都有著廣泛的應(yīng)用前景，導(dǎo)航定位一直是AUV 的一項關(guān)鍵技術(shù)。單一的慣性導(dǎo)航系統(tǒng)（INS）具有強自主性、強隱蔽性等優(yōu)點，是AUV 的理想導(dǎo)航方式，但其系統(tǒng)誤差會隨時間累積，長時間工作會導(dǎo)致其導(dǎo)航精度下降而無法滿足精度要求。SINS/DVL 組合導(dǎo)航系統(tǒng)利用DVL 速度信息對SINS 導(dǎo)航誤差進(jìn)行估計并補償，獲得較高的導(dǎo)航精度。因此，SINS/DVL 組合導(dǎo)航系統(tǒng)常被用于AUV[1]。

多普勒測速誤差是影響SINS/DVL 組合導(dǎo)航系統(tǒng)精度的重要因素，其誤差來源包括比例因子誤差、安裝偏角誤差、載體姿態(tài)誤差、波束寬度誤差、洋流速度[2]等等，其中比例因子誤差和安裝角偏誤差的標(biāo)定研究對提高SINS/DVL 組合導(dǎo)航定位精度有著重要意義[3]。因此，關(guān)于這兩種誤差標(biāo)定技術(shù)是SINS/DVL水下組合導(dǎo)航領(lǐng)域的研究熱點之一。

對于比例因子誤差和安裝誤差角的標(biāo)定方式，常用的包括在線自標(biāo)定和預(yù)標(biāo)定等。在線自標(biāo)定即把測速誤差參數(shù)作為卡爾曼濾波器的狀態(tài)進(jìn)行在線估計，是解決問題的一個思路。但是，AUV 通常采用勻速直 航的航行方式，在該航行方式下，測速誤差參數(shù)的可觀測度比較弱，而且由于在標(biāo)定過程中直接采用了DVL 的量測輸出，引入了DVL 量測噪聲，使得標(biāo)定曲線呈現(xiàn)噪聲特性，難以取得比較好的估計效果[4]。

本文針對以上問題，采用預(yù)標(biāo)定的方法，以SINS/ DVL/GNSS 作為組合導(dǎo)航方式，提出了一種基于梯度下降四元數(shù)估計理論的位置觀測標(biāo)定方法。由于該方法是基于位置矢量的標(biāo)定，通過利用四元數(shù)梯度下降法，構(gòu)造位置矢量觀測器方程，避免了DVL 量測噪聲對標(biāo)定結(jié)果的影響，可進(jìn)一步提高標(biāo)定精度。試驗結(jié)果表明，與直接利用DVL 量測速度標(biāo)定的算法相比，基于位置觀測法的標(biāo)定曲線更加平滑，精度更高。

1 DVL 主要誤差分析

1.1 比例因子誤差

在實際應(yīng)用的DVL 測速系統(tǒng)聲速計算公式中，一般將聲波在水中的傳播速度c0視為常值，即1500 m/s。但是聲波在水中的實際傳播速度受水的溫度、鹽度和水壓等因素的影響，用常值來代替一個實際變化的量必然會帶來計算誤差。其中，溫度和鹽度對聲速的影響最大：水溫每變化1℃，聲速變化所引起的測速誤差達(dá)到0.2%；海水鹽度每增加0.1%，聲速變化所引起的測速誤差為0.07%[5]。此類誤差的作用在DVL 比例因子的數(shù)值變化上呈現(xiàn)。

1.2 安裝誤差角

在SINS/DVL 組合導(dǎo)航系統(tǒng)中，安裝誤差角是指DVL 與慣導(dǎo)測量單元IMU 之間的安裝姿態(tài)關(guān)系[6]。對于SINS/DVL 組合系統(tǒng)，當(dāng)進(jìn)行速度匹配時，SINS 和DVL 的速度信息必須基于同一坐標(biāo)體系，但是當(dāng)SINS和DVL 的測量坐標(biāo)系存在誤差時，即DVL 存在安裝誤差時，必然會影響組合導(dǎo)航的精度。在工程實踐中，儀器設(shè)備都存在誤差，安裝偏角誤差無法避免，會造成DVL 和IMU 對速度信息測量不一致[7]。因此，需要明確兩者的安裝偏角關(guān)系。如圖1所示，可以更直 觀了解兩者的安裝偏角關(guān)系[8]，其中，x B-y B-zB指載體坐標(biāo)系（b系），x D-y D-zD指DVL 坐標(biāo)系（d系）。

圖1 安裝誤差角示意圖 Fig.1 Schematic of installation error angle

2 位置觀測標(biāo)定算法原理

算法原理框圖如圖2所示。AUV 在水面航行時，利用SINS/GNSS 組合導(dǎo)航系統(tǒng)，得到載體的姿態(tài)、速度和位置信息，再根據(jù)DVL 測量得到的速度信息，按照提出的標(biāo)定算法進(jìn)行誤差標(biāo)定。

圖2 標(biāo)定原理框圖 Fig.2 Block diagram of calibration principle

2.1 比例因子誤差的標(biāo)定

根據(jù)DVL 的測速原理，其輸出為：

式中，表示DVL 輸出速度；?表示比例因子誤差；表示b系到d系的姿態(tài)轉(zhuǎn)換矩陣；Cnb表示n系到b系的姿態(tài)轉(zhuǎn)換陣；vn表示載體在導(dǎo)航坐標(biāo)系（東北天坐標(biāo)系n系）中的運動速度；表示b系相對于n系的旋轉(zhuǎn)角速度在b系上的投影；表示DVL 與SINS 之間的桿臂距離在b系上的投影；ηd為DVL 的量測誤差。

根據(jù)DVL 測速原理可知，當(dāng)進(jìn)行SINS/GNSS 組合導(dǎo)航時，有如下關(guān)系式：

式中，表示DVL 輸出速度；Cbd表示b系到d系的姿態(tài)轉(zhuǎn)換矩陣，一般為常量；?表示比例因子誤差；表示SINS/GNSS 組合之后的姿態(tài)變換陣；表示SINS/GNSS 組合速度在n系下的值；表示b系相對于n系的旋轉(zhuǎn)角速度在b系上的投影；表示DVL 與SINS 之間的桿臂距離在b系上的投影。

可由下式計算：

式中，表示陀螺輸出量εb為陀螺漂移，為n系相對慣性空間角速度在n系下的投影。由于GNSS信號有效，則載體的姿態(tài)、速度和位置信息等都可以準(zhǔn)確獲得，因此可以通過計算得到。

由于姿態(tài)轉(zhuǎn)換矩陣不會改變向量的模值，而且相對AUV 的運動角速度，陀螺零偏εb為小量，其對應(yīng)的桿臂速度可以忽略，對式(2)進(jìn)行取模運算得到：

計算得到利用速度觀測算法的DVL 比例因子誤差為：

由于式(5)得到的DVL 比例因子誤差直接利用了DVL 的速度引入了量測噪聲，所以對式(2)左右兩邊進(jìn)行位置運算，式(2)左邊為：

式中，下標(biāo)k表示離散時刻，ΔtD表示DVL 采樣時間間隔。

若在每次多普勒測速儀的采樣間隔內(nèi)，SINS/GNSS 解算得出N個數(shù)據(jù)，則對SINS/GNSS 位置離散化計算得到式(2)右邊為：

所以，得到基于位置觀測標(biāo)定算法的比例因子誤差為：

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

將上述利用速度觀測方法標(biāo)定出來的結(jié)果式(5)代入式(2)有：

為了實現(xiàn)安裝角快速、準(zhǔn)確標(biāo)定，本文參考梯度下降四元數(shù)姿態(tài)估計器[9-10]，提出了一種基于梯度下降四元數(shù)估計理論的安裝角確定算法。根據(jù)上述得到的公式，令：

構(gòu)造速度觀測器矢量方程：

同樣，為了實現(xiàn)安裝誤差角的標(biāo)定不受DVL 量測噪聲的影響，利用基于位置觀測標(biāo)定算法計算得到的比例因子誤差，由式(9)得：

根據(jù)梯度下降四元數(shù)的估計理論算法，構(gòu)造目標(biāo)函數(shù)：

其中，表示由b系到d系的轉(zhuǎn)換四元數(shù)，表 示該四元數(shù)的共軛四元數(shù)。

基于式(15)的最小化，根據(jù)文獻(xiàn)[9][10]得到基于算 法的最優(yōu)姿態(tài)四元數(shù)確定的迭代方法：

式(17)中JT表示函數(shù)F關(guān)于變量qbd的雅可比矩陣函數(shù)的轉(zhuǎn)置形式。由式(15)和式(17)可知F和J的表達(dá)式：

式中，F(xiàn)是的簡單表示形式，J是的簡單表示。由式(16)可知，需 要求解權(quán)值系數(shù)μ。為了實現(xiàn)更精確的誤差標(biāo)定，本文將該系數(shù)看作為一個變化量，構(gòu)造式(19)表示的梯 度四元數(shù)，通過迭代計算，實現(xiàn)該系數(shù)的最優(yōu)化。

根據(jù)式(19)，選取合適的μk，可以保證收斂。當(dāng)?shù)介L較大時會帶來一定的擾動，因此μk應(yīng)滿足下列的約束條件：

式中，γk表示互補濾波權(quán)值。為權(quán)衡引入選取準(zhǔn)則其中σ表示的遞推發(fā)散速率，μkΔtD表示梯度控制收斂速率。將解得的γk表達(dá)式與式(13)代入式(21)，考慮到DVL 和SINS 固聯(lián)為零向量，并且μk遠(yuǎn)大于σ，整理得到以下算法迭代格式：

利用式(22)，選取合適的發(fā)散系數(shù)σ，即可得到基于位置觀測法的安裝角對應(yīng)的四元數(shù)，從而實現(xiàn)對安裝角的標(biāo)定。

至此，基于位置觀測法的比例因子誤差和安裝角均得到標(biāo)定。

根據(jù)式(11)(12)采用同樣的方法可以得到基于速度觀測方法的安裝誤差角的標(biāo)定結(jié)果，在仿真驗證及半物理試驗部分與基于位置觀測法的結(jié)果做了對比。

3 仿真驗證

本文提出了基于位置觀測的DVL 比例因子誤差以及安裝角的標(biāo)定方法，并將基于速度觀測的標(biāo)定方法也做了仿真及試驗驗證，與文獻(xiàn)[6]提出的卡爾曼濾波標(biāo)定方法進(jìn)行對比。其中，位置觀測標(biāo)定法命名為GD-p，速度觀測標(biāo)定法命名為GD-v，文獻(xiàn)[6]提出的方法命名為CKFM。

為了提高卡爾曼濾波標(biāo)定算法中DVL 比例因子誤差和安裝誤差角的可觀測度，仿真軌跡選取S 型。

仿真過程中，仿真時間為600 s，在每次方位轉(zhuǎn)彎時，AUV 都先以0.5 (°)/s 的角速度橫滾4 s，以橫滾出轉(zhuǎn)彎協(xié)調(diào)角，當(dāng)方位轉(zhuǎn)彎完成后，又以0.5 (°)/s 的角速度反向橫滾4 s，軌跡如圖3所示。系統(tǒng)工作的初始位置為東經(jīng)118.786°，北緯32.057°；慣性傳感器的參數(shù)設(shè)置為陀螺儀常值漂移0.02 (°)/h，隨機游走0.02 (°)/h；加速度計常值偏置500 μg，隨機偏置50 μg；IMU 數(shù)據(jù)輸出頻率200 Hz，捷聯(lián)結(jié)算周期5 ms；GNSS 數(shù)據(jù)輸出頻率1 Hz；DVL 的測速精度0.5%v±0.5cm/s，比例因子誤差設(shè)置為0.005；數(shù)據(jù)輸出頻率為1 Hz；DVL與SINS 之間的桿臂長度設(shè)置DVL 與SINS 之間的安裝角設(shè)置為φθ=1°，φγ=0.8°，φΨ=0.6°；SINS/ GNSS 組合導(dǎo)航系統(tǒng)的初始姿態(tài)誤差設(shè)置為 [0.5° 0.5° 1° ]T；速度誤差均設(shè)置為0.1 m/s，位置誤差為5 m。其中，圖4為3 種方法下的比例因子估計值誤差對比圖，δk表示比例因子估計值誤差。圖4～6 為3 種方法下的安裝角估計誤差的對比圖，δφ θ、δφ γ、δφ Ψ分別表示x軸安裝角估計誤差、y軸安裝角估計誤差和z軸安裝角估計誤差。

圖3 仿真軌跡 Fig.3 Simulation trajectory

圖4 比例因子估計值誤差 Fig.4 Scale factor estimation errors

圖5 x 軸安裝角估計誤差 Fig.5 x-axis mounting angle estimation error

圖6 y 軸安裝角估計誤差 Fig.6 y-axis mounting angle estimation error

圖7 z 軸安裝角估計誤差 Fig.7 z-axis mounting angle estimation error

圖4下面部分是局部放大圖，可以看出，由于是直接利用DVL 量測速度進(jìn)行的標(biāo)定，因此基于速度觀測法和卡爾曼濾波標(biāo)定法的結(jié)果是呈現(xiàn)噪聲特性的。而采用GD-p 方法后，可以有效避免這種影響，最終比例因子估計值誤差在0.06%以內(nèi)，精度有很大提高。采用位置觀測法以后，有效抑制了標(biāo)定結(jié)果的 噪聲特性。圖5表明，CKFM 和GD-v 方法標(biāo)定的x軸安裝角估計誤差在0.1°左右，而采用GD-p 方法，標(biāo)定結(jié)果可達(dá)到0.05°左右，精度有所提高。圖6表明，y軸安裝角估計誤差沒有得到標(biāo)定。由于載體一直以y軸方向為前進(jìn)軸，垂向速度和橫向速度均為小量，由文獻(xiàn)[7]可知，y軸安裝誤差角不會影響SINS/DVL 組合定位精度,因此可忽略其誤差的影響。圖7表明，CKFM 和GD-v 方法標(biāo)定的z軸安裝角估計誤差在0.15°左右，而采用GD-p 方法，標(biāo)定結(jié)果可達(dá)到0.03°左右，精度大幅度提高。

4 半物理試驗驗證

為了進(jìn)一步驗證本文提出的標(biāo)定方法的可行性和有效性，采用實驗室2017年12月5日車載數(shù)據(jù)進(jìn)行半物理仿真驗證。在本次車載試驗中，姿態(tài)、速度、位置等導(dǎo)航參數(shù)的真值由法國IXBLUE 公司研制的PHINS 和NovAtel 公司研制的FlexPark6 接收機進(jìn)行SINS/GNSS 松組合來提供。DVL 速度由PHINS 和GNSS 松組合下的速度來模擬，與捷聯(lián)慣導(dǎo)系統(tǒng)的輸出進(jìn)行SINS/DVL 組合，該系統(tǒng)信息融合周期為1 s。在車載試驗前，捷聯(lián)系統(tǒng)經(jīng)過了實驗室標(biāo)定過程，補償了慣性器件的部分常值誤差。捷聯(lián)系統(tǒng)的陀螺常值漂移為0.01 (°)/h，隨機游走為0.01 (°)/h1/2，加速度計零偏為50 μg，DVL 系統(tǒng)更新頻率設(shè)置為1 Hz，與慣導(dǎo)系統(tǒng)的安裝角設(shè)置為φθ= 1° ，φγ= 0.8° ，φΨ= 0.6°，比例因子估計值誤差設(shè)置為0.005。跑車圖以及安裝實物圖如圖8所示，圖中上側(cè)紅色圈注地方為GNSS 天線。

圖8 樣機跑車和安裝實物圖 Fig.8 Prototype car and installation physical

本次車載試驗持續(xù)3700 s，0～1540 s 期間進(jìn)行初始對準(zhǔn)，為后期的組合姿態(tài)提供高精度的導(dǎo)航信息。本次驗證試驗選取對準(zhǔn)結(jié)束后的600 s 數(shù)據(jù)進(jìn)行算法驗證，圖9為車載試驗軌跡，圖10～13 為根據(jù)本文提 出的算法進(jìn)行的標(biāo)定對比曲線。

圖9 車載試驗軌跡 Fig.9 Prototype car trajectory

圖10 比例因子估計值誤差對比 Fig.10 Comparison on scale factor estimation errors

圖11 x 軸安裝角估計誤差 Fig.11 Estimation error of x-axis mounting angle

圖12 z 軸安裝角估計誤差 Fig.12 Estimation error of z-axis mounting angle

從圖10～12 可以看出，當(dāng)采用卡爾曼濾波在線標(biāo) 定和速度觀測法時，由于直接采用了量測速度進(jìn)行標(biāo)定，曲線受到量測噪聲的影響較大。采用基于位置觀測的標(biāo)定方法后，精度有很大提高。圖10 表明，比例因子估計值誤差在5×10-4以內(nèi)；圖11 和圖12 表明，對安裝角估計值誤差的標(biāo)定，相比于采用卡爾曼在線標(biāo)定和基于速度觀測法標(biāo)定，均保持在0.02°以內(nèi)。

表1表示的是3 種方法下比例因子估計值誤差、x軸安裝角估計誤差和z軸安裝角估計誤差的均方根誤差的統(tǒng)計值。從表中可以看出，由于基于位置觀測法對DVL 輸出速度進(jìn)行了積分處理，有效降低了量測噪聲的影響，使得該方法標(biāo)定結(jié)果的均方根誤差明顯小于其他兩種方法。

表1 估計誤差均方根誤差 Tab.1 Estimation error root mean square error

5 結(jié) 論

本文為了提高SINS/DVL 水下組合導(dǎo)航的定位精度，提出了一種基于梯度下降四元數(shù)估計理論的位置觀測DVL 標(biāo)定方法，在GNSS 信號輔助下，利用多普勒測速原理標(biāo)定出比例因子估計值誤差，通過構(gòu)造位置矢量觀測方程，采用梯度下降四元數(shù)方法得到安裝角估計誤差的標(biāo)定結(jié)果，并將這種方法與卡爾曼濾波在線估計和基于速度觀測法做了對比。仿真及試驗結(jié)果表明，位置觀測法可以更精確、更快速的標(biāo)定出DVL 的比例因子值和安裝角估計值。