[发明专利]面向AUV入坞的INS/USBL组合导航定位方法

权利要求书

1.面向AUV入坞的INS/USBL组合导航定位方法，其特征在于包括以下步骤：

将USBL收发器和INS置于AUV载体中，将应答器置于入坞口处；

通过在距入坞口设定距离内设置两个定点，使AUV在这两点进行悬停，悬停点经纬度由INS获取，AUV与应答器的距离由USBL获取；

根据AUV悬停点位置、AUV与应答器的距离得到入坞口的位置；

根据入坞口的位置规划路径，实现AUV对入坞口的导航定位；

所述根据入坞口的位置规划路径，实现AUV对入坞口的导航定位包括以下步骤：

首先将INS采集的AUV当前经纬度数据转化为USBL收发器当前位置P和P’，而后与USBL收发器采集的距离数据进行整合，解算出入坞目标点位置：

1)首先计算出应答器A的经纬度(X₁，Y₁)：

2)采用同样方法计算B点经纬度(X₂，Y₂)，公式如下：

公式(1)、(2)中，a，b为收发器在P点经纬度，a为经度值，b为纬度值，a’，b’分别为收发器在P’点经、纬度；l_a、l_b分别为收发器在P点与应答器A、B的距离值，l_a’、l_b’分别为收发器在P’点与应答器A、B的距离值，A、B为布放在入坞口的应答器；

3)计算应答器A与应答器B位置连线的中点经纬度，即为入坞口的经纬度(X，Y)：

2.根据权利要求1所述的面向AUV入坞的INS/USBL组合导航定位方法，其特征在于：所述USBL收发器置于AUV艏部段；应答器为两个，分别设于入坞口两侧；INS置于AUV载体重心处，使其获取的数据为AUV载体重心的经纬度。

3.根据权利要求1所述的面向AUV入坞的INS/USBL组合导航定位方法，其特征在于所述设定距离为航行体悬停位置与应答器距离大于50m且小于200m。

4.根据权利要求1所述的面向AUV入坞的INS/USBL组合导航定位方法，其特征在于所述根据入坞口的位置规划路径具体为：解算出入坞口位置后，在入坞口位置处做中垂线，将其作为目标航线，用于进行AUV的导航控制。