基于向量投影的立體陣定位算法

孫大軍 蔡 珩 鄭翠娥 程馳宇

(哈爾濱工程大學水聲技術全國重點實驗室 哈爾濱 150001)(海洋信息獲取與安全工信部重點實驗室(哈爾濱工程大學) 工業和信息化部 哈爾濱 150001)(哈爾濱工程大學水聲工程學院 哈爾濱 150001)

1 引言

超短基線定位技術是水下航行器或潛水員進入深海、探測深海和開發深海的關鍵技術[1-5]。目前，超短基線定位技術在民用領域已實現商業化。法國iXblue公司的POSIDONIA II[6]、英國Sonardyne公司的HPT[7]、挪威Kongsberg公司的HiPAP[8]均為國際上知名的超短基線定位設備，代表著當今世界的最高水平。

超短基線通過測量目標到達聲學基陣各個基元的傳播時延來估計目標相對聲學基陣的方位和距離從而確定目標的位置[9,10]。傳統聲學基陣大多為線陣和平面陣，文獻[11]介紹了一種針對線陣的超短基線定位算法，此算法利用不同觀測點測得的空間方位角建立非線性方程，通過擬牛頓法估計目標位置。針對線陣的算法需綜合利用不同觀測點的角度信息，且要迭代求解非線性方程，定位效率低，難以滿足對運動目標的高幀率定位需求。平面陣能在每個觀測點完成一次目標位置的解算，能提供高幀率位置信息。文獻[12-14]介紹了幾種常見平面陣陣型(L型平面陣、十字平面陣、正五邊形平面陣)的定位解算方法。這些方法都存在一個特點，即能直接獲得目標的水平定位結果，目標的垂向定位結果需通過斜距和水平結果間接獲得，這就導致了目標垂向定位精度受水平定位精度影響較大。為使目標垂向定位結果與水平獨立，需要采用立體陣以直接獲得目標垂直方向的定位結果?，F有適用于立體陣的定位算法較少；文獻[15]介紹了針對4元立體陣的超短基線定位算法。此算法利用4元立體陣的對稱結構，獨立構造出目標垂直位置的觀測方程，能直接獲得目標的垂向位置。但此算法僅適用于特定幾何構型的立體陣，有一定的局限性。為實現一個算法適應不同結構立體陣的解算；文獻[16]介紹了三角分解算法。三角分解算法首先將立體陣分解成不同朝向的3元陣組合，每個組合可求解一個目標位置，其次對所有組合的定位結果進行平均，作為目標的最終定位結果。三角分解算法涉及大量坐標基轉換解算步驟極其繁瑣，且當陣元數量較多時，運算量將急劇增大。此外，針對立體陣這類復雜的陣型結構，三角分解算法的定位模型十分復雜，難以分析其誤差特性。

對于三角分解算法存在的問題，本文從向量的角度提出了另一種適用于立體陣的算法，基于向量投影的立體陣定位算法。本文算法從向量投影的角度構建立體陣中各基線向量與目標方位之間的觀測方程，通過解觀測方程組實現對目標方向余弦向量的估計。相較于三角分解算法，本文提出的算法時間復雜度更小，且待估計參數與量測數據間的關系呈線性形式，易分析定位誤差與各參量之間的影響關系。

2 三角分解算法及其缺陷

由引言部分可知，三角分解算法能適用于立體陣型的解算。算法的思想可以概括如下：(1)將立體陣分解為不同朝向的3元陣組合，每個組合定義一個本地坐標系；(2)確定目標在每個本地坐標系下的坐標；(3)通過坐標轉換獲得目標在基陣坐標系下[17]的坐標；(4)對所有組合的結果進行平均，獲得目標的最終位置。

下面以某一個3元陣組合為例，給出具體定位原理。假設3元陣定義的本地坐標系為o-xLocalyLocalzLocal；目標在本地坐標系下的位置XLocal可由式(1)和式(2)獲得。

其中，R代表目標到本地坐標系原點的距離。ν代表3 元陣中兩基線的夾角。cosα=cτ1/d1,cosβ=cτ2/d2代表目標位置向量與3元陣中兩基線夾角的余弦值。τ1,τ2分別代表聲波到達兩基線中各基元間的時延差，d1,d2代表基線長度，c代表聲波在水中的傳播速度。

由于本地坐標系與基陣坐標系并不一致，需通過坐標旋轉獲得目標在基陣坐標系下的位置XArray。

其中，RLA代表本地坐標系到基陣坐標系的旋轉矩陣，θ代表兩個坐標系之間的歐拉角矢量。

至此，可獲得3元陣組合的定位結果。文獻[16]給出了三角分解算法的詳細推導，此處不再過多贅述。三角分解算法的主要量測數據為cτ1,cτ2，將其分別記為L1=cτ1,L2=cτ2，分析量測誤差ΔL1, ΔL2對定位誤差ΔXArray的影響程度。將式(3)寫作XArray=RLA(θ)XLocal=f，對其兩邊同時求微分，有

式(4)的展開式十分復雜，從中看不到明顯的關系。但由于f與cosα, cosβ有關，可知定位誤差ΔXArray不僅與量測誤差ΔL1, ΔL2有關，還與目標的方位有關。

綜上，三角分解算法雖能適用于立體陣的解算，但也存在如下缺陷。(1)計算量大。對于一個N元立體陣，共能分解出種朝向的3元陣組合，每個組合都需要計算一次目標位置，計算量較大。(2)解算所需參數難以直觀獲取。三角分解算法需知道本地坐標系與基陣坐標系間的歐拉角，對立體陣這類復雜的陣型結構而言，該量難以直觀獲得。(3)誤差分析困難。三角分解算法的定位誤差解析式十分復雜，難以通過解析式直觀獲得定位誤差與各參量間的關系。

3 基于向量投影的立體陣定位算法

針對三角分解算法的缺陷，本文提出基于向量投影的立體陣定位算法(以下簡稱向量投影算法)，從向量投影的角度構建立體陣中各基線向量與目標方位之間的觀測方程，實現對三角分解算法定位模型的簡化。

3.1 模型構建及參數求解

假設在基陣坐標系下，立體陣中任意兩基元的坐標為Xj=[xj,yj,zj]T與Xi=[xi,yi,zi]T，此時基線i,j的方向向量可表示為Xji=Xj-Xi，如圖1(a)所示。此外假設目標在基陣坐標系下的方向余弦為e=[cosα,cosβ,cosγ]T，方向余弦各角度的定義如圖1(b)所示。

圖1 基線與目標的方向向量

基于向量投影定理，可獲得目標的方向余弦向量e與基線向量Xji夾角的余弦值為

其中， (·)代表向量點積，θ代表向量e與Xji間的夾角，dij代表基線i,j的長度。

此外，由二元測向定理[17]，可通過基線間的時延差，獲得目標與基線i,j夾角的余弦值為

τij其中， 代表聲波到達基元i,j的時延差。聯立式(5)與式(6)可得基線向量與目標方向余弦向量的觀測方程：

將不同基線對應的量測數據及待估計參數以矩陣形式表達為

其中

基于最小二乘定理，目標的方向余弦e可根據式(10)獲得

假設基陣的幾何中心為基陣坐標系的原點，可以根據式(11)獲得目標到基陣坐標系原點的斜距R。

至此，目標的斜距與方向余弦均為已知量，目標的定位結果X可由式(12)獲得

3.2 向量投影算法的誤差分析

接下來對向量投影算法的估值進行誤差分析。對式(10)兩邊同時取微分，有

由式(13)可以看出，量測誤差ΔL傳遞到估值誤差 Δe的程度與A有關，而A由基線向量組成，與立體陣的幾何構型有關，為進一步分析估值誤差與立體陣幾何構型的關系，下面以一個普遍的4元立體陣為例進行推導。4元立體陣的構型如圖2所示。

圖2 4元立體陣

圖2中，1-3基線與2-4基線的長度相同，到xoy平面的距離相等，投影到xoy平面上分別與x軸、y軸重合，并關于原點對稱。定義立體陣4個基元的坐標為Xi=[xi,yi,zi]T(i=1,2,3,4)。為便于求解式(13)中ATA的逆，基于式(7)，以1-3基線向量，2-4基線向量，1-2加3-4基線向量構建觀測方程組，有

將式(14)中A代入式(13)中，易求得估值誤差與立體陣構型的關系

定義立體陣在x,y,z方向的孔徑為|x3-x1|,|y4-y2|, |z2-z1|；則由式(1 5)可知，增加|x3-x1|, |y4-y2|, |z2-z1|，能分別減小cosα,cosβ, cosγ的估值誤差。故增加立體陣3方向的孔徑，能降低估值誤差。

3.3 算法優勢分析

從上面的分析可以得出，向量投影算法通過解線性方程組即可實現對目標方向余弦向量的估計。相比于三角分解算法，向量投影算法具有以下優勢：

(1) 時間復雜度低。由文中的式(10)可知，向量投影算法的計算量主要集中在矩陣的運算上。對于一個N元立體陣，向量投影算法的獨立量測量個數為N-1個，對應可列出N-1個觀測方程，相應的A矩陣為(N-1)×3，L向量為(N-1)×1。根據矩陣運算的時間復雜度[18]，可計算出式(10)中的時間復雜度為O(21N+6)，根據時間復雜度的定義[18]，當N較大時，可忽略常數項及系數的影響，則向量投影算法的時間復雜度可簡化為O(N)。同樣對于一個N元立體陣，三角分解算法總共能分解出個朝向的3元陣組合，對于每個3元陣組合，都需計算一次目標位置，由于文中式(1)至式(3)與N無關，則三角分解算法的時間復雜度主要取決于3元組合的數量，即。忽略低階項及系數，可將三角分解算法的時間復雜度簡化為。由于的時間復雜度遠小于，由此可得向量投影算法的時間復雜度更低。

(2) 解算所需的參數更易獲取。相比于三角分解算法，向量投影算法無本地坐標系的定義，無需知道坐標系間的歐拉角，僅需輸入各基元在基陣坐標系下的坐標、量測時延及聲速，即可估計目標位置。

(3) 誤差分析直觀。針對立體陣這類復雜的陣型結構，傳統三角分解算法難以通過解析式獲得定位誤差與各參量間的關系；而對于向量投影算法，能通過解析式獲得定位誤差與立體陣孔徑間的關系。

4 仿真分析

為驗證本文算法的性能，本部分通過算法運行時間與定位誤差評價優劣。仿真中目標的位置采用球面坐標進行描述，如圖3所示。圖中R表示目標的斜距，E表示目標的俯仰角，定義目標位于xoy平面下方時為負，A表示目標的水平方位角，定義以x軸為起點，左手規則為正。

圖3 水平方位角與俯仰角的定義

4.1 算法運行時間對比

此部分仿真對比不同基元數量下，兩種算法的運行時間。仿真過程中，選擇一個固定目標，俯仰角-45°，方位角45°，斜距50 m。仿真所用陣型為不同基元數量的圓錐陣，如圖4(a)所示，頂基元距基陣水平面的距離為0.13 m，底面基元位于xoy平面上半徑為0.13 m，以原點為中心的圓周上。底面基元的數量由4增大到10，仿真統計兩種算法定位1 000次的運行時間，如圖4(b)所示。

圖4 算法運行時間對比

由圖4可看出，三角分解算法的運行時間遠大于向量投影算法，且隨著基元數量變多增長斜率變大。

4.2 不同誤差源下性能分析

此部分仿真分析不同量測誤差源對三角分解算法及向量投影算法定位誤差的影響。

由算法的觀測方程可以看出，兩種算法的定位誤差由聲速誤差、基元位置誤差及時延誤差共同決定，3種誤差中，基元位置誤差在實際作業前會通過高精度陣型校準技術[19]進行修正，因此忽略基元位置誤差的影響，重點分析聲速誤差和時延誤差的影響。以下仿真所用的陣型參數為圖2所示的4元立體陣。其中，x,y方向的孔徑為0.26 m，z方向的孔徑為0.13 m。目標位置設置與4.1節相同。

首先分析聲速誤差的影響。聲速誤差常以固定誤差的形式存在，仿真過程中，聲速誤差從0 m/s均勻變化到10 m/s。兩種算法的定位誤差如圖5所示。

圖5 聲速誤差

由圖5可看出隨著聲速誤差的增大，兩種算法的定位誤差近似線性增大。在此斜距和方位下，10 m/s的聲速誤差對三角分解算法的影響最大不到2.4%R；對向量投影算法的影響最大不到0.52%R。

接下來分析時延誤差的影響。時延誤差常以隨機誤差的形式存在。假設立體陣各基元的時延誤差統計獨立，且都服從均值為0，標準差為σ的正態分布。仿真過程中，標準差σ的大小從0 μs均勻增加到5 μs。在每個標準差下進行1 000次蒙特卡羅實驗。其他仿真條件與圖5相同。仿真結果如圖6所示。

圖6 時延誤差

由圖6可看出隨著時延誤差的增大，向量投影算法的定位誤差近似線性增大，但三角分解算法的定位誤差呈非線性關系。在此斜距和方位下，5 μs的時延誤差對兩種算法的影響最大均超過了4%R。

從圖5、圖6的仿真結果可得出以下結論：聲速誤差對兩種算法的定位誤差影響較小，時延誤差對兩種算法的定位誤差影響較大。在實際使用中，可通過提高信噪比和采樣率提升時延量測精度。

4.3 不同目標方位下性能分析

為更普遍地分析算法性能，本部分仿真計算同一量測誤差下，目標位于不同方位時兩種算法的定位誤差。仿真所用的陣型與4.2節一致，聲速誤差設置為5 m/s，時延誤差的標準差設置為5 μs。

先分析不同水平方位角的影響。仿真過程中，固定目標的俯仰角為-45°，斜距為50 m。目標的水平方位角從-180°均勻變化到180°。兩種算法的定位誤差如圖7所示。

圖7 不同水平方位角下定位誤差

由圖7可以看出，三角分解算法的定位誤差隨水平方位角的變化上下震蕩，而向量投影算法隨水平方位角的變化不大。兩種算法在不同水平方位角下的定位誤差各有優劣。

接下來分析不同俯仰角的影響。固定目標的水平方位角為45°不變。目標的俯仰角從-90°均勻變化到0°，兩種算法的定位誤差如圖8所示。

圖8 不同俯仰角下定位誤差

由圖8可以看出，三角分解算法的定位誤差受俯仰角的影響較大，而向量投影算法的幾乎不隨俯仰角變化。兩種算法在不同俯仰角下的定位誤差同樣各有優劣。

由圖7、圖8可知：不同目標方位下，兩種算法的定位誤差在數值上各有優劣；但三角分解算法的定位誤差受目標方位的影響較大，而向量投影算法的定位誤差幾乎不受目標方位的影響，性能更加穩定。

4.4 不同立體陣孔徑下性能分析

本文在誤差分析部分，得到了向量投影算法的估值誤差與立體陣孔徑成反比的結論。本節通過仿真驗證此結論的正確性。仿真中，量測誤差的設置與4.3節相同，目標位置的設置與4.1節相同。

基于圖2所示的4元立體陣，定義x,y方向的孔徑為水平孔徑，定義z方向的孔徑為垂直孔徑。首先分析水平孔徑的影響，仿真中，垂直孔徑固定為0.26 m不變，水平孔徑從0.26 m均勻增加到0.52 m。兩種算法的定位誤差如圖9所示。

圖9 不同水平孔徑下定位誤差

由圖9可以看出，隨著水平孔徑的增大，向量投影算法的水平定位誤差明顯降低，垂直定位誤差幾乎不變；三角分解算法的水平與垂直定位誤差隨著水平孔徑的增大，呈先增大后減小的趨勢。

接下來分析垂直孔徑的影響，水平孔徑固定為0.26 m不變，垂直孔徑從0.26 m均勻增加到0.52 m。兩種算法的定位誤差如圖10所示。

圖10 不同垂直孔徑下定位誤差

由圖10可以看出，隨著垂直孔徑的增大，向量投影算法的水平定位誤差變化不明顯，垂直定位誤差明顯降低。三角分解算法的水平與垂直定位誤差隨著垂直孔徑的增大而減小。

從圖9、圖10可得以下結論，隨著基陣水平與垂直孔徑的增大，向量投影算法的定位誤差明顯降低，驗證了誤差分析結論的正確性；但三角分解算法的定位誤差與陣型孔徑間無明顯的單調規律。

5 湖試數據處理與分析

2022年2月27日，在湖北省丹江口開展了超短基線定位系統定位精度驗證試驗。試驗期間平均水深約45 m。試驗設備包括：哈爾濱工程大學自研的超短基線5元立體陣(陣型具體參數如圖11所示)、聲信標，法國iXsea公司的PHINS高精度姿態測量儀，中國華測公司P5型GNSS設備。試驗中，姿態測量設備固定安裝在5元立體陣的腔體內部；超短基線5元立體陣與GNSS天線通過4.5 m長的三腳架剛性連接，安裝于船只右舷；5元立體陣入水深度約2 m；聲信標布放在湖底，深度約40 m。

圖11 湖試所用立體陣陣型

聲信標在大地坐標系[20]下的位置為先驗已知量，在試驗前通過標定方法獲得[20]。試驗過程中，聲信標布放在湖底不動，水面船只以聲信標為中心，走半徑約100 m的正反圓加正反叉航跡，如圖12所示；期間5元立體陣利用1至4號基元在每個定位周期內完成對聲信標的一次定位解算。

圖12 水面船航跡

由于試驗過程中，聲信標在大地坐標系下的位置未曾改變，若將5元立體陣在基陣坐標系下的定位結果轉到大地坐標系，則結果應基本聚合在一點。為對比三角分解算法、向量投影算法的定位精度，將兩種算法的定位結果通過坐標轉換公式轉到大地坐標系下，以聲信標的標定位置為參考，對比兩種算法的離散程度，如圖13的直方圖所示。表1統計了圖13中兩種算法在3個方向的均方根誤差及運行時間。

表1 算法均方根誤差及運行時間統計結果

圖13 與標定結果差值的直方圖

由圖13及表1的結果可以看出，向量投影算法與三角分解算法在3方向的均方根誤差基本一致，差距在厘米級，向量投影算法略優；此外，三角分解算法的運行時間為0.122 5 s，遠大于向量投影算法的0.013 1 s，是向量投影算法的9倍多。

湖試數據處理結果表明，向量投影算法能獲得與三角分解算法幾乎一致的定位精度，且計算效率更高。

6 結論

本文針對現有適用于立體陣型的三角分解算法計算量大、定位誤差難以通過解析式精確表征的問題，從向量的角度提出了向量投影算法。與三角分解算法相比，向量投影算法的時間復雜度更低，解算所需的參數更易獲取，且定位誤差的解析式簡潔、直觀。通過對向量投影算法的誤差分析，得到增大立體陣孔徑能降低算法估值誤差的結論，該結論對立體陣的陣型設計具備指導意義。仿真分析表明，在不同目標方位下，相較于三角分解算法，向量投影算法的定位誤差幾乎維持在同一數值，受目標方位影響小，算法性能更加穩定且運算時間更少；此外，基于誤差分析的指導，增加基陣的水平與垂直孔徑能明顯減小向量投影算法的定位誤差，驗證了誤差分析結論的正確性。湖試數據處理結果表明，向量投影算法能獲得與三角分解算法幾乎一致的定位精度，但運算時間少于三角分解算法的九分之一。