CN108318034B - 一种基于声呐地图的auv回坞导航方法 - Google Patents

Abstract

本发明公开了一种基于声呐地图的AUV回坞导航方法，包括以下步骤：(1)基于AUV携带声呐的作用距离，将海底基站前方的AUV工作区域划分为估计导引区和直接导引区；对海底基站前方的AUV工作区域进行扫描，获得海底地貌特征的声呐地图；(2)AUV回坞时，航行至于海底基站同样的深度；位于估计导引区时，AUV实时扫描海底地貌特征，将当前扫描到的特征点与所述的声呐地图中的特征点进行匹配；若匹配成功，则根据所述的声呐地图对AUV的航位进行修正；若匹配失败，则根据AUV的自身导航算法进行定位导航；位于直接导引区时，AUV携带声呐扫描获取AUV与海底基站之间的相对位姿关系，对AUV进行导航。本发明的回坞导航方法能在浑浊、光线变化和非开阔的水域使用。

Description

一种基于声呐地图的AUV回坞导航方法

技术领域

本发明涉及AUV水下回坞导航定位技术领域，尤其涉及基于声呐地图的AUV回坞导航方法。

背景技术

随着人们对海洋资源的开发，自主水下航行器(AUV)的使用越来越广泛。然而，AUV在水下所携带的能源有限，因此需要对AUV回收充电。为了降低成本，人们希望这一充电过程可以在水下直接进行。

公开号为CN102320362A的中国专利文献公开了一种海底对接装置，用于回收AUV并进行充电和信息交互。为了AUV能够顺利与海底基站对接，就需要精确的导航算法。

由于电磁波在海水中会急速衰减，无法利用GPS在海底进行导航定位。因此在海洋中，主要是依靠声学定位系统进行导航定位，例如超短基线(USBL)、短基线(SBL)和长基线(LBL)等。然而利用长基线进行定位的前提是需要提前在潜器航行的区域进行基线的布放，增加了实现的难度和成本。SBL和USBL的使用相比LBL难度虽然有所降低，但这两种定位系统的成本依旧高昂，难以推广应用。

公开号为CN105501415A的中国专利文献公开了一种常见的AUV回坞组合导航方法，在AUV离接驳站较远时，利用超短基线定位系统(USBL)来修正AUV航位，在靠近接驳站时，利用视觉信息，来进行光学导引。然而USBL定位系统在非开阔的海域时常出现通讯不畅的情况，并且当AUV背向接驳站时无法获得定位信息。末端视觉导引易受到水质和光线影响，恶劣情况下，导引距离甚至无法超过5米，这些缺点很可能会造成AUV的回坞失败。

发明内容

本发明提供一种基于声呐地图的AUV回坞导航方法，能在浑浊、光线变化和非开阔的水域使用。

本发明提供了如下技术方案：

一种基于声呐地图的AUV回坞导航方法，包括以下步骤：

(1)基于AUV携带声呐的作用距离，将海底基站前方的AUV工作区域划分为估计导引区和直接导引区；

对海底基站前方的AUV工作区域进行扫描，获得海底地貌特征的声呐地图；

(2)AUV回坞时，航行至于海底基站同样的深度；

(2-1)位于估计导引区时，AUV实时扫描海底地貌特征，将当前扫描到的特征点与所述的声呐地图中的特征点进行匹配；

若匹配成功，则根据所述的声呐地图对AUV的航位进行修正；

若匹配失败，则根据AUV的自身导航算法进行定位导航；

(2-2)位于直接导引区时，AUV携带声呐扫描获取AUV与海底基站之间的相对位姿关系，对AUV进行导航。

步骤(1)中，AUV利用扩展卡尔曼滤波进行导航，同时采集海底形貌的声呐图像，从声呐图像中提取地图特征并标记位置，建立所述的声呐地图。

由于声呐地图的特征点坐标是基于AUV的航位求得的，为了保证声呐地图的精度，就需要提高AUV的导航精度，对连续两帧声呐图像的特征点进行匹配，将匹配信息用于修正AUV的航位；

对连续两帧声呐图像的特征点进行匹配的方法包括以下步骤：

(1-1)提取连续两帧声呐图像中的所有特征点，利用联合兼容(JC)的方法筛选出连续两帧声呐图像之间所有可能匹配的特征集合，形成关联假设；

(1-2)采用累积计分法判断所有关联假设发生的概率，将概率最大的关联假设作为正确关联，获得匹配信息，用于修正AUV的航位。

具体为：定义关联假设的集合为

h_i表示k时刻第i种关联假设发生；

按照累积计分公式计算各个关联假设发生的概率，累积计分公式为：

其中，

表示h_i的特征估计位置与实际观测位置之间的马氏距离；λ为自由参数，需要根据特征点的分布情况调节，数值范围为1～5，当特征点数较少时，选择较高的λ，反之选择较低的；Q_i表示h_i下的特征匹配点对；

其中，p(z_k，none|h_i)表示已知h_i发生的情况下却无实际特征点匹配发生的概率；f表示单一特征点；N_u表示未匹配到的特征点总数；

表示特征点f的可预见性方程，d_f表示特征点f被重复检测到的个数，s_c表示将特征密集区划分网格的面积，ω_f表示特征点f分布在这个网格的概率权重，权重以95％置信椭圆来判断，n_d表示利用分支界定(BB)的方法得到的特征匹配点对数。

当某一关联假设的累积总分最高时，认为这种情况下匹配发生的概率最大，将匹配的特征点对视为正确关联。由于匹配的特征点往往不止一对，在这里选择似然函数最大的一对作为最终匹配信息，修正AUV的航位。

根据上述匹配信息修正AUV航位的方法为：

利用扩展卡尔曼滤波进行AUV自身航位的观测更新，构造观测方程为：

其中，Z_k表示k时刻的观测向量；将环境特征点m_i相对于AUV的距离和方向作为观测量，ρ_i为环境特征点m_i相对于AUV的距离，ψ_i为环境特征点m_i相对于AUV的角度；(X_AUV，Y_AUV)为AUV在惯性坐标系下的绝对位置；ψ_AUV为AUV的偏航角；(x_i，y_i)为第i个环境特征点m_i在大地坐标下的坐标；V_k表示k时刻的观测噪声矩阵；

则航位修正为：

X_k＝X_k，k-1+G_k(T_k-Z_k)，

其中，X_k为修正的AUV航位；X_k，k-1为更新过程状态变量；G_k为卡尔曼增益矩阵；T_k为k时刻AUV匹配到的地图特征的实际观测位置(通过声呐获得)；

步骤(1)还包括：AUV从估计导引区进入直接导引区时刻，AUV通过声呐获得自身相对于海底基站的位姿信息，利用所述的位姿信息对所述的声呐地图进行优化；

具体为：定义AUV进入直接导引区前得到以R_i为基准的声呐地图向量为

将

转换至以海底基站原点为参考系的向量

和

分别为：

N为声呐地图中特征点总数，R_l为AUV从估计导引区进入直接导引区时刻声呐地图中AUV的参考坐标系，此时根据声呐观测求解出的AUV相对于海底基站的位置为

则修正值为

即声呐地图优化为

AUV携带声呐可以直接扫描到海底基站的区域为直接导引区；AUV工作区域中，除直接导引区外的区域为估计导引区。

优化后的声呐地图即为最终使用的导航辅助地图。得到该导航地图后，AUV在进行观测工作时，在利用扩展卡尔曼滤波(EKF)进行航位推算时，可以实时利用累积计分法进行与声呐地图的匹配。最终，AUV能实现在观测区域的实时准确定位，并根据自己在声呐地图中的相对位置，修正姿态，朝着海底基站航行，实现顺利回坞。

具体为：AUV回坞时，航行至于海底基站同样的深度；

(2-1)位于估计导引区时，AUV实时扫描海底地貌特征，将当前扫描到的特征点与所述的声呐地图中的特征点进行匹配；

方法为：将声呐地图中AUV当前估计位置周边的特征点与AUV当前观测到的特征点利用累积计分法进行匹配；

若匹配成功，则假设AUV当前位置观测到的匹配特征点在声呐地图中的位置为x_a，根据AUV导航信息推算出的该匹配特征点的位置为x_b，AUV位置为x_robot；则修正后的AUV定位位置为x_robot+(x_a-x_b)。

若匹配失败，则根据AUV的自身导航算法进行定位导航；

(2-2)位于直接导引区时，AUV携带声呐扫描获取AUV与海底基站之间的相对位姿关系，对AUV进行导航；直至AUV顺利回坞。

与现有技术相比，本发明的有益效果为：

(1)利用声呐地图来辅助导航，可以克服传统光学末端导航的不足，在浑浊或者光线变化的水质下，AUV依然能够顺利回坞；

(2)借助声呐地图辅助导航，能够在AUV背向海底基站时获得自身的定位信息；

(3)提出了累积计分法进行地图特征的匹配，匹配信息可用于修正AUV航位，提高了AUV的导航精度。

附图说明

图1为AUV扫描工作区域特征地图路线的示意图；

图2为AUV利用声呐回坞导航分区示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步详细描述，需要指出的是，以下所述实施例旨在便于对本发明的理解，而对其不起任何限定作用。

AUV在执行任务前会先按照一定的路线扫描绘制海底地貌特征的声呐地图，绘制的区域根据需要工作的面积来确定。其中，声呐地图是指海底物体的质心位置所构成的几何。

如图1所示，黑色方框内为AUV需要工作的区域，AUV按照一定的轨迹扫描工作区域的海底地貌特征，并将所有特征绘制成一张全局地图进行记录。整个导航过程AUV都是利用扩展卡尔曼滤波进行位置估计。由于海底地貌特征的声呐地图的特征坐标是基于AUV的航位求得的，为了保证地图的精度，就需要提高AUV导航精度，因此需要对连续两帧声呐图像的特征进行匹配，将匹配信息用于修正AUV的航位。

本发明提出一种累积计分法进行匹配，从两帧图像中提取所有特征点，利用联合兼容(JC)的方法，筛选出两帧图像之间所有可能匹配的特征集合，称为关联假设h。其中，h_i表示k时刻第i种地图特征匹配假设发生。

设累积计分为：

其中，

表示第i种假设的特征估计位置与实际观测位置之间的马氏距离；λ为自由参数，数值范围为1～5，当特征较为稀疏时，取值大；反之，取值小，需要根据实际特征分布情况调节；Q_i表示第i种假设下共有多少匹配点对；

表示在估计有特征将会匹配的情况下却无实际特征匹配的影响分数，

其中，p(z_k，none|h_i)表示已知假设h_i发生的情况下却无实际特征匹配发生的概率；f表示单一特征点；N_u表示未匹配到的特征总数；

表示特征点的可预见性方程，d_f表示该特征点被重复检测到的个数，s_c表示将特征密集区划分网格的面积，ω_f表示该特征分布在这个网格的概率权重，权重以95％置信椭圆来判断，n_d表示利用分支界定(BB)的方法得到的匹配对数。

当累积总分最高时，可以认为这种情况下匹配发生的概率最大，将匹配的点对视为正确关联。由于匹配点数往往不止一对，在这里选择似然函数最大的一对作为最终匹配信息，修正AUV的航位。

根据上述得到的匹配特征结果，利用EKF进行AUV自身航位的观测更新(修正)。构造观测方程为：

其中，(X_AUV，Y_AUV)为AUV在惯性坐标系下的绝对位置，ψ_AUV为AUV的偏航角，Z_k表示k时刻的观测向量，v_k表示k时刻的观测噪声矩阵。将环境特征点相对于AUV的距离和方向作为观测量，环境特征m_i(i为观测到的特征点的序号)相对于AUV的距离为ρ_i，角度为ψ_i，第i个环境特征(环境特征m_i)在大地坐标下的坐标为(x_i，y_i)。

则航位修正为：

X_k＝X_k，k-1+G_k(T_k-Z_k)，

其中，G_k为卡尔曼增益矩阵，X_k，k-1为更新过程状态变量，T_k为匹配特征的绝对位置，X_k为最终修正的AUV航位。

AUV获得海底特征地图后，需要进一步的优化和修正。在估计导引区获得的地图特征位置，是基于EKF算法推出的。如图2所示，当AUV进入直接导引区的那一刻，AUV可以通过声呐直接获得相对于接驳站的位姿信息。利用这个信息，可以修正先前存储的地图。图2中，虚线为EKF估计的AUV相对接驳站的位置，实线为通过声呐获得AUV相对位置。修正的具体方式为：

假设进入直接导引区前得到以R_i为基准的全局地图向量

和将该向量转换至以接驳站原点为参考系的向量

分别为：

N为地图中总特征数，R_l为全局地图中最后一幅局部地图的参考坐标系。在进入直接导引区的时刻，AUV的参考坐标系是R_l，此时根据声呐观测求解出的AUV相对于接驳站的位置为

则修正值为

即地图修正为

修正后的地图即为最终使用的导航辅助地图。得到该导航地图后，AUV在进行观测工作时，在利用EKF进行航位推算时，可以实时利用累积计分法进行与记录地图匹配。方法为：选取以AUV当前估计位置为中心，边长为5米的正方形区域中的地图特征(声呐地图当中的特征)与AUV当前观测到的特征利用累积积分法进行匹配，获得匹配特征信息。若匹配成功，则假设AUV当前位置观测到的特征点在记录地图中的位置为x_a，根据AUV导航信息推算出的该特征的位置为x_b，AUV位置为x_robot；则修正后的AUV定位位置为x_robot+(x_a-x_b)。最终，AUV能实现在观测区域的实时准确定位，并根据自己在地图中的相对位置，修正姿态，朝着接驳站航行，实现顺利回坞。

以上所述的实施例对本发明的技术方案和有益效果进行了详细说明，应理解的是以上所述仅为本发明的具体实施例，并不用于限制本发明，凡在本发明的原则范围内所做的任何修改、补充和等同替换等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于声呐地图的AUV回坞导航方法，其特征在于，包括以下步骤：

(1)基于AUV携带声呐的作用距离，将海底基站前方的AUV工作区域划分为估计导引区和直接导引区；

对海底基站前方的AUV工作区域进行扫描，获得海底地貌特征的声呐地图；

(2)AUV回坞时，航行至于海底基站同样的深度；

(2-1)位于估计导引区时，AUV实时扫描海底地貌特征，将当前扫描到的特征点与所述的声呐地图中的特征点进行匹配，方法为：

利用联合兼容的方法筛选出当前扫描到的特征点与所述的声呐地图中的特征点之间所有可能匹配的特征集合，形成关联假设；定义关联假设的集合为

h_i表示k时刻第i种关联假设发生；

按照累积计分公式计算各个关联假设发生的概率，累积计分公式为：

其中，

表示h_i的特征估计位置与实际观测位置之间的马氏距离；λ为自由参数，需要根据特征点的分布情况调节，数值范围为1～5，当特征点数较少时，选择较高的λ，反之选择较低的；Q_i表示h_i下的特征匹配点对；

其中，p(z_k,none|h_i)表示已知h_i发生的情况下却无实际特征点匹配发生的概率；f表示单一特征点；N_u表示未匹配到的特征点总数；

表示特征点f的可预见性方程，d_f表示特征点f被重复检测到的个数，s_c表示将特征密集区划分网格的面积，ω_f表示特征点f分布在这个网格的概率权重，权重以95％置信椭圆来判断，n_d表示利用分支界定的方法得到的特征匹配点对数；

当某一关联假设的累积总分最高时，认为这种情况下匹配发生的概率最大，将匹配的特征点对视为正确关联；

若匹配成功，则根据所述的声呐地图对AUV的航位进行修正；

若匹配失败，则根据AUV的自身导航算法进行定位导航；

(2-2)位于直接导引区时，AUV携带声呐扫描获取AUV与海底基站之间的相对位姿关系，对AUV进行导航。

2.根据权利要求1所述的基于声呐地图的AUV回坞导航方法，其特征在于，步骤(1)中，AUV利用扩展卡尔曼滤波进行导航，同时采集海底形貌的声呐图像，从声呐图像中提取地图特征并标记位置，建立所述的声呐地图。

3.根据权利要求2所述的基于声呐地图的AUV回坞导航方法，其特征在于，步骤(1)中，对连续两帧声呐图像的特征点进行匹配，将匹配信息用于修正AUV的航位；

匹配的方法包括以下步骤：

(1-1)提取连续两帧声呐图像中的所有特征点，利用联合兼容的方法筛选出连续两帧声呐图像之间所有可能匹配的特征集合，形成关联假设；

(1-2)采用累积计分法判断所有关联假设发生的概率，将概率最大的关联假设作为正确关联，获得匹配信息，用于修正AUV的航位。

4.根据权利要求3所述的基于声呐地图的AUV回坞导航方法，其特征在于，步骤(1-2)中，根据匹配信息修正AUV航位的方法为：利用扩展卡尔曼滤波进行AUV自身航位的观测更新，构造观测方程为：

其中，Z_k表示k时刻的观测向量；将环境特征点m_i相对于AUV的距离和方向作为观测量，ρ_i为环境特征点m_i相对于AUV的距离，ψ_i为环境特征点m_i相对于AUV的角度；(X_AUV,Y_AUV)为AUV在惯性坐标系下的绝对位置；ψ_AUV为AUV的偏航角；(x_i,y_i)为第i个环境特征点m_i在大地坐标下的坐标；V_k表示k时刻的观测噪声矩阵；

则航位修正为：

X_k＝X_k,k-1+G_k(T_k-Z_k)，

其中，X_k为修正的AUV航位；X_k,k-1为更新过程状态变量；G_k为卡尔曼增益矩阵；T_k为k时刻AUV匹配到的地图特征的实际观测位置。

5.根据权利要求1～4任一项所述的基于声呐地图的AUV回坞导航方法，其特征在于，步骤(1)还包括：AUV从估计导引区进入直接导引区时刻，AUV通过声呐获得自身相对于海底基站的位姿信息，利用所述的位姿信息对所述的声呐地图进行优化。

6.根据权利要求5所述的基于声呐地图的AUV回坞导航方法，其特征在于，利用所述的位姿信息对所述的声呐地图进行优化的方法为：定义AUV进入直接导引区前得到以R_i为基准的声呐地图向量为

将

转换至以海底基站原点为参考系的向量

和

分别为：

N为声呐地图中特征点总数，R_l为AUV从估计导引区进入直接导引区时刻声呐地图中AUV的参考坐标系，此时根据声呐观测求解出的AUV相对于海底基站的位置为

则修正值为

即声呐地图优化为

7.根据权利要求1所述的基于声呐地图的AUV回坞导航方法，其特征在于，步骤(2-1)中，若匹配成功，则根据所述的声呐地图对AUV的航位进行修正的方法为：

假设AUV当前位置观测到的匹配特征点在声呐地图中的位置为x_a，根据AUV导航信息推算出的该匹配特征点的位置为x_b，AUV位置为x_robot；则修正后的AUV定位位置为x_robot+(x_a-x_b)。

Images