AUV直線航跡下的單信標測距定位

曹俊， 鄭翠娥， 孫大軍， 張殿倫

(1.哈爾濱工程大學 水聲工程學院，黑龍江 哈爾濱 150001； 2.哈爾濱工程大學 水聲技術(shù)重點實驗室，黑龍江 哈爾濱 150001)

?

AUV直線航跡下的單信標測距定位

曹俊1，2， 鄭翠娥1，2， 孫大軍1，2， 張殿倫1，2

(1.哈爾濱工程大學 水聲工程學院，黑龍江 哈爾濱 150001； 2.哈爾濱工程大學 水聲技術(shù)重點實驗室，黑龍江 哈爾濱 150001)

常規(guī)的單信標測距定位基于的是非直線航跡，本文針對直線航跡下的單信標測距定位進行了研究。同時對于直線航跡，常規(guī)的直接降階求解的算法已不適用，提出線性化迭代求解的算法對載體進行定位。另一方面，引入長基線陣列的概念，從構(gòu)建虛擬陣列的角度，研究了不同誤差源對虛擬陣列的影響。仿真分析了虛擬陣列基線總長度對定位影響、虛擬陣列信標個數(shù)對定位影響，以及載體相對于信標在不同位置時的定位精度分布。海試數(shù)據(jù)結(jié)果表明：所提出的線性化迭代求解的定位算法能獲得較好的定位效果，單標定位結(jié)果與組合導航偏差在5 m之內(nèi)。

單信標測距；水聲定位；直線航跡；虛擬信標；虛擬陣列；無人水下航行器

隨著世界經(jīng)濟的發(fā)展，人類已將資源開發(fā)和利用的重點轉(zhuǎn)向海洋。海底光纜管線的鋪設(shè)及其維護，水下載體作業(yè)時是直線航跡；海洋地貌繪制，水下載體作業(yè)時是梳形航跡；深水油氣、天然氣水合物及礦產(chǎn)資源的勘查，水下載體作業(yè)時是梳形航跡。水下載體的導航定位是順利完成各項任務的首要前提和關(guān)鍵因素，基于單信標測距的定位是水下輔助導航的發(fā)展趨勢，已成為國內(nèi)外導航定位領(lǐng)域的研究重點[1-5]。文獻[6]使用最小二乘法解算出無人水下航行器(unmanned underwater vehicle，UUV)的水平初始位置，但載體需要在3個不同的直線航跡上對信標測距才能夠進行解算。文獻[7]提出單信標定位用于UUV回收的想法，UUV在信標附近進行環(huán)形機動。文獻[8]主要基于方形航跡利用協(xié)方差矩陣進行仿真分析。國內(nèi)西北工業(yè)大學和哈爾濱工程大學兩家單位從數(shù)學上研究單信標定位解算，(研究過程)既沒有從定位的角度分析各種誤差源對定位的影響，也沒有真實試驗數(shù)據(jù)的處理[9-10]。

自主式水下航行器(autonomous underwater vehicle,AUV)是UUV的一種，擺脫了系纜的牽絆。作業(yè)過程更加靈活。常規(guī)的解算方法中，需要航跡的航向必須要有改變，這也就是說當水下載體長時間直線航行時，很長一段時間內(nèi)不能夠利用聲學進行單信標定位解算。

1 單信標測距定位

傳統(tǒng)長基線(long base line，LBL)定位[11]，利用AUV至各信標間的距離信息，采用球面交匯模型解算AUV的位置。單信標測距定位如圖1所示，水下布放的單只信標為AT，要定位的位置為AUV4。原理上，單次距離測量不能確定水下載體的具體位置，所以我們需要結(jié)合水下載體工作時的運動參數(shù)如速度和姿態(tài)來實時解算出水下載體的位置。根據(jù)姿態(tài)和航速，建立AUV位于AUV4時的虛擬信標陣列VT4、VT3、VT2和VT1[12]。從圖中可以看出，虛擬信標陣列與傳統(tǒng)長基線陣列一樣。如果航向和航速均不存在誤差，則虛擬信標陣列就沒有誤差，其定位精度與傳統(tǒng)長基線完全一樣。

圖1 單信標測距定位Fig.1 Single beacon ranging

當AUV的航行軌跡為如圖2所示的直線航跡時，真實信標為AT=[ATxATyATz]T，航向角為φ，測距周期為T，測距個數(shù)為n，要定位的位置為AUVn=[xayaza]T，AUV深度za通過深度傳感器獲得，AUVi處的測距信息為Ri。根據(jù)航向和航速，建立的虛擬信標為VTi=[xtiytizti]T，虛擬信標的總長度為L。對于所有測量點中的前n個測量點，要定位的位置是AUVn，前n-1個測量點是不求解的。對于第n個測量點，利用從1～n的信息用于解算；對于第n+1個測量點，利用從2～n+1的信息用于解算；依次類推。則從第n個測量點及以后，每個點都能夠直接求解。

圖2 直線航跡示意圖Fig.2 Sketch of straight line track

第i個測距信息對應的定位方程為

(xa-xti)2+(ya-yti)2+(za-zti)2=Ri2

(1)

常規(guī)的LBL解算方法是經(jīng)過降次處理，得到二元一次方程組：

B=AX

(2)

其中

Ai×2=[(xt(i+1)-xti)，(yt(i+1)-yti)]

當矩陣ATA為可逆陣時，根據(jù)最小二乘法，可得到最優(yōu)解為

(3)

當航行軌跡為直線時，構(gòu)建的虛擬信標也是直線，由虛擬信標構(gòu)成的矩陣A的秩為1，小于未知數(shù)個數(shù)，方程無解。所以對于直線航跡，常規(guī)解算方法的系數(shù)矩陣不滿秩，不能夠進行解算。為了能夠?qū)χ本€航跡進行解算，令

(4)

將方程(4)利用泰勒級數(shù)展開進行線性化[13]，構(gòu)成方程組為

N=MΔX

(5)

其中，初值X=(x0，y0)，i=1，2，…，n

當矩陣MTM為可逆陣時，根據(jù)最小二乘法，可得到最優(yōu)解為：

(6)

要使方程有解，AUV航行軌跡不能為過信標的直線。因為當航行軌跡為過信標的直線時，矩陣M的秩為1，小于未知數(shù)個數(shù)，方程無解。對修正參數(shù)ΔX進行精度判斷，多次迭代直至修正參數(shù)的精度高于直接觀測量測距精度一個數(shù)量級。由第k次迭代到第k+1次迭代的公式為：Xk+1=Xk+ΔXk。

2 虛擬陣列對定位影響

定位方程式(1)中的已知量為虛擬信標的位置。下面引入長基線陣列的概念，將各虛擬信標構(gòu)建成虛擬信標陣列，分析其對定位的影響。

2.1 構(gòu)建虛擬陣列

載體姿態(tài)和載體速度是在載體坐標系下測量得到，而信標的位置是在大地坐標下，所以虛擬信標與實際布放的單只信標的關(guān)系式如下

(7)

(8)

式中：vx為載體東向速度，vy為載體北向速度，vz為載體天向速度，φ為載體航向，vf為載體前向速度，vl為載體左向速度，vv為載體垂向速度。

2.2 誤差源對虛擬陣列影響

虛擬信標的位置由信標標定結(jié)果、載體前向速度、載體左向速度和載體航向角共同決定。下面分別分析這些誤差源對定位的影響。

2.2.1 信標標定誤差

當AT存在誤差ΔAT=[ΔATxΔATy]T時，虛擬陣列信標的位置偏差為ΔVTi=ΔAT。

從公式可以看出，虛擬陣列的每一個虛擬信標都會存在同樣大小的位置偏差，位置偏差的大小與信標標定誤差相同。

2.2.2 載體前向速度誤差

當vf存在誤差Δvf時，引起的速度誤差為

(9)

虛擬陣列信標的位置偏差為

(10)

其中，Δv=[Δvf·sinφΔvf·cosφ]T。

從式(10)可以看出，虛擬陣列的虛擬信標存在位置偏差，且后構(gòu)建虛擬信標的位置偏差按照Δv逐漸增大。

2.2.3 載體左向速度誤差

當vl存在誤差Δvl時，引起的速度誤差為

(11)

虛擬陣列信標的位置偏差為

(12)

其中，Δv=[ΔvlcosφΔvlsinφ]T。

從公式可以看出，虛擬陣列的虛擬信標存在位置偏差，且后構(gòu)建虛擬信標的位置偏差按照Δv逐漸增大。

2.2.4 載體航向誤差

當φ存在誤差Δφ時，引起的速度誤差為

(13)

虛擬陣列信標的位置偏差為：

(14)

其中，

從式(14)可以看出，虛擬陣列的虛擬信標存在位置偏差，且后構(gòu)建虛擬信標的位置偏差按照Δv逐漸增大。

2.3 仿真分析

仿真參數(shù)：布放的單只信標位置為(0，0)。直線航跡如圖2所示，以1 m/s的前向航速勻速運動，航向角為90°。假設(shè)誤差分別為5 m的固定東向標定誤差，0.01 m/s的固定前向速度誤差，0.01 m/s的固定左向速度誤差和0.01°的固定測向誤差。

2.3.1 虛擬陣列基線總長度對定位影響

要定位的位置AUVn為定值(1 000，1 000)。虛擬陣列信標個數(shù)為11個，虛擬陣列信標間距從1m增加至400m，仿真分析虛擬陣列基線總長度對水平定位結(jié)果的影響。

定位結(jié)果如圖3所示。從圖中可以看出，存在信標標定誤差、左向速度誤差和測向誤差時，定位誤差為固定值，虛擬陣列基線總長度對定位誤差沒有影響。存在前向速度誤差時，定位偏差會隨著基線總長度的增加先減小再增大。當虛擬信標VT1與要定位的位置AUVn的距離最小時，即虛擬陣列基線總長度為1 000m時，載體前向速度誤差引起的單標測距定位誤差最小。

圖3 虛擬陣列基線總長度對水平定位結(jié)果的影響Fig.3 Effect of baseline total length of virtual array on positioning results

2.3.2 虛擬陣列信標個數(shù)對定位影響

要定位的位置AUVn為定值(1 000，1 000)。虛擬陣列基線總長度為固定值400m，虛擬陣列信標個數(shù)從5個增加至401個，仿真分析虛擬陣列信標個數(shù)對水平定位結(jié)果的影響。

定位結(jié)果如圖4所示。從圖中可以看出，存在信標標定誤差、前向速度誤差、左向速度誤差和測向誤差時，改變虛擬陣列信標個數(shù)對定位誤差沒有影響，定位誤差為固定值。

圖4 虛擬陣列信標個數(shù)對水平定位結(jié)果的影響Fig.4 Effect of beacon number of virtual array on positioning results

2.3.2 載體相對于信標在不同位置時的定位精度分布

AUV在6 000m×6 000m的范圍內(nèi)運動，目標在區(qū)域內(nèi)的定位精度分布如圖5所示。

從圖5(a)可知，存在信標標定誤差時，定位偏差為固定值，大小與信標標定誤差相同。從圖5(b)可知，存在固定前向測速誤差時，經(jīng)過信標的航跡不能夠定位，水平定位誤差是對稱分布的，對稱軸是航向為方向經(jīng)過信標的直線，對稱軸的兩端定位誤差大，遠離對稱軸兩端的定位誤差小。從圖5(c)可知，存在固定左向速度誤差時，水平定位誤差近似呈對稱分布，信標處定位誤差最小，以信標為中心向外輻射，距離信標越遠的區(qū)域定位誤差越大。從圖5(d)可知，存在固定測向誤差時，水平定位誤差是對稱分布的，信標處定位誤差最小，以信標為中心向外輻射，距離信標越遠的區(qū)域定位誤差越大。

3 海試數(shù)據(jù)分析

3.1 試驗1

2015年4月，在中國南海進行了6 000 m AUV實用化改造的綜合試航海上試驗。試驗區(qū)域平均水深3 720 m左右。試驗平臺為中科院沈陽自動化研究所自主研制的AUV。試驗設(shè)備包括：慣性導航系統(tǒng)、多普勒計程儀、深度傳感器和長基線定位系統(tǒng)。長基線定位系統(tǒng)選擇的是法國IXSEA公司的RAMSES6000測距儀及RT系列信標。試驗期間，LBL的定位周期為24 s；基線長度3 000 m；AUV離底50 m工作；航行時的前向速度大概為1 m/s。AUV航行軌跡如圖6所示，S為起點；布放的4只聲信標，標號為1、2、3、4；長基線陣列的中心為原點。AUV的深度通過高精度壓力傳感器測得。

單信標測距定位結(jié)果如圖7所示，從圖中可以看出，單標定位1表示信標1的單信標測距定位結(jié)果，定位偏差隨著AUV與信標1距離的減小而減?。粏螛硕ㄎ?表示信標3的單信標測距定位結(jié)果，定位偏差隨著AUV與信標1距離的增大而增大。綜合仿真分析可知，這說明AUV航行參數(shù)存在著固定偏差。

本次試驗中，通過LBL對水面試驗船進行自主定位，并將定位結(jié)果與GPS結(jié)果比對，驗證了標定結(jié)果是比較精確的。下面從載體航向、載體前向速度和載體左向速度進行分析，如圖8所示。從圖中可以看出，左向速度的精度和前向速度的精度也不高，起伏達到0.04 m/s。載體航向角的起伏范圍從90°～94°，起伏程度嚴重，航向角誤差很大。

3.1 試驗2

2015年7月，在中國南海第二次進行了4 500 m AUV的綜合試航海上試驗。與2015年4月不同的是慣性導航系統(tǒng)選用的是法國IXSEA公司的PHINS，該設(shè)備的測向精度和速度精度都比較高。與RAMSES測距系統(tǒng)結(jié)合使用的情況下，航向角測量精度能達到0.05°。圖9為AUV航行過程中測量的載體航向、載體前向速度和載體左向速度。航向角的起伏范圍從-0.25°～0.25°，航向角精度得到了很大的提高。

圖5 不同誤差源下的定位精度分布Fig.5 Distribution of positioning accuracy under different error sources

圖6 AUV航行軌跡Fig.6 AUV navigation trajectory

圖7 單信標測距定位結(jié)果Fig.7 Positioning result based on single beacon ranging

圖8 AUV航行參數(shù)Fig.8 AUV navigation parameters

圖9 AUV航行參數(shù)Fig.9 AUV navigation parameters

單信標測距定位結(jié)果如圖10所示。從圖中可以看出，定位結(jié)果與組合導航偏差不大。對定位結(jié)果與組合導航偏差進行統(tǒng)計，傳統(tǒng)LBL定位偏差1.616 m，信標1單標測距定位偏差4.268 m，信標2單標測距定位偏差2.805 m，信標3單標測距定位偏差4.339 m，信標4單標測距定位偏差3.395 m。海試數(shù)據(jù)處理結(jié)果表明：基于單信標測距的定位是以載體的航速和航向精度比較高為前提的。試驗2使用了高精度的PHINS，誤差比較小，虛擬陣列的信標誤差就小，單信標定位能夠獲得比較好的定位效果。

圖10 單信標測距定位Fig.10 Positioning deviation based on single beacon ranging

4 結(jié)論

仿真分析結(jié)果表明：1)信標標定誤差對每一個虛擬信標引入大小等于標定誤差的位置偏差，載體前向速度誤差、載體左向速度誤差和載體測向誤差對每一個虛擬信標引入大小不等的位置偏差，后構(gòu)建的虛擬信標的位置偏差逐漸增大。2)存在載體前向速度誤差時，虛擬陣列基線長度影響定位結(jié)果。對于其他誤差，虛擬陣列基線總長度和虛擬陣列信標個數(shù)對定位結(jié)果沒有影響。

海試數(shù)據(jù)結(jié)果表明：1)存在載體測向誤差時，單標定位結(jié)果與組合導航的偏差隨著AUV航跡與信標距離的增大而增大。2)當AUV航行參數(shù)精度比較高時，單標定位結(jié)果與組合導航的偏差在5 m之內(nèi)，虛擬陣列的信標誤差越小，單信標定位能夠獲得比較好的定位效果。

[1]DJAPIC V，NAD D，F(xiàn)ERRI G，et al. Novel method for underwater navigation aiding using a companion underwater robot as a guiding platforms[C]// Proceedings of Oceans 2013，Bergen，Norway， 2013： 1-10.

[2]HEGRENAS O，GADE K，HAGEN O K，et al. Underwater transponder positioning and navigation of autonomous underwater vehicles[C]// IEEE Oceanic Engineering Society and Marine Technology Society. MTS/IEEE Biloxi-Marine Technology for Our Future: Global and Local Challenges， OCEANS 2009， Biloxi， MS， USA， 2009： 1-7.

[3]MCEWEN R， THOMAS H， WEBER D， et al. Performance of an AUV navigation system at arctic latitudes[J]. Oceanic engineering， IEEE Journal of， 2005， 30(2)： 443-454.

[4]THOMSON D， DOSSO S. AUV localization in an underwater acoustic positioning system[J]. Inverse theory， 2013， 40(3)： 1-6.

[5]孫大軍， 鄭翠娥. 水聲導航、定位技術(shù)發(fā)展趨勢探討[J]. 海洋技術(shù)學報， 2015，34(3)： 64-65.

SUN Dajun, ZHENG Cuie. Discussion on development trend of underwater acoustic navigation and positioning technology[J]. Journal of ocean technology， 2015， 34(3)： 64-65.

[6]ALEXANDER P S. The AUV positioning using ranges from one transponder LBL[C]// Proceedings of the 1995 MTS/IEEE Oceans Conference， San Diego， CA， USA， 1995:1620-1623

[7]VAGANAY J， BACCOU P， JOUVENCEL B. Homing by acoustic ranging to a single beacon [C]// Proceedings of the 2000 MTS/IEEE Oceans Conference， Providence， RI， USA， 2000:1457-1462.

[8]LARSEN M. Synthetic long baseline navigation of underwater vehicles[C]// Proceedings of the 2000 MTS/IEEE Oceans Conference， Providence， RI， USA， 2000: 2043-2050.

[9]劉明雍， 李聞白， 劉富檣， 等. 基于單信標測距的水下導航系統(tǒng)可觀測性分析[J]. 西北工業(yè)大學學報， 2011， 29(1)： 87-92.

LIU Mingyong， LI Wenbai， LIU Fuqiang， et al. Observability analysis of underwater navigation system based on single beacon ranging[J]. Journal of Northwestern Polytechnical University， 2011， 29(1)： 87-92.

[10]王久光. 基于UTP的UUV遠程航海水下位置校準及控制方法研究[D]. 哈爾濱:哈爾濱工程大學， 2012: 38-52.

WANG Jiuguang. The calibration and control method of the UTP based UUV remote navigation water position calibration and control method[D]. Harbin :Harbin Engineering University， 2012: 38-52.

[11]田坦.水下定位與導航[M].北京：國防工業(yè)出版社， 2007.

[12]CARE E G, LA P. Virtual long baseline (VLBL) autonomous underwater vehicle navigation using a single transponder[D]. Massachusetts: Massachusetts institute of technology， 2006: 66-75.

[13]YAMASHITA N， FUKUSHIMA M. On the rate of convergence of the levenberg-marquardt method [J]. Journal of global optimization, 2010， 47(1)： 133-159.

[14]潘國榮， 汪大超， 周躍寅. 兩種大轉(zhuǎn)角空間坐標轉(zhuǎn)換模型研究[J]. 山東科技大學學報， 2015， 34(1)： 61-67.

PAN Guorong， WANG Dachao， ZHOU Yueyin. Two spatial coordinate transformation model of large angle[J]. Journal of Shandong University of Science and Technology， 2015， 34(1)： 61-67.

本文引用格式：

曹俊， 鄭翠娥， 孫大軍，等.AUV直線航跡下的單信標測距定位[J]. 哈爾濱工程大學學報， 2017， 38(5)： 733-738.

CAO Jun，ZHENG Cuie，SUN Dajun，et al. AUV positioning based on single-beacon ranging in straight-line trajectory[J]. Journal of Harbin Engineering University， 2017， 38(5)： 733-738.

AUV positioning based on single-beacon ranging in straight-line trajectory

CAO Jun1，2，ZHENG Cuie1，2，SUN Dajun1，2，ZHANG Dianlun1，2

(1.College of Underwater Acoustic Engineering， Harbin Engineering University， Harbin 150001， Heilongjiang， China; 2.Acoustic Science and Technology Laboratory， Harbin Engineering University， Harbin 150001， Heilongjiang， China)

The conventional localization of single-beacon ranging is based on the nonlinear trajectory. In this paper， the localization of single-beacon ranging was studied based on the linear trajectory. On the one hand， the conventional direct order-reducing method was not applicable for the straight-line trajectory; a linear iterative algorithm was proposed to locate the vector. On the other hand， the concept of the long baseline array was introduced; the effects of different error sources on the virtual array were studied from the aspect of constructing the virtual array. The influence of total length of the virtual array baseline， the influence of the number of the virtual array beacons， and the location accuracy of the vector with respect to the beacon at different positions were analyzed. Sea trial results show that the proposed algorithm can realize good localization results， the deviation between the single-beacon positioning and integrated navigation is within 5 m.

single-beacon ranging; acoustic positioning; straight-line track; virtual beacon; virtual array; linear iterative; unmanned underwater vehide (UUV)

2015-12-28.

日期：2017-04-26.

國家國防科技工業(yè)局基礎(chǔ)科研項目(B2420133002);國家自然科學基金(61531012).

曹俊(1987-)， 男， 博士研究生； 孫大軍(1972-)， 男， 教授， 博士生導師.

孫大軍，E-mail:sundajun@hrbeu.edu.cn.

10.11990/jheu.201512093

TB568

A

1006-7043(2017)05-0733-06

網(wǎng)絡出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20170426.1040.012.html