[发明专利]一种基于水下机动平台的声信标测向及定位方法

权利要求书

1.一种基于水下机动平台的声信标测向及定位方法，其特征在于，所述方法包括以下步骤：

步骤1：信号测向：基于迭代更新的思想使波束方向逐步接近声信标方向，并通过多项式求根计算更新波束方向；

步骤2：声信标方向跟踪：建立一阶角度跟踪模型，并通过粒子滤波算法利用历史信息实现声信标测向结果的平滑及测向精度的进一步提高；

步骤3：声信标定位：使用新型角交汇定位模型，建立解算方程，并采用拟牛顿迭代法进行非线性方程组求解，实现定位解算；

所述步骤2的一阶角度跟踪模型具体为：

根据声信标方向跟踪，所建立的状态转移方程为：

Zⁿ⁺¹＝HZⁿ+R (14)

其中：为状态矩阵，θ_n'为声信标方向变化率；为状态转移矩阵；R为状态转移噪声矩阵；

相应的观测方程为：

Yⁿ＝θ_n+Q (15)

其中：Q为观测噪声；

所述步骤3角交汇定位模型所建立三维定位解算方程为：

式中：[x_s,y_s]为待求解的声信标水平坐标；z_s为声信标深度，由压力传感器测量；[x₁,y₁,z₁]及[x₂,y₂,z₂]为水下机动平台在不同测量点的三维坐标，由惯导系统测量；[x_a,y_a,z_a]为水听器阵列的单位方向矢量，由水下机动平台的姿态测量仪测量；及为声信标与水下机动平台的距离；

所述步骤3中新型角交汇定位模型，即公式(18)的具体求解为：

步骤3.1：确定未知数初值：

步骤3.2：计算偏微分矩阵初值：

步骤3.3：计算函数值初值：

步骤3.4：更新未知数：

步骤3.5：更新函数值：

步骤3.6：计算残差：rⁱ＝xⁱ⁺¹-xⁱ，yⁱ＝F(xⁱ⁺¹)-F(xⁱ)；

步骤3.7：更新偏微分矩阵：

步骤3.8：重复步骤3.4至步骤3.7，直至两次迭代的间隔小于某一门限β；

即：(xⁱ⁺¹-xⁱ)·(xⁱ⁺¹-xⁱ)^T＜β时，停止迭代；停止迭代时未知数的数值即为声信标水平坐标的定位结果。