悬浮式水下自主航行器的定位和导航方法

1.一种悬浮式水下自主航行器的定位和导航方法，其特征在于，包括以下步骤：S10，采集水下图像；S20，对采集到的图像进行显著性分析；S30，对采集到的图像进行预处理和特征提取输入特征词袋；S40，进行特征匹配、多传感器实时定位及推进器控制；S50，将S20-S40的输出构建图像数据库；S60，对图像根据图像数据库进行闭环检测；S70，对图像数据库进行离线优化；其中，所述多传感器实时定位，包括以下步骤：S41，设k时刻位姿x,y, 通过惯性导航单元推算位姿变化Δx i ,Δy i , 通过电子罗盘获取方位角变化 S42，如 为阈值，则确定航向角变化量 S43，如 则判定电子罗盘受到外部磁场干扰 S44，进行视觉图像匹配；S45，如匹配，则通过单应矩阵推算位姿变化Δx v ,Δy v , S46，如不匹配，则判定为误匹配，k+1时刻位姿为x+Δx i ,y+Δy i , S47，是否位置偏差 且 S48，是则判定为正确匹配，k+1时刻位姿为x+Δx v ,y+Δy v , 否则S46。 2.如权利要求1所述的悬浮式水下自主航行器的定位和导航方法，其特征在于，所述显著性分析通过信息熵来衡量。 3.如权利要求1所述的悬浮式水下自主航行器的定位和导航方法，其特征在于，所述特征匹配采用ORB特征算子检测特征点，采用RANSAC算法排除无效特征。 4.如权利要求1所述的悬浮式水下自主航行器的定位和导航方法，其特征在于，所述闭环检测包括以下步骤：S61，从图像数据库中找出跟当前位置的距离在设定值以内的图像；S62，逐一比较当前图像与其余弦相似度；S63，如果余弦相似度高于设定值，则进行特征匹配。 5.如权利要求1所述的悬浮式水下自主航行器的定位和导航方法，其特征在于，所述离线优化采用BA非线性法。