[发明专利]一种基于多传感器融合的水下机器人定位与构图方法

说明书

本发明属于水下机器人定位技术领域，具体涉及一种基于多传感器融合的水下机器人定位与构图方法。本发明克服了水下环境恶劣而导致传感器无法精确使用的问题，利用水下机器人安装的视觉传感器和声觉传感器，结合传感器工作原理，优化所得的路标信息，再与惯性测量元件相互作用下，获得水下机器人的航行状态，进而达成传感器融合的效果。本发明通过相机、声呐和惯性测量元件三个传感器融合，通过紧耦合的方式对水下机器人的状态估计和周围环境路标进行了滤除干扰和优化，使得水下机器人在航行任务的过程中，能够根据融合声、光传感器的数据信息，达到优化路标的目的来提升水下航行的续航时间，增强了同步定位与构图系统的实时性和鲁棒性。

技术领域

本发明属于水下机器人定位技术领域，具体涉及一种基于多传感器融合的水下机器人定位与构图方法。

背景技术

水下定位技术是水下机器人领域的重要研究方向，是使水下机器人进行有效任务作业的关键。当水下机器人处于一个完全陌生的环境时，需要具体的环境信息用以自主航行，但由于水下环境的复杂多变，仅凭单成本传感器无法实现高精度的航海任务。

发明内容

本发明的目的在于克服水下环境恶劣而导致传感器无法精确使用的问题，提供一种基于多传感器融合的水下机器人定位与构图方法。

本发明的目的通过如下技术方案来实现：包括以下步骤：

步骤1：获取相机在时间间隔t内测量的路标序列

路标序列中的路标为相机测量的第i个路标点在大地坐标系下的坐标；

步骤2：根据声呐在时间间隔t内测量的路标的物理信息z＝[γ_j,θ_j]，计算声呐在时间间隔t内测量的路标序列

路标序列中的路标为声呐测量的第j个路标点在大地坐标系下的坐标，其中，γ_j为声呐到第j个路标点的距离；θ_j为声呐与第j个路标点的角度；T_WS为声呐坐标系到大地坐标系下的转移矩阵；

步骤3：计算声呐在时间间隔t内测量的路标序列的均值，得到均值路标

步骤4：根据路标序列中各路标与均值路标之间的马氏距离对路标序列进行筛选，仅保留满足的路标

其中，S为相机和声纳路标之间的协方差矩阵；‖DΔl‖²为设定的信赖区间；

步骤5：根据筛选后的路标序列结合惯性测量元件进行水下机器人的定位与构图。

从筛选后的相机测量的路标序列中选取特征点用来定位；

ZP_uv＝KP_w

其中，K为相机内参；Z为路标到相机的距离，通过相机成像原理获得；P_uv为特征点的像素坐标；P_w为水下机器人的坐标。

本发明的有益效果在于：

本发明克服了水下环境恶劣而导致传感器无法精确使用的问题。本发明利用水下机器人安装的视觉传感器和声觉传感器，结合传感器工作原理，优化所得的路标信息；再与惯性测量元件相互作用下，获得水下机器人的航行状态，进而达成传感器融合的效果。本发明通过相机、声呐和惯性测量元件三个传感器融合，通过紧耦合的方式对水下机器人的状态估计和周围环境路标进行了滤除干扰和优化，使得水下机器人在航行任务的过程中，能够根据融合声、光传感器的数据信息，达到优化路标的目的来提升水下航行的续航时间，增强了同步定位与构图系统的实时性和鲁棒性。

附图说明

图1本发明的整体框架图。

图2本发明中多传感器融合的流程图。