一种深潜长航潜水器SINS/DVL洋流速度估计方法

本发明属于载人潜水器水下导航定位技术领域，涉及一种深潜长航潜水器sins/dvl洋流速度估计方法。

背景技术：

因为电磁信号在水下的衰减特性，因此全球定位系统gps不可用于海洋中层水域中的导航定位，同样，远离海底的多普勒测速仪dvl也无法直接用于导航。在中层水域中导航信息源稀缺，只有惯性测量单元imu和声学传感器可用。目前，特别是对于大深度执行长时间、远距离潜航任务的深潜长航潜水器，如何利用深海区稀缺的导航信息源进行中层水域的导航研究具有重要的科学意义。

对于深潜长航潜水器而言，中层水域洋流速度未知条件下，无法直接实现sins/dvl组合导航，需要实时获得洋流速度；甚至中层水域的洋流速度信息可能还需要被实时观测进行军事防务。因此洋流速度信息的重要性不可言喻，但是在可用导航信息源缺乏的中层水域条件下，实时获取洋流速度是很困难的。因此在现有水下导航传感器的技术条件下，设计一种可行准确的中层水域洋流速度估计方案至关重要。

专利文献(申请号：cn201910509338.2)：一种深潜载人潜水器sins自辅助导航方法。首先，通过进行两次sins导航解算，得到sins1和sins2，接着将sins1输出的水平速度滤波后与sins2构建速度匹配组合导航。其关键技术是利用载体下潜/上浮运动特征以及捷联惯性导航sins误差传播特性提取出载体真实的运动信息，再通过kalman滤波实现sins自辅助导航。该方法基于特定轨迹运动，仅适用于深载潜水器下潜/上浮阶段的导航过程，但是无法针对深潜长航潜水器在中层水域的巡航任务提供有效的导航方法；而且该方法也不能获取洋流速度信息。

技术实现要素：

针对以上问题，本发明提供一种深潜长航潜水器sins/dvl洋流速度估计方法。针对中层水域导航信息源缺乏，难以获取洋流速度的现状，提出一种深潜长航潜水器sins/dvl洋流速度估计方法，其特征在于，具体步骤如下：

(1)基于特定轨迹运动下的捷联性惯导系统sins进行两次导航解算输出，分别为sins1和sins2，设计高通滤波器获取sins1的真实水平速度，与sins2进行组合实现sins自辅助导航；

所述步骤1中基于特定轨迹运动下的捷联性惯导系统sins进行两次导航解算输出，分别为sins1和sins2，设计高通滤波器获取sins1的真实水平速度，与sins2进行组合实现sins自辅助导航如下：

s1.1：根据载人潜水器螺旋下潜运动的轨迹特性分析，以及sins误差传播特性分析；将圆周运动轨迹下sins1真实的水平速度视为高频信号，而sins1工作机理产生水平速度误差看做低频信号；

sins1输出的水平速度包括：真实水平速度和水平速度误差，由于初始对准误差、安装误差、导航算法误差的存在，得到sins系统误差模型：

失准角误差方程：

速度误差方程：

位置误差方程：

以上公式中，i代表惯性坐标系，n代表导航坐标系，e代表地球坐标系，b代表机体坐标系；φ＝[φnφeφd]^t为欧拉失准角，表示sins计算的导航坐标系和真实的导航坐标系n之间的旋转；δvⁿ＝[δvnδveδvd]^t为速度误差；δl,δλ,δh分别为纬度、经度和高度误差；其中，水平速度误差为δvn和δve包含三种不同的振动误差，分别为：

休拉振荡ts：

地球振荡te：

傅科振荡tf：

以上，r是地球半径，ω是地球自转角速度，l是纬度，g是重力加速度；

深潜载人潜水器螺旋下潜/上浮表现为圆周轨迹运动，其水平方向速度呈周期性变化；潜水器下潜或上浮过程，考虑了水平速度误差中周期t＝84.4分钟的休拉振荡；真实水平速度变化周期相对于休拉振荡周期要小，因此真实的水平速度视为高频信号，而水平速度误差就看作低频信号；

s1.2：设计高通无时延zd-hpf数字滤波器，对sins1输出的水平速度进行处理，滤除低频速度误差分量，进而获取载人潜水器的真实水平速度；

根据要保留、滤除的不同信号的频率差异特性，设置数字高通滤波器的技术指标(ωp,ωs,αp,αs)，分别是通带截止频率、阻带截止频率、阻带允许的最大衰减、阻带允许的最小衰减，由双线性变换的映射关系，经过频率转换为相应的模拟高通滤波器技术指标，转换公式为：

基于模拟高通滤波器技术指标(ωp,ωs,αp,αs)，经过频率变换公式转化为模拟低通滤波器技术指标(λp,λs,αp,αs)，为了计算简单，取归一化模拟低通原型系统函数g(p)的通带边界频率为λp＝1，从而可求得归一化阻带边界频率为：

根据频率变换后的(λp,λs,αp,αs)，计算滤波器阶数n：

设计巴特沃斯滤波器，查询归一化滤波器参数表，得到n对应的归一化低通原型系统函数g(p)，去归一化得到模拟高通滤波器传递函数hh(s)，利用双线性变换法将其转换为数字低通滤波器hl(z)；

再利用互补的思想将其转化为无时延数字高通滤波器hh(z)：

hh(z)＝1-hl(z)(12)

s1.3：通过kalman滤波器将步骤1.2中获取的真实水平速度与sins2进行组合实现sins自辅助导航；

选取姿态失准角、速度误差、位置误差、陀螺仪常值零偏、加速度计常值零偏为状态变量：

状态方程为：

其中，系统转移矩阵为：

系统干扰矩阵：

系统噪声向量：

由s1.2得到sins1经过高通滤波器zd-hpf处理的水平速度

选取sins2输出的水平速度和之差作为量测量z：

离散化的卡尔曼滤波状态方程和量测方程：

根据kalman滤波器进行求解，获得状态估计量对sins2输出进行修正，实现sins自辅助导航，从而输出高精度导航解和

(2)根据多普勒效应的原理，在中层水域工作的深潜长航潜水器利用多普勒计程仪dvl的水跟踪模式，测量对流速度；

所述步骤2中根据多普勒效应的原理，在中层水域工作的深潜长航潜水器利用多普勒计程仪dvl的水跟踪模式，测量对流速度的方法如下：

s2.1：根据多普勒效应的原理，基于单波束配置dvl测速情况下，求解载体速度vx；

多普勒测速仪dvl是通过安装在载体的超声换能器向海底发射超声波，根据多普勒效应对载体速度进行测量，多普勒效应就是指发射声源作相对介质的运动时，其频率接收频率不同的物理学现象，已知发射声波的频率f0、角度α、传播速度c0，只要测量得到多普勒频移fd的大小得到载体速度：

s2.2：实现四波束配置dvl测速，并对在中层水域工作的dvl进行水跟踪模式的选择，求解对流速度

假设双波束dvl系统前向和后向的波束发射频率均是f0，前、后向的多普勒频移差fd1和fd2为：

由上式可得：

由式(21)得到双波束配置dvl下的速度：

由式(24)可知四波束dvl系统下的速度：

针对的深潜长航潜水器所处的中层水域的条件下，潜水器对海底的距离远远超过了dvl测速的射程范围，因此无法使用dvl的底跟踪模式，此时dvl只能工作在水跟踪模式，测量相对水流的速度

(3)结合sins自辅助导航提供的高精度导航参数和利用dvl获取的对流速度，利用最小二乘估计算法rls估计洋流速度；

所述步骤3中结合sins自辅助导航提供的高精度导航参数和利用dvl获取的对流速度，利用最小二乘估计算法rls估计洋流速度方法如下：

s3.1：整理由步骤1得到sins自辅助导航输出的高精度导航解和步骤2得到的dvl测量的对流速度作为洋流估计的数据准备；

s3.2：对实际dvl测量值建模，以求解洋流速度为目的，建立与sins自辅助导航输出相关的关系式；

经过对dvl测速原理分析，建立dvl测速模型为：

以上，是dvl测量值，即对流速度；δk表示dvl刻度因子误差，v^d表示在dvl坐标系下测量的无误差的理想值；δv^d表示dvl随机测量误差，在洋流估计的过程中，和还有vcⁿ的关系式为：

由步骤1可知，姿态矩阵和速度是由sins自辅助导航输出的导航解；vcⁿ＝[vcnvcevcd]^t则表示表示洋流速度在导航系n下的投影；视为dvl仪表坐标d系到潜水器载体b系的坐标变换矩阵，可由下公式获取：

其中，i为3阶单位矩阵；表示d系和b系之间的安装误差角，αγ、αθ为水平安装误差角，为航向安装误差角；

将式(28)代入式(27)中：

整理关系式，将需要求解的未知项：洋流速度vcⁿ和安装误差角移至等式右边，已知项则转至等式的另一边，最终得：

s3.3：利用递归最小二乘估计recursiveleastsquare,rls算法估计水平洋流速度；

由于sins自辅助导航输出水平速度，深度计输出高度信息，所以只需要考虑水平洋流速度vcⁿ，为了估计出未知的水平洋流速度vcⁿ和安装误差角α，设状态变量：

系统状态方程为：

根据步骤3.2的式(29)，选取量测量为：

其中，姿态阵

进一步，系统量测方程：

z2＝h2x2+v2(35)

其中

利用递归最小二乘估计recursiveleastsquare,rls算法估计安装误差角和水平洋流速度vcⁿ，其计算公式为：

通过该计算公式，即可以估计安装角误差和洋流速度信息vcⁿ。

作为本发明进一步改进，为了精确测量载体的三维速度，减少载体因颠簸与起伏带来的影响，通常需要在载体的前后左右四个方向同时各发射一个超声波束，构成四波束janus正交配置。虽然仅需要三条波束就能够提供速度的三个分量，但是四波束配置平面阵列天线产生四条波束相对更容易；多出一个量测方程可以提高dvl速度解算的精度和测量冗余度，实现系统可靠性的改善。

本发明公开一种深潜长航潜水器sins/dvl洋流速度估计方法，包括以下步骤：1、基于特定轨迹运动下的捷联性惯导系统sins进行两次导航解算输出，分别为sins1和sins2，设计高通滤波器获取sins1的真实水平速度，与sins2进行组合实现sins自辅助导航；2、根据多普勒效应原理，在中层水域工作的深潜长航潜水器利用多普勒计程仪dvl的水跟踪模式测量对流速度；3、结合sins自辅助导航提供的高精度导航参数和利用dvl获取的对流速度，利用最小二乘估计算法rls估计洋流速度。本发明能够在可有导航信息源缺乏，难以实时获取洋流速度的条件下，通过sins/dvl估计出中层水域的洋流速度信息。

附图说明

图1是根据本发明实施例的sins自辅助导航框架图；

图2是根据发明实施例的无时延高通滤波器(zd-hpf)设计流程图；

图3是根据本发明实施例的四波束配置多普勒计程仪测试示意图；

图4是本发明深潜长航潜水器sins/dvl洋流速度估计流程图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

本发明提供一种深潜长航潜水器sins/dvl洋流速度估计方法。针对中层水域导航信息源缺乏，难以获取洋流速度的现状，提出一种结合sins自辅助导航输出的高精度导航信息和利用利用dvl获取的对流速度，利用rls算法实现安装误差角和洋流速度估计的方法。

作为本发明一种具体实施方法，本发明提供流程图如图4所示一种深潜长航潜水器sins/dvl洋流速度估计方法，具体步骤如下：

步骤1：基于特定轨迹运动的捷联性惯导系统sins进行两次导航解算输出，分别为sins1和sins2，设计高通滤波器获取sins1的真实水平速度，与sins2进行组合实现sins自辅助导航。

图1是sins自辅助导航框架图，深潜载人潜水器在下潜过程，采用螺旋下潜方案可以具备抗流的作用，可以分成水平方向上的圆周运动和竖直方向的匀速直线运动。而sins工作机理产生的水平速度误差被认为是低频成分，而潜水器螺旋下潜的真实水平速度则认为是高频成分。基于sins工作原理和误差传播方程，利用高/低频的区别可以对sins1输出的水平速度进行处理，获取真实水平速度。然后，通过kalman滤波器将该水平速度与sins2进行组合实现sins自辅助导航。

步骤1.1：根据载人潜水器螺旋下潜运动的轨迹特性分析，以及sins误差传播特性分析；将圆周运动轨迹下sins1真实的水平速度视为高频信号，而sins1工作机理产生水平速度误差看做低频信号。

sins1输出的水平速度包括：真实水平速度和水平速度误差，由于初始对准误差、安装误差、导航算法误差的存在，得到sins系统误差模型：

失准角误差方程：

速度误差方程：

位置误差方程：

以上公式中，i代表惯性坐标系，n代表导航坐标系，e代表地球坐标系，b代表机体坐标系；φ＝[φnφeφd]^t为欧拉失准角，表示sins计算的导航坐标系和真实的导航坐标系n之间的旋转；δvⁿ＝[δvnδveδvd]^t为速度误差；δl,δλ,δh分别为纬度、经度和高度误差；其中，水平速度误差为δvn和δve包含三种不同的振动误差，分别为：

休拉振荡ts：

地球振荡te：

傅科振荡tf：

以上，r是地球半径，ω是地球自转角速度，l是纬度，g是重力加速度。

深潜载人潜水器螺旋下潜/上浮表现为圆周轨迹运动，其水平方向速度呈周期性变化；潜水器下潜或上浮过程一般不多于3小时，所以本发明考虑了水平速度误差中周期t＝84.4分钟的休拉振荡；真实水平速度变化周期相对于休拉振荡周期要小的多，因此真实的水平速度可以视为高频信号，而水平速度误差就可以看作低频信号。

步骤1.2：如图1所示，设计高通无时延zd-hpf数字滤波器，对sins1输出的水平速度进行处理，滤除低频速度误差分量，进而获取载人潜水器的真实水平速度。

图2是无时延高通滤波器zd-hpf设计流程图，如图2所示：根据要保留、滤除的不同信号的频率差异特性，设置数字高通滤波器的技术指标(ωp,ωs,αp,αs)，分别是通带截止频率、阻带截止频率、阻带允许的最大衰减、阻带允许的最小衰减。由双线性变换的映射关系，经过频率转换为相应的模拟高通滤波器技术指标，转换公式为：

基于模拟高通滤波器技术指标(ωp,ωs,αp,αs)，经过频率变换公式转化为模拟低通滤波器技术指标(λp,λs,αp,αs)，为了计算简单，取归一化模拟低通原型系统函数g(p)的通带边界频率为λp＝1，从而可求得归一化阻带边界频率为：

根据频率变换后的(λp,λs,αp,αs)，计算滤波器阶数n：

设计巴特沃斯滤波器，查询归一化滤波器参数表，可以得到n对应的归一化低通原型系统函数g(p)，去归一化得到模拟高通滤波器传递函数hh(s)，利用双线性变换法将其转换为数字低通滤波器hl(z)。

再利用互补的思想将其转化为无时延数字高通滤波器hh(z)：

hh(z)＝1-hl(z)(12)

步骤1.3：通过kalman滤波器将步骤1.2中获取的真实水平速度与sins2进行组合实现sins自辅助导航。

选取姿态失准角、速度误差、位置误差、陀螺仪常值零偏、加速度计常值零偏为状态变量：

状态方程为：

其中，系统转移矩阵为：

系统干扰矩阵：

系统噪声向量：

由步骤1.2得到sins1经过高通滤波器(zd-hpf)处理的水平速度

选取sins2输出的水平速度和之差作为量测量z：

离散化的卡尔曼滤波状态方程和量测方程：

如图1所示，根据kalman滤波器进行求解，获得状态估计量对sins2输出进行修正，实现sins自辅助导航，从而输出高精度导航解和

步骤2：根据多普勒效应的原理，在中层水域工作的深潜长航潜水器利用dvl的水跟踪模式，测量对流速度；

步骤2.1：根据多普勒效应的原理，基于单波束配置dvl测速情况下，求解载体速度vx；

多普勒测速仪dvl是通过安装在载体的超声换能器向海底发射超声波，根据多普勒效应对载体速度进行测量。多普勒效应就是指发射声源作相对介质的运动时，其频率接收频率不同的物理学现象。一般已知发射声波的频率f0、角度α、传播速度c0，只要测量得到多普勒频移fd的大小可以得到载体速度：

步骤2.2：实现四波束配置dvl测速，并对在中层水域工作的dvl进行水跟踪模式的选择，求解对流速度

图3是四波束配置多普勒计程仪测试示意图，在实际应用中，为了精确测量载体的三维速度，减少载体因颠簸与起伏带来的影响，通常需要在载体的前后左右四个方向同时各发射一个超声波束，构成四波束janus正交配置。虽然仅需要三条波束就能够提供速度的三个分量，但是四波束配置平面阵列天线产生四条波束相对更容易；多出一个量测方程可以提高dvl速度解算的精度和测量冗余度，实现系统可靠性的改善。

假设双波束dvl系统前向和后向的波束发射频率均是f0，前、后向的多普勒频移差fd1和fd2为：

由上式可得：

由式(21)得到双波束配置dvl下的速度：

由式(24)可知四波束dvl系统下的速度：

对于本发明所针对的深潜长航潜水器所处的中层水域的条件下，潜水器对海底的距离远远超过了dvl测速的射程范围，因此无法使用dvl的底跟踪模式，此时dvl只能工作在水跟踪模式，测量相对水流的速度

步骤3：结合sins自辅助导航提供的高精度导航参数和dvl测量的对流速度，利用最小二乘估计算法rls估计洋流速度；

步骤3.1：整理由步骤1得到sins自辅助导航输出的高精度导航解和步骤2得到的dvl测量的对流速度作为洋流估计的数据准备。

步骤3.2：对实际dvl测量值建模，以求解洋流速度vcⁿ为目的，建立与sins自辅助导航输出相关的关系式。

经过对dvl测速原理分析，建立dvl测速模型为：

以上，是dvl测量值，即对流速度；δk表示dvl刻度因子误差，v^d表示在dvl坐标系下测量的无误差的理想值；δv^d表示dvl随机测量误差。在洋流估计的过程中，和还有vcⁿ的关系式为：

由步骤1可知，姿态矩阵和速度是由sins自辅助导航输出的导航解；vcⁿ＝[vcnvcevcd]^t则表示表示洋流速度在导航系n下的投影；视为dvl仪表坐标d系到潜水器载体b系的坐标变换矩阵，可由下公式获取：

其中，i为3阶单位矩阵；表示d系和b系之间的安装误差角，αγ、αθ为水平安装误差角，为航向安装误差角。

将式(28)代入式(27)中：

整理关系式，将需要求解的未知项：洋流速度vcⁿ和安装误差角移至等式右边，已知项则转至等式的另一边，最终得：

步骤3.3：利用递归最小二乘估计recursiveleastsquare,rls算法估计水平洋流速度vcⁿ。

由于sins自辅助导航输出水平速度，深度计输出高度信息，所以只需要考虑水平洋流速度vcⁿ，为了估计出未知的水平洋流速度vcⁿ和安装误差角α，设状态变量：

系统状态方程为：

根据步骤3.2的式(29)，选取量测量为：

其中，姿态阵

进一步，系统量测方程：

z2＝h2x2+v2(35)

其中

利用递归最小二乘估计recursiveleastsquare,rls算法估计安装误差角和水平洋流速度，其计算公式为：

通过该计算公式，即可以估计安装角误差和洋流速度信息vcⁿ。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。