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

徐曉蘇，楊 陽(yáng)，李 瑤

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

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

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

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

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

1 DVL 主要誤差分析

1.1 比例因子誤差

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

1.2 安裝誤差角

在SINS/DVL 組合導(dǎo)航系統(tǒng)中，安裝誤差角是指DVL 與慣導(dǎo)測(cè)量單元IMU 之間的安裝姿態(tài)關(guān)系[6]。對(duì)于SINS/DVL 組合系統(tǒng)，當(dāng)進(jìn)行速度匹配時(shí)，SINS 和DVL 的速度信息必須基于同一坐標(biāo)體系，但是當(dāng)SINS和DVL 的測(cè)量坐標(biāo)系存在誤差時(shí)，即DVL 存在安裝誤差時(shí)，必然會(huì)影響組合導(dǎo)航的精度。在工程實(shí)踐中，儀器設(shè)備都存在誤差，安裝偏角誤差無(wú)法避免，會(huì)造成DVL 和IMU 對(duì)速度信息測(cè)量不一致[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 位置觀測(cè)標(biāo)定算法原理

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

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

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

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

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

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

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

可由下式計(jì)算：

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

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

計(jì)算得到利用速度觀測(cè)算法的DVL 比例因子誤差為：

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

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

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

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

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

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

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

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

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

根據(jù)梯度下降四元數(shù)的估計(jì)理論算法，構(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)是的簡(jiǎn)單表示形式，J是的簡(jiǎn)單表示。由式(16)可知，需 要求解權(quán)值系數(shù)μ。為了實(shí)現(xiàn)更精確的誤差標(biāo)定，本文將該系數(shù)看作為一個(gè)變化量，構(gòu)造式(19)表示的梯 度四元數(shù)，通過(guò)迭代計(jì)算，實(shí)現(xiàn)該系數(shù)的最優(yōu)化。

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

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

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

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

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

3 仿真驗(yàn)證

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

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

仿真過(guò)程中，仿真時(shí)間為600 s，在每次方位轉(zhuǎn)彎時(shí)，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，隨機(jī)游走0.02 (°)/h；加速度計(jì)常值偏置500 μg，隨機(jī)偏置50 μg；IMU 數(shù)據(jù)輸出頻率200 Hz，捷聯(lián)結(jié)算周期5 ms；GNSS 數(shù)據(jù)輸出頻率1 Hz；DVL 的測(cè)速精度0.5%v±0.5cm/s，比例因子誤差設(shè)置為0.005；數(shù)據(jù)輸出頻率為1 Hz；DVL與SINS 之間的桿臂長(zhǎng)度設(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 種方法下的比例因子估計(jì)值誤差對(duì)比圖，δk表示比例因子估計(jì)值誤差。圖4～6 為3 種方法下的安裝角估計(jì)誤差的對(duì)比圖，δφ θ、δφ γ、δφ Ψ分別表示x軸安裝角估計(jì)誤差、y軸安裝角估計(jì)誤差和z軸安裝角估計(jì)誤差。

圖3 仿真軌跡 Fig.3 Simulation trajectory

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

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

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

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

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

4 半物理試驗(yàn)驗(yàn)證

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

圖8 樣機(jī)跑車和安裝實(shí)物圖 Fig.8 Prototype car and installation physical

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

圖9 車載試驗(yàn)軌跡 Fig.9 Prototype car trajectory

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

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

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

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

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

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

5 結(jié) 論

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