CN116433760A - 一种水下导航定位系统和方法 - Google Patents

Abstract

本发明涉及一种水下导航定位系统和方法，属于水下双目立体视觉技术领域。本发明首先对设置在水下航行器上的双目相机进行立体标定，得到标定参数，通过双目相机获取对接装置的图像信息，利用标定参数对获取的图像信息进行立体匹配和三维重建获取对接装置的三维信息，航行器正确姿态拍摄到的模板图像和双目相机当前姿态拍摄的图像进行比对，确定航行器自身的姿态信息；根据对接装置的三维信息及航行器自身的姿态信息对航行器进行姿态调整，直至按照正确姿态完成与对接装置的对接，本发明实现了航行器更加精准的自主对接过程。

Description

一种水下导航定位系统和方法

技术领域

本发明涉及一种水下导航定位系统和方法，属于水下双目立体视觉技术领域。

背景技术

水下航行器自主对接技术是解决水下回收以及深海观测网络扩展和组建的关键技术，能够有效避免水面布放、回收的诸多不利因素，是连接深海空间站以及深海观测网络各节点的关键。在对水下航行器进行自主对接时，需要确定自身相对于目标的位姿，采用单目相机无法对自身相对于目标的位置进行确定。而双目相机能够基于视差原理，通过计算左右两个相机中图像坐标计算视差，利用视差和相机的标定参数确定图像中的目标进行定位。目前，现有技术中基于双目视觉系统水下引导对接的技术主要是采用双目视觉获取水下导引光源的图像，用改进的均值滤波对原始光源去噪，去噪后循环获取图像连通域与二值化的阈值、提取出光源图像并计算出光源中心坐标、匹配光源并最终结算位置信息。该方法存在了两个问题，首先未考虑到航行器自身相对于对接装置的姿态，导致了航行器对自身姿态调整时没有参照物比对；其次该方案使用加权质心检测算法判断光源中心，牺牲了对接过程中的精准度。

发明内容

本发明的目的是提供一种水下导航定位系统和方法，用以解决水下航行器对接过程中未考虑自身姿态、对接过程不精准的问题。

为实现上述目的，本发明的方案包括：

本发明的一种水下导航定位方法，包括以下步骤：

1)对设置在水下航行器上的双目相机进行立体标定，得到标定参数；

2)通过双目相机获取水下航行器的对接装置的图像信息；

3)利用标定参数对获取的图像信息进行立体匹配和三维重建，以获取对接装置的三维信息，并将航行器正确姿态拍摄到的模板图像和双目相机当前姿态拍摄的图像进行比对，确定航行器当前姿态信息；

4)根据对接装置的三维信息及航行器自身的姿态信息对航行器进行姿态调整，直至按照正确姿态完成与对接装置的对接。

本发明首先利用双目相机获取对接装置的图像，然后利用双目相机的标定参数将对接装置的图像进行立体匹配和三维重建来获取对接装置的三维坐标信息(相对于航行器)，并基于航行器正确姿态拍摄到的模板图像和双目相机当前姿态拍摄的图像进行比对，确定航行器自身的当前姿态信息，对航行器的姿态进行调整，令航行器按照正确姿态行驶完成对接。此过程中不断对获取的图像进行处理，能够实时确定对接装置的相对位置和航行器的姿态信息，为精确对接提供了可靠的支撑，提高了航行器对接的控制精度。

进一步地，所述双目相机的立体标定包括：对两个单目相机分别标定，根据两相机的标定参数计算出两相机间的转换矩阵完成双目相机的立体标定，所述单目相机的标定采用张正友标定法。

在本发明中，对双目相机进行立体标定，获得双目相机内参矩阵、外参矩阵和畸变系数，进行计算，减小相机所采集图像畸变的误差，由于双目相机无法实现立体标定，所以采用张正友标定法分别对两相机进行标定，根据两个相机的标定参数最终完成双目相机的立体标定，此方式提升了标定的精度，也便于人工操作。

进一步地，该方法还包括对双目相机获取的水下航行器的对接装置的图像信息进行预处理，以去除噪声干扰。

本发明将获取的对接装置图像进行预处理，消除噪声的影响，增强了对接装置图像信息，为后续对对接装置定位和航行器姿态的确定提供了可靠的图像数据。

进一步地，所述的立体匹配过程包括两部分，第一部分是采用约束准则对预处理后的图像进行约束；第二部分是利用匹配代价函数解算两个图像像素点之间的相似度，进而得到视差图。

进一步地，所述约束准则采用极线约束，一致性约束或相似性约束，所述匹配代价函数采用BM匹配算法或SAD匹配算法。

本发明利用立体匹配得到视差图，在立体匹配过程中，要采用约束准则对图像进行约束，缩小立体匹配搜索的范围，降低误匹配，同时提高匹配效率，其次还需利用匹配代价函数解算两个图像像素点之间的相似度，快速得到较为精准的视差图。

进一步地，所述的三维重建过程是提取对接装置的特征信息，对特征点进行匹配，根据匹配的特征点和相机标定参数获取到对接装置在图像中的相对位置，并映射到视差图中，进而得到的对接装置的三维信息。

本发明利用特征点信息对所得的对接装置图像进行三维重构，得到对接装置的立体三维信息，相对于现有技术更加清晰准确，为精准对接过程提供便利。

一种水下导航定位系统，包括双目相机、图像处理装置和控制装置，所述双目相机用于设置在水下航行器上，用于拍摄航行过程中水下航行器的对接装置的图像信息，并将其发送给图像处理装置；所述的图像处理装置用于利用双目相机的标定参数对接收到的图像信息进行立体匹配和三维重建，以得到对接装置的三维信息，并基于航行器正确姿态拍摄到的模板图像和双目相机当前姿态拍摄的图像进行比对，确定航行器自身的姿态信息；所述控制装置基于对接装置的三维信息和航行器当前的姿态信息对航行器进行控制，直至航行器按照正确姿态完成与对接装置的对接。

本发明中采用双目立体视觉系统对对接装置进行识别定位，通过标定的双目相机获取对接装置的图像，然后进行立体匹配与三维重建，进而得到对接装置的三维信息以及航行器的位姿信息，控制装置进行调整，直至航行器与对接装置对接完成，该过程所使用的双目相机系统安装简单稳固，能够适应水下复杂环境，图像处理装置对处理过程进行进一步改进，利用三维信息和航行器自身姿态信息对航行器进行调整，为精确进行对接提供了可靠的支撑。

进一步地，所述双目相机的标定参数是通过立体标定得到，双目立体标定首先要对两个单目相机分别标定，根据两相机的标定参数计算出两相机间的转换矩阵完成双目相机的立体标定。

本发明对双目相机进行立体标定，获得双目相机内参矩阵、外参矩阵和畸变系数，进行计算，减小相机所采集图像畸变的误差，由于双目相机无法实现立体标定，所以采用张正友标定法分别对两相机进行标定，根据两个相机的标定参数最终完成双目相机的立体标定，此方式提升了标定的精度，也便于人工操作。

进一步地，所述图像处理装置进行立体匹配的过程包括两部分，第一部分是采用约束准则对预处理后的图像进行约束；第二部分是利用匹配代价函数解算两个图像像素点之间的相似度，得到视差图。

本发明的图像处理装置利用立体匹配得到视差图，在立体匹配过程中，要采用约束准则对图像进行约束，缩小立体匹配搜索的范围，降低误匹配，同时提高匹配效率，其次还需利用匹配代价函数解算两个图像像素点之间的相似度，快速得到较为精准的视差图。

进一步地，所述图像处理装置进行三维重建的过程是根据从图像中提取对接装置的特征信息，匹配特征点，根据匹配的特征点和相机标定参数获取到对接装置在图像中的相对位置，并映射到视差图中，即可得到的对接装置的三维信息。

本发明利用特征点信息对所得的对接装置图像进行三维重构，得到对接装置的立体三维信息，相对于现有技术更加清晰准确，为精准对接过程提供便利。

附图说明

图1是发明水下航行器导航定位方法的流程示意图。

具体实施方式

下面结合附图对本发明做进一步详细的说明。

方法实施例：

首先要对双目相机进行立体标定；将采集到的图像进行图像预处理；利用标定后的参数将处理后的图片进行立体匹配和三维重建来获取对接装置的三维信息及自身的姿态信息；对航行器的姿态进行调整，使航行器按照的正确姿态到达对接装置，期间不断对对接装置进行识别处理来获取水下航行器实时姿态信息，对航行器进行调整，控制水下航行器进行对接。该方法的实现流程图1所示，具体实施步骤如下：

1.对双目相机进行立体标定，获得相机自身参数信息。

双目立体相机由两个水平放置、轴心距离固定的单目相机组成，用于固定安装在水下航行器的前端，由于安装误差等因素的影响，两个单目相机的光心轴线不一定完全平行，因此需要进行立体标定。立体标定时首先分别对两个单目相机进行标定，通过单目相机的标定可以得到相机的内参矩阵、外参矩阵及畸变参数等参数信息，根据两个相机的标定参数可以计算得到两相机间的转换矩阵完成双目相机的立体标定。

本发明为提高标定的精度，采用张正友标定法进行两个单目相机的标定任务，通过计算得到两相机之间的转换矩阵完成双目相机立体标定，得到双目相机的标定参数，标定参数包括内参矩阵、外参矩阵及畸变参数。

2.对双目相机采集到的图片进行图像预处理，过滤掉干扰噪声，增强对接装置的光源图像信息。

本发明主要是应用在水下航行器的对接定位过程，为了能够准确表征对接装置，本发明在需要对接的位置设置若干个光源，例如对接装置设置五个构成T字形的灯，作为其他实施方式，也可以根据实际情况对对接装置进行设置。

水下航行器前端的双目相机在水下航行器行驶的过程中实时地进行图像采集，当采集到表征对接装置的光源图片时对图片进行预处理，以消除图片中的干扰噪声，增强光源信息，例如可以采用均值滤波算法对光源图片进行去噪处理。

3.利用标定后的参数将预处理后的图像进行立体匹配和三维重建来获取对接装置的三维信息，及航行器自身的姿态信息。

立体匹配主要包括两部分，第一部分是采用约束准则对预处理后的图像进行约束；第二部分是利用匹配代价函数解算两个图像像素点之间的相似度，进而得到视差图，实现立体匹配。

本发明采用的匹配约束准则包括极线约束、一致性约束、连续性约束、相似性约束和唯一性约束，其中极线约束是一种点对直线的约束，使得特征匹配由二维搜索降为一维搜索，能够大大加快计算速度，因此本实施例选用极限约束对预处理的图像进行约束。

匹配代价函数用于检测两点之间的相似程度，相似度越高，匹配率越高；匹配代价函数主要有SAD匹配算法、BM算法、SGBM半全局匹配算法，本实施例选用BM匹配算法对对接装置进行立体匹配，得到视差图。

从预处理后的双目相机的图像中提取对接装置的特征信息，根据左右两个相机的图像中的特征信息进行特征点匹配，根据匹配的特征点和相机标定参数获取到对接装置的在两幅图像中的相对位置，将其映射到立体匹配的视差图中，即可得到的目标未知的三维信息，实现对对接装置的定位。

为了获取航行器的自身姿态信息，本发明航行器采集到的对接装置图像与正确姿态拍摄到的模板图像进行比对，并根据对接装置的三维坐标信息和双目相机的标定参数确定当前水下航行器的姿态信息。具体过程如下：

1)首先分别定义双目相机坐标系(也即航行器坐标系)、对接装置坐标系，双目相机坐标系以双目相机中心为原点，前右下分别为x-y-z轴；对接装置坐标系以对接装置为原点，前右下分别为x-y-z轴；

2)取对接装置较为明显的n个特征点(n≥3)，分别得到每个特征点在左右相机图像中的二维坐标；

3)根据双目视觉的标定参数，利用双目视觉视差原理，可以得到上述n个特征点在双目相机坐标系下的三维坐标P＝{p₁,p₂,···,p_n}；

4)假设上述n个特征点在对接装置坐标系下的三维坐标为P'＝{p₁',p'₂,···,p'_n}，那么存在一组欧式变换R和t，使得：

定义第i点误差项：

e_i＝p_i-(Rp_i'+t)

构建最小二乘问题，求使误差平方和达到最小的R和t：

5)R和t即代表了水下航行器的姿态信息。

4.对航行器的姿态进行调整，使航行器按照的正确姿态到达对接装置。

为保证水下航行器能够按照正确姿态在对接装置完成对接，水下航行器需要根据自身的当前的姿态信息进行姿态调整，使航行器不断靠近对接装置，对接装置不断地进行识别处理，借此获取水下航行器的位置和姿态信息，使水下航行器对前进的运动和方向进行调整。

系统实施例：

一种水下导航定位系统，包括双目相机、图像处理装置和控制装置。

双目相机设置在水下航行器的前端上，由两个水平放置、轴心距离固定的单目相机组成，用于拍摄航行过程中水下航行器的对接装置的图像信息，并将其发送给图像处理装置，双目相机在拍摄之前需要进行立体标定。

图像处理装置利用双目相机的标定参数对接收到的图像信息进行立体匹配和三维重建，首先采用约束准则对预处理后的图像进行约束；其次是利用匹配代价函数解算两个图像像素点之间的相似度，进而得到视差图，从双目相机中获取的图像进行特征信息的提取，对特征点进行匹配，根据匹配的特征点和标定参数获取对接装置的相对位置，映射到视差图中得到对接装置的三维信息；并基于航行器正确姿态拍摄到的模板图像和双目相机当前姿态拍摄的图像进行比对，确定航行器自身的姿态信息。

控制装置基于对接装置的三维信息和航行器当前的姿态信息对航行器进行控制，直至航行器按照正确姿态完成与对接装置的对接完成。

具体的控制过程已在方法实施例中进行详述，这里不再赘述。本发明利用双目相机获取对接装置的图像，利用双目相机的标定参数确定对接装置的相对位置，并在此基础上利用正确姿态的模板图像确定航行器的姿态，根据对接装置的位置和航行器的姿态控制航行器对接，使令航行器按照正确姿态进行对接，提高对接的成功率。利用水下双目视觉系统，具有较强的稳定性，此过程中不断对获取的图像进行处理，能够实时确定对接装置的相对位置和航行器的姿态信息，使对接过程精准可靠。

Claims (10)

1.一种水下导航定位方法，其特征在于，包括以下步骤：

1)对设置在水下航行器上的双目相机进行立体标定，得到标定参数；

2)通过双目相机获取水下航行器的对接装置的图像信息；

3)利用标定参数对获取的图像信息进行立体匹配和三维重建，以获取对接装置的三维信息，并将航行器正确姿态拍摄到的模板图像和双目相机当前姿态拍摄的图像进行比对，确定航行器当前姿态信息；

4)根据对接装置的三维信息及航行器当前的姿态信息对航行器进行姿态调整，直至按照正确姿态完成与对接装置的对接。

2.根据权利要求1所述的水下导航定位方法，其特征在于，所述双目相机的立体标定包括：对两个单目相机分别标定，根据两相机的标定参数计算出两相机间的转换矩阵完成双目相机的立体标定，所述单目相机的标定采用张正友标定法。

3.根据权利要求1所述的水下导航定位方法，其特征在于，该方法还包括对双目相机获取的水下航行器的对接装置的图像信息进行预处理，以去除噪声干扰。

4.根据权利要求3所述的水下导航定位方法，其特征在于，所述的立体匹配过程包括两部分，第一部分是采用约束准则对预处理后的图像进行约束；第二部分是利用匹配代价函数解算两个图像像素点之间的相似度，进而得到视差图。

5.根据权利要求4所述的水下导航定位方法，其特征在于，所述约束准则采用极线约束，一致性约束或相似性约束，所述匹配代价函数采用BM匹配算法或SAD匹配算法。

6.根据权利要求1所述的水下导航定位方法，其特征在于，所述的三维重建过程是提取对接装置的特征信息，对特征点进行匹配，根据匹配的特征点和相机标定参数获取到对接装置在图像中的相对位置，并映射到视差图中，进而得到的对接装置的三维信息。

7.一种水下导航定位系统，包括双目相机、图像处理装置和控制装置，其特征在于，所述双目相机用于设置在水下航行器上，用于拍摄航行过程中水下航行器的对接装置的图像信息，并将其发送给图像处理装置；所述的图像处理装置用于利用双目相机的标定参数对接收到的图像信息进行立体匹配和三维重建，以得到对接装置的三维信息，并对航行器正确姿态拍摄到的模板图像和双目相机当前姿态拍摄的图像进行比对，确定航行器自身的姿态信息；所述控制装置基于对接装置的三维信息和航行器当前的姿态信息对航行器进行控制，直至航行器按照正确姿态完成与对接装置的对接。

8.根据权利要求7所述的水下导航定位系统，其特征在于，所述双目相机通过立体标定得到标定参数，标定过程包括：对两个单目相机分别标定，根据两相机的标定参数计算出两相机间的转换矩阵，完成双目相机的立体标定。

9.根据权利要求7所述的水下导航定位系统，其特征在于，所述图像处理装置进行立体匹配的过程包括两部分，第一部分是采用约束准则对预处理后的图像进行约束；第二部分是利用匹配代价函数解算两个图像像素点之间的相似度，得到视差图。

10.根据权利要求7所述的水下导航定位系统，其特征在于，所述图像处理装置进行三维重建的过程是根据从图像中提取对接装置的特征信息，匹配特征点，根据匹配的特征点和相机标定参数获取到对接装置在图像中的相对位置，并映射到视差图中，即可得到的对接装置的三维信息。

Images