[发明专利]垂直出水航行体特征位置到达时刻估计方法

权利要求书

1.一种垂直出水航行体特征位置到达时刻估计方法，其特征在于，其步骤包括：

S1，待测航行体于水下运载平台上以一定的垂向作用力释放，在经历舱内段、水中段、出水段三个过程后完成出水，航行体上安装的惯性传感器获取视加速度、角速率的时间序列测量数据，航行体释放时刻为时间序列测量数据的时间零点；

S2，以惯性传感器安装位置为参考质点，建立参考质点水下运动全过程的速度、位移以及俯仰角、偏航角时间序列；

S3，将所述质点运动轨迹换算到航行体头部和尾部的特征点，并迭加航行体姿态修正，分别得到头部特征点和尾部特征点的运动轨迹；以头部特征点和尾部特征点轨迹随时间变化序列为依据，由航行体尺寸和运载平台水下航行深度信息确定特征位置，通过线性插值得到特征位置的到达时刻信息；

所述的步骤S1具体包括，

S11，建立坐标系，选取航行体从运载平台上释放时惯性传感器所在位置为原点，X轴铅直指向海面，Y轴指向运载平台航行方向，Z轴与X轴、Y轴构成右手坐标系；

S12，已知航行体长度为L，根据惯性传感器安装位置，沿航行体轴向，将其分为上、下2段，长度分别为L_a和L_b，L＝L_a+L_b；运载平台航行深度，即从舱外甲板到水面的距离，为H_war，航行体运载舱深度，即从舱外甲板到舱底的距离，为H_can；

S13，航行体从运载平台上的释放时刻记为T₀，尾部出舱时刻记为T₁，头部触水时刻记为T₂，尾部出水时刻记为T₃；

S14，获取的时间序列数据包括X轴、Y轴和Z轴三个方向的视加速度a_X(t_i)、a_Y(t_i)、a_Z(t_i)，以及俯仰角速率偏航角速率w_ψ(t_i)、滚动角速率w_γ(t_i)，取样的离散时间向量为T＝[t₀,t₁,…,t_N]；

所述的步骤S2具体包括，

S21，以惯性传感器的位置作为参考质点，根据X方向的视加速度时间序列可得到该质点速度参数的时间序列：

其中，a_X(t_i)、a_X(t_i+1)分别为与时间点t_i、t_i+1对应的X方向视加速度数据，i＝0,1,...,N；v_i+1、v_i分别为第i+1个、第i个时间点的X方向速度，v₀＝0；g为重力加速度；

进一步可得到该质点在X方向位移参数的时间序列：

其中，x_i+1、x_i分别为第i+1个、第i个时间点的X方向位移，x₀＝L_b-H_can；

S22，以惯性传感器的位置作为参考质点，根据俯仰角速率、偏航角速率的时间序列可得到俯仰角、偏航角参数的时间序列：

其中，分别为俯仰角速率时间序列与时间点t_i、t_i+1对应的数据，i＝0,1,...,N；w_ψ(t_i)、w_ψ(t_i+1)分别为偏航角速率时间序列与时间点t_i、t_i+1对应的数据；分别为第i+1个、第i个时间点的俯仰角，ψ_i+1、ψ_i分别为第i+1个、第i个时间点的偏航角，ψ₀＝0；

所述的步骤S3具体包括，

S31，将航行体作为刚体，对俯仰角和偏航角变化产生的头部和尾部到达位置偏差进行修正；将以惯性传感器的位置作为参考质点的航行体运动轨迹变换为以头部和尾部作为参考质点的轨迹：

由此得到与时间标度T＝[t₀,t₁,…,t_N]对应的沿X方向的航行体头部位移序列和航行体尾部位移序列

S32，根据数据序列X_d(t_i)和T，通过线性插值求出满足X_d(t^*)＝0的时刻，即为尾部出舱时刻T₁；

S33，根据数据序列X_u(t_i)和T，通过线性插值求出满足X_u(t^*)＝H_war的时刻，即为头部触水时刻T₂；

S34，根据数据序列X_d(t_i)和T，通过线性插值求出满足X_d(t^*)＝H_war的时刻，即为尾部出水时刻T₃。