基于羅德里格矩陣變換的水下導(dǎo)航系統(tǒng)校準(zhǔn)算法研究

張森， 郭錦標(biāo)， 吳媛媛

(1.海軍工程大學(xué) 電子工程學(xué)院， 湖北 武漢 430033； 2.31003部隊(duì)， 北京 100089)

收稿日期：2019-05-07

基金項(xiàng)目：國(guó)家高技術(shù)研究發(fā)展計(jì)劃項(xiàng)目(2014AA093405)；國(guó)家自然科學(xué)基金項(xiàng)目(61101205、41976177)

作者簡(jiǎn)介：張森(1982—)，男，副教授，碩士生導(dǎo)師。E-mail：johnson_xh@sina.com

0 引言

在國(guó)家“863”計(jì)劃項(xiàng)目支持下，作者團(tuán)隊(duì)提出了一種新型水下導(dǎo)航系統(tǒng)，稱(chēng)之為多正交信號(hào)水下導(dǎo)航系統(tǒng)[1]。該系統(tǒng)由發(fā)射端和接收端組成，發(fā)射端包含4個(gè)發(fā)射陣元，同時(shí)發(fā)射一組相互正交的信號(hào)，用一個(gè)水聽(tīng)器作為接收端接收信號(hào)。多正交信號(hào)水下導(dǎo)航系統(tǒng)可為水下用戶(hù)提供廣播式的定位信息，能實(shí)現(xiàn)潛艇、無(wú)人水下航行器、潛水員及水下平臺(tái)(如固定式水聲監(jiān)聽(tīng)陣等)等用戶(hù)的自身隱蔽定位，有廣闊的應(yīng)用前景。

發(fā)射端可安裝于運(yùn)動(dòng)平臺(tái)，稱(chēng)為運(yùn)動(dòng)平臺(tái)型，也可布放于海底，稱(chēng)為海底布放型。當(dāng)海底布放型系統(tǒng)布放發(fā)射端后，無(wú)法直接獲取該發(fā)射端的精確位置以及姿態(tài)角信息，因而需要通過(guò)校準(zhǔn)得到這些參數(shù)，用以實(shí)現(xiàn)位置解算。即使發(fā)射端安裝高精度的姿態(tài)傳感器，可測(cè)量基陣的姿態(tài)，但基陣與姿態(tài)傳感器安裝時(shí)通常存在一定的偏差，也需要通過(guò)校準(zhǔn)進(jìn)行估計(jì)得出。

目前應(yīng)用于水下導(dǎo)航定位系統(tǒng)校準(zhǔn)的方法多是采用非線(xiàn)性最小二乘法[2-4]，該方法的核心是將非線(xiàn)性方程用泰勒公式展開(kāi)，非線(xiàn)性方程線(xiàn)性化后采用迭代的計(jì)算方法得到方程未知量的解。姿態(tài)角估計(jì)時(shí)需要設(shè)置初始值，一般都是小角度的近似，而多正交信號(hào)水下導(dǎo)航系統(tǒng)的發(fā)射端位于水下，無(wú)法獲得發(fā)射端姿態(tài)的先驗(yàn)知識(shí)，并且其角度也可能很大，使得需要初始值的傳統(tǒng)非線(xiàn)性最小二乘法可能無(wú)法收斂。歸納起來(lái)，這是一個(gè)在無(wú)姿態(tài)先驗(yàn)知識(shí)條件下對(duì)發(fā)射端姿態(tài)的估計(jì)問(wèn)題。

在忽略聲線(xiàn)彎曲或進(jìn)行等效聲速近似[5]的情形下，發(fā)射端姿態(tài)的估計(jì)問(wèn)題是一個(gè)空間坐標(biāo)變換問(wèn)題，除了最小二乘法外，還有學(xué)者利用四元數(shù)[6-7]、奇異值分解[8]、羅德里格矩陣變換[9-11]等方法進(jìn)行相關(guān)研究，但羅德里格矩陣法只需估計(jì)3個(gè)參數(shù)，具有計(jì)算方便、適應(yīng)任意旋轉(zhuǎn)角的估計(jì)等優(yōu)點(diǎn)。為此，本文將研究基于羅德里格矩陣變換的多正交信號(hào)水下導(dǎo)航系統(tǒng)姿態(tài)角估計(jì)，以此完成系統(tǒng)校準(zhǔn)。

1 水下導(dǎo)航系統(tǒng)校準(zhǔn)模型

圖1所示為本系統(tǒng)的校準(zhǔn)模型，主要包括水下的發(fā)射端和接收端以及試驗(yàn)船上的數(shù)據(jù)處理部分，船上安裝的全球定位系統(tǒng)(GPS)用于測(cè)量接收端的位置信息，進(jìn)行性能的比對(duì)。

圖1 多正交信號(hào)水下導(dǎo)航系統(tǒng)校準(zhǔn)模型示意圖Fig.1 Calibration model of underwater navigation system based on multi-orthogonal signals

需要說(shuō)明的是，圖1為校準(zhǔn)示意圖，在實(shí)際應(yīng)用時(shí)，將發(fā)射端置于水底，經(jīng)過(guò)系統(tǒng)校準(zhǔn)獲取校準(zhǔn)參數(shù)后，接收端和信號(hào)處理模塊將安裝在水下運(yùn)動(dòng)載體上，為其實(shí)時(shí)解算自身位置，不再需要GPS.

接收端通過(guò)接收信號(hào)完成自身位置的解算，自身位置分為兩種：一種是相對(duì)于發(fā)射端的位置，稱(chēng)為相對(duì)位置；另外一種是絕對(duì)位置，即在地理坐標(biāo)系下的位置。利用聲學(xué)原理解算相對(duì)位置，采用坐標(biāo)間的轉(zhuǎn)換關(guān)系將相對(duì)位置轉(zhuǎn)換為絕對(duì)位置，即采用(1)式完成：

(1)

2 基于羅德里格矩陣變換的姿態(tài)角估計(jì)算法

基于羅德里格矩陣變換的旋轉(zhuǎn)角估計(jì)在三維坐標(biāo)轉(zhuǎn)換中表現(xiàn)出比非線(xiàn)性最小二乘法更好的效果[9]，下面分析其姿態(tài)角估計(jì)的過(guò)程。

旋轉(zhuǎn)矩陣是一個(gè)正交矩陣，故而有下列的關(guān)系：

(2)

式中：上標(biāo)T表示矩陣轉(zhuǎn)置。

(3)

式中：I為單位向量。

坐標(biāo)轉(zhuǎn)換(1)式用羅德里格矩陣變換代替旋轉(zhuǎn)矩陣，則可以得到

LXR=LXB+(I+S)(I-S)-1AXJ,

(4)

通過(guò)數(shù)學(xué)處理，可以將(4)式改寫(xiě)為

(I-S)(LXR-LXB)=(I+S)AXJ,

(5)

化簡(jiǎn)可得

S(AXJ+LXR-LXB)=LXR-LXB-AXJ.

(6)

為表達(dá)方便，令

AXJ+LXR-LXB=[p1,p2,p3]T，LXR-LXB-AXJ=[q1,q2,q3]T，

(7)

式中：p1、p2、p3、q1、q2、q3為實(shí)數(shù)。

此時(shí)，將(6)式展開(kāi)可化簡(jiǎn)得

(8)

令

(9)

這樣便可建立方程：

q=p×A.

(10)

在校準(zhǔn)路徑上進(jìn)行n次測(cè)量，則可列n個(gè)方程，寫(xiě)成矩陣形式則可表示為

Q=P×A,

(11)

其中，

P=[p1;p2;…;pn]3n×3,Q=[q1;q2;…;qn]3n×1，

(12)

式中：p1、p2、…、pn，q1、q2、…、qn均為(9)式中的矩陣，下標(biāo)表示測(cè)量的序號(hào)。

采用最小二乘方法解算出未知量[11-12]，則可求得

A=(PTP)-1PTQ.

(13)

(14)

最后再結(jié)合旋轉(zhuǎn)矩陣的表達(dá)式即可解算出發(fā)射端的姿態(tài)角參數(shù)。

3 兩種姿態(tài)角估計(jì)方法性能比較

本節(jié)將通過(guò)仿真比較兩種姿態(tài)角估計(jì)方法的性能。仿真采用MATLAB 2014a軟件，具體的仿真環(huán)境為：選擇文獻(xiàn)[2]的“⊕”字型的校準(zhǔn)航跡，船繞著發(fā)射端航行，發(fā)射端的水平位置位于繞行軌跡內(nèi)部，接收端在航跡上均勻取測(cè)量點(diǎn)，在每個(gè)測(cè)量點(diǎn)上記錄下數(shù)據(jù)。假設(shè)水面深度為0，向下為正。圖2所示為接收端的航行軌跡與發(fā)射端的位置關(guān)系，軌跡上“⊕”字型的點(diǎn)代表航跡上的測(cè)量點(diǎn)，下方一個(gè)單獨(dú)點(diǎn)表示為發(fā)射端位置，經(jīng)驗(yàn)聲速值取1 500 m/s. 假設(shè)發(fā)射端的地理坐標(biāo)為L(zhǎng)XB=[xB,yB,zB]=[50 m,100 m,500 m]，接收端繞行的半徑為400 m.

圖2 “⊕”字型校準(zhǔn)航跡Fig.2 Calibrated track of ‘⊕’font

3.1 可收斂性比較

文獻(xiàn)[2]研究了非線(xiàn)性最小二乘法的校準(zhǔn)過(guò)程，姿態(tài)角估計(jì)是基于小角度近似展開(kāi)，而本系統(tǒng)在布放信標(biāo)時(shí)不能保證其姿態(tài)角為小角度，因而基于本系統(tǒng)的非線(xiàn)性最小二乘法姿態(tài)角估計(jì)采用非小角度近似。由于非線(xiàn)性最小二乘姿態(tài)角估計(jì)算法的開(kāi)始需要預(yù)先設(shè)置一個(gè)初始值，不失一般性，可設(shè)為(0°，0°，0°). 下面比較非線(xiàn)性最小二乘法與羅德里格矩陣變換法估計(jì)角度的可收斂性，假設(shè)系統(tǒng)信號(hào)傳播模型不存在測(cè)量誤差，僅用上述兩種方法分別估計(jì)發(fā)射端的姿態(tài)角，得到估計(jì)結(jié)果如表1所示。

非線(xiàn)性最小二乘法的迭代控制精度為1×10-8，在小角度估計(jì)時(shí)其估計(jì)結(jié)果也是十分接近真實(shí)值，但隨著角度的增大，所需迭代次數(shù)越來(lái)越多，到了較大的角度時(shí)出現(xiàn)錯(cuò)誤，最后一行則是由于迭代發(fā)散而得不到估計(jì)結(jié)果。而羅德里格矩陣法沒(méi)有非線(xiàn)性的近似，估計(jì)角度不存在誤差，沒(méi)有估計(jì)角度大小的限制，并且僅需要一次計(jì)算，簡(jiǎn)便高效。實(shí)際的水下發(fā)射端姿態(tài)角，往往可能是橫搖角和縱搖角為較小的值，而航向角的值就可能很大。下面將橫搖角和縱搖角設(shè)定為某個(gè)較小的值(不失一般性地設(shè)定為2°和3°)，改變航向角，兩種方法估計(jì)結(jié)果如圖3所示。從圖3可以看出，當(dāng)航向角超過(guò)±60°左右，非線(xiàn)性最小二乘算法會(huì)出現(xiàn)無(wú)法收斂的情況，而羅德里格矩陣變換法則能穩(wěn)定地估計(jì)。

表1 兩種算法姿態(tài)角估計(jì)結(jié)果比較Tab.1 Estimated attitude angles of different algorithms

圖3 改變航向角時(shí)估計(jì)誤差對(duì)比Fig.3 Comparison of estimated errors at different heading angles

3.2 估計(jì)精度比較

下面將研究不同GPS測(cè)量誤差、不同信噪比(SNR)和發(fā)射端不同的姿態(tài)角時(shí)算法的估計(jì)精度，其中SNR決定了時(shí)延測(cè)量誤差和陣元間時(shí)延差測(cè)量誤差，如圖4所示，圖中估計(jì)誤差為500次蒙特卡羅仿真的均方根誤差。

圖4(a)是SNR為20 dB、發(fā)射端姿態(tài)橫搖角、縱搖角和航向角分別為5°、10°和25°時(shí)，估計(jì)誤差隨著GPS定位誤差變化的情況，從中可以看出，隨著GPS誤差的增大，兩種方法估計(jì)誤差均增大，但羅德里格矩陣變換法估計(jì)誤差小于非線(xiàn)性最小二乘法。

圖4(b)是GPS定位誤差為0.5 m、發(fā)射端姿態(tài)橫搖角、縱搖角和航向角分別為5°、10°和25°時(shí)，估計(jì)誤差隨著SNR變化的情況，從中可以看出，隨著SNR增大，二者估計(jì)誤差均下降，且羅德里格矩陣變換法優(yōu)于非線(xiàn)性最小二乘方法。當(dāng)SNR大于25 dB時(shí)，誤差下降變得平緩。

圖4 不同條件時(shí)姿態(tài)角估計(jì)誤差的對(duì)比Fig.4 Comparisons of estimated errors

圖4(c)和圖4(d)是GPS定位誤差為0.5 m、SNR為20 dB時(shí)，發(fā)射端姿態(tài)角(橫搖角、縱搖角和航向角)為不同時(shí)估計(jì)誤差的變化情況。圖中橫坐標(biāo)為圖中參考角度的倍數(shù)，如圖4(c)和圖4(d)中參考角度分別為1°、3°、5°和2°、3°、5°，若當(dāng)選擇倍數(shù)為4時(shí)，則發(fā)射端姿態(tài)角分別為4°、12°、20°和8°、12°和20°. 比較圖4(c)和圖4(d)姿態(tài)角發(fā)生小的變化，非線(xiàn)性最小二乘方法對(duì)于不同的姿態(tài)角估計(jì)出現(xiàn)較大誤差起伏，而羅德里格矩陣變換法起伏很小。從總體上看，姿態(tài)角越大，非線(xiàn)性最小二乘方法估計(jì)誤差越大，而羅德里格矩陣變換法估計(jì)誤差隨角度變化較小。因此，羅德里格矩陣變換法對(duì)發(fā)射端姿態(tài)角具有更優(yōu)的適應(yīng)性和穩(wěn)健性。

基于羅德里格矩陣變換的姿態(tài)角估計(jì)，不需要姿態(tài)角的先驗(yàn)信息，沒(méi)有非線(xiàn)性化方程的線(xiàn)性近似，消除線(xiàn)性化過(guò)程帶來(lái)的誤差，計(jì)算過(guò)程簡(jiǎn)便、高效，不存在待估計(jì)角度大小的限制，因而與傳統(tǒng)非線(xiàn)性最小二乘法的姿態(tài)角估計(jì)方法相比，具有更高的估計(jì)效率、估計(jì)精度和更強(qiáng)的適應(yīng)性。

4 試驗(yàn)分析

2016年12月在湖北宜昌的隔河巖水庫(kù)對(duì)多正交信號(hào)水下導(dǎo)航系統(tǒng)進(jìn)行了湖上試驗(yàn)。試驗(yàn)區(qū)域水深約100 m.

試驗(yàn)時(shí)信標(biāo)發(fā)射端布放于湖底，通過(guò)浮球標(biāo)識(shí)信標(biāo)位置，信號(hào)接收端測(cè)量船繞信標(biāo)航行，船上同時(shí)采用差分GPS進(jìn)行位置測(cè)量，試驗(yàn)現(xiàn)場(chǎng)如圖5所示。測(cè)量船采取繞行的方式收集數(shù)據(jù)，經(jīng)過(guò)數(shù)據(jù)處理可以獲得校準(zhǔn)參數(shù)，以此進(jìn)行位置解算。與未經(jīng)過(guò)校準(zhǔn)的位置解算對(duì)比時(shí)，可以看出校準(zhǔn)前后的變化。

圖5 湖上試驗(yàn)圖Fig.5 Lake trial

圖6是測(cè)量船所采取的校準(zhǔn)航跡，“GPS”代表的路線(xiàn)是由測(cè)量船上GPS測(cè)量出的航跡，“解算”代表的路線(xiàn)是水下導(dǎo)航系統(tǒng)解算出的接收端位置，此時(shí)的解算結(jié)果是系統(tǒng)未經(jīng)過(guò)校準(zhǔn)所得，可以看到解算結(jié)果偏離真實(shí)位置。

圖6 未經(jīng)過(guò)校準(zhǔn)的位置解算與GPS測(cè)量的比較Fig.6 Comparison of calculated and GPS-measured results of position without calibration

圖7 非線(xiàn)性最小二乘法校準(zhǔn)結(jié)果Fig.7 Result of calibration by nonlinear least square method

圖8 羅德里格矩陣法校準(zhǔn)結(jié)果Fig.8 Result of calibration by Rodrigo matrix method

發(fā)射端位置估計(jì)采用相同的方法[13]，而姿態(tài)角估計(jì)則分別采用非線(xiàn)性最小二乘法和羅德里格矩陣法。水下航行體的深度信息一般會(huì)由自身攜帶的深度計(jì)等設(shè)備獲取，因此這里僅比較水平方向的位置解算結(jié)果。將通過(guò)校準(zhǔn)得到的參數(shù)用于校準(zhǔn)接收端的位置解算結(jié)果，經(jīng)校準(zhǔn)后的結(jié)果與GPS測(cè)量結(jié)果比較，觀(guān)察解算結(jié)果的誤差分布，如圖7、圖8所示，最大作用距離Rmax約390 m. 從圖7、圖8上可以看出，采用非線(xiàn)性最小二乘法和羅德里格矩陣變換法得到的校準(zhǔn)參數(shù)均能達(dá)到校準(zhǔn)位置解算結(jié)果的目的，采用羅德里格矩陣變換法的校準(zhǔn)結(jié)果誤差點(diǎn)分布更為集中在原點(diǎn)附近。為更直觀(guān)比較校準(zhǔn)后的誤差點(diǎn)分布，采用均方根誤差統(tǒng)計(jì)校準(zhǔn)的位置解算誤差大小，如表2所示。從表2中可以看出，采用本文所研究的羅德里格矩陣姿態(tài)角估計(jì)方法，比非線(xiàn)性最小二乘法具有更佳的校準(zhǔn)性能。

表2 校準(zhǔn)結(jié)果及校準(zhǔn)誤差的比較Tab.2 Comparison of calibration results and errors

為驗(yàn)證校準(zhǔn)參數(shù)的可用性，將兩種校準(zhǔn)方法得到的校準(zhǔn)參數(shù)用于解算新的航跡，得到結(jié)果如圖9、圖10所示。該航跡最大作用距離Rmax約305 m. 采用均方根誤差統(tǒng)計(jì)校準(zhǔn)后的位置解算誤差大小，如表3所示。從圖9、圖10和均方根誤差均可看出，羅德里格矩陣變換法的測(cè)量誤差明顯小于非線(xiàn)性最小二乘法。

圖9 非線(xiàn)性最小二乘法校準(zhǔn)結(jié)果Fig.9 Result of calibration by nonlinear least square method

圖10 羅德里格矩陣法校準(zhǔn)結(jié)果Fig.10 Result of calibration by Rodrigo matrix method

表3 校準(zhǔn)后誤差比較

Tab.3 Comparison of errors after calibration

校準(zhǔn)算法水平方向均方根誤差/m相對(duì)定位精度非線(xiàn)性最小二乘法2.370.78%Rmax羅德里格矩陣變換法1.700.56%Rmax

5 結(jié)論

本文基于多正交信號(hào)水下導(dǎo)航系統(tǒng)，研究了利用羅德里格矩陣變換的系統(tǒng)校準(zhǔn)算法，進(jìn)行了姿態(tài)角估計(jì)。得到以下結(jié)論：

1)采用羅德里格矩陣變換法的姿態(tài)角估計(jì)，無(wú)需姿態(tài)角的先驗(yàn)信息，無(wú)需線(xiàn)性化近似，算法簡(jiǎn)單、高效，避免了線(xiàn)性化過(guò)程的誤差。

2)非線(xiàn)性最小二乘法姿態(tài)角估計(jì)具有角度大小的限制，適用于3個(gè)方向的角度(橫搖角、縱搖角、航向角)均為小角度的情況，而羅德里格矩陣變換法的姿態(tài)角估計(jì)不存在限制，適用性更強(qiáng)。

3)湖試結(jié)果表明，采用本文研究的姿態(tài)角估計(jì)方法校準(zhǔn)效果更好。