基于羅德里格矩陣變換的水下導航系統校準算法研究

張森， 郭錦標， 吳媛媛

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

收稿日期：2019-05-07

基金項目：國家高技術研究發展計劃項目(2014AA093405)；國家自然科學基金項目(61101205、41976177)

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

0 引言

在國家“863”計劃項目支持下，作者團隊提出了一種新型水下導航系統，稱之為多正交信號水下導航系統[1]。該系統由發射端和接收端組成，發射端包含4個發射陣元，同時發射一組相互正交的信號，用一個水聽器作為接收端接收信號。多正交信號水下導航系統可為水下用戶提供廣播式的定位信息，能實現潛艇、無人水下航行器、潛水員及水下平臺(如固定式水聲監聽陣等)等用戶的自身隱蔽定位，有廣闊的應用前景。

發射端可安裝于運動平臺，稱為運動平臺型，也可布放于海底，稱為海底布放型。當海底布放型系統布放發射端后，無法直接獲取該發射端的精確位置以及姿態角信息，因而需要通過校準得到這些參數，用以實現位置解算。即使發射端安裝高精度的姿態傳感器，可測量基陣的姿態，但基陣與姿態傳感器安裝時通常存在一定的偏差，也需要通過校準進行估計得出。

目前應用于水下導航定位系統校準的方法多是采用非線性最小二乘法[2-4]，該方法的核心是將非線性方程用泰勒公式展開，非線性方程線性化后采用迭代的計算方法得到方程未知量的解。姿態角估計時需要設置初始值，一般都是小角度的近似，而多正交信號水下導航系統的發射端位于水下，無法獲得發射端姿態的先驗知識，并且其角度也可能很大，使得需要初始值的傳統非線性最小二乘法可能無法收斂。歸納起來，這是一個在無姿態先驗知識條件下對發射端姿態的估計問題。

在忽略聲線彎曲或進行等效聲速近似[5]的情形下，發射端姿態的估計問題是一個空間坐標變換問題，除了最小二乘法外，還有學者利用四元數[6-7]、奇異值分解[8]、羅德里格矩陣變換[9-11]等方法進行相關研究，但羅德里格矩陣法只需估計3個參數，具有計算方便、適應任意旋轉角的估計等優點。為此，本文將研究基于羅德里格矩陣變換的多正交信號水下導航系統姿態角估計，以此完成系統校準。

1 水下導航系統校準模型

圖1所示為本系統的校準模型，主要包括水下的發射端和接收端以及試驗船上的數據處理部分，船上安裝的全球定位系統(GPS)用于測量接收端的位置信息，進行性能的比對。

圖1 多正交信號水下導航系統校準模型示意圖Fig.1 Calibration model of underwater navigation system based on multi-orthogonal signals

需要說明的是，圖1為校準示意圖，在實際應用時，將發射端置于水底，經過系統校準獲取校準參數后，接收端和信號處理模塊將安裝在水下運動載體上，為其實時解算自身位置，不再需要GPS.

接收端通過接收信號完成自身位置的解算，自身位置分為兩種：一種是相對于發射端的位置，稱為相對位置；另外一種是絕對位置，即在地理坐標系下的位置。利用聲學原理解算相對位置，采用坐標間的轉換關系將相對位置轉換為絕對位置，即采用(1)式完成：

(1)

2 基于羅德里格矩陣變換的姿態角估計算法

基于羅德里格矩陣變換的旋轉角估計在三維坐標轉換中表現出比非線性最小二乘法更好的效果[9]，下面分析其姿態角估計的過程。

旋轉矩陣是一個正交矩陣，故而有下列的關系：

(2)

式中：上標T表示矩陣轉置。

(3)

式中：I為單位向量。

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

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

(4)

通過數學處理，可以將(4)式改寫為

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

(5)

化簡可得

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

(6)

為表達方便，令

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

(7)

式中：p1、p2、p3、q1、q2、q3為實數。

此時，將(6)式展開可化簡得

(8)

令

(9)

這樣便可建立方程：

q=p×A.

(10)

在校準路徑上進行n次測量，則可列n個方程，寫成矩陣形式則可表示為

Q=P×A,

(11)

其中，

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

(12)

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

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

A=(PTP)-1PTQ.

(13)

(14)

最后再結合旋轉矩陣的表達式即可解算出發射端的姿態角參數。

3 兩種姿態角估計方法性能比較

本節將通過仿真比較兩種姿態角估計方法的性能。仿真采用MATLAB 2014a軟件，具體的仿真環境為：選擇文獻[2]的“⊕”字型的校準航跡，船繞著發射端航行，發射端的水平位置位于繞行軌跡內部，接收端在航跡上均勻取測量點，在每個測量點上記錄下數據。假設水面深度為0，向下為正。圖2所示為接收端的航行軌跡與發射端的位置關系，軌跡上“⊕”字型的點代表航跡上的測量點，下方一個單獨點表示為發射端位置，經驗聲速值取1 500 m/s. 假設發射端的地理坐標為LXB=[xB,yB,zB]=[50 m,100 m,500 m]，接收端繞行的半徑為400 m.

圖2 “⊕”字型校準航跡Fig.2 Calibrated track of ‘⊕’font

3.1 可收斂性比較

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

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

表1 兩種算法姿態角估計結果比較Tab.1 Estimated attitude angles of different algorithms

圖3 改變航向角時估計誤差對比Fig.3 Comparison of estimated errors at different heading angles

3.2 估計精度比較

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

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

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

圖4 不同條件時姿態角估計誤差的對比Fig.4 Comparisons of estimated errors

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

基于羅德里格矩陣變換的姿態角估計，不需要姿態角的先驗信息，沒有非線性化方程的線性近似，消除線性化過程帶來的誤差，計算過程簡便、高效，不存在待估計角度大小的限制，因而與傳統非線性最小二乘法的姿態角估計方法相比，具有更高的估計效率、估計精度和更強的適應性。

4 試驗分析

2016年12月在湖北宜昌的隔河巖水庫對多正交信號水下導航系統進行了湖上試驗。試驗區域水深約100 m.

試驗時信標發射端布放于湖底，通過浮球標識信標位置，信號接收端測量船繞信標航行，船上同時采用差分GPS進行位置測量，試驗現場如圖5所示。測量船采取繞行的方式收集數據，經過數據處理可以獲得校準參數，以此進行位置解算。與未經過校準的位置解算對比時，可以看出校準前后的變化。

圖5 湖上試驗圖Fig.5 Lake trial

圖6是測量船所采取的校準航跡，“GPS”代表的路線是由測量船上GPS測量出的航跡，“解算”代表的路線是水下導航系統解算出的接收端位置，此時的解算結果是系統未經過校準所得，可以看到解算結果偏離真實位置。

圖6 未經過校準的位置解算與GPS測量的比較Fig.6 Comparison of calculated and GPS-measured results of position without calibration

圖7 非線性最小二乘法校準結果Fig.7 Result of calibration by nonlinear least square method

圖8 羅德里格矩陣法校準結果Fig.8 Result of calibration by Rodrigo matrix method

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

表2 校準結果及校準誤差的比較Tab.2 Comparison of calibration results and errors

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

圖9 非線性最小二乘法校準結果Fig.9 Result of calibration by nonlinear least square method

圖10 羅德里格矩陣法校準結果Fig.10 Result of calibration by Rodrigo matrix method

表3 校準后誤差比較

Tab.3 Comparison of errors after calibration

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

5 結論

本文基于多正交信號水下導航系統，研究了利用羅德里格矩陣變換的系統校準算法，進行了姿態角估計。得到以下結論：

1)采用羅德里格矩陣變換法的姿態角估計，無需姿態角的先驗信息，無需線性化近似，算法簡單、高效，避免了線性化過程的誤差。

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

3)湖試結果表明，采用本文研究的姿態角估計方法校準效果更好。