基于泰勒級數(shù)展開的總體最小二乘水下短基線定位算法研究

侯 華，黃鼎盛，郭勝杰，程 萌

（河北工程大學(xué) 信息與電氣工程學(xué)院，河北 邯鄲 056038）

迄今為止，在水下環(huán)境中，聲波仍是完成水下定位最常用的載體［1］。按照基線長度的不同，水下聲學(xué)定位系統(tǒng)可分為長基線（Long Baseline，LBL）定位系統(tǒng)、短基線（Short Baseline，SBL）定位系統(tǒng)以及超短基線（Ultra-short Baseline，USBL）定位系統(tǒng)3 類［2］。短基線定位系統(tǒng)價格低廉、安裝便捷且定位精度高［3］，是中小型自主式水下導(dǎo)航器（Autonomous Underwater Vehicle，AUV）定位的首選方案。

在短基線定位系統(tǒng)中，需要通過測量聲波信號從換能器到達(dá)水聽器陣列中不同陣元之間的時間差，得到換能器與水聽器基陣之間的距離和方位信息，再對這些測距信息進(jìn)行分析解算，最后根據(jù)己知的換能器或是水聽器坐標(biāo)得到待定位目標(biāo)的具體位置而完成定位［4］。在水環(huán)境中，引起AUV定位誤差的原因主要有：（1）水聲信號在水下傳播時聲速測量的偏差；（2）多徑效應(yīng)的存在導(dǎo)致信號在傳播時產(chǎn)生衰落，使得信號傳播時延出現(xiàn)偏差；（3）基陣自身位置的擾動會使定位出現(xiàn)誤差等［5］。此外，定位算法的不同更是直接影響到AUV 的定位結(jié)果。因此，對水下短基線定位系統(tǒng)中算法的研究十分重要。

Chan 氏定位算法在定位時要求噪聲服從高斯分布，因此它在對AUV 定位時適用性不足［6］。最小二乘定位算法計(jì)算量小、定位快速，但是精度不高、魯棒性較差［7］。人工蜂群算法定位精度較高，但無法滿足定位的實(shí)時性要求［8］。因此，文獻(xiàn)［9］基于牛頓迭代對最小二乘定位法進(jìn)行改進(jìn)，提出了一種改進(jìn)最小二乘法的水下測距與定位方法，明顯改善了定位精度。文獻(xiàn)［10］將多水聽器距離值進(jìn)行信息融合，提出了一種基于卡爾曼濾波的多水聽器水下定位算法。文獻(xiàn)［11］提出了一種基于并行蟻群算法的長基線定位方法。該方法將水聲目標(biāo)定位問題簡化為帶約束條件的非線性優(yōu)化問題，降低了各個誤差源對水聲目標(biāo)導(dǎo)航定位精度的影響?？紤]到AUV 定位算法的綠色和實(shí)時性需求，本文針對淺水環(huán)境提出了一種基于泰勒級數(shù)展開的總體最小二乘定位算法。與人工蜂群算法相比，該算法的復(fù)雜度較低。與最小二乘算法相比，該算法能夠大幅度提升定位精度，且穩(wěn)定性較強(qiáng)。最后，對本文算法進(jìn)行實(shí)驗(yàn)仿真，并與最小二乘算法進(jìn)行了對比分析。

1 系統(tǒng)設(shè)計(jì)方案

設(shè)計(jì)的短基線定位系統(tǒng)包括水面部分和水下部分。水面部分由GPS 接收機(jī)、數(shù)據(jù)處理中心、5G 收發(fā)器以及水聽器基陣組成，主要完成水聲定位信號的接收與定位算法的解算等工作。水下部分安裝在水下AUV 上，主要任務(wù)是完成水聲信號的調(diào)制與發(fā)射，由擴(kuò)頻信號發(fā)生儀、數(shù)據(jù)處理中心、驅(qū)動控制中心以及發(fā)射換能器組成，且與AUV 時間同步?；囉? 個水聽器和1 個全向發(fā)射換能器組成，布放形式為岸基布放。4 個水聽器用于接收待定位目標(biāo)發(fā)射的水聲信號，且其與水面浮標(biāo)相連接，按照一定陣型擺放。由于浮標(biāo)位置是已知的，故水聽器坐標(biāo)位置也是已知的。全向發(fā)射換能器被安裝在AUV 上，用來發(fā)射定位信號。系統(tǒng)通過測量水聲信號在AUV 與各水聽器間的傳播時延差計(jì)算其坐標(biāo)信息。

2 泰勒級數(shù)展開的總體最小二乘定位算法原理

在淺水環(huán)境下，聲速存在的梯度變化可以忽略［12］。本文認(rèn)為，聲波在水中的傳播速率為恒定速率1 500 m/s。定位過程中，換能器發(fā)射的水聲信號經(jīng)傳播到達(dá)水聽器基陣的時間差可以用廣義互相關(guān)時延估計(jì)算法得到。因此，在短基線定位系統(tǒng)中，可以用距離差代替時間差。待定位AUV 與4 個水聽器所在的浮標(biāo)陣距離可以表示為：

其中，第i個浮標(biāo)的坐標(biāo)為(xi,yi,zi)，待定位水下AUV 的位置為(x,y,z)，ti為從AUV 上的發(fā)射換能器發(fā)射聲波經(jīng)傳播達(dá)到第i個水聽器所在的浮標(biāo)陣的時間，c為聲波傳播的速度。由定位空間方程求差處理得到線性方程：

總體最小二乘法的思想在于同時利用擾動矩陣和擾動向量調(diào)整A和B，以校正二者內(nèi)存在的噪聲，構(gòu)造增廣矩陣C=[-B|A]，利用增廣矩陣的奇異值分解求得最小二乘解。當(dāng)同時存在陣元位置誤差和測距誤差時，rank{C}=4，因此對其進(jìn)行奇異值分解得：

式中：U、V分別為增廣矩陣的左、右奇異矩陣；Σ是半正定對角矩陣；uk和vk分別為左、右奇異向量；uk為矩陣U的第k列；vk為矩陣V的第k列；σk為C對應(yīng)的奇異值，并滿足σ1≥σ2≥σ3≥σ4。當(dāng)沒有測量誤差時，則有rank{C}=3，此時σ4=0，且Ax=B存在唯一解；當(dāng)測量誤差存在時，則有rank{C}=4，此時σ4≠0。但是，當(dāng)測量誤差不是很大時，Ax=B存在唯一的總體最小二乘算法的解為：

將此總體最小二乘估計(jì)的待定位AUV 位置作為初值(x0,y0,z0)，在初值點(diǎn)進(jìn)行泰勒級數(shù)展開，并采用多次加權(quán)最小二乘迭代運(yùn)算完成目標(biāo)的定位。

令：

對式（10）在(x0,y0,z0)處進(jìn)行泰勒級數(shù)展開，忽略高階項(xiàng)，則有：

則一階項(xiàng)Un可以表示為：

初值(x0,y0,z0)為總體最小二乘估計(jì)值，則通過AUV 到水聽器陣的測量距離Rn即可求出(Δx,Δy,Δz)。根據(jù)給出的門限η判斷式（14）是否成立：

若滿足條件，則迭代結(jié)束；若不滿足條件，則將(x0+Δx,y0+Δy,z0+Δz)作為新的初始坐標(biāo)進(jìn)行計(jì)算，直到滿足迭代條件，則坐標(biāo)為AUV 定位的結(jié)果。

3 仿真實(shí)驗(yàn)與性能分析

本文的短基線定位系統(tǒng)環(huán)境設(shè)置如下：水聽器陣的4 個浮標(biāo)陣元的坐標(biāo)為（464，241，75）、（628，347，118）、（726，532，132）和（948，682，154），待定位的水下AUV 的位置設(shè)置為（754，153，200）。在不同測距條件下，通過1 000 次蒙特卡洛仿真實(shí)驗(yàn)，對本文提出的算法與最小二乘算法進(jìn)行定位誤差比對，結(jié)果如圖1 所示。

從圖1 可以看出，隨著AUV 與水聽器基陣測距誤差的增大，最小二乘定位算法的誤差曲線增長較為急促，而本文算法的誤差曲線增長平緩。這是因?yàn)楸疚乃惴ㄔ跇?gòu)造最小二乘解時不但考慮了測距誤差，還考慮了基陣自身的擾動誤差，故在測距誤差相同的條件下，定位誤差后者始終優(yōu)于前者。顯然，泰勒級數(shù)展開的總體最小二乘定位算法較最小二乘定位算法有較高的定位精度。

在兩種不同定位算法下對AUV 進(jìn)行實(shí)時運(yùn)動軌跡定位仿真。AUV 從初始位置（754，153，200）出發(fā)，在水下進(jìn)行勻速直線運(yùn)動。圖2 為AUV在二維空間下最小二乘定位算法與本文提出算法的實(shí)時定位軌跡及預(yù)設(shè)軌跡圖。

由圖2 可知，在AUV 二維空間勻速直線運(yùn)動時，最小二乘算法的定位軌跡隨著時間的變化在AUV 運(yùn)動附近上下波動較大，而本文提出的算法定位軌跡在AUV 運(yùn)動附近波動較小，曲線較為穩(wěn)定，軌跡更加平穩(wěn)，實(shí)時定位精度較最小二乘算法好。這是由于本文算法在求解非線性方程組時考慮了基陣的擾動誤差，并構(gòu)造誤差的增廣矩陣進(jìn)行奇異值分解，大大提高了對AUV 的實(shí)時定位精度。此外，泰勒級數(shù)展開的思想使得定位結(jié)果在初始值進(jìn)行迭代優(yōu)化，增強(qiáng)了實(shí)時定位的穩(wěn)定性。顯然，在AUV 進(jìn)行勻速直線運(yùn)動時，本文算法對AUV 的實(shí)時定位性能強(qiáng)且定位軌跡波動較小，在提高最小二乘定位精度的同時具有較強(qiáng)的魯棒性。

4 結(jié)語

本文提出的基于泰勒級數(shù)展開的總體最小二乘定位算法不依賴于水壓計(jì)、羅經(jīng)以及DVL 等復(fù)雜設(shè)備和AUV 的先驗(yàn)信息，在工程應(yīng)用中對中小型AUV 的定位適應(yīng)性較好。仿真實(shí)驗(yàn)表明，本文算法精度較高。在AUV 實(shí)時定位中，該算法定位軌跡較為平穩(wěn)，總體上滿足AUV 在淺水中實(shí)現(xiàn)高精度實(shí)時定位的應(yīng)用需求。本文提出的算法為提升水聲定位系統(tǒng)的精度提供了一種有效途徑，今后還需進(jìn)一步開展不同環(huán)境下的AUV 深水實(shí)驗(yàn)來提高定位算法的可靠性和精確性。