一种水下航行器位姿确定方法及控制系统技术方案

本发明专利技术属于水下航行器回收领域，具体涉及一种水下航行器位姿确定方法及控制系统。该位姿确定方法通过两组安装在不同位置、具备不同视场的相机，分阶段采用不同方式进行位姿确定；首先通过双目相机由较远处确定航行器相对于回收平台的位姿，待航行到回收平台上方脱离双目视觉范围后，通过单目相机进一步更新位姿，因此该方法能够适用于斜向下的坐落式回收过程，解决了相机视场范围不足的问题；同时在确认位姿时，本方法在使用的相机确定的引导光源位置同时还使用了引导光源的先验位置信息，即参考了引导光源布置的真实位置，因此能够减小相机确定引导光源位置带来的误差，提高了位姿确定的精度，提高了回收的成功率。提高了回收的成功率。提高了回收的成功率。

【技术实现步骤摘要】
一种水下航行器位姿确定方法及控制系统


[0001]本专利技术属于水下航行器回收领域，具体涉及一种水下航行器位姿确定方法及控制系统。

技术介绍

[0002]水下无人航行器在执行完水下任务后，需要返回回收平台，即对水下航行器进行回收。在距离回收平台较远时，通常使用声信号引导水下航行器返回；行驶至距离回收平台较近时，由于声信号杂波严重，且精度较差，则改为采用精度较高的光视觉继续引导回收。
[0003]由于水下视觉存在昏暗环境下难以直接观测到回收平台的问题，因此可以通过感知布置在回收平台上的水下导引光源，引导水下航行器向回收平台靠近。且由于单目视觉相比双目视觉具有安装简便、精度高等优点，现有技术中通常采用相机对水下无人航行器进行位姿确定的方式进行航行器水下回收。然而，在位姿确定的过程中，安装在特定位置的相机的视场范围往往会受到限制，因此在位姿确定过程中可能存在视觉盲区，并且相机可能会受到环境干扰，导致确定的引导光源位置不准确，影响位姿确定的精确程度。

技术实现思路

[0004]本专利技术的目的在于提供一种水下航行器位姿确定方法及控制系统，用于解决现有技术中进行航行器位姿确定时相机视场范围受限且确定的引导光源位置不准确的问题。
[0005]为了实现上述目的，本专利技术提供了一种水下航行器位姿确定方法，步骤如下：
[0006]1)根据航行器前方斜向下设定角度安装的双目相机、航行器下方垂直向下安装的单目相机以及航行器自身，分别建立双目相机坐标系、单目相机坐标系和航行器坐标系，并根据相机的安装位置，分别确定双目相机坐标系与航行器坐标系、单目相机坐标系与航行器坐标系之间的转换关系；
[0007]2)水下回收平台上布置设定数量的引导光源，建立回收平台坐标系，确定引导光源在回收平台坐标系下的坐标；
[0008]3)通过双目相机获取包含引导光源的图像，确定引导光源在双目相机坐标系下的坐标，并根据双目相机坐标系与航行器坐标系转换关系，将引导光源在双目相机坐标系下的坐标转换到航行器坐标系下；
[0009]4)根据引导光源在航行器坐标系下的坐标以及在回收平台坐标系下的坐标，确定航行器相对于回收平台的相对位姿；
[0010]5)当引导光源脱离双目相机视场范围时，以双目相机确定的当前位姿作为初始坐标，通过单目相机得到引导光源在单目相机坐标系下的坐标，并根据单目相机坐标系与航行器坐标系转换关系，将引导光源在单目相机坐标系下的坐标转换到航行器坐标系下，然后持续按照步骤4)确定航行器相对于回收平台的相对位姿，直至完成航行器回收。
[0011]该位姿确定方法通过两组安装在不同位置、具备不同视场的相机，分阶段采用不同方式进行位姿确定；首先通过双目相机由较远处确定航行器相对于回收平台的位姿，待
航行到回收平台上方脱离双目视觉范围后，通过单目相机进一步更新位姿，因此该方法能够适用于斜向下的坐落式回收过程，解决了相机视场范围不足的问题；同时在确认位姿时，使用了引导光源的先验位置信息，即参考了引导光源布置的真实位置，因此能够减小相机确定引导光源位置带来的误差，提高了位姿确定的精度以及回收的成功率。
[0012]进一步地，步骤3)中，通过双目相机确定引导光源在双目相机坐标系下的坐标的方式为：
[0013]根据双目相机获取的图像，得到引导光源在相机图像上对应的拟合圆，通过拟合圆心的位置和半径，对双目相机中的左相机和右相机的拟合圆心进行匹配，利用双目视觉深度原理，得到该引导光源在双目相机坐标系下的空间坐标。
[0014]进一步地，根据引导光源在航行器坐标系下的坐标以及在回收平台坐标系下的坐标，确定航行器相对于回收平台的相对位姿的方式如下：
[0015]n个引导光源在航行器坐标系下的空间坐标为P＝{p1,p2,
···
,p
n
}，在回收平台坐标系下的坐标为P'＝{p1',p'2,
···
,p'
n
}，获取使二者误差平方和达到最小的欧式变换R和t：
[0016][0017]求得的使误差平方和达到最小的R和t表示航行器相对于回收平台的相对位姿。
[0018]进一步地，布置的引导光源的数量至少为三个，且不在同一直线上。
[0019]进一步地，通过单目相机得到引导光源在单目相机坐标系下坐标的方式为：
[0020]根据单目相机获取的图像，得到引导光源在相机图像上对应的拟合圆及其圆心位置；选取任意三个空间位置能够构成三角形的引导光源，通过其在回收平台坐标系下的坐标以及相机图像上的拟合圆心坐标，根据三角形相似原理，得到各个引导光源在相机坐标系下的坐标。
[0021]进一步地，对单目相机和双目相机获取的图像使用中值滤波算法进行图像处理，去除噪声点，然后使用Otus自适应阈值对图像做二值化处理。
[0022]对相机获取的图像进行处理能够从数据来源方面减小误差，间接提高位姿确定精度。
[0023]进一步地，对二值化后的图像采用Canny边缘检测算法，使用霍夫圆检测拟合圆心，得到圆心位置。
[0024]进一步地，所述引导光源的布置方式为T型布置。
[0025]本专利技术还提供了一种水下航行器控制系统，用于实现上述的水下航行器位姿确定方法。
[0026]该水下航行器控制系统能够实现与上述水下航行器位姿确定方法相同的有益效果。
附图说明
[0027]图1为本专利技术水下航行器回收位姿确定方法实施例中水下航行器回收位姿确定方法的流程图；
[0028]图2为本专利技术水下航行器位姿确定方法实施例中水下航行器的回收过程示意图；
[0029]图3a为本专利技术水下航行器位姿确定方法实施例中通过相机获取的引导光源图像；
[0030]图3b为本专利技术水下航行器位姿确定方法实施例中对引导光源图像处理得到的引导光源拟合圆及其圆心；
[0031]图4为本专利技术水下航行器位姿确定方法实施例中通过单目相机得到引导光源在单目相机坐标系下坐标的原理图。
具体实施方式
[0032]为了使本专利技术的目的、技术方案及优点更加清楚明了，以下结合附图及实施例，对本专利技术进行进一步详细说明。
[0033]水下航行器位姿确定方法实施例
[0034]本实施例提供了一种水下航行器位姿确定方法的技术方案，参照图1，具体包括如下步骤：
[0035]1)根据航行器前方斜向下设定角度安装的双目相机、航行器下方垂直向下安装的单目相机以及航行器自身，分别分别建立双目相机坐标系、单目相机坐标系和航行器坐标系；并根据相机的安装位置，分别确定双目相机坐标系与航行器坐标系、单目相机坐标系与航行器坐标系之间的转换关系。其中航行器坐标系以航行器质心为原点，航行方向为正前方，x轴向前，y轴向右，z轴向下。
[0036]双目相机可以安装在航行器前方偏下位置(视场角朝前下方)，单目相机安装在航行器下方偏前或者偏后位置都可以；相机安装角度是为了能够捕捉到光源，在捕捉到光源的前提下不本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种水下航行器位姿确定方法，其特征在于，步骤如下：1)根据航行器前方斜向下设定角度安装的双目相机、航行器下方垂直向下安装的单目相机以及航行器自身，分别建立双目相机坐标系、单目相机坐标系和航行器坐标系，并根据相机的安装位置，分别确定双目相机坐标系与航行器坐标系、单目相机坐标系与航行器坐标系之间的转换关系；2)水下回收平台上布置设定数量的引导光源，建立回收平台坐标系，确定引导光源在回收平台坐标系下的坐标；3)通过双目相机获取包含引导光源的图像，确定引导光源在双目相机坐标系下的坐标，并根据双目相机坐标系与航行器坐标系转换关系，将引导光源在双目相机坐标系下的坐标转换到航行器坐标系下；4)根据引导光源在航行器坐标系下的坐标以及在回收平台坐标系下的坐标，确定航行器相对于回收平台的相对位姿；5)当引导光源脱离双目相机视场范围时，以双目相机确定的当前位姿作为初始坐标，通过单目相机得到引导光源在单目相机坐标系下的坐标，并根据单目相机坐标系与航行器坐标系转换关系，将引导光源在单目相机坐标系下的坐标转换到航行器坐标系下，然后持续按照步骤4)确定航行器相对于回收平台的相对位姿，直至完成航行器回收。2.根据权利要求1所述的水下航行器位姿确定方法，其特征在于，步骤3)中，通过双目相机确定引导光源在双目相机坐标系下的坐标的方式为：根据双目相机获取的图像，得到引导光源在相机图像上对应的拟合圆，通过拟合圆心的位置和半径，对双目相机中的左相机和右相机的拟合圆心进行匹配，利用双目视觉深度原理，得到该引导光源在双目相机坐标系下的空间坐标。3.根据权利要求1
‑
2任一项所述的水下航行器位姿确定方法，其特征在于，根据引导光源在航行器坐标系下的坐标以及在回收平台坐标系下的坐标，确定航行器相对于回收平台的相对...

【专利技术属性】
技术研发人员：王凯，段要全，秦丽萍，侯冬冬，经慧祥，
申请(专利权)人：中国船舶重工集团公司第七一三研究所，
类型：发明
国别省市：