基于波束域信息的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 组合导航算法，能够提升组合导航定位精度以及导航系统对水下复杂环境的适应性，具有较好的工程适用性和应用价值。