[发明专利]视觉伺服的仿生水下机器人悬停控制方法、系统和装置

权利要求书

1.一种视觉伺服的仿生水下机器人悬停控制方法，其特征在于，该方法包括以下步骤：

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

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

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

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

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

2.根据权利要求1所述的视觉伺服的仿生水下机器人悬停控制方法，其特征在于，所述图像平面坐标为所述目标物体特征点在所述采集图像上的坐标。

3.根据权利要求2所述的视觉伺服的仿生水下机器人悬停控制方法，其特征在于，所述目标物体特征点为目标物体的几何中心。

4.根据权利要求1所述的视觉伺服的仿生水下机器人悬停控制方法，其特征在于，所述图像定位和测距技术为基于双目视觉系统的图像定位和测距技术。

5.根据权利要求1所述的视觉伺服的仿生水下机器人悬停控制方法，其特征在于，所述期望距离和实际距离为所述目标物体与所述水下作业机器人质心间的距离。

6.根据权利要求1所述的视觉伺服的仿生水下机器人悬停控制方法，其特征在于，所述T_n时刻实际速度获取方法为，基于T_n时刻目标物体的三维坐标，通过离散二阶跟踪微分器获取。

7.根据权利要求1所述的视觉伺服的仿生水下机器人悬停控制方法，其特征在于，所述仿生水下机器人第一期望速度、第二期望速度和实际速度分别包括偏航角速度、进退速度和浮潜速度三个速度分量。

8.根据权利要求1所述的视觉伺服的仿生水下机器人悬停控制方法，其特征在于，所述T_n时刻最终期望速度确定方法为：

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

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

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

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