張延順 ，郭雅靜，黃小娟，李春雨，汪湛清

(1.北京航空航天大學 儀器科學與光電工程學院，北京100191; 2.北京理工大學 自動化學院，北京100081)

航位推算(Dead Reckoning，DR)/水聲定位系統(tǒng)組合定位既能抑制DR 系統(tǒng)隨著時間增加由誤差累積引起的誤差發(fā)散，又能平滑水聲定位系統(tǒng)輸出位置的波動［1-2］.在水聲定位系統(tǒng)短時間失效的情況下，可以依靠DR 進行導航定位［3-5］.此方法在水下導航系統(tǒng)中得到廣泛應(yīng)用.DR 系統(tǒng)的初始位置精度影響DR 系統(tǒng)性能［6-7］，也影響DR/水聲組合定位系統(tǒng)導航數(shù)據(jù)的性能.在對姿態(tài)和速度平穩(wěn)性有較高要求的水下精確測繪等［8-9］領(lǐng)域，對DR 系統(tǒng)的初始位置精度要求更高.

水下導航信息貧乏［10-13］，通常只能用水聲定位系統(tǒng)輸出的位置信息作為DR 系統(tǒng)的初始位置.水聲定位系統(tǒng)是根據(jù)聲波在水底應(yīng)答器和載體接收機間傳播的時間來確定距離進而進行定位的，受聲速誤差、航行噪聲和載體搖擺等影響，整體數(shù)據(jù)的優(yōu)劣程度不一，表現(xiàn)為數(shù)據(jù)波動較大［14-15］.所以，需要從一段時間內(nèi)水聲定位系統(tǒng)輸出的位置信息中計算提取出DR 系統(tǒng)需要的初始位置.

當水下運載體為靜基座狀態(tài)時，通常采用對一段時間內(nèi)水聲定位系統(tǒng)輸出信號求平均值的方法來得到DR 系統(tǒng)的初始位置，此方法在水下載體處于系泊狀態(tài)時精度較高.在海洋探索和科學考察時，水下運載體通常為運動狀態(tài)，在運動狀態(tài)下，應(yīng)用求平均值法則會引入較大的計算誤差.因此，急需解決水下運載體行進過程中確定DR 系統(tǒng)初始位置的問題.基于此背景，本文提出了運動過程中水下運載體DR 系統(tǒng)初始位置確定的方法.該方法綜合DR 和水聲定位系統(tǒng)的特點，能得到優(yōu)化的初始位置，可提高DR/水聲組合定位系統(tǒng)姿態(tài)和速度的穩(wěn)定性，有助于水下地形測繪、勘探等系統(tǒng)提高成圖質(zhì)量.

1 DR 系統(tǒng)初始位置確定方法

水聲定位系統(tǒng)利用聲波在水下傳播良好的特性，通過檢測其傳輸?shù)臅r間差進行定位，但是其輸出信號隨機誤差較大，特別是遠距離傳輸時誤差更大.目前常用的水聲定位系統(tǒng)主要有長基線系統(tǒng)(LBL)、短基線系統(tǒng)(SBL)、超短基線系統(tǒng)(USBL)，本文應(yīng)用海試實驗中的USBL 數(shù)據(jù)進行研究.某次海試實驗初始定位時間(75 s)內(nèi)的超短基線定位系統(tǒng)USBL 的輸出數(shù)據(jù)如圖1 所示.

圖1 初始定位時間內(nèi)水聲定位系統(tǒng)輸出位置數(shù)據(jù)Fig.1 Output location data of hydroacoustic system in initial positioning time

由圖1 可見，USBL 水聲系統(tǒng)輸出數(shù)據(jù)隨機性較大，該段時間段內(nèi)平均位置為(122.400°，21.640 77°)，最大定位誤差為88.50 m，最小定位誤差為11.98 m.因此以任一點數(shù)據(jù)作為DR 系統(tǒng)初始值都可能會有較大的誤差，但通常只能從一段時間內(nèi)的水聲定位系統(tǒng)數(shù)據(jù)中提取DR 系統(tǒng)初始值，這就需要針對運載體運動特點采用相應(yīng)方法確定初始位置.水下運載體高度信息由深度計提供，所以實際應(yīng)用時本文只考慮二維平面內(nèi)的初始位置信息，即經(jīng)度、緯度信息.

1.1 準靜態(tài)情況的平均值法定位

準靜態(tài)情況下水下載體姿態(tài)有輕微變化，但無線運動.此情況下，對短時間內(nèi)水聲定位系統(tǒng)提供的經(jīng)度、緯度進行求平均值運算，在一定程度上能減小定位系統(tǒng)的誤差.通常采用的方法是在短時間內(nèi)采集1 組水聲定位系統(tǒng)輸出值，然后對此數(shù)據(jù)進行求平均運算，以平均值作為位置的初始值，初始緯度、經(jīng)度分別為

式中:Lk和λk分別為水聲定位系統(tǒng)k 時刻輸出的緯度和經(jīng)度數(shù)值.

在靜態(tài)情況下平均值法能有效抑制水聲定位系統(tǒng)的隨機誤差，但是在運動情況下，則會產(chǎn)生與運動狀態(tài)相關(guān)的位置誤差，且位置誤差隨著運動速度增大而增大，需要研究動態(tài)情況下初始位置確定方法.

1.2 最小二乘擬合方法

動態(tài)情況是指水下載體具有線運動.動態(tài)情況下采用平均值法計算位置初始會產(chǎn)生明顯的誤差.水下載體以某一速度直線運動時，可采用最小二乘直線擬合方法對水聲定位系統(tǒng)提供的經(jīng)度和緯度信息分別進行擬合，并計算出位置初值.

最小二乘法(又稱最小平方法)是一種數(shù)學優(yōu)化技術(shù)，它通過最小化誤差的平方和尋找數(shù)據(jù)的最佳函數(shù)匹配.利用最小二乘法可以簡便地求得1 組未知位置數(shù)據(jù)，并使得這些求得的位置數(shù)據(jù)與實際水聲定位系統(tǒng)輸出的位置數(shù)據(jù)之間誤差的平方和為最小.

水聲定位系統(tǒng)輸出1 組緯度、經(jīng)度數(shù)據(jù)分別為

分別對緯度L、經(jīng)度λ 進行最小二乘擬合，假設(shè)最小二乘擬合直線函數(shù)為

分別計算擬合系數(shù)a1、a2、b1和b2使得計算得出的緯度、經(jīng)度與實測的緯度、經(jīng)度離差的平方和最小:

將計算得到的a1、a2、b1和b2代入式(3)，即可得到擬合直線.如果后續(xù)工作為實時定位解算，應(yīng)以擬合直線的末尾點為該時間點以后DR 的起始點.本文所研究的實驗數(shù)據(jù)為離線后處理，為了對全部數(shù)據(jù)進行定位解算研究，以擬合直線的初始點為全部數(shù)據(jù)DR 系統(tǒng)的初始位置.

采用最小二乘法確定初始位置后，能在一定程度上提高載體運動狀態(tài)情況下初始位置的計算精度.但最小二乘法擬合出的零階和一階系數(shù)是在對1 組數(shù)據(jù)整體誤差最小情況下計算出來的.當水聲定位數(shù)據(jù)輸出誤差正態(tài)性不好時，如前段數(shù)據(jù)誤差正偏大，而后段數(shù)據(jù)負偏大情況下最小二乘法擬合出曲線的一階系數(shù)會偏小，這會導致初始位置計算偏大，且該方法計算得出的運動軌跡具有隨機性，不同區(qū)段內(nèi)計算得出的運動軌跡偏差較大.因此，在實用中此方法還需要改進.

1.3 軌跡平移法

水聲位置數(shù)據(jù)分布影響擬合的運動軌跡，其誤差分布的非正態(tài)性將導致最小二乘擬合直線的斜率偏離真實值，從而引起初始位置計算誤差.為解決此問題，本文依據(jù)DR 系統(tǒng)短期精度高、數(shù)據(jù)平滑的特點，提出利用軌跡平移法計算初始位置的方案.軌跡平移法原理如圖2 所示.

圖2 軌跡平移法原理Fig.2 Principle of trajectory translation method

首先以水聲定位系統(tǒng)USBL 數(shù)據(jù)初始點坐標為初始位置，利用羅經(jīng)提供的姿態(tài)信息和多普勒速度計程儀(Doppler Velocity Log，DVL)提供的速度信息，進行DR 得到水下運載體在初始定位階段的運行軌跡.羅經(jīng)輸出的姿態(tài)信息在短時間內(nèi)精度較高，所以用DR 得到的位置數(shù)據(jù)能真實反映水下載體的運動趨勢和運動軌跡的形狀，此運動趨勢與真實運動軌跡相比誤差較小.但由于水聲定位系統(tǒng)提供的初始位置具有隨機性，所以應(yīng)用DR 系統(tǒng)計算的軌跡相對真實軌跡在位置上是有偏移的，但將DR 計算得到的軌跡進行平移則會得到與真實軌跡比較接近的運行軌跡.為得到軌跡的準確初始位置，本文在水聲定位系統(tǒng)數(shù)據(jù)和DR 系統(tǒng)數(shù)據(jù)擬合直線距離平方和最小準則下，對DR 系統(tǒng)計算出的軌跡進行平移，直至水聲定位系統(tǒng)位置數(shù)據(jù)到該直線距離平方和最小的判斷準則實現(xiàn)后，提取得到的軌跡即為在統(tǒng)計意義下與真實軌跡最接近的軌跡.而該軌跡的初始點即為所求的初始位置.軌跡平移法確定初始位置算法流程圖如圖3 所示.

利用軌跡平移法確定初始位置方法充分利用了DR 導航系統(tǒng)短期精度高和水聲定位系統(tǒng)誤差不發(fā)散的特點，進一步減小了初始的位置誤差.

圖3 軌跡平移法算法流程圖Fig.3 Flow diagram of trajectory translation method

2 實驗研究

2.1 實驗條件

本文在理論研究的基礎(chǔ)上，利用一次深度為4 700 m 左右的深拖系統(tǒng)海試實驗數(shù)據(jù)對第1 節(jié)初始位置確定方法進行了實驗驗證與分析.實驗數(shù)據(jù)包括約20 min 的羅經(jīng)、DVL 以及水聲定位系統(tǒng)USBL 的原始實際實驗數(shù)據(jù)，其中羅經(jīng)和DVL數(shù)據(jù)用于進行DR，USBL 數(shù)據(jù)用于定位.數(shù)據(jù)中羅經(jīng)為OCTANS，航向動態(tài)精度為0.1°/cos L，航向靜態(tài)誤差為0.05°/cos L;DVL 對底測速誤差為0.5%V，V 為速度;USBL 誤差為0.5%S，S 為位移.海試實驗中，不同傳感器輸出數(shù)據(jù)頻率不同，需要進行時間對準和匹配，所以定位結(jié)果分析中統(tǒng)一取計算周期為15 s.

2.2 初始位置確定方法實驗

圖4 USBL 原始位置數(shù)據(jù)Fig.4 Raw position data of USBL

應(yīng)用本文方法在第2.1 節(jié)介紹的實驗條件下開展初始位置確定方法的實驗研究，由于水下定位沒有絕對的參考基準，所以只能用水下航行器搭載導航系統(tǒng)來計算運行軌跡基準.USBL 原始位置數(shù)據(jù)如圖4 所示，其中前75 s(T1時間段，T1為初始定位時間)數(shù)據(jù)用于求取初始位置.圖4中水下航行器直線行駛的數(shù)據(jù)軌跡為直線，但存在波動，這是由于水聲定位原理和行駛過程中非理想因素的影響造成的，但這并沒有影響水下航行器直線行駛的趨勢.采樣點間隔為15 s，為減小非理想因素對運行軌跡的影響，本文采用卡爾曼濾波方法對軌跡數(shù)據(jù)進行事后處理，得到圖5、圖6中的水下航行器運行軌跡R，R 為直線.R 比較真實地反映了水下航行器的運行方向，依據(jù)T1時間段內(nèi)的R 確定的初始點是最準確的.但是實際航行中無法事先獲得此直線，本文利用后處理方法獲得此直線，只用于對本文方法計算的T1時間段內(nèi)運行軌跡進行評價.也就是說，將R 作為本文方法評價的基準，換句話說就是利用與R 最接近的直線確定的初始位置是最準確的.

本文分別采用平均值法、最小二乘法和軌跡平移法，利用在圖4 所示數(shù)據(jù)中T1時間段的數(shù)據(jù)來研究初始位置確定方法.應(yīng)用不同方法在T1時間段內(nèi)獲得的運動軌跡和全部時間段內(nèi)的后處理軌跡參考基準R 如圖5 所示.

圖5 初始位置確定與全部數(shù)據(jù)的數(shù)據(jù)參考基準Fig.5 Determination of initial position and data reference of all data

為清晰觀察T1時間段內(nèi)不同方法確定的運行軌跡，截取T1時間段內(nèi)不同方法獲得的運動軌跡如圖6 所示.圖6 中，直線R 為經(jīng)后處理獲得的直線參考基準，曲線1 為利用平均值法獲得的運行軌跡，曲線2 為利用本文提出的軌跡平移法計算確定的運行軌跡，曲線3 為對USBL 數(shù)據(jù)進行最小二乘擬合獲得的運行軌跡.從圖6 中可以看出，以USBL 在T1時間段內(nèi)的平均值為運行軌跡，受USBL 水聲定位系統(tǒng)數(shù)據(jù)波動的影響，引入計算誤差;對USBL 數(shù)據(jù)進行最小二乘擬合獲得的運行軌跡，受USBL 水聲定位系統(tǒng)數(shù)據(jù)誤差分布非正態(tài)性影響，經(jīng)度和緯度軌跡方向偏離直線R，且計算得出的運動軌跡具有隨機性;軌跡平移法利用DR 系統(tǒng)輸出的位置信息和USBL 水聲定位系統(tǒng)的位置信息進行曲線擬合和平移，得到新擬合曲線的斜率和距離更加接近真實運動軌跡.因此本文利用軌跡平移法確定的擬合曲線的起始點作為全部數(shù)據(jù)DR 的起始位置，這樣有助于提高DR 的精度和組合導航的性能.

圖6 不同方法對應(yīng)的初始運動軌跡Fig.6 Initial trajectory corresponding with different methods

不同方法確定的初始定位時間(T1時間段)內(nèi)的運行軌跡與直線R 作對比的誤差曲線如圖7所示.圖7 中曲線1 為平均值法確定的運行軌跡誤差曲線;曲線2 為本文提出的軌跡平移法確定的運行軌跡誤差曲線;曲線3 為USBL 最小二乘擬合方法確定的運行軌跡誤差.

圖7 不同方法確定運行軌跡誤差Fig.7 Error of initial trajectory corresponding with different methods

采用平均值法、最小二乘法和本文方法確定的初始位置(經(jīng)度、緯度)分別是:(122.399 81°，21.640 77°)、(122.399 52°，21.640 32°)、(122.399 68°，21.640 72°).采用以上3 種方法確定的初始位置與由通過直線R 確定的初始位置間的距離誤差分別為:39.35、30.68、23.68 m.以其中采用本文提出的軌跡平移法得到的初始位置誤差最小，分別是平均值法和最小二乘擬合方法的60.2%和77.2%.由以上結(jié)果可知，軌跡平移法確定初始位置誤差最小，以此方法確定的DR系統(tǒng)精度有所提高，此方法可減小DR 誤差提高組合導航系統(tǒng)性能.

3 結(jié) 論

本文綜合研究水下運載體動基座直線航行條件下的DR/水聲定位系統(tǒng)輸出數(shù)據(jù)的特點，提出了一種DR 軌跡平移法確定最優(yōu)初始位置的方法，經(jīng)實驗驗證表明:

1)本文提出的運動過程中水下運載體DR系統(tǒng)初始位置確定方法，利用水聲定位系統(tǒng)提供的位置信息和DR 得到的方向信息計算得到優(yōu)化的初始位置，該方法確定的初始位置誤差最小，分別是平均值法和最小二乘擬合方法的60.2%和77.2%.

2)利用本文方法進行初始位置的確定，提高了初始時段DR 定位方法的準確程度.

3)提高DR 的基礎(chǔ)上，也提高了組合導航的性能，有助于水下地形測繪、勘探等系統(tǒng)提高成圖質(zhì)量.

為使本文方法適應(yīng)海試實驗在線定位，仍需優(yōu)化算法，提高其實時性.

References)

［1］ 王淑煒，張延順.基于羅經(jīng)/DVL/水聲定位系統(tǒng)的水下組合導航方法研究［J］.海洋技術(shù)學報，2014，33(1):19-23.

Wang S W，Zhang Y S.Research on the underwater integrated navigation method based on compass/DVL/hydroacoustic position system［J］.Journal of Ocean Technology，2014，33(1):19-23(in Chinese).

［2］ Rigby P，Pizarro O，Williams S B.Towards geo-referenced AUV navigation through fusion of USBL and DVL measurements［C］∥Proceedings of OCEANS 2006.Piscataway，NJ:IEEE Press，2006:1-6.

［3］ 馮子龍，劉健，劉開周.AUV 自主導航航位推算算法的研究［J］.機器人，2005，27(2):168-172.

Feng Z L，Liu J，Liu K Z.Dead reckoning method for autonomous navigation of autonomous underwater vehicles［J］.Robot，2005，27(2):168-172(in Chinese)

［4］ Jirawimut R，Ptasinski P，Garaj V，et al.A method for dead reckoning parameter correction in pedestrian navigation system［J］.IEEE Transactions on Instrumentation and Measurement，2003，52(1):209-215.

［5］ 萬磊，李璐，劉建成，等.一種基于航位推算的水下機器人導航算法［J］.中國造船，2005，45(4):77-82.

Wan L，Li L，Liu J C，et al.Navigation algorithm based on dead reckoning of automatic underwater vehicle［J］.Shipbuilding of China，2005，45(4):77-82(in Chinese).

［6］ Yang X Y，He H.GPS/DR integrated navigation system based on adaptive robust Kalman filtering［C］∥Proceedings of International Conference on Microwave and Millimeter Wave Technology.Piscataway，NJ:IEEE Press，2008，4:1946-1949.

［7］ Chung H，Ojeda L，Borenstein J.Accurate mobile robot deadreckoning with a precision-calibrated fiber-optic gyroscope［J］.IEEE Transactions on Robotics and Automation，2001，17(1):80-84.

［8］ 朱維慶，朱敏，劉曉東，等.海底微地貌測量系統(tǒng)［J］.海洋測繪，2003，23(3):27-31.

Zhu W Q，Zhu M，Liu X D，et al.The survey system for detailed relief on sea floor［J］.Hydrographic Surveying and Charting 2003，23(3):27-31(in Chinese).

［9］ 朱維慶，劉曉東，張東升，等.高分辨率測深側(cè)掃聲納［J］.海洋技術(shù)，2005，24(4):29-35.

Zhu W Q，Liu X D，Zhang D S，et al.High resolution bathymetric sidescan sonar［J］.Ocean Technology，2005，24(4):29-35(in Chinese).

［10］ 孫樹民，李悅.淺談水下定位技術(shù)的發(fā)展［J］.廣東造船，2004(4):19-24.

Sun S M，Li Y.The development of underwater positioning technology［J］.Shipbuilding of Guangdong，2004(4):19-24(in Chinese).

［11］ Kim K，Choi H T，Lee C M.Underwater precise navigation using multiple sensor fusion［C］∥Proceedings of 2013 IEEE International Underwater Technology Symposium(UT).Piscataway，NJ:IEEE Press，2013:1-4.

［12］ 劉曉東，張方生，朱維慶，等.深水聲學拖曳系統(tǒng)［J］.海洋測繪，2006，25(6):37-40.

Liu X D，Zhang F S，Zhu W S，et al.Towed deep water acoustics system［J］.Hydrographic Surveying and Charting，2006，25(6):37-40(in Chinese).

［13］ 蔣青吉，肖昌榮，肖波.HiPAP100 水下定位系統(tǒng)在海底攝像中的應(yīng)用［J］.海洋地質(zhì)前沿，2013，29(11):62-66.

Jiang Q J，Xiao C R，Xiao B.The application of HiPAP 100 underwater positioning system to the seabed video system survey［J］.Marine Geology Frontiers，2013，29(11):62-66(in Chinese).

［14］ 李守軍，包更生，吳水根.水聲定位技術(shù)的發(fā)展現(xiàn)狀與展望［J］.海洋技術(shù)，2005，24(1):130-135.

Li S J，Bao G S，Wu S G.A practical overview and prospect of acoustic positioning technology［J］.Ocean Technology，2005，24(1):130-135(in Chinese).

［15］ 吳永亭，周興華，楊龍.水下聲學定位系統(tǒng)及其應(yīng)用［J］.海洋測繪，2003，23(4):18-21.

Wu Y T，Zhou X H，Yang L.Underwater acoustic positioning system and its application［J］.Hydrographic Surveying and Charting，2003，23(4):18-21(in Chinese).