一种用于水下装置定点悬停的方法和装置与流程

本发明涉及水下移动平台定点悬停领域，尤其涉及一种用于水下装置定点悬停的方法和装置。

背景技术：

定点悬停能力是水下移动平台(rov、auv)的重要性能之一。水下移动平台在执行水下定点观测任务时，常会遇到波浪和海流等影响，使得水下移动平台难以维持稳定的观测。现有的技术方案主要采用水下移动平台本身携带的惯性导航器件(imu、电子罗盘等)和流速计来测量平台的运动参数，然后控制动力系统来对偏差进行修正，以此达到稳定观测的目的。由于高精度的惯性导航器件和流速计的价格非常高昂，许多成本较低的水下移动平台难以使用，而价格较低的惯性导航器件精度就差许多，难以实现精准的定点悬停。

技术实现要素：

本发明所要解决的技术问题是针对现有技术中高精度的惯性导航器件和流速计的价格非常高昂，许多成本较低的水下移动平台难以使用，而价格较低的惯性导航器件精度就差，难以实现精准的定点悬停的不足，提供了一种用于水下装置定点悬停的方法和装置。

本发明解决上述技术问题的技术方案如下：

一种用于水下装置定点悬停的方法，包括以下步骤：

s1，通过安装在水下装置上的摄像头采集环境图像；

s2，根据预设选取规则在所述环境图像中选取预设数量的待匹配像素块，根据预设匹配算法和参考图像对所述预设数量的待匹配像素块进行匹配运算，获得每个待匹配像素块的光流矢量；

s3，根据所述每个待匹配像素块的光流矢量计算得到所述摄像头的运动矢量参数；

s4，根据所述摄像头的运动矢量参数计算所述水下装置的运动参数；

s5，根据所述水下装置的运动参数对所述水下装置进行运动偏差修正。

本发明的有益效果是：本申请基于摄像头采集的图像计算像素块的光流矢量参数矩阵，结合图像中的所有待匹配像素块的光流矢量参数，得到摄像头的运动矢量参数矩阵，摄像头的运动矢量参数矩阵估计出摄像头的运动参数，以得出水下装置的运动参数，并控制动力系统对运动偏差进行修正，由于光流参数的精度为像素级，因此可以达到很高的定点悬停精度。相比现有高精度的惯性导航器件和流速计本申请使用的方案成本更低，相比将价格较低的惯性导航器件，本申请使用的方案精度更高，能够实现精准的定点悬停。

在上述技术方案的基础上，本发明还可以做如下改进。

进一步地，所述根据预设选取规则在所述环境图像中选取预设数量的待匹配像素块，具体包括：

将所述环境图像分辨率调整为第一预设分辨率；

以所述环境图像的中心点作为中心，选取第二预设分辨率区域，将所述第二预设分辨率区域均分成多个方格区域；

以每个方格区域的中心点为中心，选取第三预设分辨率区域，计算所述第三预设分辨率区域的每个像素值的梯度绝对值，将梯度值最大的像素点作为所述第三预设分辨率区域的像素中心点；

以所述第三预设分辨率区域的像素中心点为中心，选取第四预设分辨率区域，作为所述每个方格区域的备选像素块；

在全部所述备选待匹配像素块中选出所述预设数量的像素块，作为参与所述匹配运算的待匹配像素块。

采用上述进一步方案的有益效果是：在图像尺寸较大时，分割会产生数量较多的像素块，每个像素块都需要通过滑窗的方式与参考帧的对应像素块进行匹配，运算量会非常大，很难达到实时性。通过将采集的图像种的每帧图像选出预设数量的像素块作为待匹配的像素块，在可视场景静态的情况下，图像的光流场是均匀的，因此不需要对每个像素块进行匹配，可选择典型的像素块，作为待匹配像素块，以此减少待匹配的像素块数量，从而大幅度降低运算量，提高算法的实时性。

进一步地，所述根据预设匹配算法和参考图像对所述预设数量的待匹配像素块进行匹配运算，获得每个待匹配像素块的光流矢量，具体包括：

在所述参考图像中，分别以每个所述待匹配像素块为中心，在预设搜索范围内，通过预设匹配算法和滑动窗口匹配与所述待匹配像素块对应的相似像素块；

根据每个所述待匹配像素块与对应的相似像素块的差值，得到每个所述待匹配像素块的光流矢量。

采用上述进一步方案的有益效果是：通过预设匹配算法对待匹配像素块进行匹配运算，获得待匹配像素块的光流矢量，水下装置的运动状态符合匹配算法的要求，使得匹配结果更加稳定，实时优化性能更好。

进一步地，所述预设匹配算法为快速匹配算法，具体包括：在匹配区域进行匹配计算，并根据匹配的结果调整步长，得出最终的计算结果。

采用上述进一步方案的有益效果是：通过快速匹配算法在匹配区域进行匹配计算时，根据当前匹配的结果调整遍历步长，当匹配距离较大时增大步长，匹配距离较小时减小步长，以此提高匹配速度。

进一步地，所述s3，具体包括：

构造用于计算所述摄像头的运动矢量参数的超定方程组，所述超定方程组中以所述每个所述待匹配像素块的光流矢量为已知量，以所述摄像头的运动矢量参数为未知量；

根据最小二乘法对所述超定方程组求解，解出所述超定方程组的最小二乘解，将所述最小二乘解作为所述摄像头的运动矢量参数。

进一步地，所述超定方程组为：

其中，δt为两帧图像采集间隔时间，vx表示沿摄像头成像平面水平方向的运动速度，vy表示沿摄像头成像平面垂直方向的运动速度，vz表示垂直于摄像头成像平面的运动速度；由于缺少参照物的尺度信息，因此所估计的这三个运动速度均非绝对速度，而是尺度归一化后的运动速度。

是第m个待匹配像素块的光流矢量，其中m值为1，2，3，...，m，m为所述预设数量的待匹配像素块的数量值；

根据摄像头标定得到。

采用上述进一步方案的有益效果是：根据摄像头的标定参数和全部待匹配像素块的光流矢量，得到求出摄像头的运动矢量参数的超定方程组，通过最小二乘法求解超定方程组，得出摄像头的运动矢量参数。

进一步地，所述s5，具体包括：

设定控制期望值，根据所述水下装置的运动参数和所述控制期望值，通过pid控制器计算得到所述水下装置的被控量；

根据所述被控量对所述水下装置进行运动偏差修正。

进一步地，所述s5，具体包括：

设定控制期望值，根据所述水下装置的运动参数和所述控制期望值，通过模糊控制器计算得到所述水下装置的被控量；

根据所述被控量对所述水下装置进行运动偏差修正。

采用上述进一步方案的有益效果是：根据摄像头的运动矢量参数结合摄像头与水下装置的位置关系，得到水下装置的运动参数；根据水下装置的运动参数和控制期望值，通过pid控制器或者模糊控制器，计算水下装置的被控制量，根据控制量修正水下装置的运动偏差，实现水下装置的定点悬停。

本发明解决上述技术问题的另一种技术方案如下：

一种存储介质，包括：所述存储介质中存储有指令，当计算机读取所述指令时，使所述计算机执行如上述技术方案的一种用于水下装置定点悬停的方法。

本发明解决上述技术问题的另一种技术方案如下：

一种用于水下装置定点悬停的装置，包括：存储器，用于存储计算机程序；

处理器，用于执行所述计算机程序，实现如上述技术方案的一种用于水下装置定点悬停的方法。

本发明的有益效果是：本申请基于摄像头采集的图像计算像素块的光流矢量参数矩阵，结合图像中的所有待匹配像素块的光流矢量参数，得到摄像头的运动矢量参数矩阵，摄像头的运动矢量参数矩阵估计出摄像头的运动参数，以得出水下装置的运动参数，并控制动力系统对运动偏差进行修正，由于光流参数的精度为像素级，因此可以达到很高的定点悬停精度。相比现有高精度的惯性导航器件和流速计本申请使用的方案成本更低，相比将价格较低的惯性导航器件，本申请使用的方案精度更高，能够实现精准的定点悬停。

附图说明

图1为本发明的实施例提供的水下装置定点悬浮的方法流程示意图；

图2为本发明的其他实施例提供的水下装置定点悬浮系统的结构框图；

图3为本发明的实施例提供的块匹配光流计算方法原理示意图。

图4为本发明的实施例提供的pid控制器的控制过程示意图。

图5为本发明的实施例提供的模糊控制器的控制过程示意图。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实施例只用于解释本发明，并非用于限定本发明的范围。

如图1所示，为本发明实施例提供的一种用于水下装置定点悬停的方法，该方法包括：s1，通过安装在水下装置上的摄像头采集环境图像；可以通过双目摄像头采集环境图像。

s2，根据预设选取规则在环境图像中选取预设数量的待匹配像素块，根据预设匹配算法和参考图像对预设数量的待匹配像素块进行匹配运算，获得每个待匹配像素块的光流矢量；其中预设选取规则，考虑到在可视场景静态的情况下，图像的光流场应该是均匀的，因此不需要对每个像素块进行匹配，可选择典型的像素块，作为待匹配像素块，以此减少待匹配的像素块数量，从而大幅度降低运算量，提高算法的实时性。

典型像素块的选取步骤包括：将环境图像分辨率调整为第一预设分辨率；其中，第一预设分辨率是摄像头采集图像的分辨率，根据平台的计算能力选取分辨率。

以环境图像的中心点作为中心，选取第二预设分辨率区域，将第二预设分辨率区域均分成多个方格区域；

以每个方格区域的中心点为中心，选取第三预设分辨率区域，计算第三预设分辨率区域的每个像素值的梯度绝对值，将梯度值最大的像素点作为第三预设分辨率区域的像素中心点；

以第三预设分辨率区域的像素中心点为中心，选取第四预设分辨率区域，作为每个方格区域的备选像素块；

其中，第二预设分辨率、第三预设分辨率和第四预设分辨率的选取根据平台的计算能力确定，不做具体限定。

在全部备选待匹配像素块中选出预设数量的像素块，作为参与匹配运算的待匹配像素块。

其中，每个像素块在参考帧中进行匹配时，可以通过块匹配的方法，也可以采用特征点匹配的方法，例如sift、surf、角点检测，在参考帧中根据预设匹配算法对预设数量的待匹配像素块进行匹配运算，中的匹配算法可以采用快速匹配算法，在匹配区域进行匹配计算时，根据当前匹配的结果调整遍历步长，当匹配距离较大时增大步长，匹配距离较小时减小步长，以此提高匹配速度，也可以使用能实现该效果的其他匹配算法。

s3，根据每个待匹配像素块的光流矢量计算得到摄像头的运动矢量参数；

例如，可以通过构建光流矢量和运动矢量参数的超定方程组，通过求解超定方程组得到摄像头的运动矢量参数，下面给出构建超定方程组的示例性说明。

l为摄像头世界坐标系到图像坐标系的投影变换矩阵。其定义如下：

其中，fx，fy，u0，v0为摄像头的内参数，r和t为摄像头外参数，r是3×3的旋转矩阵，t是3×1的平移矩阵，这些参数均可以通过摄像头标定得到。0为(0，0，0)。

设p(xw,yw,zw)为世界坐标系下的一个点，(x,y)为其在摄像头的成像点坐标，则下列方程成立：

假设短时间内zw变化很小，由上述公式可得到下列公式：

令，

u＝δx，v＝δy，

δt为两帧图像采集间隔时间，则得出每个待匹配像素块的光流矢量参数矩阵(u，v)^t：

其中，第m个待匹配像素块的光流矢量对应的光流矢量参数矩阵vm为

vm＝(um，vm)^t，

m值为1，2，3，...，m，m为预设数量的待匹配像素块的数量值；

结合上述过程，可以得出超定方程组为，

其中，vx表示沿摄像头成像平面水平方向的运动速度，vy表示沿摄像头成像平面垂直方向的运动速度，vz表示垂直于摄像头成像平面的运动速度；由于缺少参照物的尺度信息，因此所估计的这三个运动速度均非绝对速度，而是尺度归一化后的运动速度。其中vx，vy表示在摄像头成像平面的x轴运动和y轴运动速度，其中vz表示在垂直于摄像头成像平面的z轴的运动速度。

根据最小二乘法对摄像头的运动矢量参数矩阵的超定方程组求解，解出摄像头的运动矢量参数。

s4，根据摄像头的运动矢量参数计算水下装置的运动参数；

根据摄像头的运动矢量参数，结合摄像头与水下装置的位置关系，通常摄像头与水下装置是前置位置关系，即摄像头的运动参数就是水下装置的运动参数；如果摄像头与水下装置是其他位置关系，根据具体的位置关系，通过已求出的摄像头的运动参数结合具体位置关系计算出水下装置的运动参数。

s5，根据水下装置的运动参数对水下装置进行运动偏差修正。

水下移动装置的运动偏差修正可采用pid控制器来控制，pid控制过程如图4，其中，vin为控制的期望值，vout为被控量。将vin设为(0,0,0)时，即可实现定点悬停控制。

pid根据水下装置的运动参数，通过比例、积分和微分的方式，输出调整参数，控制动力系统对水下装置的运动偏差进行修正，修正完再次采集图像，获取水下装置的运动参数，如果依然存在偏差，则pid控制继续控制动力系统对水下装置的运动偏差进行修正，如果偏差为零，则不需要控制动力系统，但是摄像头继续采集环境图像，并获取水下装置的运动参数。对水下装置的运动偏差修正也可以采用模糊控制器，模糊控制器的控制过程如图5，其中e和ec分别表示误差和误差的变化率，ke和kec为量化因子，ku为比例因子。

有双目摄像头的水下移动平台还可以根据双目视差图获得图像中参照物的尺度信息，借助尺度信息可估计出平台的实际运动速度，可实现更好的控制效果。

本申请基于摄像头采集的图像计算像素块的光流矢量参数矩阵，结合图像中的所有待匹配像素块的光流矢量参数，得到摄像头的运动矢量参数矩阵，摄像头的运动矢量参数矩阵估计出摄像头的运动参数，以得出水下装置的运动参数，并控制动力系统对运动偏差进行修正，由于光流参数的精度为像素级，因此可以达到很高的定点悬停精度。相比现有高精度的惯性导航器件和流速计本申请使用的方案成本更低，相比将价格较低的惯性导航器件，本申请使用的方案精度更高，能够实现精准的定点悬停。

优选地，在上述任意实施例中，根据预设选取规则在环境图像中选取预设数量的待匹配像素块，具体包括：

将环境图像分辨率调整为第一预设分辨率；其中，第一预设分辨率是摄像头采集图像的分辨率，根据平台的计算能力选取分辨率。

以环境图像的中心点作为中心，选取第二预设分辨率区域，将第二预设分辨率区域均分成多个方格区域；

以每个方格区域的中心点为中心，选取第三预设分辨率区域，计算第三预设分辨率区域的每个像素值的梯度绝对值，将梯度值最大的像素点作为第三预设分辨率区域的像素中心点；

以第三预设分辨率区域的像素中心点为中心，选取第四预设分辨率区域，作为每个方格区域的备选像素块；其中，第二预设分辨率、第三预设分辨率和第四预设分辨率的选取根据平台的计算能力确定，不做具体限定。

在全部备选待匹配像素块中选出预设数量的像素块，作为参与匹配运算的待匹配像素块。其中，每个像素块在参考帧中进行匹配时，可以通过块匹配的方法，也可以采用特征点匹配的方法，例如sift、surf、角点检测，在参考帧中根据预设匹配算法对预设数量的待匹配像素块进行匹配运算，中的匹配算法可以采用快速匹配算法，在匹配区域进行匹配计算时，根据当前匹配的结果调整遍历步长，当匹配距离较大时增大步长，匹配距离较小时减小步长，以此提高匹配速度，也可以使用能实现该效果的其他匹配算法。

下面给出一种示例性的说明。

假设摄像头采集的图像分辨率调整为640×480。截取图像中心480×320的区域作为待匹配块选取区域，将该区域均分为12×8个方格区域，每个方格边长为40像素。

每个方格选取中心的32×32像素区域计算每个像素的梯度绝对值，以梯度值最大的像素点作为中心，选取8×8像素区域作为该方格的待匹配像素块，整幅图像共得到12×8＝96个待匹配像素块。

从这96个像素块中，选取中心像素梯度值前32大的像素块作为最终参与匹配计算光流的像素块。选取32个是经验值，也可为其他值，具体根据平台的处理速度和选择的准确度来选取。

在图像尺寸较大时，分割会产生数量较多的像素块，每个像素块都需要通过滑窗的方式与参考帧的对应像素块进行匹配，运算量会非常大，很难达到实时性。通过将采集的图像种的每帧图像选出预设数量的像素块作为待匹配的像素块，在可视场景静态的情况下，图像的光流场是均匀的，因此不需要对每个像素块进行匹配，可选择典型的像素块，作为待匹配像素块，以此减少待匹配的像素块数量，从而大幅度降低运算量，提高算法的实时性。

优选地，在上述任意实施例中，根据预设匹配算法和参考图像对预设数量的待匹配像素块进行匹配运算，获得每个待匹配像素块的光流矢量，具体包括：

在参考图像中，分别以每个待匹配像素块为中心，在预设搜索范围内，通过预设匹配算法和滑动窗口匹配与待匹配像素块对应的相似像素块；

根据每个待匹配像素块与对应的相似像素块的差值，得到每个待匹配像素块的光流矢量。

获得待匹配像素块的光流矢量的过程，通过块匹配的方法，如图3所示，将当前帧图像分割成固定大小(如m×n)的像素块，每个像素块需要在参考帧中进行匹配。在参考帧中，以待匹配像素块为中心的一定搜索范围内，采用一定的匹配原则采用滑窗匹配的方式找到最优匹配像素块，两个像素块的矢量差值即为运动光流矢量(u,v)。

每个像素块在参考帧中进行匹配时，可以通过块匹配的方法，也可以采用特征点匹配的方法，例如sift、surf、角点检测。

通过预设匹配算法对待匹配像素块进行匹配运算，获得待匹配像素块的光流矢量，水下装置的运动状态符合匹配算法的要求，使得匹配结果更加稳定，实时优化性能更好。

优选地，在上述任意实施例中，预设匹配算法为快速匹配算法，具体包括：在匹配区域进行匹配计算，并根据匹配的结果调整步长，得出最终的计算结果。

在参考帧中根据预设匹配算法对预设数量的待匹配像素块进行匹配运算，中的匹配算法可以采用快速匹配算法，在匹配区域进行匹配计算时，根据当前匹配的结果调整遍历步长，当匹配距离较大时增大步长，匹配距离较小时减小步长，以此提高匹配速度，也可以使用能实现该效果的其他匹配算法。

通过快速匹配算法在匹配区域进行匹配计算时，根据当前匹配的结果调整遍历步长，当匹配距离较大时增大步长，匹配距离较小时减小步长，以此提高匹配速度。

优选地，在上述任意实施例中，s3，具体包括：

构造用于计算摄像头的运动矢量参数的超定方程组，超定方程组中以每个待匹配像素块的光流矢量为已知量，以摄像头的运动矢量参数为未知量；

根据最小二乘法对超定方程组求解，解出超定方程组的最小二乘解，将最小二乘解作为摄像头的运动矢量参数。

优选地，在上述任意实施例中，超定方程组为：

其中，δt为两帧图像采集间隔时间，vx表示沿摄像头成像平面水平方向的运动速度，vy表示沿摄像头成像平面垂直方向的运动速度，vz表示垂直于摄像头成像平面的运动速度；其中vx，vy表示在摄像头成像平面的x轴运动和y轴运动速度，其中vz表示在垂直于摄像头成像平面的z轴的运动速度，由于缺少参照物的尺度信息，因此所估计的这三个运动速度均非绝对速度，而是尺度归一化后的运动速度。

是第m个待匹配像素块的光流矢量，其中m值为1，2，3，...，m，m为预设数量；

根据摄像头标定得到。

根据摄像头的标定参数和全部待匹配像素块的光流矢量，得到求出摄像头的运动矢量参数的超定方程组，通过最小二乘法求解超定方程组，得出摄像头的运动矢量参数。

优选地，在上述任意实施例中，s5，具体包括：

设定控制期望值，根据水下装置的运动参数和控制期望值，通过pid控制器计算得到水下装置的被控量；

根据被控量对水下装置进行运动偏差修正。

优选地，在上述任意实施例中，s5，具体包括：

设定控制期望值，根据水下装置的运动参数和控制期望值，通过模糊控制器计算得到水下装置的被控量；

根据被控量对水下装置进行运动偏差修正。

水下移动装置的运动偏差修正可采用pid控制器来控制，pid控制过程如图4，其中，vin为控制的期望值，vout为被控量。将vin设为(0,0,0)时，即可实现定点悬停控制。

pid根据水下装置的运动参数，通过比例、积分和微分的方式，输出调整参数，控制动力系统对水下装置的运动偏差进行修正，修正完再次采集图像，获取水下装置的运动参数，如果依然存在偏差，则pid控制继续控制动力系统对水下装置的运动偏差进行修正，如果偏差为零，则不需要控制动力系统，但是摄像头继续采集环境图像，并获取水下装置的运动参数。对水下装置的运动偏差修正也可以采用模糊控制器，模糊控制器的控制过程如图5，其中e和ec分别表示误差和误差的变化率，ke和kec为量化因子，ku为比例因子。

有双目摄像头的水下移动平台还可以根据双目视差图获得图像中参照物的尺度信息，借助尺度信息可估计出平台的实际运动速度，可实现更好的控制效果。

根据摄像头的运动矢量参数结合摄像头与水下装置的位置关系，得到水下装置的运动参数；根据水下装置的运动参数和控制期望值，通过pid控制器或者模糊控制器，计算水下装置的被控制量，根据控制量修正水下装置的运动偏差，实现水下装置的定点悬停。

在某一实施例中，给出了一种存储介质，该存储介质中存储有指令，当计算机读取指令时，使计算机执行如任意实施例的一种用于水下装置定点悬停的方法。

在某一实施例中，给出了一种用于水下装置定点悬停的装置，如图2所示，该装置包括：存储器11，用于存储计算机程序；

处理器12，用于执行计算机程序，实现如任意实施例的一种用于水下装置定点悬停的方法。

本申请基于摄像头采集的图像计算像素块的光流矢量参数矩阵，结合图像中的所有待匹配像素块的光流矢量参数，得到摄像头的运动矢量参数矩阵，摄像头的运动矢量参数矩阵估计出摄像头的运动参数，以得出水下装置的运动参数，并控制动力系统对运动偏差进行修正，由于光流参数的精度为像素级，因此可以达到很高的定点悬停精度。相比现有高精度的惯性导航器件和流速计本申请使用的方案成本更低，相比将价格较低的惯性导航器件，本申请使用的方案精度更高，能够实现精准的定点悬停。

读者应理解，在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的方法实施例仅仅是示意性的，例如，步骤的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个步骤可以结合或者可以集成到另一个步骤，或一些特征可以忽略，或不执行。

上述方法如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。