馬小艷，陸明泉，張 輝，張吉先

（1. 清華大學(xué)電子工程系，北京 100084；2. 北京自動化控制設(shè)備研究所，北京 100074； 3. 陸軍裝備部駐北京地區(qū)軍事代表室，北京 100074）

水下無人航行器在海洋資源的探測和開發(fā)、石油天然氣管道的維修檢測以及維護海洋安全等方面發(fā)揮著重要的作用[1]。長航時高精度自主水下導(dǎo)航定位技術(shù)是關(guān)系水下無人航行器航行安全以及任務(wù)成功的關(guān)鍵技術(shù)之一。與陸空導(dǎo)航不同，由于海水介質(zhì)對光波和電磁波能量的大幅度衰減，使得無線電、衛(wèi)星等常規(guī)導(dǎo)航方式在水下均不可用[2,3]。目前，國內(nèi)外常用的水下導(dǎo)航定位技術(shù)有：慣性導(dǎo)航技術(shù)（Inertial Navigation System, INS）、航位推算技術(shù)、聲學(xué)導(dǎo)航與定位技術(shù)、海洋地球物理導(dǎo)航技術(shù)、組合導(dǎo)航技術(shù)等。組合導(dǎo)航是將兩種或更多的不同導(dǎo)航方式的導(dǎo)航信息進行組合處理，得到一種綜合導(dǎo)航信息的導(dǎo)航方式。組合導(dǎo)航利用了各種不同導(dǎo)航方式的優(yōu)點，實現(xiàn)取長補短，可得到更為理想的導(dǎo)航精度和性能。當(dāng)前水下導(dǎo)航的主要組合導(dǎo)航方式有：慣性/DVL 導(dǎo)航、慣性/地球物理場信息匹配（重力、地磁場、地形）組合導(dǎo)航、水下多源信息組合導(dǎo)航。地球物理場導(dǎo)航均要求事先存儲精確海底重力場/地磁場/地形等地圖信息；聲學(xué)定位系統(tǒng)要求事先在工作海域布設(shè)位置精確已知的參考基陣，且維護費用較高，因此由慣性導(dǎo)航系統(tǒng)以及DVL 構(gòu)成組合導(dǎo)航系統(tǒng)是目前國內(nèi)外應(yīng)用最廣泛、最成熟水下導(dǎo)航的技術(shù)方案[4-6]。SINS/DVL 通常采用速度匹配方式[2-6]，將載體坐標(biāo)系下的慣導(dǎo)系統(tǒng)的速度信息與DVL 的速度信息的差值作為觀測量，采用濾波估計方法，實現(xiàn)對慣導(dǎo)系統(tǒng)和DVL 的誤差的估計并進行修正，從而獲得較高的定位精度。

對于水下復(fù)雜環(huán)境影響使DVL 回波個數(shù)小于4，而導(dǎo)致DVL 測速信息失效情況下的INS/DVL 組合導(dǎo)航方法鮮有研究。文獻(xiàn)[6]對載體對底的距離大于DVL的工作距離，導(dǎo)致DVL 的對底速度失效問題，提出了一種考慮洋流的SINS/DVL 組合導(dǎo)航算法，假定短期小范圍內(nèi)DVL 設(shè)定水流層的洋流速度為恒定值，分析了DVL 水跟蹤模式下洋流速度對組合導(dǎo)航精度的影響，基于速度匹配組合導(dǎo)航的方式提出了SINS/DVL 組合導(dǎo)航系統(tǒng)設(shè)計方案，從而能夠有效提高DVL 僅對水輸出時系統(tǒng)的組合導(dǎo)航定位精度。而英國Sonardyne 公司2018 年推出的Sprint-Nav 系列高精度組合導(dǎo)航產(chǎn)品，通過將其DVL 與捷聯(lián)慣導(dǎo)一體化設(shè)計，實現(xiàn)了基于DVL 波束域信息輔助下的聲學(xué)與慣性緊耦合導(dǎo)航能力，最高導(dǎo)航精度0.04%航程。基于以上的研究，本文主要研究如何利用波束域信息以及在回波個數(shù)少于4 的情況下，采用何種組合修正策略，提高組合導(dǎo)航精度以及回波信息的利用率。

1 DVL 原理及誤差特性

1.1 DVL 工作原理

DVL 以多普勒效應(yīng)為基本原理[7]，利用水聲換能器向水底或水中介質(zhì)發(fā)射特定頻率的定向聲脈沖信號，接收從水底或水中聲波散射體上反射的回波信號，通過分析聲波回波信號的多普勒頻移，實現(xiàn)對載體速度的測量。為了提高測速精度，實際的多普勒測速聲吶常用兩個波束俯角相同，其中一個波束指向前下，另一波束指向后下方的詹納斯（Janus）配置。實際使用的DVL，為了能夠測得載體的前向和側(cè)向速度，通常采用四波束Janus 配置。除了前后配置的一對換能器外，還有左右方向也有一對換能器，用于測量載體的側(cè)向速度，四波束Janus 配置如圖1 所示。

圖1 四波束Janus 配置DVL 原理圖Fig.1 The schematic of four Janus beams DVL

由四波束Janus 配置的四個波束頻移求解速度計算公式如式(1)所示：

式中，fd1，fd2，fd3，fd4分別為四個波束的多普勒頻移，單位：Hz；vdx，vdy，vdz分別為DVL 儀器坐標(biāo)系的三維速度，單位：m/s；f0為DVL 換能器發(fā)射信號的頻率，單位：Hz；c為水中聲速，單位：m/s；α為波束俯角，通常為60°，單位：rad。

由速度求解四波束Janus 配置的四個波束頻移計算公式如式(2)所示。

寫成矩陣形式為：

DVL 儀器坐標(biāo)系定義為，原點位于DVL 的重心，xd軸與波束1、2 連線平行指向前方，zd軸與波束3、4 連線平行指向右方，yd軸由右手定則確定。為了提高DVL 對橫向速度分量的敏感性，擴大對水下航行器前向速度的測量范圍，通常DVL 在載體上有兩種方法，一種是十字型安裝，即1、2 波束與載體的前后向平行；另一種是“X”型配置，是使四個波束與載體的縱軸線交叉且對稱排列的X 型配置。

1.2 DVL 分類及誤差特性

按照換能器基陣的不同，DVL 分為兩類[7]，一種是活塞式，一種為相控陣，實物分別如圖2、圖3 所示。相對活塞式，相控陣由于利用相控陣進行收發(fā)，可以消除海水聲速不同對測速精度的影響。另外在增大DVL 的射程、減少聲吶基陣的尺寸和重量方面，相控陣也更具優(yōu)勢。

圖2 活塞式DVLFig.2 piston type DVL

圖3 相控陣DVLFig.3 phased array DVL

根據(jù)DVL 的射程，通常將DVL 劃分為三個等級：200 m 以內(nèi)的叫小深度計程儀；400～500 m 的稱為中深度計程儀；4000 m 以上的稱為大深度計程儀。國內(nèi)外主流的DVL 工作頻率主要在38 KHz～1200 KHz 的6 個典型值，以RTI 公司的產(chǎn)品為例，給出DVL 的主要技術(shù)指標(biāo)如表1 所示。

表1 典型DVL 主要技術(shù)指標(biāo)Tab.1 Main technical specifications of typical DVL

由此可見，DVL 工作頻率高，則測速精度高，但是隨著DVL 工作頻率越高，其射程就會越小，系統(tǒng)的體積、重量、功耗也越大，因此一般大型船舶安裝低頻的射程大的DVL，而對于小型的水下航行器而言，則選用高頻的，射程300 m 以內(nèi)的。

DVL 的測速誤差主要包括解算公式引起的誤差、載體搖擺引起的誤差、聲速引起的誤差以及有限波束寬度的影響等[7]，雖然通過增加傾斜傳感器、溫度、鹽度傳感器等手段，對上述的誤差進行補償，但是仍有在單DVL 級別不可補償?shù)恼`差，一般在工程應(yīng)用中引入DVL 刻度因子誤差以及測量噪聲，因此DVL的實際測量值如式(4)所示。

2 SINS/DVL 組合導(dǎo)航算法

常用的SINS/DVL 組合導(dǎo)航算法，慣導(dǎo)系統(tǒng)速度和 DVL 速度信息差值作為觀測量，針對影響SINS/DVL 組合導(dǎo)航系統(tǒng)的慣導(dǎo)系統(tǒng)誤差、DVL 刻度系數(shù)誤差以及兩者之間的安裝誤差、桿臂誤差等，建立詳細(xì)誤差模型，通過卡爾曼濾波器估計慣導(dǎo)系統(tǒng)誤差和DVL 誤差，并實時進行反饋修正，得到高精度的組合導(dǎo)航定位信息?；谒俣绕ヅ涞腟INS/DVL 組合導(dǎo)航算法原理如圖4 所示。

圖4 基于DVL 速度測量量SINS/DVL 組合導(dǎo)航算法原理圖Fig.4 The schematic diagram of Algorithm of inertial/DVL integrated navigation based on velocity integral

與常規(guī)的SINS/DVL 組合導(dǎo)航算法不同，基于四波束Janus 配置的聲學(xué)多普勒頻移的SINS/DVL 組合導(dǎo)航算法，需要DVL 輸出每個波束的頻移值，并需要由慣導(dǎo)系統(tǒng)的速度信息利用式(2)計算四個波束的頻移，而且觀測量是變維數(shù)的，其維數(shù)為實際收到的DVL 的有效回波數(shù)。具體的算法原理如圖5 所示。

圖5 基于頻移測量的SINS/DVL 組合導(dǎo)航算法原理圖Fig.5 The schematic diagram of Algorithm of inertial/DVL integrated navigation based on velocity integral

理論上僅需三個線性不相關(guān)的多普勒頻移即可得到載體三個方向的速度，而采用雙Janus 配置的多普勒可以得到四個頻移量，利用頻移解算速度即式(1)相當(dāng)于進行了降維處理，造成對冗余回波信息的利用不充分，而基于頻移測量的SINS/DVL 組合導(dǎo)航算法由于充分利用了回波信息，因此理論上導(dǎo)航精度以及信息的利用率均比常規(guī)速度組合有所提升。

2.1 組合導(dǎo)航濾波模型

影響INS/DVL 組合導(dǎo)航系統(tǒng)精度的因素主要可以分為三大類：一是捷聯(lián)慣性導(dǎo)航系統(tǒng)的誤差；二是DVL 的測速誤差；三是兩者之間的安裝誤差。兩者的安裝誤差主要包括安裝角誤差、桿臂誤差，其中桿臂誤差可以通過測量獲得。因此SINS/DVL 組合導(dǎo)航系統(tǒng)狀態(tài)變量通常選取如下：慣導(dǎo)系統(tǒng)的北向、天向、東向速度誤差δVn、δVu、δVe，北向、天向、東向姿態(tài)誤差φn、φu、φe，北向、天向、東向位置誤差δL、δh、δλ，載體系各軸向的加表零偏?x、?y、?z，陀螺漂移εx、εy、εz，DVL 的刻度系數(shù)誤差δKD，DVL 儀器坐標(biāo)系與慣導(dǎo)系統(tǒng)的b 系向安裝角誤差φx、φy、φz，共19 維?？梢缘玫絊INS/DVL組合導(dǎo)航系統(tǒng)誤差狀態(tài)方程如下：

根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)誤差方程可以得到系統(tǒng)狀態(tài)矩陣SINSF[7]。

假設(shè)δKD、φx、φy、φz為常數(shù)，因此FDVL為4 ×4零矩陣。理想情況下，DVL 的四個波束的頻移為：

將式(4)代入式(6)得到DVL 實際的頻移測量值為：

定義慣導(dǎo)系統(tǒng)載體坐標(biāo)系（b 系）原點位于載體重心，xb軸指向載體前方，yb軸指向載體上方，zb軸指向載體右方。慣導(dǎo)系統(tǒng)的導(dǎo)航坐標(biāo)系n 系原點位于載體重心，xn軸指向北，yn軸指向天，nz軸指向東。理論上，慣導(dǎo)系統(tǒng)的b 系和DVL 的儀器系（d 系）是重合的，而實際上b 系到DVL 儀器坐標(biāo)系（d 系）之間存在小角度的誤差角φx、φy、φz，因此可得慣導(dǎo)系統(tǒng)理想情況下速度對應(yīng)的四個波束的頻移為：

而實際慣導(dǎo)系統(tǒng)的頻移為：

將上式展開，并忽略二階小量，可得

將式(9)與(7)做差，即可得濾波器的觀測方程為：

將式(10)展開，可以得到：

式中，Cij(i=1,2,3;j=1,2,3)為姿態(tài)轉(zhuǎn)換矩陣中相應(yīng)的元素。

寫成矩陣的形式為：

其中：

H 為19× 4 的系統(tǒng)量測矩陣。非零元素分別為：

綜上所述，利用式(5)(15)建立的系統(tǒng)方程以及觀測方程，選擇合適的狀態(tài)初始值X、初始估計均方誤差陣P0、系統(tǒng)噪聲初始方差陣Q0以及量測噪聲方差陣Rk，按照離散型Kalman 濾波計算公式即可進行SINS/DVL 組合導(dǎo)航Kalman 濾波計算。

3 仿真驗證

3.1 仿真條件

根據(jù)水下環(huán)境特點以及SINS/DVL 組合導(dǎo)航系統(tǒng)的實際使用情況，設(shè)置如圖6 所示的仿真運行軌跡。

圖6 仿真運行軌跡Fig.6 Simulation running track

具體仿真條件設(shè)置如下：

（1）軌跡設(shè)置

仿真總時間：t=3600s ；轉(zhuǎn)彎時間：400 s；初始位置：φ0=39.8°，λ0=116.2°，h0=-80m ；初始速度：VN0=0m/s ，VU0=0m/s ，VE0=0m/s ；前向速度：3m/s；初始姿態(tài)角：航向角ψ= 45°，俯仰角θ= 0°，滾動角γ= 0°；初始姿態(tài)誤差：δψ= 0.1°，δθ=0.01°，δγ=-0.01°。

（2）慣性器件設(shè)置

慣性器件采樣周期：Tn=5ms ；陀螺漂移：0.01 °/h的常值漂移+0.01 °/h的隨機噪聲；加速度計零偏：0.0005m/s2常值偏+0.0005m/s2的隨機噪聲。

（3）DVL 參考信息設(shè)置

DVL 測速采樣周期：1sT= ；DVL 測速誤差：0.005m/s 的隨機噪聲；DVL 的刻度系數(shù)誤差為：0.2%；

（4）工況設(shè)置

考慮到DVL 由于水下地形等原因收不到回波的情況，會有15 種組合，但是考慮到前后的1、2 波束以及左右的3、4 波束等效的前提，簡化后，設(shè)置以下4 種典型工況：

工況1：四波束均有效；

工況2：僅1、2、3 波束有效；

工況3：僅1、3 波束有效；

工況4：僅1 波束有效。

而其余三種三波束有效的結(jié)果參照工況2，僅2、3 波束有效，僅1、4 波束或者僅2、4 波束有效等同于工況3，而僅1、2 波束、僅3、4 波束有效以及2、3、4 單獨有效的情況等同工況4。

3.2 仿真結(jié)果及分析

根據(jù)設(shè)定的仿真條件，得到四種工況以及速度匹配的組合定位誤差，由于僅1 個波束有效情況下的位置誤差相比其他四種工況差別較大，因此圖7、8、9給出了前三種工況與常規(guī)的速度匹配的SINS/DVL 組合導(dǎo)航北向位置誤差、東向位置誤差以及徑向位置誤差對比。

測得開孔圓柱殼孔上邊測點和孔中心高度左邊測點的軸向及環(huán)向應(yīng)變隨時間變化曲線如圖6所示。其中應(yīng)變片1測孔上邊測點的軸向應(yīng)變，應(yīng)變片2測孔上邊測點的環(huán)向應(yīng)變，應(yīng)變片3測孔左邊測點的軸向應(yīng)變，應(yīng)變片4測孔左邊測點的環(huán)向應(yīng)變。屈曲后各測點處應(yīng)變出現(xiàn)了突變(應(yīng)變片1除外)，孔上邊測點軸向應(yīng)變片1應(yīng)變變化不明顯，因為下方是圓孔，軸向無支撐反力，但環(huán)向應(yīng)變片2應(yīng)變變化很明顯?？鬃筮厹y點軸向應(yīng)變片3和環(huán)向應(yīng)變片4變化規(guī)律剛開始不一樣，屈曲后出現(xiàn)突變，變化規(guī)律一致，但環(huán)向應(yīng)變比軸向應(yīng)變大，開孔改變了殼體應(yīng)力分布?？赘浇l(fā)生了明顯屈曲。

圖7 波束大于2 以及速度匹配組合北向位置誤差對比Fig.7 North position error comparison of four integrated navigation

圖8 波束大于2 以及速度匹配組合東向位置誤差對比Fig.8 East position error comparison of four integrated navigation

圖9 波束大于2 以及速度匹配組合徑向位置誤差對比Fig.9 Radial position error comparison of four integrated navigation

對比工況1、2、3 與速度匹配的定位誤差曲線可知，四波束匹配的組合導(dǎo)航誤差最小，三波束和速度匹配的結(jié)果相當(dāng)，2 波束的組合導(dǎo)航誤差最大。主要是從理論上而言，三個波束即可完成三個方向的速度測量，一般DVL 也可通過設(shè)置，在僅三個波束有效的情況下輸出速度，四個波束主要是構(gòu)成兩對Janus，進一步提高速度的測量精度，因此四波束匹配的組合導(dǎo)航精度也是最高的。而由于文中的仿真軌跡，為二維平面運動，因此工況3 即前后左右各有一個波束有效的情況，對于測量二維平面的速度信息的情況是充分的，因此工況3 也可以得到很好的組合定位結(jié)果。表2 給出了波束大于2 以及速度匹配組合位置誤差統(tǒng)計，也可以得到相同的結(jié)果。

表2 有效回波大于2 以及速度匹配組合位置均方根誤差Tab.2 Position RMSE error of echo beams above 2 and velocity intergraded navigation

僅一個方向的波束有效時，相當(dāng)于僅可測得水平兩個速度誤差的線性組合，無法將兩個方向的誤差進行分離，即使采用“X”型安裝，即繞DVL 的Y 軸轉(zhuǎn)45°，使波束1、2 的連線與航行器的前向成45°，僅在直線運動段，也無法對慣導(dǎo)的兩個方向的誤差進行有效估計，而當(dāng)航行器轉(zhuǎn)彎后，即可有效地分離兩個方向的速度誤差。

圖10-12 分別給出了兩種安裝方式下單波束有效情況下的組合導(dǎo)航定位誤差、純慣性定位誤差，表3給出了轉(zhuǎn)彎前、轉(zhuǎn)彎后、全過程單波束組合導(dǎo)航與純慣性導(dǎo)航的誤差統(tǒng)計，可見一次轉(zhuǎn)彎后，單波束組合導(dǎo)航的定位誤差也降到50 m 以內(nèi)，遠(yuǎn)高于慣性導(dǎo)航定位精度。

圖10 兩種安裝方式單波束組合北向位置誤差對比Fig.10 Single echo beam integrated north position error of two installation ways

圖12 純慣性與兩種安裝方式單波束組合徑向位置誤差Fig.12 radial position error of SINS and Single wave beam integrated

表3 純慣及兩種安裝單回波組合導(dǎo)航位置均方根誤差Tab.3 Position RMSE error of single echo beam intergraded navigation and pure INS

4 湖面試驗結(jié)果及分析

為了對算法的有效性進行驗證，對湖上試驗的數(shù)據(jù)進行了半實物仿真。湖上試驗系統(tǒng)，主要包括光纖捷聯(lián)慣性導(dǎo)航系統(tǒng)、DVL 和GPS 接收機。GPS 接收機用于考核組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。湖上試驗所在水域水深較淺，平均水深34 米，最大深度不到100米，沒有超過DVL 的有效射程，且航行器上無其它的聲吶設(shè)備對DVL 造成干擾，但是實驗過程中，仍出現(xiàn)了DVL 測速異常的情況，以其中一條次為例，圖13 給出了試驗過程DVL 速度曲線，速度為30 m/s的是無效情況，統(tǒng)計無效測速比例達(dá)到了0.2%。而試驗中的DVL，暫不支持輸出頻移信息，為了驗證算法的有效性，通過式(2)，利用DVL 的速度信息計算得到四個有效的DVL 的頻移信息，并進行了數(shù)學(xué)仿真。圖14-16 分別給出了速度匹配以及頻移匹配的組合定位誤差曲線。

圖13 湖上試驗DVL 的速度輸出Fig.13 DVL velocity output in lake test

圖14 湖上試驗北向位置誤差對比Fig.14 Comparison of north position error in lake test

圖15 湖上試驗東向位置誤差對比Fig.15 Comparison of east position error in lake test

圖16 湖上試驗徑向位置誤差對比Fig.16 Comparison of radial position error in lake test

從上述結(jié)果中可以看出，采用基于波束域信息的 組合比基于速度匹配的SINS/DVL 組合導(dǎo)航精度更高，組合導(dǎo)航誤差由6.2889 m 提高到4.229 m，定位精度提高30%以上。

5 結(jié) 論

1）當(dāng)聲學(xué)多普勒回波數(shù)量為4 時，可以得到比常規(guī)速度組合更高的導(dǎo)航精度；

2）當(dāng)回波數(shù)量為3 時，波束域組合與速度組合的精度相當(dāng)；

3）對于二維平面的運動，若能保證兩對Janus 配置均至少有一個波束有效，則仍可進行高精度的組合導(dǎo)航。

由此可見，本文提出的波束域SINS/DVL 組合導(dǎo)航算法，能夠提升組合導(dǎo)航定位精度以及導(dǎo)航系統(tǒng)對水下復(fù)雜環(huán)境的適應(yīng)性，具有較好的工程適用性和應(yīng)用價值。