基于关键约束帧的前视成像声呐水下定位与导航方法技术

本发明专利技术属于水下自主定位导航技术领域，公开了一种基于关键约束帧的前视成像声呐水下定位与导航方法。其目的在于识别并充分利用过往声呐图像中的有效约束信息对当前位姿和水下特征进行定位。主要步骤有：前端数据处理，包括二维声呐图像的特征提取与匹配、惯性测量数据处理；后端弹性滑动窗口优化，包括声呐关键约束帧的识别与存储、从关键约束帧库中选择可加入滑动窗口优化的过往帧、窗口大小控制与优化、更新机器人位姿和水下特征位置。本发明专利技术方法可提高水下机器人定位和导航系统的精度，同时保证有限的优化规模，在声呐图像前端出现异常值时也可以及时在后端检测并处理，以保证系统的鲁棒性。

【技术实现步骤摘要】
基于关键约束帧的前视成像声呐水下定位与导航方法
本专利技术属于水下自主定位导航
，主要涉及一种基于关键约束帧的前视成像声呐水下定位与导航方法。
技术介绍
近年来，世界各国对海洋安全、海洋资源和海洋经济等方面日益重视，海洋机器人，尤其是无人潜航器(AutonomousUnderwaterVehicle,AUV)，作为执行各类海洋水下任务必不可缺的高端装备，可适应不确定、无法预知的复杂水下环境，在民用和军事上都具备无可比拟的优越性，正受到越来越广泛的关注。由于复杂海洋环境海浪、海流、碍航物、湍流和内波等不确定环境因素，水下精确可靠的导航定位是AUV执行任务及安全航行的关键。AUV导航定位系统能够为其提供位置、航向、深度、速度和姿态等信息，以保障AUV的安全航行、水下作业。现有的水下定位导航技术一般分为两大类[1]:从外部定位导航来看，GPS信号在水下衰减迅速，很难依赖；水声定位系统如超短基线定位声呐(Ultra-ShortBaseline，USBL)、短基线定位声呐(ShortBaseline,SBL)、长基线定位声呐(Lon本文档来自技高网...

【技术保护点】
1.基于关键约束帧的前视成像声呐水下定位与导航方法，其特征在于，包括以下步骤：/n步骤1：前端数据处理：水下机器人依据惯性导航设备产生的数据得到推测的机器人位姿；通过二维前视成像声呐获取二维声呐图像，经过特征提取与匹配后，得到声呐连续两帧之间的特征匹配，并进行特征点全局编号，以feature_id表示，每一帧观测到的所有特征点均以像素坐标形式按feature_id顺序存储到特征点数据库，若匹配成功或检测到已经存在于特征点数据库中，则判定为旧特征点，全局编号不更新；否则判定为当前帧观测到的新特征点，按特征观测的先后顺序依次编号；/n步骤2：声呐关键约束帧的识别与存储：读取连续两帧中机器人在世界坐...

【技术特征摘要】
1.基于关键约束帧的前视成像声呐水下定位与导航方法，其特征在于，包括以下步骤：
步骤1：前端数据处理：水下机器人依据惯性导航设备产生的数据得到推测的机器人位姿；通过二维前视成像声呐获取二维声呐图像，经过特征提取与匹配后，得到声呐连续两帧之间的特征匹配，并进行特征点全局编号，以feature_id表示，每一帧观测到的所有特征点均以像素坐标形式按feature_id顺序存储到特征点数据库，若匹配成功或检测到已经存在于特征点数据库中，则判定为旧特征点，全局编号不更新；否则判定为当前帧观测到的新特征点，按特征观测的先后顺序依次编号；
步骤2：声呐关键约束帧的识别与存储：读取连续两帧中机器人在世界坐标系下的位姿和所有特征点像素坐标，并将特征点像素坐标转换成基于声呐极坐标系的球面坐标；判定当前帧是否为欠约束帧，若是，则另行处理；若否，则进一步判定当前帧是否为声呐关键约束帧，若是，则将该声呐帧存储至关键约束帧库；
步骤3：从关键约束帧库中选择可加入滑动窗口优化的过往帧：对于关键约束帧库中存储的过往帧，判断其能否加入到当前滑动窗口优化中，若能则选择性加入，若否则不加入；
步骤4：窗口大小控制与优化：判断当前滑动窗口的大小是否超过最大限制，若是，则从当前窗口内移除部分过往帧；若否，则在窗口确定后开始通过非线性优化器对滑动窗口进行优化；优化完成后，得到估计的机器人相对位姿及水下特征点球面坐标，同时更新机器人和水下特征点的全局坐标信息，并反馈至下一次优化的输入中。


2.根据权利要求1所述的基于关键约束帧的前视成像声呐水下定位与导航方法，其特征在于所述的前视成像声呐为多波束二维前视成像声呐。


3.根据权利要求1所述的基于关键约束帧的前视成像声呐水下定位与导航方法，其特征在于步骤2的具体方法为：
步骤2.1：读取连续两帧中所有的特征点像素坐标，并将上一帧中的特征点像素坐标转换成基于声呐...

【专利技术属性】
技术研发人员：郑荣濠，徐阳，刘妹琴，张森林，
申请(专利权)人：浙江大学，
类型：发明
国别省市：浙江;33