CN107084723B - 一种海洋环境下的水下航行体运动轨迹估计方法 - Google Patents

Abstract

本发明公开了一种海洋环境下的水下航行体运动轨迹估计方法，属于目标运动轨迹估计领域。在本发明的估计方法中，首先建立水下航行体的水下空间运动方程组，针对对其运动轨迹影响较大的海浪和海流两个海洋环境特性，分别建立波浪、海流的数值模型，研究分析水下航行体垂直出水时运动轨迹参数受波浪、海流影响时的变化规律，并利用该规律对水下航行体的运动轨迹进行估计。本发明可以为决策者选择水下航行体适当的启动时机及姿态控制等提供科学依据，并可为后续水下航行体水下运动时机及方式的制定等相关工作提供技术支撑。

Description

一种海洋环境下的水下航行体运动轨迹估计方法

技术领域

本发明属于目标运动轨迹估计领域，尤其涉及一种海洋环境影响下的水下航行体运动轨迹估计技术。

背景技术

水下航行体在水下运动过程中，海流、海浪等海洋环境因素将对水下航行体的运动轨迹产生重要影响。当水下航行体从开始运动至飞行接近水面时，由于波浪的作用，其运动轨迹因受到强烈的干扰而可能产生偏离，偏离大小受波浪的参数，如波高、波长、周期、波速等的制约，具有较大的随机性。波浪的影响主要体现在两个方面：首先，在波浪的冲击下，水下航行体瞬时位于波面附近的部分承受较大的切应力载荷，航行体按照预定规律运动的能力和稳定性将面临巨大考验；其次，若波浪力作用在水下航行体质心两侧，则会产生较大的偏转力矩，使其运动姿态发生改变，就可能导致其运动航迹发生彻底改变。因此，波浪是影响水下航行体运动轨迹的重要因素。水下垂直运动与水平运动相比，整个运动过程中水下航行体运动轨迹受海流影响要大得多。而为了实现既定目标，水下航行体的出水角度要控制在90度附近，因此水下航行体在整个水下运动过程中将处于大攻角状态，此种状态下的水下流体动力建模、运动轨迹控制等都是全新的技术问题。

在诸多海洋环境因素的影响下，水下航行体水下运动轨迹变得极其复杂并具有不确定性。水下航行体能否按照既定轨迹运行，直接影响到水下航行体运动的安全性、运动的可靠性及完成任务的有效性。根据海洋环境因素对其运动轨迹进行估计，可以为决策者选择水下航行体适当的启动时机及姿态控制等提供科学依据，具有重要的工程价值和理论意义。本发明可为后续水下航行体水下运动时机及方式的制定等相关工作提供技术支撑。

发明内容

本发明首先建立水下航行体的水下空间运动方程组，针对对其运动轨迹影响较大的海浪和海流两个海洋环境特性，分别建立波浪、海流的数值模型，研究分析水下航行体垂直出水时运动轨迹参数受波浪、海流影响时的变化规律，并利用该规律对水下航行体的运动轨迹进行估计。

建立固连于大地的地面坐标系ox₀y₀z₀，原点o置于运动时刻航行体浮心在水面上的投影点，ox₀z₀坐标平面与水面重合，y₀轴垂直向上。再建立原点位于浮心的航行体坐标系oxyz，ox轴指向航行体头部，oy轴在航行体纵对称面内与ox轴垂直，oz轴垂直于oxy平面，指向按右手系确定。速度坐标系原点与航行体坐标系原点重合，ox'轴与原点处航行体的速度矢量方向重合，oy'轴位于航行体的纵对称面内，垂直于ox'轴并指向上方，oz'轴与ox'y'z'平面垂直，指向按右手系确定。地面坐标系、速度坐标系和航行体坐标系示意图如附图1所示。

航行体在空间的位置由航行体浮心在地面坐标系内的坐标给出，航行体在空间的姿态由三个欧拉角确定。这三个欧拉角的定义为：将航行体坐标系平移至与其原点与地面坐标系原点重合的位置，再从该位置开始，依次绕oy、oz、ox轴旋转ψ,θ,三个角度使得两个坐标系完全重合，我们将三个角度ψ,θ,的组合称为欧拉角，并将这三个角度用于表示水下航行体的空间姿态，ψ,θ,分别称为偏航角、俯仰角和横滚角。

由地面系ox₀y₀z₀到航行体系oxyz的转换矩阵为

反之，由航行体系oxyz到地面系ox₀y₀z₀的转换矩阵为的转置，即

水下航行体的攻角与侧滑角由航行体坐标系与速度坐标系之间的相对位置定义。速度轴ox'在航行体纵对称面oxy内的投影与航行体纵轴ox之间的夹角，称为攻角，记为α。速度轴ox'与航行体纵对称面oxy平面的夹角称为侧滑角，记为β。

根据动量定理和动量矩定理，得到水下航行体的水下空间运动方程组的一般形式：

式中，

为航行体浮心处的线加速度在三个坐标轴上的分量；

为航行体角加速度在三个坐标轴上的分量；

v_ox,v_oy,v_oz为航行体浮心处的速度在三个坐标轴上的分量；

ω_x,ω_y,ω_z为航行体角速度在三个坐标轴上的分量；

A_mλ为惯性矩阵，即

其中：

m为航行体质量；

λ_ij,i,j＝1,2,...,6为附加质量；

x_c,y_c,z_c为为航行体质心位置；

J_i,j,i,j＝x,y,z为航行体转动惯量、惯性矩；

惯性矩阵A_mλ中的各参数可根据水下航行体的外形尺寸、质量等固有参数在对其轨迹估计前获得，各参数均为常数量；

A_vω为速度矩阵，即

A_FM为力矩阵，即

其中：

X_B,Y_B,Z_B为浮力在航行体坐标系下的三个坐标轴上的分量；

X_G,Y_G,Z_G为重力在航行体坐标系下的三个坐标轴上的分量；

M_Gx,M_Gy,M_Gz为重力矩在航行体坐标系下的三个坐标轴上的分量；

X_αμ,Y_αμ,Z_αμ为粘性位置力在航行体坐标系下的三个坐标轴上的分量；

M_αμx,M_αμy,M_αμz为粘性位置力矩在航行体坐标系下的三个坐标轴上的分量；

Y_ωμ,Z_ωμ为粘性阻尼力在航行体坐标系下的Y和Z两个坐标轴上的分量；

M_ωμx,M_ωμy,M_ωμz为粘性阻尼力矩在航行体坐标系下的三个坐标轴上的分量；

T_x,T_y,T_z为推力矢量在航行体坐标系下的三个坐标轴上的分量；

M_Ty,M_Tz为推力矢量产生的力矩在航行体坐标系下的Y和Z两个坐标轴上的分量。

下面分别给出各个力的表达式。

航行体浮力大小为

B＝ρVg (2)

式中，V为航行体的排水体积，ρ为海水密度。

浮力作用方向沿地面坐标系的oy₀轴正向，其在航行体坐标系内的三个分量为

航行体重力的大小为

G＝mg (4)

m为航行体质量。重力作用方向沿地面坐标系的oy₀轴负向，其在航行体坐标系下的三个分量为

重力矩在航行体坐标系下的三个分量为

在需要对航迹进行估计的有限的时间及空间范围内，粘性阻尼力可看做一个常数。水下航行体的推力是一个分段线性函数，其表达式可根据先验公式得到：推力T＝T₀(1-3.45e-3H)，其中T₀为发动机在空气中的推力，H为发动机在水下的工作深度，e为自然对数。这里的粘性位置力由海浪扰动力产生。在得到A_FM中各个力的表达式后，将水下空间运动方程组进行离散化，对目标运动参数 进行迭代求解。利用海流传感器测量海流流向和流速，计算海流对水下航行体的影响，并利用海流扰动力对每一时刻得到的运动参数进行修正。

水下航行体在近海面运行时，其运动轨迹不可避免地受到海洋波浪等海洋环境因素的影响，其中海流和海浪是两个主要的影响因素，因此这里主要考虑这两个因素的影响。首先考虑海浪影响所产生的海浪扰动力。海浪的波浪场可以分成压力场和速度场，因此，波浪对水下航行体的作用力可分为两部分：由波浪压力场引起的作用力和由于波浪质点的轨迹速度引起的作用力。下面分别对其进行建模。

假设流体为理想不可压缩流体，并且为无限深度，波高相较于波长属于小量，根据波浪的线性边界条件，即可得到二元进行波。

波浪的速度势函数为

波浪自由表面函数为

式中，A为波幅；ω为圆周频率；k为波数，相当于2π距离内波的数目。其中圆周频率ω和波数k之间的关系为

ω²＝kg (9)

式中，g为重力加速度。

波浪方向是指波浪的传播方向。一般情况下，波浪的方向用其与地面坐标系x₀轴正向的夹角γ表示。利用坐标转换可得到不同方向的波浪在地面坐标系中的速度势函数表达式，即

波浪的速势也经常以复数的形式(复势)表示，对应于式(10)的速势φ的复势Φ为

海洋上的实际波浪是非常复杂的，且随时间与空间而变化，具有很强的随机性，这里用概率理论进行建模。描述海洋实际不规则波浪的方法是利用不同初始相位的规则余弦波进行叠加。假定波系中每个波浪的初始相位以相同的概率在0～2π之间随机散布，而且不同频域规则波的初始相位之间是互不相关的。于是，不规则波浪可用下式描述

η(x₀,z₀,t)＝Re{∫dA(ω)exp[-ik(x₀ cosγ+z₀ sinγ)+iωt+iε(ω)]} (12)

式中，Re{}为对括号内的值取实部；ε(ω)为不同频率波浪的初始相位角。而规则波的波幅dA(ω)为

式中，S_X(ω)为随机波浪的频谱。经过充分验证的波浪S_X(ω)的半经验公式为

式中，ν_w为自由表面以上19.5m标准高度处的风速；C₁、C₂为无量纲参数，C₁和C₂最常用的值为

C₁＝8.1×10-3，C₂＝0.74

不规则波浪复势为

水下航行体的动力学方程是在航行体坐标系中建立的，波浪复势也应转换到航行体坐标系中。将波浪复势进行转换可得

其中

B＝ky_0B+i(ωt+ε-kx_0B cosγ-kz_0B sinγ) (18)

式中，x_0B，y_0B，z_0B为水下航行体坐标系原点在地面坐标系中的坐标，在水下航行体的动力学方程中为x₀，y₀，z₀，这里加下标B是为了进行区分。根据切片理论，水下航行体的切片示意图如附图2所示。

水下航行体沿轴线上各切片所受的由海浪压力场引起的作用力可由波浪复势求得。

由式(19)可导出波浪在水中形成的附加压力场p，如式(20)所示：

式中，ν为水下航行体的运动速度。

该压力场在航行体坐标系中y、z方向上对单位长度切片产生的附加扰动力为

式中，S(x)为水下航行体在位置x处切片横截面的面积。

在波浪传播过程中，流场中的流体质点在水下作非定常运动，这将引起水下航行体周围流场的附加运动，从而对水下航行体形成附加惯性力。水下航行体单个切片在航行体坐标系中y、z方向产生的附加惯性力分量为

式中，ν′_y、ν′_z为由于波浪的运动，流体质点在切片形心处产生的扰动速度在航行体坐标系中y、z方向的分量；S_ey(x)、S_ez(x)为海浪在单个切片上产生的附加质量力。

由式(20)～(24)得到了水下航行体各个切片在波浪扰动下受到的压力场力以及附加质量力，则在航行体坐标系中，水下航行体在Y轴和Z轴上所受到的总的波浪扰动力和力矩为

式中

F_j(ω，θ，ψ)＝∫dx·{iω[S_ej(x)+S(x)]+νS′_ej(x)}·exp[kx sinθ-ikx(cosθcosψcosγ-cosθsinψsinγ)] j＝y，z (29)

取Y、Z、M_y、M_z的实部即为作用于水下航行体的波浪扰动力和力矩在航行体坐标系中的表达式。其中，夹角γ表示波浪方向，这里用波浪方向与地面坐标x₀系列轴正向的夹角表示，ω为波浪的圆周频率，A为波浪波幅，γ、ω和A均可通过测波仪测量得到，ρ为流体密度。水下航行体的水下空间运动方程组中的粘性位置力Y_aμ和Z_aμ分别为Y和Z，相应的粘性位置力矩M_aμy和M_aμz分别为M_y和M_z。

粘性位置力和粘性位置力矩运用切片法求解，切片法具体操作是将水下航行体沿轴向进行切分，将积分方程沿坐标x轴进行离散化，在各个切片内分别求解，最后累加得到积分值。将式(25)～(28)进行离散化后，即可运用切片法求解。切片具体操作是将水下航行体沿轴向进行切分，其中切片时应遵循以下原则：

(1)在水下航行体头部和尾部外形尺寸变化较大的地方需较细切分；

(2)在水下航行体中部圆柱段部分可进行较粗的切分。

一般说来，切片总数增加可以提高计算轨迹的精度。实际计算表明，切片总数达到100左右，切片个数对波浪力计算轨迹精度的影响己可忽略不计。本发明中，将水下航行体沿x轴向共切分100段，其中头部30段、平行中体25段、尾部45段。通过大量的实际计算结果表明，切片总个数达到100左右时，波浪力的计算精度变化随切片个数的增加已为微小量，可忽略不计。每一个切片横截面处的波浪扰流流动可局部地看成是简单的二元流动，每一切片的附加质量与其几何外形有关，因此每一切片处的附加质量系数可以根据无限长圆柱体附加质量系数进行保角变换求得。

将计算得到的海浪扰动力带入水下空间运动方程组式(1)中，将该方程组离散化，对水下航行体的运动参数进行迭代求解，得到目标运动参数初始估计值，从而完成对某一时刻水下航行体运动参数的估计。水下航行体在初始运动时刻的位置坐标是已知的，根据后续运动时刻所估计的目标运动参数如速度矢量，即可得到目标的完整运动轨迹，从而实现对目标运动轨迹的估计。

自然界中的海流是异常复杂的，其影响因素和制约条件复杂多样。根据某测试单位对众多海域中的数千个监测点海流的测试结果可以看出，地理位置对测试点海流的大小和方向有一定的影响，主要相关因素为测试点离水面的高度以及海域的经纬度；测试的时间也对海流的大小和方向有影响；因此海流既是空间的函数又是时间的函数，并且还有着相当大的随机性。目前，国内外文献中还未见到公开发表的自然海流相关的数据库或者数学模型。这里在海流对水下航行体运动的研究中，假定海流的速度为常量，即

v_w0＝v_wx0i₀+v_wy0j₀+v_wz0k₀ (32)

式中，v_w0为海流在地面系中的速度矢量，v_wx0,v_wy0,v_wz0为海流速度矢量v_w0在地面系中的x，y，z方向的三个分量，并且在一定时间和空间范围内，海流速度矢量不随着时间与空间的变化而变化。海流的流向和流速可通过海流传感器测量得到。

式(32)中定义了一个定常的均匀流场，当水下航行体在存在海流的流场中运动时，相当于在原来运动的流场中又叠加了一个由海流引起的附加流场，即

v_T0＝v_Tw0+v_w0 (33)

式中，v_T0为水下航行体在地面系中运动时的速度矢量，v_Tw0为水下航行体相对于海流的运动速度。

由式(33)可见，在无海流条件下，即海流流速为零条件下，水下航行体相对于海流的运动速度和水下航行体在地面系中的运动速度(绝对速度)是相同的，因此海流速度即为速度坐标系中的牵连速度，海流的运动是定常的平移运动。当观察者在运动坐标系中随海流一起运动时，对于观察者来说，海流是不存在的，所观察到的水下航行体运动与没有海流时是相同的。

水下航行体在流体中运动时所受到的流体动力和力矩，如升力、阻力等均由水下航行体相对于流体的速度、攻角与侧滑角决定，因此，水下航行体运动数学模型中所建立的各个运动学参数在有流体运动情况下均是相对于流体的，只有在流体处于静止状态时，得到的运动轨迹参数才是相对于地面坐标系的，即

因此，在存在海流的条件下，运用水下航行体空间运动方程组对目标运动轨迹进行估计时，需要按照式(34)对其估计得到的运动参数初始条件进行修正，将海流速度矢量线性叠加到水下航行体速度矢量上。水下航行体空间运动方程组是建立在航行体坐标系下的，因此首先需要对其进行坐标系转换，即

式中，上标“0”表示仿真初始时刻的参数值，下标“0”表示地面坐标系下的参数值，为不存在海流条件下水下航行体速度在地面系下x,y,z轴的三个分量。初始时刻的攻角α，侧滑角β，偏航角ψ，俯仰角θ的表达式分别为：

研究海流对水下航行体运动轨迹的影响时，人们比较感兴趣或者需要得到的往往是水下航行体运动轨迹在地面系下的参数，而并非相对于海流时的运动参数。因此需要运用绝对运动、相对运动以及牵连运动之间的关系来获取在有海流条件下水下航行体运动相对于地面系的运动参数，其转换方程为：

式中，t为仿真初始时刻累积的时间；x₀(t),y₀(t),z₀(t)为有海流条件下水下航行体运动t时间后在地面坐标系中的坐标；x_w0(t),y_w0(t),z_w0(t)为由水下空间运动方程组估计出的t时刻水下航行体相对于海流的三个位移分量；v_Tx0(t),v_Ty0(t),v_Tz0(t)为有海流条件下水下航行体运动t时间后相对于地面系三个方向速度分量；v_Twx0(t),v_Twy0(t),v_Twz0(t)为由水下空间运动方程组求解出的t时刻水下航行体相对于海流的三个速度分量；θ(t),ψ(t)为水下航行体t时刻在地面坐标系下的俯仰角与偏航角。

由于海流的复杂性，在水下航行体运动轨迹计算中，仅以恒定海流速度求解其附加扰动力的常规方法存在一定误差，影响了水下航行体的出水姿态。假定海流速度沿深度方向呈梯度分布V_h＝V₀+0.03×h，其中V_h为水下h深度的海流流速，V₀为水面出海流流速，并将航行体沿轴向切片。在计算的每一时间步内，首先，循环遍历每一个切片，通过切片中心的高度，插值求得所在分层海流的速度，对初始估计得到的运动参数进行修正，计算总的攻角和侧滑角。

定义水下航行体的运动参数为其中，(ν_x,ν_y,ν_z,ω_x,ω_y,ω_z)为航行体坐标系下的水下航行体的速度和角速度，分别为水下航行体的3个姿态角——俯仰角、偏航角和横滚角，(x₀,y₀,z₀)为水下航行体在地面系的位置坐标。

插值求切片中心处的海流速度，经过转换矩阵从地面坐标系转换到航行体坐标系中，(u,v,w)为航行体坐标系中的海流速度矢量。考虑相对运动，则叠加海流即利用海流修正之后，水下航行体速度见式(43)，叠加海流即利用海流修正之后水下航行体的攻角α，侧滑角β见式(44)。

综上所示，本发明的具体步骤为：

第一步：首先建立表示某一时刻水下航行体运动参数与受力关系的水下航行体空间运动方程组，水下航行体所受的力包括浮力，重力，粘性位置力，粘性阻尼力和推力，其中重力、浮力和推力可通过已知公式计算得到，粘性阻尼力为一常数；

第二步：粘性位置力由海浪扰动力产生，其大小和方向由海浪参数包括波浪波幅A、波浪方向γ和波浪圆周频率ω决定，波浪参数可通过测波仪测量得到，将水下航行体沿轴向进行切分，利用波浪参数对水下航行体各个切片所受的波浪扰动力进行计算，得到水下航行体所受的海浪扰动力，将海浪扰动力作为粘性位置力带入水下空间运动方程组中，将该方程组离散化，对水下航行体的运动参数进行迭代求解，从而得到初始运动参数；

第三步：利用海流扰动力对初始运动参数进行修正，利用海流传感器测量海流流向和流速，将海流速度矢量线性叠加到上一步估计得到的水下航行体速度矢量上，从而得到该时刻的水下航行体运动参数估计值；

第四步：对后续时刻，重复第二步和第三步，得到后续时刻的运动参数估计值，结合初始运动时刻水下航行体位置坐标，即可估计得到完整的目标运动轨迹。

本发明的工作主要集中在以下三个方面：

(1)本发明研究中对于波浪影响，以运动载体表面的流体动力系数和附加质量为基础，从运动学观点出发建立了波浪扰动力影响下的水下航行体水下运动轨迹模型，研究波浪力对水下航行体运动轨迹轨迹的影响，并给出具体的仿真结果分析。

(2)本发明研究中对于海流影响，研究确定水下航行体水中和出水过程中相对铅垂线的出水偏差角度和角速度，建立铅垂平面内运动轨迹的水中运动方程组，建立了海流扰动力影响下的水下航行体水下运动轨迹模型，研究海流力对水下航行体运动轨迹轨迹的影响，并给出具体的仿真结果分析。

(3)在分析海浪、海流对水下航行体运动轨迹轨迹的基础上，利用实测海洋环境参数，在海浪和海流共同作用下水下航行体的运动轨迹运动轨迹进行估计。

实施例

(1)不同浪级条件下水下航行体运动轨迹估计

分别设置2/4/5级三种不同浪级，相同浪向角90度，相同初始运动深度，相同初始速度，进行仿真计算，对不同浪级条件下的水下航行体运动轨迹进行了估计，结果如附图3至附图4所示。

仿真结果表明：

1)水下航行体初始垂直运动较水平运动时运动参数受到波浪力的扰动小；

2)浪级越高，对运动姿态角的扰动越强烈，侧向偏距越大；

3)由于波浪的随机性，同一入射角条件下的波浪扰动力出现了不同的方向，而扰动力幅值随浪级的变高而变大，且同一海况下近水面处的幅值最大。

(2)不同海流流速条件下轨迹参数估计

分别设置2/4/6kn三种不同海流流速，相同初始运动深度，相同初始速度，对不同海流速度条件下的水下航行体运动轨迹进行了估计，结果如下附图5至附图7所示。

仿真结果表明：

1)水下航行体海流作用下，攻角和侧滑角逐渐增大，当水下航行体最终随海流一起运动时攻角和侧滑角逐渐减小；

2)海流流速越大，运动轨迹参数受扰动越严重，侧向偏距越大，且同一流速条件下靠近水面处海流力越大，这是因为采用分层流，海面处流速最大；

3)海流流速越大，Y向速度衰减越大，且当海流流速为6kn时，水下航行体由于Y方向的推力分量不够而速度继续减小，航行体出水需用时间也变长。

(3)不同海流流向条件下轨迹参数估计

在相同初始速度和相同初始深度，海流流速为3kn，流向角分别为0°，45°，90°，135°和无海流条件下，对水下航行体在不同海流流向条件下运动轨迹参数进行了估计，结果如下附图8至附图10所示。

仿真结果表明：

1)在海流流向为0°时对运动轨迹参数影响最小，在流向为90°时，影响最大。

2)流向角越大，出水时间越长，出水偏距越大；

3)水下航行体的法向Y方向，运动运动轨迹基本不受海流流向角的影响。

(4)海流和海浪共同作用下水下航行体运动轨迹估计

选取的真实海流模型条件下，对水下航行体在不同海浪浪级情况下的运动运动轨迹进行了估计。估计结果如附图11至附图13所示。

附图说明

附图1是地面坐标系、速度坐标系和航行体坐标系示意图；

附图2是水下航行体切片示意图；

附图3是不同浪级条件下对水下航行体的俯仰角估计结果；

附图4是不同浪级条件下对水下航行体Y方向位移的估计结果；

附图5是不同海流流速条件下对水下航行体俯仰角的估计结果；

附图6是不同海流流速条件下对水下航行体X方向速度的估计结果；

附图7是不同海流流速条件下对水下航行体的Z方向位移估计结果；

附图8是不同海流流向条件下对水下航行体的俯仰角估计结果；

附图9是不同海流流向条件下对水下航行体的速度估计结果；

附图10是不同海流流向条件下对水下航行体的Y方向位移估计结果；

附图11是海流和海浪共同作用下对水下航行体的俯仰角估计结果；

附图12是海流和海浪共同作用下对水下航行体的X方向速度估计结果；

附图13是海流和海浪共同作用下对水下航行体的Y方向位移估计结果。

Claims (3)

1.一种海洋环境下的水下航行体运动轨迹估计方法，其步骤是：

第一步：首先建立表示某一时刻水下航行体运动参数与受力关系的水下航行体空间运动方程组，水下航行体所受的力包括浮力，重力，粘性位置力，粘性阻尼力和推力，其中重力、浮力和推力可通过已知公式计算得到，粘性阻尼力为一常数；

第二步：粘性位置力由海浪扰动力产生，其大小和方向由海浪参数包括波浪波幅A、波浪方向γ和波浪圆周频率ω决定，波浪参数可通过测波仪测量得到，将水下航行体沿轴向进行切分，利用波浪参数对水下航行体各个切片所受的波浪扰动力进行计算，得到水下航行体所受的海浪扰动力，将海浪扰动力作为粘性位置力带入水下空间运动方程组中，将该方程组离散化，对水下航行体的运动参数进行迭代求解，从而得到初始运动参数；所述的水下航行体所受到的波浪扰动力和力矩通过如下方式计算：

Y＝∫dx·Aρωe^B[kcosγ(sinψsinφ-sinθcosψcosφ)+ksinγ(cosψsinφ+sinθsinψcosφ)+ikcosθcosφ]F_y(ω,θ,ψ)

Z＝∫dx·Aρωe^B[kcosγ(sinψcosφ+sinθcosψsinφ)+ksinγ(cosψcosφ-sinθsinψsinφ)-ikcosθsinφ]F_z(ω,θ,ψ)

M_y＝∫dx·Aρωe^B[kcosγ(sinψcosφ+sinθcosψsinφ)+ksinγ(cosψcosφ-sinθsinψsinφ)-ikcosθsinφ]M_y(ω,θ,ψ)

M_z＝∫dx·Aρωe^B[kcosγ(sinψsinφ-sinθcosψcosφ)+ksinγ(cosψsinφ+sinθsinψcosφ)+ikcosθcosφ]M_z(ω,θ,ψ)

式中

F_j(ω，θ，ψ)＝∫dx·{iω[S_ej(x)+S(x)]+νS′_ej(x)}·exp[kxsinθ-ikx(cosθcosψcosγ-cosθsinψsinγ)]，j＝y，z

M_j(ω，θ，ψ)＝∫dx·x{iω[S_ej(x)+S(x)]+νS′_ej(x)}·exp[kxsinθ-ikx(cosθcosψcosγ-cosθsinψsinγ)]，j＝y，z

取Y、Z、M_y、M_z的实部即为作用于水下航行体的波浪扰动力和力矩在航行体坐标系中的表达式，其中，夹角γ表示波浪方向，这里用波浪方向与地面坐标x₀系列轴正向的夹角表示，ω为波浪的圆周频率，k为波数，A为波浪波幅，γ、ω和A均可通过测波仪测量得到，ρ为流体密度，分别为水下航行体的偏航角、俯仰角和横滚角，S(x)为水下航行体在位置x处切片横截面的面积，S_ey(x)、S_ez(x)为海浪在单个切片上产生的附加质量力，B为浮力，水下航行体的水下空间运动方程组中的粘性位置力Y_aμ和Z_aμ分别为Y和Z，相应的粘性位置力矩M_aμy和M_aμz分别为M_y和M_z；

粘性位置力和粘性位置力矩运用切片法求解，切片法具体操作是将水下航行体沿轴向进行切分，将积分方程沿坐标x轴进行离散化，在各个切片内分别求解，最后累加得到积分值，其中切片时应在水下航行体头部和尾部外形尺寸变化较大的地方需较细切分，在水下航行体中部圆柱段部分可进行较粗的切分；

第三步：利用海流扰动力对初始运动参数进行修正，利用海流传感器测量海流流向和流速，将海流速度矢量线性叠加到上一步估计得到的水下航行体速度矢量上，从而得到该时刻的水下航行体运动参数估计值；利用海流扰动力对初始运动参数进行修正的方法是，假定海流速度沿深度方向呈梯度分布，根据海流传感器测量得到的某一深度处的海流流向和流速，利用公式V_h＝V₀+0.03×h计算其他深度处的海流，其中V_h为水下h深度的海流流速，V₀为水面出海流流速，将航行体沿轴向切片，在每一次参数估计过程中，首先，循环遍历每一个切片，通过切片中心的高度，插值求得所在分层海流的速度，对初始估计得到的运动参数进行修正，利用海流修正后的水下航行体速度表达式为：利用海流修正后的水下航行体攻角α和侧滑角β的表达式是(u,v,w)为航行体坐标系中的海流速度矢量，(ν_x,ν_y,ν_z)为初始估计得到的水下航行体速度矢量；

第四步：对后续时刻，重复第二步和第三步，得到后续时刻的运动参数估计值，结合初始运动时刻水下航行体位置坐标，即可估计得到完整的目标运动轨迹。

2.如权利要求1所述的水下航行体运动轨迹估计方法，其特征是，所述的水下航行体空间运动方程组是：

式中：是航行体浮心处的线加速度在三个坐标轴上的分量；是航行体角加速度在三个坐标轴上的分量；v_ox,v_oy,v_oz是航行体浮心处的速度在三个坐标轴上的分量；ω_x,ω_y,ω_z是航行体角速度在三个坐标轴上的分量；

A_mλ是惯性矩阵，即

其中：m是航行体质量；λ_ij,i,j＝1,2,...,6是附加质量；x_c,y_c,z_c是为航行体质心位置；J_i,j,i,j＝x,y,z是航行体转动惯量、惯性矩,惯性矩阵A_mλ中的各参数均为常数量；A_vω是速度矩阵，即

A_FM是力矩阵，即

其中：X_B,Y_B,Z_B是浮力在航行体坐标系下的三个坐标轴上的分量；X_G,Y_G,Z_G是重力在航行体坐标系下的三个坐标轴上的分量；M_Gx,M_Gy,M_Gz是重力矩在航行体坐标系下的三个坐标轴上的分量；X_αμ,Y_αμ,Z_αμ是粘性位置力在航行体坐标系下的三个坐标轴上的分量；M_αμx,M_αμy,M_αμz是粘性位置力矩在航行体坐标系下的三个坐标轴上的分量；Y_ωμ,Z_ωμ是粘性阻尼力在航行体坐标系下Y和Z坐标轴上的分量；M_ωμx,M_ωμy,M_ωμz是粘性阻尼力矩在航行体坐标系下的三个坐标轴上的分量；T_x,T_y,T_z是推力矢量在航行体坐标系下的三个坐标轴上的分量；M_Ty,M_Tz是推力矢量产生的力矩在航行体坐标系下Y和Z坐标轴上的分量。

3.如权利要求1所述的水下航行体运动轨迹估计方法，其特征是，将水下航行体沿x轴向共切分100段，其中头部30段、平行中体25段、尾部45段。