基于波束域信息的SINS/DVL組合導航算法

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

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

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

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

1 DVL 原理及誤差特性

1.1 DVL 工作原理

DVL 以多普勒效應為基本原理[7]，利用水聲換能器向水底或水中介質發射特定頻率的定向聲脈沖信號，接收從水底或水中聲波散射體上反射的回波信號，通過分析聲波回波信號的多普勒頻移，實現對載體速度的測量。為了提高測速精度，實際的多普勒測速聲吶常用兩個波束俯角相同，其中一個波束指向前下，另一波束指向后下方的詹納斯（Janus）配置。實際使用的DVL，為了能夠測得載體的前向和側向速度，通常采用四波束Janus 配置。除了前后配置的一對換能器外，還有左右方向也有一對換能器，用于測量載體的側向速度，四波束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 儀器坐標系的三維速度，單位：m/s；f0為DVL 換能器發射信號的頻率，單位：Hz；c為水中聲速，單位：m/s；α為波束俯角，通常為60°，單位：rad。

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

寫成矩陣形式為：

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

1.2 DVL 分類及誤差特性

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

圖2 活塞式DVLFig.2 piston type DVL

圖3 相控陣DVLFig.3 phased array DVL

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

表1 典型DVL 主要技術指標Tab.1 Main technical specifications of typical DVL

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

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

2 SINS/DVL 組合導航算法

常用的SINS/DVL 組合導航算法，慣導系統速度和 DVL 速度信息差值作為觀測量，針對影響SINS/DVL 組合導航系統的慣導系統誤差、DVL 刻度系數誤差以及兩者之間的安裝誤差、桿臂誤差等，建立詳細誤差模型，通過卡爾曼濾波器估計慣導系統誤差和DVL 誤差，并實時進行反饋修正，得到高精度的組合導航定位信息。基于速度匹配的SINS/DVL 組合導航算法原理如圖4 所示。

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

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

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

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

2.1 組合導航濾波模型

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

根據捷聯慣導系統誤差方程可以得到系統狀態矩陣SINSF[7]。

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

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

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

而實際慣導系統的頻移為：

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

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

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

式中，Cij(i=1,2,3;j=1,2,3)為姿態轉換矩陣中相應的元素。

寫成矩陣的形式為：

其中：

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

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

3 仿真驗證

3.1 仿真條件

根據水下環境特點以及SINS/DVL 組合導航系統的實際使用情況，設置如圖6 所示的仿真運行軌跡。

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

具體仿真條件設置如下：

（1）軌跡設置

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

（2）慣性器件設置

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

（3）DVL 參考信息設置

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

（4）工況設置

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

工況1：四波束均有效；

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

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

工況4：僅1 波束有效。

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

3.2 仿真結果及分析

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

測得開孔圓柱殼孔上邊測點和孔中心高度左邊測點的軸向及環向應變隨時間變化曲線如圖6所示。其中應變片1測孔上邊測點的軸向應變，應變片2測孔上邊測點的環向應變，應變片3測孔左邊測點的軸向應變，應變片4測孔左邊測點的環向應變。屈曲后各測點處應變出現了突變(應變片1除外)，孔上邊測點軸向應變片1應變變化不明顯，因為下方是圓孔，軸向無支撐反力，但環向應變片2應變變化很明顯。孔左邊測點軸向應變片3和環向應變片4變化規律剛開始不一樣，屈曲后出現突變，變化規律一致，但環向應變比軸向應變大，開孔改變了殼體應力分布。孔附近發生了明顯屈曲。

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

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

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

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

圖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 純慣及兩種安裝單回波組合導航位置均方根誤差Tab.3 Position RMSE error of single echo beam intergraded navigation and pure INS

4 湖面試驗結果及分析

為了對算法的有效性進行驗證，對湖上試驗的數據進行了半實物仿真。湖上試驗系統，主要包括光纖捷聯慣性導航系統、DVL 和GPS 接收機。GPS 接收機用于考核組合導航系統的導航精度。湖上試驗所在水域水深較淺，平均水深34 米，最大深度不到100米，沒有超過DVL 的有效射程，且航行器上無其它的聲吶設備對DVL 造成干擾，但是實驗過程中，仍出現了DVL 測速異常的情況，以其中一條次為例，圖13 給出了試驗過程DVL 速度曲線，速度為30 m/s的是無效情況，統計無效測速比例達到了0.2%。而試驗中的DVL，暫不支持輸出頻移信息，為了驗證算法的有效性，通過式(2)，利用DVL 的速度信息計算得到四個有效的DVL 的頻移信息，并進行了數學仿真。圖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

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

5 結 論

1）當聲學多普勒回波數量為4 時，可以得到比常規速度組合更高的導航精度；

2）當回波數量為3 時，波束域組合與速度組合的精度相當；

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

由此可見，本文提出的波束域SINS/DVL 組合導航算法，能夠提升組合導航定位精度以及導航系統對水下復雜環境的適應性，具有較好的工程適用性和應用價值。