一种水下航行器位姿确定方法及控制系统与流程

1.本发明属于水下航行器回收领域，具体涉及一种水下航行器位姿确定方法及控制系统。


背景技术：

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


技术实现要素：

4.本发明的目的在于提供一种水下航行器位姿确定方法及控制系统，用于解决现有技术中进行航行器位姿确定时相机视场范围受限且确定的引导光源位置不准确的问题。
5.为了实现上述目的，本发明提供了一种水下航行器位姿确定方法，步骤如下：
6.1)根据航行器前方斜向下设定角度安装的双目相机、航行器下方垂直向下安装的单目相机以及航行器自身，分别建立双目相机坐标系、单目相机坐标系和航行器坐标系，并根据相机的安装位置，分别确定双目相机坐标系与航行器坐标系、单目相机坐标系与航行器坐标系之间的转换关系；
7.2)水下回收平台上布置设定数量的引导光源，建立回收平台坐标系，确定引导光源在回收平台坐标系下的坐标；
8.3)通过双目相机获取包含引导光源的图像，确定引导光源在双目相机坐标系下的坐标，并根据双目相机坐标系与航行器坐标系转换关系，将引导光源在双目相机坐标系下的坐标转换到航行器坐标系下；
9.4)根据引导光源在航行器坐标系下的坐标以及在回收平台坐标系下的坐标，确定航行器相对于回收平台的相对位姿；
10.5)当引导光源脱离双目相机视场范围时，以双目相机确定的当前位姿作为初始坐标，通过单目相机得到引导光源在单目相机坐标系下的坐标，并根据单目相机坐标系与航行器坐标系转换关系，将引导光源在单目相机坐标系下的坐标转换到航行器坐标系下，然后持续按照步骤4)确定航行器相对于回收平台的相对位姿，直至完成航行器回收。
11.该位姿确定方法通过两组安装在不同位置、具备不同视场的相机，分阶段采用不同方式进行位姿确定；首先通过双目相机由较远处确定航行器相对于回收平台的位姿，待
航行到回收平台上方脱离双目视觉范围后，通过单目相机进一步更新位姿，因此该方法能够适用于斜向下的坐落式回收过程，解决了相机视场范围不足的问题；同时在确认位姿时，使用了引导光源的先验位置信息，即参考了引导光源布置的真实位置，因此能够减小相机确定引导光源位置带来的误差，提高了位姿确定的精度以及回收的成功率。
12.进一步地，步骤3)中，通过双目相机确定引导光源在双目相机坐标系下的坐标的方式为：
13.根据双目相机获取的图像，得到引导光源在相机图像上对应的拟合圆，通过拟合圆心的位置和半径，对双目相机中的左相机和右相机的拟合圆心进行匹配，利用双目视觉深度原理，得到该引导光源在双目相机坐标系下的空间坐标。
14.进一步地，根据引导光源在航行器坐标系下的坐标以及在回收平台坐标系下的坐标，确定航行器相对于回收平台的相对位姿的方式如下：
15.n个引导光源在航行器坐标系下的空间坐标为p＝{p1,p2,
···
,pn}，在回收平台坐标系下的坐标为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]
双目相机可以安装在航行器前方偏下位置(视场角朝前下方)，单目相机安装在航行器下方偏前或者偏后位置都可以；相机安装角度是为了能够捕捉到光源，在捕捉到光源的前提下不会对位姿确定产生影响；其中，双目相机能够通过双目视觉原理进行定位，因此安装在航行器前方能够在识别出光源后第一时间建立航行器相对于引导光源的初始位姿，而单目相机虽然没有建立初始位姿的功能，但是能够以双目相机已经确定的位姿作为已知初始位姿，实现实时的位姿确定；因此将单目相机安装在航行器下方，在双目相机无法进行定位时，由单目相机实现继续定位，能够节省定位成本；在其他实施例中，如果不考虑成本，航行器前方和下方安装的相机均可以是双目相机。
[0037]
图2为水下航行器的回收过程示意图，根据图2可以看出，双目相机安装在航行器前方，视场范围大致为航行器正前方与斜下方区域，难以获取航行器正下方区域图像，而航行器下方安装的单目相机即能够弥补该视场范围的不足；为尽可能地避免视场范围不足带来的视觉盲区，单目相机和双目相机的安装位置必须保证二者之间的视场范围有重合部分。图2中以ov为原点的坐标系即表示了航行器坐标系。
[0038]
2)水下回收平台上布置设定数量的引导光源，建立回收平台坐标系，确定引导光源在回收平台坐标系下的坐标。
[0039]
在本实施例中，由于利用pnp原理使用单目相机确定位姿时，有效目标点至少为3个，因此水下回收平台上布置的引导光源的数量至少为三个，且为便于使用单目相机进行航行器位姿确定，所有引导光源不在同一直线上，即其中至少有三个引导光源所在的位置能够构成三角形。在一个优选实施例中，引导光源的布置方式为t型布置。横向和纵向上存在引导光源数量或者引导光源之间的距离有明显区别的，都可以算作t型布置，如图2所示，纵向上的引导光源为2个，横向上的引导光源为3个，因此可以算作t型布置；t型布置的设置目的在于能让航行器仅仅通过引导光源就能分辨清楚回收平台的前方和后方；如图2所示，
以o
p
为原点的坐标系即表示了回收平台坐标系，回收平台坐标系与步骤1)中的航行器坐标系之间的相对关系即可用于确定航行器相对于回收平台的相对位姿。
[0040]
由于回收平台本身与引导光源之间具有固定位置关系，因此引导光源在回收平台坐标系下的坐标即可用于表征引导光源先验位置信息，能够真实反映引导光源的客观位置，不受采集方式带来的干扰。
[0041]
3)通过双目相机获取包含引导光源的图像，确定引导光源在双目相机坐标系下的坐标，并根据双目相机坐标系与航行器坐标系转换关系，将引导光源在双目相机坐标系下的坐标转换到航行器坐标系下。
[0042]
如图2所示，航行器位于位置i附近时，采用双目相机，即双目视觉来确定航行器的相对位姿。双目视觉首先需要通过左、右两组相机，获取包含引导光源的图像：
[0043]mil
,m
ir
(i＝1,2,3,
…
)，
[0044]
其中m
il
表示第i张左相机图像，m
ir
表示第i张右相机图像；获取图像后，对获取的任一张图像m
il
或m
ir
使用中值滤波算法，去除噪声点，然后使用otus自适应阈值对图像做二值化处理。对二值化后的图像采用canny边缘检测算法，使用霍夫圆检测拟合圆心，得到引导光源在图像上拟合圆的圆心位置(xi,yi)，其中i＝1,2,
…
n。通过相机获取的引导光源图像如图3a所示，与该图像对应的对引导光源图像进行处理后得到的引导光源拟合圆及其圆心则如图3b所示。
[0045]
双目左右相机分别取引导光源拟合圆的圆心作为引导光源的正中心，同一个引导光源分别在左相机图像和右相机图像中都拟合出了圆心；分别使用拟合圆心的位置(xi,yi)和半径ri作为特征描述子，对左相机和右相机拟合圆心进行匹配，即确定左相机和右相机的拟合圆心处于同一位置，左相机和右相机的拟合圆为同一拟合圆，利用双目视觉深度原理，得到该光源在双目相机坐标系下的空间坐标，进而得到在航行器坐标系下的空间坐标。
[0046]
由于双目相机安装在航行器之后，双目相机坐标系和航行器坐标系之间有一个旋转和平移的关系；因此设旋转矩阵为r
dcv
，平移矩阵为t
dcv
，引导光源在双目视觉坐标系下的坐标为(x
dc
,y
dc
,z
dc
)，则转换到航行器坐标系下的坐标为(xv,yv,zv)
t
＝r
dcv
·
(x
dc
,y
dc
,z
dc
)
t
+t
dcv
；4)根据引导光源在航行器坐标系下的坐标以及在回收平台坐标系下的坐标，确定航行器相对于回收平台的相对位姿。
[0047]
根据步骤2)和步骤3)中得到的引导光源在回收平台坐标系下和航行器坐标系下的坐标，如n个引导光源在航行器坐标系下的空间坐标为p＝{p1,p2,
···
,pn}，在回收平台坐标系下的坐标为p'＝{p1',p'2,
···
,p'n}，需要找到一组欧式变换r和t，使得：
[0048][0049]
定义第i点误差项：
[0050]ei
＝p
i-(rpi'+t)
[0051]
构建最小二乘问题，求使误差平方和达到最小的r和t：
[0052][0053]
利用奇异值分解，求得r和t，也即得到了相对位姿。在确认位姿时，使用了引导光源的先验位置信息(引导光源在回收平台坐标系下的坐标)，也即参考了引导光源布置的真实位置，因此能够减小相机确定引导光源位置带来的误差，提高了位姿确定的精度，同时提
高了回收的成功率。
[0054]
5)当引导光源脱离双目相机视场范围时，以双目相机确定的当前位置作为初始坐标，通过单目相机得到引导光源在单目相机坐标系下的坐标，并根据单目相机坐标系与航行器坐标系转换关系，将引导光源在单目相机坐标系下的坐标转换到航行器坐标系下，重复步骤4)，实时获取航行器相对于回收平台的相对位姿，直至完成航行器回收；本实施例中，当双目相机无法观察到引导光源时，即视为脱离视场范围；进一步来说，无法从双目相机图像中识别到引导光源则视为脱离视场范围。
[0055]
如图2所示，航行器位于位置ii附近时，相当于已经行驶至回收平台上方，引导光源已经脱离双目相机视场范围，进入单目相机视场范围，此时则由单目相机继续捕获引导光源图像。采用上述步骤3)采用的图像处理方法，对单目相机获取的任一张图像使用中值滤波算法，去除噪声点，然后使用otus自适应阈值对图像做二值化处理。对二值化后的图像采用canny边缘检测算法，使用霍夫圆检测拟合圆并得到引导光源在图像中的圆心位置。在进入单目相机视场范围之后的初始时刻，使用双目相机确定的当前位姿作为初始坐标，对连续多帧图像，利用多光源单目定位技术，可以得到引导光源在单目相机坐标系下的坐标，进一步转换为航行器坐标系下的坐标。根据单目相机坐标系与航行器坐标系转换关系，将引导光源在单目相机坐标系下的坐标转换到航行器坐标系下的具体原理及方式如下：
[0056]
单目相机安装在航行器之后，单目相机坐标系和航行器坐标系之间有一个旋转和平移的关系，因此设旋转矩阵为r
scv
，平移矩阵为t
scv
，引导光源在单目相机坐标系下的坐标为(x
sc
,y
sc
,z
sc
)；
[0057]
则转换到航行器坐标系下的坐标为(xv,yv,zv)
t
＝r
scv
·
(x
sc
,y
sc
,z
sc
)
t
+t
scv
；
[0058]
其中，得到引导光源在单目相机坐标系下的坐标的具体方式如下：选取任意三个空间位置能够构成三角形的引导光源，通过其在回收平台坐标系下的坐标以及相机图像上的拟合圆心坐标，根据三角形相似原理，利用单目相机确定位姿的pnp算法，得到各个引导光源在相机坐标系下的坐标。如图4所示，a、b、c为空间点，表示在回收平台坐标系下的任意三个能够构成三角形的引导光源；a、b、c为2d点，表示这三个引导光源在相机图像上的拟合圆心；根据三角形相似原理，可以得到a、b、c三个点在相机坐标系下的坐标，进而得到对应的三个引导光源在航行器坐标系下的坐标。
[0059]
得到引导光源在航行器坐标系下的坐标后，同样采用步骤4)的方式根据引导光源在航行器坐标系下以及在回收平台坐标系下的坐标确定航行器相对位姿的方法，利用误差平方和最小得到航行器与回收平台的相对位姿，并实时按照上述方法确定该相对位姿，直至完成航行器回收。
[0060]
水下航行器控制系统实施例
[0061]
本实施例提供了一种水下航行器控制系统的技术方案，该控制系统用于实现如上述水下航行器位姿确定方法实施例中的水下航行器位姿确定方法。该控制系统的具体控制逻辑和步骤已经在水下航行器位姿确定方法实施例中进行了详细说明，此处不再赘述。
[0062]
本发明的特点在于：该水下航行器位姿确定方法及控制系统可以先使用双目相机由较远处确定航行器相对于回收平台的位姿，待航行到回收平台上方脱离双目视觉范围后，继续使用单目相机进一步更新位姿，直至完成回收。因此该方法能够适用于斜向下的坐落式回收方式，解决了单个相机视场范围不足的问题，同时引入了引导光源的先验位置信
息，提高了位姿确定的精度以及回收的成功率。
[0063]
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细地说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。

技术特征：
1.一种水下航行器位姿确定方法，其特征在于，步骤如下：1)根据航行器前方斜向下设定角度安装的双目相机、航行器下方垂直向下安装的单目相机以及航行器自身，分别建立双目相机坐标系、单目相机坐标系和航行器坐标系，并根据相机的安装位置，分别确定双目相机坐标系与航行器坐标系、单目相机坐标系与航行器坐标系之间的转换关系；2)水下回收平台上布置设定数量的引导光源，建立回收平台坐标系，确定引导光源在回收平台坐标系下的坐标；3)通过双目相机获取包含引导光源的图像，确定引导光源在双目相机坐标系下的坐标，并根据双目相机坐标系与航行器坐标系转换关系，将引导光源在双目相机坐标系下的坐标转换到航行器坐标系下；4)根据引导光源在航行器坐标系下的坐标以及在回收平台坐标系下的坐标，确定航行器相对于回收平台的相对位姿；5)当引导光源脱离双目相机视场范围时，以双目相机确定的当前位姿作为初始坐标，通过单目相机得到引导光源在单目相机坐标系下的坐标，并根据单目相机坐标系与航行器坐标系转换关系，将引导光源在单目相机坐标系下的坐标转换到航行器坐标系下，然后持续按照步骤4)确定航行器相对于回收平台的相对位姿，直至完成航行器回收。2.根据权利要求1所述的水下航行器位姿确定方法，其特征在于，步骤3)中，通过双目相机确定引导光源在双目相机坐标系下的坐标的方式为：根据双目相机获取的图像，得到引导光源在相机图像上对应的拟合圆，通过拟合圆心的位置和半径，对双目相机中的左相机和右相机的拟合圆心进行匹配，利用双目视觉深度原理，得到该引导光源在双目相机坐标系下的空间坐标。3.根据权利要求1-2任一项所述的水下航行器位姿确定方法，其特征在于，根据引导光源在航行器坐标系下的坐标以及在回收平台坐标系下的坐标，确定航行器相对于回收平台的相对位姿的方式如下：n个引导光源在航行器坐标系下的空间坐标为p＝{p1,p2,
···
,p
n
}，在回收平台坐标系下的坐标为p'＝{p1',p'2,
···
,p'
n
}，获取使二者误差平方和达到最小的欧式变换r和t：求得的使误差平方和达到最小的r和t表示航行器相对于回收平台的相对位姿。4.根据权利要求1-2任一项所述的水下航行器位姿确定方法，其特征在于，布置的引导光源的数量至少为三个，且不在同一直线上。5.根据权利要求4所述的水下航行器位姿确定方法，其特征在于，通过单目相机得到引导光源在单目相机坐标系下坐标的方式为：根据单目相机获取的图像，得到引导光源在相机图像上对应的拟合圆及其圆心位置；选取任意三个空间位置能够构成三角形的引导光源，通过其在回收平台坐标系下的坐标以及相机图像上的拟合圆心坐标，根据三角形相似原理，得到各个引导光源在相机坐标系下的坐标。6.根据权利要求1-2任一项所述的水下航行器位姿确定方法，其特征在于，对单目相机
和双目相机获取的图像使用中值滤波算法进行图像处理，去除噪声点，然后使用otus自适应阈值对图像做二值化处理。7.根据权利要求6所述的水下航行器位姿确定方法，其特征在于，对二值化后的图像采用canny边缘检测算法，使用霍夫圆检测拟合圆心，得到圆心位置。8.根据权利要求4所述的水下航行器位姿确定方法，其特征在于，所述引导光源的布置方式为t型布置。9.一种水下航行器控制系统，其特征在于，所述水下航行器控制系统用于实现权利要求1-8任一项所述的水下航行器位姿确定方法。

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


技术研发人员：王凯 段要全 秦丽萍 侯冬冬 经慧祥
受保护的技术使用者：中国船舶重工集团公司第七一三研究所
技术研发日：2022.12.08
技术公布日：2023/7/19