[发明专利]基于改进的高斯距离迭代算法对接回收组合导航方法

权利要求书

1.一种基于改进的高斯距离迭代算法对接回收组合导航方法，其特征在于，包含下列步骤：

1)将AUV的航迹分割成若干条连接的直线段，初始直线段的初始点已知，再用超短基线系统的测距法和测向法求出直线段上一点的坐标；

2)已知两点的坐标，然后利用航位推算将每个位置点的坐标替换成待求的位置点的坐标，从而得到一个关于待求的位置点的坐标的方程组；

3)针对步骤2)方程组计算过程中产生误差用泰勒级数展开进行线性化，设定AUV的位置初始值和迭代阈值，用最小二乘法求出AUV位置的修正量，将修正量与迭代阈值进行比较，若修正量小于迭代阈值则停止迭代；

4)若修正量大于迭代阈值，则采用信赖域的方法来更新步长继续迭代；

5)求出该直线段的最后一个点的坐标，作为下一直线段的初始点，再重复以上步骤进行迭代。

2.如权利要求1所述的基于改进的高斯距离迭代算法对接回收组合导航方法，其特征在于，步骤1)具体为：选取所述AUV航迹中的一段，并假设AUV为匀速运动，回收坞上发射器的位置已知，由深度计测得AUV的深度信息，将AUV航迹中的一段与回收坞上声源发射器的位置信息放到二维平面上，形成一个二维平面关系图。

3.如权利要求2所述的基于改进的高斯距离迭代算法对接回收组合导航方法，其特征在于，步骤2)中的方程组具体如下：设P_s点坐标为P_s(X₀,Y₀)，任取AUV航迹上的三个点p₁(X₁,Y₁),p₂(X₂,Y₂)和p₃(x,y)，三个点与P_s点的距离分别为r₁,r₂,r₃于是可得：

p₁位置到p₃位置的时间为t₁₃，p₂位置到p₃位置的时间为t₂₃，AUV的航向角为于是得：

将式(2)代入式(1)，整理可得关于x,y的方程组

式中，

4.如权利要求3所述的基于改进的高斯距离迭代算法对接回收组合导航方法，其特征在于，步骤3)具体包括：用泰勒级数展开后引入改进的高斯距离迭代算法进行迭代求解，式(3)可以看成是坐标(x,y)与已知坐标的距离函数，令

式中，i＝1,2,3；

改进的高斯距离迭代法步骤为：

STEP1：对式(5)给定初始值(a₀,b₀)，

STEP2：在(a₀,b₀)处进行泰勒展开，且x＝a₀+Δa，y＝b₀+Δb，(Δa,Δb)为步长略去高次项，取一阶近似项，可得展开式：

STEP3：再将式(6)带到式(3)，再进行最小二乘法求解，获得调整增量(Δa,Δb)，判断式(7)是否成立；

式中，δ为迭代阈值，若式(7)成立，停止迭代计算，则(a₀,b₀)为所求的解，否则更新步长(Δa,Δb)。

5.如权利要求4所述的基于改进的高斯距离迭代算法对接回收组合导航方法，其特征在于，步骤4)具体包括：所述采用信赖域方法来更新步长，首先给定一个信赖域半径作为位移长度的上界，并以当前迭代点为中心以此上界为半径确定一个称之为信赖域的闭球区域；然后，通过求解这个区域内的信赖域子问题(目标函数的二次近似模型)的最优点来确定候选位移；若候选位移能使目标函数值有充分的下降量，则接受该候选位移作为新的位移，并保持或扩大信赖域半径，继续新的迭代；否则，说明二次模型与目标函数的近似度不够理想，需要缩小信赖域半径，再通过求解新的信赖域内的子问题得到新的候选位移。