CN112907505B - 一种水下机器人电缆三维形状估计方法 - Google Patents

Abstract

本发明公开了一种水下机器人电缆三维形状估计方法，其特征在于：包括步骤：获取水下机器人电缆图片，得到电缆分割图；基于电缆分割图使用悬链线模型得到电缆分割图中悬链线的投影方程；基于所述图像中悬链线的投影方程和电缆分割图中的检测点，通过最小二乘拟合方法得到电缆三维形状参数，进而估计电缆三维形状。本发明能够实时估计水下机器人电缆三维形状并且准确度高。

Description

一种水下机器人电缆三维形状估计方法

技术领域

本发明涉及人工智能技术领域，具体涉及一种水下机器人电缆三维形状估计方法。

背景技术

近年来，遥控水下机器人(ROV)在各种水下作业中发挥着重要作用。但由于水下通信困难，ROV往往需要连接一根电缆，用于水下机器人和水面无人艇之间或者其他水下机器人之间传输数据，也能够在ROV进行水下作业中用于提供动力。实时监测电缆的三维形状有助于ROV勘探和干预任务更加安全，避免碰撞和与水下环境的纠缠，特别是如果ROV装备了可能被电缆卡住的机械臂。

现有方案中，如集中质量法，梁方程的有限单元法可以用来表示电缆的形状。但这些方法计算复杂，难以实现实时的电缆形状估计。

发明内容

为解决现有技术中的不足，本发明提供一种水下机器人电缆三维形状估计方法，解决了现有电缆形状估计方法计算复杂难以实时估计的问题。

为了实现上述目标，本发明采用如下技术方案：一种水下机器人电缆三维形状估计方法，包括步骤：

获取水下机器人电缆图片，得到电缆分割图；

基于电缆分割图使用悬链线模型得到电缆分割图中悬链线的投影方程；

基于所述图像中悬链线的投影方程和电缆分割图中的检测点，通过最小二乘拟合方法得到电缆三维形状参数，进而估计电缆三维形状。

进一步的，所述电缆分割图中悬链线的投影方程为：

其中，y(x)是悬链线投影到相机捕获的电缆画面图坐标系中的纵坐标值，x 为悬链线投影到相机捕获的电缆画面图坐标系中的横坐标值；A是悬链线常数；

在以电缆与水下机器人的连接点P₂为坐标原点，水下机器人的水平轴为X 轴，水下机器人的纵轴为Z轴，垂直于X轴与Z轴且方向指向摄像机为Y轴的坐标系F2中悬链线的点P坐标表达式表示为：

θ是电缆所在平面与水下机器人纵轴的水平夹角；X0是坐标系F2下电缆上的点到点P2的水平距离，Z0是坐标系F2下电缆上的点到P2的垂直距离，Y0 是摄像机到电缆所在平面的距离；h是电缆与水下机器人的连接点P₂到F₀坐标系横坐标的距离，坐标系F₀为基于分割图像中电缆的两个端点和最低点所在平面，以电缆的最低点P₀为坐标原点，图像横坐标方向为横坐标，图像纵坐标方向为纵坐标所建的坐标系。

进一步的，所述悬链线常数为：

其中，H是电缆的长度，h是电缆与水下机器人的连接点P₂到F₀坐标系横坐标的距离，Δh是电缆与摄像机的连接点P₁到点P₂的垂直方向上的距离。

进一步的，所述电缆三维形状参数包括：

c＝sinθ

Δh

参数b表示电缆相对于机器人的松弛度，参数c表示电缆相对于机器人的方向，h_max是h的最大值。

进一步的，所述最小二乘拟合方法为：高斯-牛顿算法的最小二乘拟合方法。

进一步的，所述通过最小二乘拟合方法得到电缆三维形状参数，包括：

选取悬链线最低点为高斯-牛顿算法的最小二乘拟合方法迭代的初始值；

设置代价函数L(u)，通过求解代价函数的最小值，得到对应的电缆三维形状参数；

r_i(u)＝y_i-y(x_i ^u)

其中，特征向量u，定义为：

代价函数L(u)是特征向量为u时，所述电缆分割图中检测点残差的总和，r_i(u) 为分割图中第i个检测点的纵坐标值与将其横坐标带入电缆分割图中悬链线的投影方程得到的值y(x_i ^u)的差值，x_i ^u为特征向量为u时电缆分割图像中第i个检测点的横坐标；K为电缆分割图像中检测到的电缆点总数，(x_i,y_i)为电缆分割图像中第i个检测点的坐标；

进一步的，所述代价函数L(u)的最小值通过以下迭代过程得到：

u^m+1＝u^m-J^rr(u^m)

u^m为第m次迭代时的特征向量u，m是迭代次数，从m＝1开始迭代，每次迭代得到u后，代入代价函数L(u)，当L(u)值增加而不是减少，迭代过程停止得到L(u)的最小值；

r(u^m)是残差r_i(u)组成的列向量，J_r是雅可比矩阵：

雅可比矩阵第i行有如下形式：

进一步的，所述每次迭代得到的u后，还包括：

判断是否满足条件：

若满足，则丢弃该u，继续迭代过程。

本发明所达到的有益效果：本发明用悬链线模型来拟合一个电缆的三维形状，并使用高斯-牛顿算法来拟合电缆三维形状，考虑到高斯-牛顿算法是一种局部收敛算法，它只能收敛到局部最小值，可能导致错误的电缆三维形状参数估计，所以，为了求得全局最小值，本发明首先选取悬链线最低点为初始值，然后通过计算残差得到特征值u，去除不满足阈值的特征值得到精确的电缆三维形状参数。本发明方法能够实时估计水下机器人电缆三维形状并且准确度高。

附图说明

图1是本发明实施例中的估计方法流程图；

图2是本发明实施例中的相机拍摄的电缆图(左)及HSV空间分割图(右)；

图3是本发明实施例中的悬链线模型示意图；

图4是本发明实施例中的电缆三维形状估计结果图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

如图1所示，一种水下机器人电缆三维形状估计方法，包括步骤：

步骤1，获取水下机器人电缆图片，通过图像分割方法识别电缆，得到电缆分割图；

电缆的像素点使用HSV空间分割方法识别。由于颜色吸收，水下图像背景大多是蓝色的。所以选择电缆的颜色为橙色，这样它是远离主要的蓝色色调空间。如图2所示，图2(左)是连接有电缆线的水下机器人源图像(已灰度化)，图2(右)是分割结果图。

步骤2，基于电缆分割图，使用悬链线模型得到电缆分割图中悬链线的投影方程；

悬链线模型是一种用于描述在重力作用下两端固定的链条所具有的曲线形状的数学模型；

电缆的三维形状初步使用如图3所示的悬链线模型表示，坐标系F₀(基于分割图像中电缆的两个端点和最低点所在平面，以电缆的最低点P₀为坐标原点，图像横坐标i₀方向为横坐标，图像纵坐标k₀方向为纵坐标所建的坐标系，)中的悬链线方程为：

其中，Z₀是F₀坐标系下纵坐标对应的变量，X₀是F₀坐标系下横坐标对应的变量，A是悬链线常数；cosh()是数学中的双曲余弦函数，

其中，H是电缆的长度，h是电缆与水下机器人的连接点P₂到F₀坐标系横坐标的距离，Δh是电缆与摄像机的连接点P₁到点P₂的垂直方向上的距离。

由式(1)可以推导出在坐标系F2(以点P₂为坐标原点，水下机器人的水平轴为X轴，水下机器人的纵轴为Z轴，垂直于X轴与Z轴且方向指向摄像机为Y轴的坐标系)中悬链线的点坐标表达式表示为：

其中，

d是F₀坐标系下点P₀到P₂的水平距离；θ是电缆所在平面与水下机器人纵轴的水平夹角；X0是坐标系F2下电缆上的点到电缆与水下机器人连接点P2的水平距离，Z0是坐标系F2下电缆上的点到电缆与水下机器人连接点P2的垂直距离，Y0是摄像机到电缆所在平面(即：分割图像中电缆的两个端点和最低点所在平面)的距离。

在水下机器人保持水平的情况下，搭载摄像机的无人艇或者另一个水下机器人坐标轴与所观测的水下机器人的坐标系坐标轴对齐，将坐标系F2下的悬链线点坐标(公式(3))投影到相机捕获的电缆画面图中，坐标系是以矩形相机视野画面(图4)的左下角最低点为原点，水平方向向右为x轴，垂直方向向上为y轴，得到图像中悬链线的投影方程：

其中，

其中，y(x)是悬链线投影到相机捕获的电缆画面图坐标系中的纵坐标值，x 为悬链线投影到相机捕获的电缆画面图坐标系中的横坐标值。

步骤3，基于所述图像中悬链线的投影方程和电缆分割图中的检测点，通过高斯-牛顿算法的最小二乘拟合方法得到估计的电缆三维形状参数，进而估计电缆三维形状。

得到图像中悬链线的投影方程公式(5)后，因为悬链线模型只能近似拟合实际的电缆形状，并不能得到精确的水下机器人电缆形状。为了得到精确的电缆形状参数，再采用基于高斯-牛顿算法的非线性最小二乘拟合方法估计电缆的三维形状参数。此方法将悬链线投影模型(式(5))拟合到相机图像中检测到的电缆点上(相机图像中检测到的电缆点组成实际的电缆形状)。

在使用高斯-牛顿算法拟合电缆三维形状前，考虑到高斯-牛顿算法是一种局部收敛算法，它只能收敛到局部最小值，可能导致错误的电缆三维形状参数估计。为了求得全局最小值，本发明选取悬链线最低点为高斯-牛顿算法的最小二乘拟合算法最后一步进行重复迭代的初始值。

电缆的形状由以下参数描述：

c＝sinθ

Δh (8)

参数b用来表示电缆相对于机器人的松弛度，参数c表示电缆相对于机器人的方向，h_max是电缆与水下机器人的连接点P₂到F₀坐标系横坐标的距离h的最大值。电缆连接的两个设备的高度差Δh是通过机器人的外部压力传感器来测量的。因此，只有式(8)的前两个参数通过曲线拟合估计。这些参数构成特征向量u，定义为：

设置代价函数L(u)，通过求解代价函数的最小值，可得到对应的u；

r_i(u)＝y_i-y(x_i ^u) (11)

其中，代价函数L(u)是特征向量为u时，所有图像中检测点残差的总和，r_i(u) 为残差，是特征向量为u时，图像中第i个检测点的纵坐标值与将其横坐标带入公式(5)中得到的值的差值；K为电缆分割图像中检测到的电缆点总数，(x_i,y_i) 为电缆分割图像中第i个检测点的坐标，y(x_i ^u)高斯-牛顿算法的最小二乘拟合算法中的非线性回归模型，可以从公式(5)得到；x_i ^u为特征向量为u时电缆分割图像中第i个检测点的横坐标；

代价函数L(u)的最小值可以通过以下迭代过程得到：

u^m+1＝u^m-J_rr(u^m) (12)

u^m为第m次迭代时的特征向量u，m是迭代次数，式(12)从m＝1开始迭代，每次迭代得到的u代入式(10)得到对应的L(u)，当L(u)值增加而不是减少，迭代过程停止得到L(u)的最小值。r(u^m)是残差r_i(u)组成的列向量，J_r是雅可比矩阵：

雅可比矩阵第i行(第i行对应第i个检测点)有如下形式：

使得代价函数L(u)最小的u就是最终需要的结果，通过特征向量u中的参数b与参数c就能估计出电缆的三维形状。

实际中在使用高斯-牛顿算法的非线性最小二乘拟合方法拟合时会存在一些与悬链线点相关的但物理上不存在的残差，因为它们超过了电缆长度。所以最后在高斯-牛顿算法中再加入去除无界残差(当悬链线投影接近垂直于图4坐标轴x坐标轴时所求得的残差)操作。

通过分析式(13)给出的雅可比矩阵的奇异性，可以设置条件来去除不符合条件的u。

每次迭代得到u后，判断满足条件：

若满足，则丢弃该u，继续迭代过程；

其中，∈是保证雅可比矩阵不出现病态的参数。该条件过滤图像平面中与图像中x坐标轴接近垂直的悬链线点，当悬链线投影退化为图像中的一条垂直线时出现这种情况。

通过此条件其实是限制了特征向量u中参数c的范围，将不满足条件的特征向量u去除，从而得到最终的特征向量u。这样就能得到最终电缆的形状参数。

u是包含电缆形状主要参数的特征向量，有了u就能知道电缆相对于机器人的松弛度和水下机器人相对于电缆的位置。因为电缆的三维形状是不规则的，只是类似悬链线模型，而不能完全用悬链线模型式(5)精确表示电缆三维形状。用高斯-牛顿算法的最小二乘拟合算法得到描述电缆三维形状参数的特征向量u 并通过仿真(仿真过程就是将步骤3迭代过程中的每个点描绘出来)得到图4 的电缆三维形状。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、 CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/ 或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (5)

1.一种水下机器人电缆三维形状估计方法，其特征在于：包括步骤：

获取水下机器人电缆图片，得到电缆分割图；

基于电缆分割图使用悬链线模型得到电缆分割图中悬链线的投影方程；

基于所述电缆分割图中悬链线的投影方程和电缆分割图中的检测点，通过最小二乘拟合方法得到电缆三维形状参数，进而估计电缆三维形状；

其中，所述电缆分割图中悬链线的投影方程为：

其中，y(x)是悬链线投影到相机捕获的电缆画面图坐标系中的纵坐标值，x为悬链线投影到相机捕获的电缆画面图坐标系中的横坐标值；A是悬链线常数；

在以电缆与水下机器人的连接点P₂为坐标原点，水下机器人的水平轴为X轴，水下机器人的纵轴为Z轴，垂直于X轴与Z轴且方向指向摄像机为Y轴的坐标系F2中悬链线的点P坐标表达式表示为：

θ是电缆所在平面与水下机器人纵轴的水平夹角；X0是坐标系F2下电缆上的点到点P2的水平距离，Z0是坐标系F2下电缆上的点到P2的垂直距离，Y0是摄像机到电缆所在平面的距离；h是电缆与水下机器人的连接点P₂到F₀坐标系横坐标的距离，坐标系F₀为基于分割图像中电缆的两个端点和最低点所在平面，以电缆的最低点P₀为坐标原点，图像横坐标方向为横坐标，图像纵坐标方向为纵坐标所建的坐标系；

所述悬链线常数为：

其中，H是电缆的长度，h是电缆与水下机器人的连接点P₂到F₀坐标系横坐标的距离，Δh是电缆与摄像机的连接点P₁到点P₂的垂直方向上的距离；

所述电缆三维形状参数包括：

c＝sinθ

Δh

参数b表示电缆相对于机器人的松弛度，参数c表示电缆相对于机器人的方向，h_max是h的最大值。

2.根据权利要求1所述的一种水下机器人电缆三维形状估计方法，其特征是：所述最小二乘拟合方法为：高斯-牛顿算法的最小二乘拟合方法。

3.根据权利要求2所述的一种水下机器人电缆三维形状估计方法，其特征是：所述通过最小二乘拟合方法得到电缆三维形状参数，包括：

选取悬链线最低点为高斯-牛顿算法的最小二乘拟合方法迭代的初始值；

设置代价函数L(u)，通过求解代价函数的最小值，得到对应的电缆三维形状参数；

r_i(u)＝y_i-y(x_i ^u)

其中，特征向量u，定义为：

代价函数L(u)是特征向量为u时所述电缆分割图中检测点残差的总和，r_i(u)为特征向量为u时，分割图中第i个检测点的纵坐标值与将其横坐标带入电缆分割图中悬链线的投影方程得到的值y(x_i ^u)的差值，x_i ^u为特征向量为u时电缆分割图像中第i个检测点的横坐标，K为电缆分割图像中检测到的电缆点总数，(x_i,y_i)为电缆分割图像中第i个检测点的坐标；

4.根据权利要求3所述的一种水下机器人电缆三维形状估计方法，其特征是：所述代价函数L(u)的最小值通过以下迭代过程得到：

u^m+1＝u^m-J_rr(u^m)

u^m为第m次迭代时的特征向量u，m是迭代次数，从m＝1开始迭代，每次迭代得到u后，代入代价函数L(u)，当L(u)值增加而不是减少，迭代过程停止得到L(u)的最小值；

r(u^m)是残差r_i(u)组成的列向量，J_r是雅可比矩阵：

雅可比矩阵第i行有如下形式：

5.根据权利要求4所述的一种水下机器人电缆三维形状估计方法，其特征是：所述每次迭代得到的u后，还包括：

判断是否满足条件：

若满足，则丢弃该u，继续迭代过程。