视觉伺服的仿生水下机器人悬停控制方法、系统和装置与流程

本发明属于仿生学领域，具体涉及一种视觉伺服的仿生水下机器人悬停控制方法、系统和装置。

背景技术：

水下作业机器人在执行很多实际的水下作业任务时，如水下目标监控、水下设施维修等，往往要求其能够进行自主悬停。据报道，悬停模式在水下作业机器人一次典型水下作业任务中占比高达35％。而水下作业机器人作为一种水中自由悬浮设备，并不能够像陆地上机器人那样能够自然而然地稳定在目标前方，为实现自主悬停往往需要克服未知水动力对其造成的干扰。因此，设计一种有效且实用的仿生水下机器人悬停控制方法显得十分必要。

现有技术采用基于视觉的水下机器人悬停控制方法，相较于依靠定位传感器(惯性器件、深度传感器等)来进行悬停控制的方法，基于视觉的悬停控制方法具有成本低、精度较高等特性。基于视觉的水下机器人悬停控制问题可描述为：在视觉可观测的水下目标周围，基于视觉信息控制水下作业机器人与目标之间保持稳定的相对位姿关系。对于基于视觉的水下机器人悬停控制问题,国内外学者也开展了一些研究工作，提出了基于视觉的水下机器人悬停控制方法：例如马克等人通过假设主动控制系统可以将位置误差保持在较小水平来简化传感问题，机器人位置通过图像滤波和特征展开进行估计，并通过三维位置误差和航向误差反馈实现水下机器人悬停控制；高等人设计了一种基于图像的自适应神经网络视觉伺服悬停控制器，并基于李雅普诺夫稳定性判据证明了视觉伺服误差和神经网络权值矩阵的归一化最终有界性，最后通过仿真验证了控制器的有效性。

现有技术中对水下机器人悬停控制方法的研究已取得较大进展，但针对新型仿生波动鳍推进水下机器人的悬停控制方法很少涉及，因其控制量及运动特性与传统水下机器人差异较大，现有方法不能对其进行悬停控制，因此急需一种针对新型仿生水下机器人的悬停控制方法。

技术实现要素：

为了解决现有技术中的上述问题，即为了解决现有技术中缺少新型仿生水下机器人的悬停控制方法，而现有水下机器人悬停控制方法无法实现仿生水下机器人悬停控制的问题，本发明提出了一种视觉伺服的仿生水下机器人悬停控制方法，该方法包括以下步骤：

步骤s100，设置仿生水下机器人的悬停位置与目标物体的距离作为期望距离；设置所述目标物体在采集图像中的图像平面坐标作为期望图像特征；所述采集图像通过设置于所述仿生水下机器人上的图像采集装置采集得到；

步骤s200，基于tn时刻包含目标物体的图像数据，通过图像定位和测距技术获取tn时刻所述目标物体在所述仿生水下机器人随体坐标系下的三维坐标和所述目标物体与所述仿生水下机器人的实际距离，并将所述目标物体在所述设定侧图像中的图像平面坐标作为实际图像特征；

步骤s300，基于所述期望图像特征和所述实际图像特征间的图像特征误差，通过基于pid算法建立的第一悬停控制器获取tn时刻第一期望速度；基于所述期望距离和所述实际距离间的距离误差，通过基于pid算法建立的第二悬停控制器获取tn时刻第二期望速度；基于所述第一期望速度和所述第二期望速度输出tn时刻最终期望速度；

步骤s400，基于所述tn时刻最终期望速度和tn时刻实际速度间的速度误差，通过基于pid算法建立的速度控制器获取tn时刻仿生水下机器人的动力学控制量，并基于该动力学控制量对所述仿生水下机器人进行动力学控制；所述动力学控制量包括进退方向推进力、深度方向推进力、偏航力矩；

步骤s500，按照设定的时间间隔重复步骤s200至步骤s400进行所述仿生水下机器人的悬停控制。

在一些优选的实施方式中，所述图像平面坐标为所述目标物体特征点在所述采集图像上的坐标。

在一些优选的实施方式中，所述目标物体特征点为目标物体的几何中心。

在一些优选的实施方式中，所述图像定位和测距技术为基于双目视觉系统的图像定位和测距技术。

在一些优选的实施方式中，所述期望距离和实际距离为所述目标物体与所述水下作业机器人质心间的距离。

在一些优选的实施方式中，所述tn时刻实际速度获取方法为，基于tn时刻目标物体的三维坐标，通过离散二阶跟踪微分器获取。

在一些优选的实施方式中，所述仿生水下机器人第一期望速度、第二期望速度和实际速度分别包括偏航角速度、进退速度和浮潜速度三个速度分量。

在一些优选的实施方式中，所述tn时刻最终期望速度确定方法为：

当图像特征误差绝对值大于等于阈值elt时，最终期望速度为第一期望速度；

当图像特征误差绝对值缩小至小于阈值elt时，最终期望速度为第一期望速度与第二期望速度之和；

当图像特征误差绝对值从小于阈值elt增大至大于eut时，最终期望速度为第一期望速度；

其中eut>elt，elt和eut分别为预先设定的图像特征误差阈值。

在一些优选的实施方式中，“基于该动力学控制量对所述仿生水下机器人进行动力学控制”的方法为，基于所述动力学控制量，通过模糊参数映射模型将tn时刻动力学控制量映射为tn时刻仿生水下机器人实际控制量；所述模糊参数映射模型为基于模糊规则建立，用于将输入变量通过模糊推理和解模糊运算得到相应的输出变量。

在一些优选的实施方式中，“通过模糊参数映射模型将tn时刻动力学控制量映射为tn时刻仿生水下机器人实际控制量”的步骤包括：

步骤s910，确定所述动力学控制量和所述实际控制量的论域，并进行模糊化；

步骤s920，建立模糊规则库；

步骤s930，通过模糊推理与解模糊运算得到水下机器人实际控制量。

在一些优选的实施方式中，第一期望速度为：

其中，kps∈r^2×2、kis∈r^2×2、kds∈r^2×2为可调参数矩阵，jcam为图像雅可比矩阵，为速度雅可比矩阵jrob的第1、3、6列组成的矩阵，es为图像坐标误差，为es的微分。

在一些优选的实施方式中，第二期望速度获取步骤包括：

步骤s310，基于距离误差得到所述图像采集装置的沿光轴远离目标物体方向的速度vz；

vz＝-(kpded+kid∫ed+kdde·d)

其中，kpd、kid、kdd分别为比例、积分、微分系数，ed为距离误差，为的ed微分；

步骤s320，基于所述图像采集装置的沿光轴远离目标物体方向的速度获取第二期望速度

其中，为速度雅可比矩阵jrob逆矩阵的第1、3、6列组成的矩阵，vcam图像采集装置的速度。

本发明的第二方面提出了一种视觉伺服的仿生水下机器人悬停控制系统，该系统包括：期望图像特征和期望距离获取模块、实际图像特征和实际距离获取模块、最终期望速度获取模块、动力学控制量获取模块、悬停控制模块；

所述期望图像特征和期望距离获取模块，配置为设置仿生水下机器人的悬停位置与目标物体的距离作为期望距离；设置所述目标物体在采集图像中的图像平面坐标作为期望图像特征；所述采集图像通过设置于所述仿生水下机器人上的图像采集装置采集得到；

所述实际图像特征和实际距离获取模块，配置为基于tn时刻包含目标物体的图像数据，获取tn时刻所述目标物体在所述仿生水下机器人随体坐标系下的三维坐标和所述目标物体与所述仿生水下机器人的实际距离，并将所述目标物体在所述设定侧图像中的图像平面坐标作为实际图像特征；

所述最终期望速度获取模块，配置为基于所述期望图像特征和所述实际图像特征间的图像特征误差，通过基于pid算法建立的第一悬停控制器获取tn时刻第一期望速度；基于所述期望距离和所述实际距离间的距离误差，通过基于pid算法建立的第二悬停控制器获取tn时刻第二期望速度；基于所述第一期望速度和所述第二期望速度输出tn时刻最终期望速度；

所述动力学控制量获取模块，配置为基于所述tn时刻最终期望速度和tn时刻实际速度间的速度误差，通过基于pid算法建立的速度控制器获取tn时刻仿生水下机器人的动力学控制量，并基于该动力学控制量对所述仿生水下机器人进行动力学控制；所述动力学控制量包括进退方向推进力、深度方向推进力、偏航力矩；

所述悬停控制模块，配置为按照设定的时间间隔，依次通过实际图像特征和实际距离获取模块、最终期望速度获取模块、动力学控制量获取模块进行所述仿生水下机器人的悬停控制。

本发明的第三方面提出了一种存储装置，其中存储有多条程序，所述程序适于由处理器加载并执行以实现上述的视觉伺服的仿生水下机器人悬停控制方法。

本发明的第四方面提出了一种处理装置，包括处理器、存储装置；处理器，适于执行各条程序；存储装置，适于存储多条程序；所述程序适于由处理器加载并执行以实现上述的视觉伺服的仿生水下机器人悬停控制方法。

本发明的有益效果：本发明根据双目视觉系统进行目标识别与定位，并将图像特征误差及距离误差空间映射至期望速度空间，基于与实际速度的误差实时输出仿生水下机器人实际控制量，本发明能够稳定控制仿生水下机器人实现自主悬停，且具有一定的抗干扰能力。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：

图1是本发明一种实施例的视觉伺服的仿生水下机器人悬停控制方法流程示意图；

图2是本发明一种实施例的视觉伺服的仿生水下机器人悬停控制方法原理示意图；

图3是本发明一种实施例的双目视觉系统图像采集装置坐标系和水下机器人随体坐标系示意图；

图4是本发明一种实施例的悬停控制实验视频截图序列；

图5是本发明一种实施例的悬停控制图像像素特征及像素误差变化曲线；

图6是本发明一种实施例的悬停控制实验水下作业机器人与目标之间距离变化曲线；

图7是本发明一种实施例的悬停控制扰动实验视频截图序列；

图8是本发明一种实施例的悬停控制扰动实验图像像素特征及像素误差变化曲线。

图9是本发明一种实施例的悬停控制扰动实验水下作业机器人与目标之间距离变化曲线。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

本发明第一方面提出了一种视觉伺服的仿生水下机器人悬停控制方法，该方法包括以下步骤：

步骤s100，设置仿生水下机器人的悬停位置与目标物体的距离作为期望距离；设置目标物体在采集图像中的图像平面坐标作为期望图像特征；采集图像通过设置于仿生水下机器人上的图像采集装置采集得到；

步骤s200，基于tn时刻包含目标物体的图像数据，通过图像定位和测距技术获取tn时刻目标物体在仿生水下机器人随体坐标系下的三维坐标和目标物体与仿生水下机器人的实际距离，并将目标物体在设定侧图像中的图像平面坐标作为实际图像特征；

步骤s300，基于期望图像特征和实际图像特征间的图像特征误差，通过基于pid算法建立的第一悬停控制器获取tn时刻第一期望速度；基于期望距离和实际距离间的距离误差，通过基于pid算法建立的第二悬停控制器获取tn时刻第二期望速度；基于第一期望速度和第二期望速度输出tn时刻最终期望速度；

步骤s400，基于tn时刻最终期望速度和tn时刻实际速度间的速度误差，通过基于pid算法建立的速度控制器获取tn时刻仿生水下机器人的动力学控制量，并基于该动力学控制量对仿生水下机器人进行动力学控制；动力学控制量包括进退方向推进力、深度方向推进力、偏航力矩；

步骤s500，按照设定的时间间隔重复步骤s200至步骤s400进行仿生水下机器人的悬停控制。

为了更清晰地对本发明进行说明，下面结合附图1和图2先对本发明方法一种实施例各步骤进行展开描述，然后结合图3-图8对本发明一种实施例的悬停过程进行描述。

步骤s100，设置仿生水下机器人的悬停位置与目标物体的距离作为期望距离dn；设置目标物体在采集图像中的图像平面坐标作为期望图像特征sd；采集图像通过设置于仿生水下机器人上的图像采集装置采集得到。

本实施例中通过双目视觉系统进行图像采集；其中，双目视觉系统的图像采集装置的光轴指向目标物体，目标物体同时位于采集的两幅图像中，图像像素总量大于320*240。图像平面为双目视觉系统中预先设定的图像采集装置的归一化图像平面。

波动鳍推进水下作业机器人前部安装有双目视觉系统，如图3所示，建立双目视觉系统左相机和右相机坐标系分别为olcxlcylczlc和orcxrcyrczrc，坐标系原点olc和orc在相机光心上，zlc和zrc轴沿着光轴指向目标。随体坐标系为obxbybzb的坐标原点在水下作业机器人质心上。

以t为采样间隔，通过双目视觉系统采集目标物体tn时刻的图像数据，其中，n表示第n个采样周期，n为大于等于1的正整数。

在其他实施例中，也可以采用图像采集装置的光轴为平行的双目视觉系统，图像采集要求与本实施例相同，不再详述。

步骤s200，基于tn时刻包含目标物体的图像数据，通过图像定位和测距技术获取tn时刻目标物体在仿生水下机器人随体坐标系下的三维坐标和目标物体与仿生水下机器人的实际距离，并将目标物体在设定侧图像中的图像平面坐标作为实际图像特征。

图像定位和测距技术为基于双目视觉系统的图像定位和测距技术。对采集图像进行图像处理，目标物体在随体坐标系下的位置坐标ηt以及距离d通过三维重建的方法获得，其中目标物体与仿生水下机器人的实际距离d为波动鳍推进水下作业机器人质心与目标之间的距离。基于阈值分割获得目标物体在归一化图像平面上成像区域，并基于图像矩获得成像区域的目标物体几何中心像素坐标，将此坐标作为目标物体的实际图像特征。

基于tn时刻目标物体的三维坐标，通过离散二阶跟踪微分器获取仿生水下机器人tn时刻实际速度

由于通过处理双目视觉系统图像获得水下目标在随体坐标系下的三维位置坐标ηt，其中ηt＝[xt,yt,zt]^t。该位置坐标不可避免地会混入高频噪声，需要通过跟踪微分器抑制微分噪声。对于给定速度v，离散二阶跟踪微分器如公式(1)所示。

其中，h为跟踪微分器的采样周期，r0为速度因子，h0为滤波因子，fhan(·)为最速控制综合函数，v1为给定v的跟踪信号，v2为v1的微分信号。

算法1描述了采用跟踪微分器对水下作业机器人偏航角速度进行估计的算法。其中，采样周期h与双目视觉系统的采样间隔t相等。

算法1：偏航角速度估计算法

1:n＝1；

2:repeat

3:获取目标位置反馈ηt；

4:计算夹角ψt(n)＝atan2(yt,xt)；

5:u＝fhan(ψ1(n)-ψt(n),ψ2(n),rψ0,hψ0)；

6:ψ1(n+1)＝ψ1(n)+hψψ2(n)；

7:ψ2(n+1)＝ψ2(n)+hψu；

8:输出角速度估计量

9:until停止速度估计

其中n代表第n个采样周期，atan2(y,x)函数返回y和x的四象限反正切值，rψ0为速度因子，hψ0为滤波因子。在偏航角速度估计算法中，首先计算水下目标在obxbyb平面上投影与ob点的连线与obxb轴的夹角(以下统称夹角)ψt(n)，然后跟踪微分器根据当前ψt(n)值计算其跟踪信号ψ1(n)及微分信号ψ2(n)，ψ2(n)即为当前目标物体相对于水下作业机器人的偏航角速度。在水下目标是静止的情况下，水下作业机器人的偏航角速度估计值即为-ψ2(n+1)。

跟踪微分器根据当前三维位置坐标中的xt值计算其跟踪信号x1(n)及微分信号x2(n)，x2(n)即为当前目标物体相对于水下作业机器人的进退速度,水下作业机器人的进退速度估计值为-x2(n+1)。

跟踪微分器根据当前三维位置坐标中的zt值计算其跟踪信号z1(n)及微分信号z2(n)，z2(n)即为当前目标物体相对于水下作业机器人的浮潜速度,水下作业机器人的浮潜速度估计值为-z2(n+1)。

步骤s300，基于期望图像特征和实际图像特征间的图像特征误差，通过基于pid算法建立的第一悬停控制器获取tn时刻第一期望速度；基于期望距离和实际距离间的距离误差，通过基于pid算法建立的第二悬停控制器获取tn时刻第二期望速度；基于第一期望速度和第二期望速度输出tn时刻最终期望速度

tn时刻最终期望速度确定方法为：

当图像特征误差绝对值大于等于阈值elt时，最终期望速度为第一期望速度；

当图像特征误差绝对值缩小至小于阈值elt时，最终期望速度为第一期望速度与第二期望速度之和；

当图像特征误差绝对值从小于阈值elt增大至大于eut时，最终期望速度为第一期望速度；

其中eut>elt，elt和eut分别为预先设定的图像特征误差阈值。

1、第一期望速度的计算过程

为使得图像特征误差es趋于0，其中es＝sd-s，sd、s分别为期望图像特征和实际图像特征，图像特征sd选取为(xg,yg)^t，其中，xg,yg表示目标物体在归一化图像平面上成像区域的几何中心坐标。

图像特征变化的速度和相机位姿的变化速度vcam之间存在一定的关系，该关系可以用图像雅可比矩阵表示，如公式(2)所示。

其中，

jcam为图像雅可比矩阵，zg为目标物体在相机坐标系下的深度，vcam＝[vx,vy,vz,wx,wy,wz]^t∈r⁶为相机相对于目标物体的速度。

仿生水下机器人速度和相机速度之间关系，如公式(3)所示，也可用雅可比矩阵表示。

vcam＝jrobvrob(3)

其中，vrob∈r⁶为仿生水下机器人速度，jrob为速度雅可比矩阵，可根据相机在随体坐标系下的位姿计算得到。

由于仿生水下机器人为一个欠驱动系统，可控速度为即进退速度、浮潜速度以及偏航角速度。相机速度和水下作业机器人期望速度之间的关系可通过公式(4)表示。

其中，为jrob的第1、3、6列组成的矩阵。

通过以上公式可得图像特征变化速度和水下作业机器人可控速度之间的关系，如公式(5)所示。

为了使es收敛，令λs为一正常数，则通过公式(6)计算得到水下作业机器人第一期望速度。

其中(·)⁺为矩阵伪逆。

为保证图像特征误差的快速收敛，对图像特征误差添加比例积分微分作用，则通过公式(7)可计算得到水下作业机器人第一期望速度。

其中kps∈r^2×2，kis∈r^2×2，kds∈r^2×2为可调参数矩阵，jcam为图像雅可比矩阵，为速度雅可比矩阵jrob的第1、3、6列组成的矩阵，es为图像坐标误差，为es的微分。

2、第二期望速度的计算过程

为使得距离误差ed趋于0，其中ed＝dn-d，dn、d分别为水下作业机器人质心与目标物体之间的期望距离和实际距离。当ed<0时，应使相机具有正zrc轴方向的速度来增加ed，反之当ed>0时，应使相机具有负zrc轴方向的速度以减小ed，相机速度vz可通过公式(8)计算得到。

其中，kpd、kid、kdd分别为比例、积分、微分系数，为的ed微分，vz为相机相对于目标物体的速度vcam的zrc轴分量，即vcam＝[0,0,vz,0,0,0]^t。因此，设计控制律，如公式(9)所示计算得到第二期望速度可使得距离d收敛至期望值dn。

其中，为的第1、3、6列组成的矩阵，vcam图像采集装置的速度。

3、最终期望速度

图像特征误差绝对值|es|大于等于阈值elt时，最终期望速度为第一期望速度。

当图像特征误差绝对值|es|收敛至某一阈值elt时，可认为图像采集装置的光轴已对准水下目标，增加对水下作业机器人质心与目标物体之间的距离控制，最终期望速度为第一期望速度和第二期望速度之和。

为防止es在阈值附近震荡，切换至添加距离控制模式后，当|es|大于eut时，关闭对距离的控制，且eut>elt，即此时最终期望速度为第一期望速度。

通过悬停控制器得到的第一期望速度、第二期望速度均包括进退速度、浮潜速度以及偏航角速度。第一悬停控制器和第二悬停控制器的控制周期与双目视觉系统的采样间隔相等。

步骤s400，基于tn时刻最终期望速度和tn时刻实际速度间的速度误差ev，通过基于pid算法建立的速度控制器获取tn时刻仿生水下机器人的动力学控制量，并基于该动力学控制量对仿生水下机器人进行动力学控制；动力学控制量包括进退方向推进力、深度方向推进力、偏航力矩。

速度控制器基于pid算法进行设计，可根据速度误差输出水下作业机器人达到期望速度所需推进力和力矩控制量，其数学表达式如公式(10)所示。

其中为3×3可调参数矩阵；为速度误差，具体包括进退速度误差、浮潜速度误差以及偏航角速度误差；为的微分，τuτwτr依次为进退方向推进力，深度方向推进力，偏航力矩。

基于动力学控制量，通过波动推进力/力矩-行波参数映射模型将tn时刻动力学控制量映射为tn时刻仿生水下机器人实际控制量，本实施例中波动推进力/力矩-行波参数映射模型采用模糊参数映射模型，即将波动推进力/力矩映射为行波参数；仿生水下机器人行波参数为左鳍和右鳍波动频率、左鳍和右鳍波动幅值、相邻鳍条相位差、偏置角；模糊参数映射模型为基于模糊规则建立，用于将输入变量通过模糊推理和解模糊运算得到相应的输出变量。具体步骤包括：

步骤s910，确定动力学控制量和实际控制量的论域，并进行模糊化。

首先让仿生水下机器人在一些行波参数下进行游动，并计算游动的平均速度。然后根据平均速度和仿真分析估计在此行波参数下产生的推进力和推进力矩。根据三维运动控制实验结果以及波动鳍推进水下机器人的运动特点，取进退方向推进力τu的论域为[-7,7]，偏航力矩τr的论域为[-5,5]，深度方向推进力τw的论域为[-5,5]，左、右鳍波动频率和的论域均为[-40,40]，波动幅值的论域取为[10,40]，相邻鳍条相位差的论域为[0,120]，偏置角的论域为[-170,170]。左、右波动鳍的波动频率可以为负值，其符号代表行波传播方向。波动频率为正表示行波从水下航行器头部向尾部传播，反之，波动频率为负表示行波传播方向为从水下航行器尾部到头部。最后根据三角形隶属度函数进行模糊化。

步骤s920，建立模糊规则库。

模糊规则库是模糊模型的核心，主要依据以下4个原则建立模糊规则库：

(1)浮潜力与长鳍偏置角紧密相关。在一定范围内，当浮潜推力变大时，偏置角也应增大，反之亦然。

(2)当偏航力矩较小而进退推力较大时，两侧长鳍行波传播方向应一致，通过改变两侧行波频率来同时输出较大的进退推力和较小的偏航力矩。

(3)航向控制和深度控制优先级高于进退控制。即当需要产生的偏航力矩、浮潜推力、进退推力都较大时，优先保证产生较大的偏航力矩和浮潜推力。

(4)当进退推力和偏航力矩都较小时，相邻鳍条间的相位差变小，行波频率、幅值和偏置角根据浮潜推力确定。即当需要的浮潜推力较大时，行波频率、幅值和偏置角设置为较大值，反之亦然。

步骤s930，通过模糊推理与解模糊运算得到水下机器人实际控制量。

采用mamdani法进行模糊推理，并采用中心平均法进行解模糊运算以获得两侧波动鳍行波参数。

步骤s500，在tn时刻以实际控制量控制仿生水下机器人运行，运行时间长度为t。按照设定的时间间隔t重复步骤s200至步骤s400进行仿生水下机器人的悬停控制。

本发明一种实施例的悬停控制实验在在5m×4m×1.1m(长×宽×深)的室内水池进行，实验过程中，用一根竹杆悬吊一个红色小球浸入水中模拟水下目标物体，期望图像特征与悬停控制仿真中设置一致，仿生水下作业机器人与目标之间期望距离为0.75m。初始时，水下作业机器人运动至目标前方一定距离，此时目标物体在右相机归一化图像平面上图像特征为(0.64,-0.38)，即图像像素坐标为(272.36,44.03)，初始距离为0.97m。同时，还进行了扰动实验以验证本发明悬停控制方法的抗扰性能。在扰动实验过程中，波动鳍推进水下作业机器人首先悬停在目标前方，进入稳态后，对其施加外部扰动(用打捞网拨动水下作业机器人并拨水制造紊流)。

图4-图6给出了水下机器人悬停控制实验的结果。

图4是悬停控制实验视频截图序列，图3中a至i分别为以5秒为采样间隔的采用本发明一种实施例的仿生水下机器人工作过程的视频图像，以及在该时刻本发明一种实施例的双目视觉系统左相机和右相机采集的目标物体图像。

图5是基于右相机采集的图像计算得到的悬停控制图像像素特征及像素误差变化曲线，本实施例中目标物体图像特征为目标物体的几何中心在归一化图像平面上的坐标，为了直观显示，图5中目标物体几何中心的坐标均为视觉图像平面的像素坐标。图5上部为目标物体几何中心在图像平面u轴像素特征变化曲线，实线表示图像像素特征实际值，虚线表示图像像素特征期望值，横坐标为时间(单位：秒)、纵坐标为图像坐标系的u轴；图5中部为目标物体几何中心在图像平面v轴像素特征变化曲线，实线表示图像像素特征实际值，虚线表示图像像素特征期望值，横坐标为时间(单位：秒)、纵坐标为图像坐标系的v轴；图5下部为目标物体几何中心像素误差变化曲线，横坐标为时间(单位：秒)、纵坐标为像素误差，其中

图6是悬停控制实验水下作业机器人与目标物体之间距离变化曲线，横坐标为时间(单位：秒)、纵坐标为水下作业机器人与目标物体之间距离(单位：米)，实线表示水下作业机器人与目标物体之间的距离实际值，虚线表示水下作业机器人与目标物体之间的距离期望值。

从图4-图6中可以看出本发明能够控制仿生水下机器人，使图像特征误差迅速减少，并快速到达期望距离，最终稳态误差保持在40个像素之内，距离误差在0.08m之内。

图7-图9给出了水下机器人悬停控制扰动实验的结果，测试本实施例的抗扰性能。

图7是悬停控制扰动实验视频截图序列，图7中a至j分别为以9秒为采样间隔的采用本发明一种实施例的仿生水下机器人工作过程的视频图像，以及在该时刻本发明一种实施例的双目视觉系统左相机和右相机采集的目标物体图像。

图8是基于右相机采集的图像计算得到的悬停控制扰动实验图像像素特征及像素误差变化曲线，本实施例中目标物体图像特征为目标物体的几何中心在归一化图像平面上的坐标，为了直观显示，图8中的图像平面坐标均为目标物体几何中心在视觉图像平面的像素坐标。图8上部为目标物体几何中心在图像平面u轴像素特征变化曲线，实线表示图像像素特征实际值，虚线表示图像像素特征期望值，横坐标为时间(单位：秒)、纵坐标为图像坐标系的u轴；图8中部为目标物体几何中心在图像平面v轴像素特征变化曲线，实线表示图像像素特征实际值，虚线表示图像像素特征期望值，横坐标为时间(单位：秒)、纵坐标为图像坐标系的v轴；图8下部为目标物体几何中心像素误差变化曲线，横坐标为时间(单位：秒)、纵坐标为像素误差，其中

图9是悬停控制扰动实验水下作业机器人与目标之间距离变化曲线，横坐标为时间(单位：秒)、纵坐标为水下作业机器人与目标物体之间距离(单位：米)，实线表示水下作业机器人与目标物体之间的距离实际值，虚线表示水下作业机器人与目标物体之间的距离期望值。

从图7-图9中可以看出在基于视觉伺服的悬停控制系统作用下，水下作业机器人能够快速达到期望位置。在20s左右，由于受到外部扰动作用，图像像素特征误差增大至125个像素，距离增大至1m左右。此后，在本发明悬停控制系统作用下，水下作业机器人很快恢复并稳定在水下目标前方。因为仍存在较大的水流扰动，图像像素稳态误差较前述悬停控制实验更大，但仍在70个像素之内，距离稳态误差在0.11m之内，表明本发明的悬停控制方法具有一定的抗干扰能力。

本发明实施例的一种视觉伺服的仿生水下机器人悬停控制系统，该系统包括：期望图像特征和期望距离获取模块、实际图像特征和实际距离获取模块、最终期望速度获取模块、动力学控制量获取模块、悬停控制模块；

期望图像特征和期望距离获取模块，配置为设置仿生水下机器人的悬停位置与目标物体的距离作为期望距离；设置目标物体在采集图像中的图像平面坐标作为期望图像特征；采集图像通过设置于仿生水下机器人上的图像采集装置采集得到；

实际图像特征和实际距离获取模块，配置为基于tn时刻包含目标物体的图像数据，获取tn时刻目标物体在仿生水下机器人随体坐标系下的三维坐标和目标物体与仿生水下机器人的实际距离，并将目标物体在设定侧图像中的图像平面坐标作为实际图像特征；

最终期望速度获取模块，配置为基于期望图像特征和实际图像特征间的图像特征误差，通过基于pid算法建立的第一悬停控制器获取tn时刻第一期望速度；基于期望距离和实际距离间的距离误差，通过基于pid算法建立的第二悬停控制器获取tn时刻第二期望速度；基于第一期望速度和第二期望速度输出tn时刻最终期望速度；

动力学控制量获取模块，配置为基于tn时刻最终期望速度和tn时刻实际速度间的速度误差，通过基于pid算法建立的速度控制器获取tn时刻仿生水下机器人的动力学控制量，并基于该动力学控制量对仿生水下机器人进行动力学控制；动力学控制量包括进退方向推进力、深度方向推进力、偏航力矩；

悬停控制模块，配置为按照设定的时间间隔，依次通过实际图像特征和实际距离获取模块、最终期望速度获取模块、动力学控制量获取模块进行仿生水下机器人的悬停控制。

需要说明的是，上述实施例提供的视觉伺服的仿生水下机器人悬停控制系统，仅以上述各功能模块的划分进行举例说明，在实际应用中，可以根据需要而将上述功能分配由不同的功能模块来完成，即将本发明实施例中的模块或者步骤再分解或者组合，例如，上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块，以完成以上描述的全部或者部分功能。对于本发明实施例中涉及的模块、步骤的名称，仅仅是为了区分各个模块或者步骤，不视为对本发明的不当限定。

所属技术领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统的具体工作过程及有关说明，可以参考前述方法实施例中的对应过程，在此不再赘述。

本发明实施例的一种存储装置，其中存储有多条程序，程序适于由处理器加载并执行以实现上述的视觉伺服的仿生水下机器人悬停控制方法。

本发明实施例的一种处理装置，包括处理器、存储装置；处理器，适于执行各条程序；存储装置，适于存储多条程序；程序适于由处理器加载并执行以实现上述的视觉伺服的仿生水下机器人悬停控制方法。

所属技术领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的存储装置、处理装置的具体工作过程及有关说明，可以参考前述装置实施例中的对应过程，在此不再赘述。

本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的模块、方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，软件模块、方法步骤对应的程序可以置于随机存储器(ram)、内存、只读存储器(rom)、电可编程rom、电可擦除可编程rom、寄存器、硬盘、可移动磁盘、cd-rom、或技术领域内所公知的任意其它形式的存储介质中。为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

术语“第一”、“第二”等是用于区别类似的对象，而不是用于描述或表示特定的顺序或先后次序。

术语“包括”或者任何其它类似用语旨在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备/装置不仅包括那些要素，而且还包括没有明确列出的其它要素，或者还包括这些过程、方法、物品或者设备/装置所固有的要素。

至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征做出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。