CN111136687B - 一种水下机器人视觉控制目标抓取测试系统与方法 - Google Patents

Abstract

一种水下机器人视觉控制目标抓取测试系统与方法，属于水下机器人作业领域。本发明的水面主控工控机通过光端机与嵌入式计算机相连，嵌入式计算机分别与水下视觉摄像机、水下机械手相连，水面主控工控机通过DA板卡与所述三轴悬臂吊相连。通过水下视觉摄像机对目标的实时跟踪，得出目标与水下视觉摄像机的位置关系并转化为水下机械手与目标的位置关系；计算水动力环境模型；计算分析得出测试系统在水中的运动模拟结果及其响应；控制三轴悬臂吊的运动并控制水下机械手对目标进行抓取。本发明在机器人下水前调试控制舱设备，验证水下机器人目标抓取视觉控制过程的可行性和系统的可靠性，具有测试准确可靠、仿真测试充分模拟试验等优点。

Description

一种水下机器人视觉控制目标抓取测试系统与方法

技术领域

本发明属于水下机器人作业领域，具体涉及一种水下机器人视觉控制目标抓取测试系统与方法。

背景技术

近年来，机器人的手眼融合目标抓取已成为科学研究的重要热点领域。水下机器人的视觉控制目标抓取作业，可以用于水下目标的自主捕获、海底生物及岩石标本的自主采集、海底设施的维护与装置的回收、水下打捞和援潜救生等方面。但目前，潜水员和水下机器人遥控操作抓取仍然是水下工程和作业的主要手段。但随着水下机器人的广泛使用，需要不断地提高水下机器人目标抓取的自主性以提高抓取的效率、减少操作人员的干预、并适于更多层次的使用人员。随着水下机器人的发和陆地或空间机器人相比，水下机器人视觉控制目标抓取不仅包含复杂的软硬件系统，而且面临更复杂的水下环境，不仅面临退化和低对比度的水下图像，而且要处理机器人在水中的浮游运动，所以在载体下水前的调试过程中，非常有必要进行视觉控制目标抓取的全实物测试。

专利文献“一种智能潜水器控制舱的全实物测试装置及测试方法(CN200910072924.1)”和“一种水下平台控制的半实物仿真测试系统(CN2014 20533048.4)”，涉及到的水下机器人实物仿真测试方法，但主要用于测试水下机器人航行时的控制舱的软硬件功能，无法模拟水下机器人视觉控制目标抓取的手艇协调运动、目标跟踪抓取的实际过程。

和普通的潜水器全实物或半实物仿真系统相比，水下机器人视觉控制目标抓取的全实物系统更为复杂。它涉及到水下机器人的视觉和机械手的坐标变换和运动学分析，全实物系统需要模拟水下机器人在发现目标后的抓取运动，并根据模拟的结果分析实际试验场景中可能出现的响应，从而对系统软硬件进行接近于真实的调试。中船重工七O二研究所的谢俊元等人在《船舶力学》发表的论文载人潜水器-机械手系统动力学研究论及了水下机器人目标抓取的运动学和动力学响应分析方法，但该方法无法为半实物的水下机器人目标抓取提供仿真；中国科学院沈阳自动化所的张奇峰等人在《机器人》杂志所发表的论文《基于能源消耗最小的自治水下机器人-机械手系统协调运动研究》论及了水下机器人目标抓取运动的手艇协调运动控制方法，但该方法直接不能用于水下机器人的视觉控制。

因此，非常有必要设计一种视觉控制目标抓取的全实物仿真测试系统与测试方法，进行水下机器人-机械手系统视觉控制目标抓取的全实物测试。

发明内容

针对上述问题，本发明提供了一种水下机器人视觉控制目标抓取测试系统与方法。

为实现上述目的，一种水下机器人视觉控制目标抓取测试系统，其结构包括水面主控工控机、多关节水下机械手、三轴悬臂吊、水下视觉摄像机、嵌入式计算机，所述水面主控工控机通过光端机与所述嵌入式计算机相连，嵌入式计算机分别与所述水下视觉摄像机、所述多关节水下机械手相连，水面主控工控机通过DA板卡与所述三轴悬臂吊相连。

所述嵌入式计算机通过CAN卡、水密线缆与所述多关节水下机械手相连；所述嵌入式计算机通过视频采集卡、水密线缆与所述水下视觉摄像机相连；所述光端机包括耐压舱光端机、脐带缆和水面光端机。

一种水下机器人视觉控制目标抓取测试系统的测试方法，具体的步骤为：

步骤1.通过水下视觉摄像机对目标的实时跟踪，得出目标与水下视觉摄像机的位置关系；

步骤2.根据运动学方程将目标与水下视觉摄像机的位置关系转化为多关节水下机械手与目标的位置关系；

步骤3.应用控制算法进行轨迹规划并获得控制响应，在水面主控工控机内将控制响应转化为测试系统的实际输出响应；

步骤4.根据潜艇操纵性方程以及各项水动力系数，在Vega视景仿真软件中加载测试系统模型并计算得出水动力环境模型；

步骤5.利用Vega视景仿真软件将得到的输出响应结合测试系统模型和水动力环境模型，计算分析得出测试系统在水中的运动模拟结果及其响应；

步骤6.根据视景仿真软件的模拟结果和响应，以三轴悬臂吊的运动代替测试系统在水中的运动，并控制多关节水下机械手对目标进行抓取，实现测试系统在现实环境下的全实物抓取操作。

步骤2所述的运动学方程为

其中

为某点在水下机械手末端坐标系下的坐标；

为该点在摄像机坐标系下的坐标；/>

为摄像机坐标系到水下机器人质心坐标系的旋转矩阵；/>

为摄像机坐标系到水下机器人质心坐标系的三维平移向量；ⁱT_i+1为从机械手第i+1个关节到第i个关节的变换矩阵，^vT₁为从水下机器人质心到机械手基关节的变换矩阵，具体的公式为：

其中x₁、y₁和z₁代表机械手基关节相对于水下机器人质心坐标系的位移，d_i代表关节两法线间的距离，a_i代表第i个机械手连杆的长度，α_i代表z_i和z_i+1轴之间的夹角，θ_i代表两连杆之夹角。

步骤4所述的潜艇操纵性方程包括多关节水下机械手对机器人的受力影响X_effect，具体的计算公式为

X_effect＝g(θ)+F(θ)+H(θ)

g(θ)代表多关节水下机械手在竖直方向所受的重力和浮力的合力，

代表多关节水下机械手各臂杆所受重力，/>

代表多关节水下机械手各臂杆所受浮力，/>

和/>

分别代表多关节水下机械手各臂杆的重心和浮心位置，m_i代表机械手第i个臂杆的质量；

F(θ)代表多关节水下机械手各关节运动给机器人载体带来的动量矩

式中ⁱI_i代表多关节水下机械手第i个臂杆绕自身质心旋转的转动惯量，ⁱw_i代表多关节水下机械手第i个臂杆绕自身质心旋转的角速度向量，ⁱv_ci代表多关节水下机械手第i个臂杆在其质心的线速度向量；

H(θ)代表机械手在运动时受到的水动力

其中H_Mi代表多关节水下机械手第i个臂杆运动带来的附加质量力，

ⁱV_ci＝[u_ci v_ci w_ci p_ci q_ci r_ci]代表多关节水下机械手第i个臂杆在其质心的速度向量，包括线速度和角速度；

H_Ci＝C_Ai ⁱV_ci代表附加质量带来的哥氏力，

代表水动力系数

代表粘性水动力ρ代表水的密度，D(x_i)代表多关节水下机械手第i个臂杆的截面积，C_D(R_n)代表粘性水动力的拖曳系数，R_n代表雷诺数，dl_i代表机械手第i个臂杆的长度微元。

本发明的有益效果在于：

本发明提供的一种水下机器人视觉控制目标抓取测试系统与方法，可在机器人下水前，水下机器人-机械手系统总体集成安装前后，不开耐压控制舱的前提下，有效地完成水下机器人-机械手系统视觉控制目标抓取的闭环控制调试的全实物仿真测试系统和测试方法。该发明可以在机器人下水前调试控制舱设备，并验证水下机器人目标抓取视觉控制过程的可行性和系统的可靠性，具有测试准确可靠、仿真测试充分模拟试验等优点。

附图说明

图1为本发明的测试系统结构图。

图2为本发明的测试系统原理图。

图3为本发明的测试方法流程图。

图4为本发明的仿真结果测试图。

具体实施方式：

下面结合附图对本发明做进一步的描述：

实施例1

本发明设计了一种水下机器人-机械手系统视觉控制目标抓取的全实物仿真测试系统与测试方法，主要目的是在机器人下水前，水下机器人-机械手系统总体集成安装前后，不开耐压控制舱的前提下，有效地完成水下机器人-机械手系统视觉控制目标抓取的闭环控制调试的全实物仿真测试系统和测试方法。该发明可以在机器人下水前调试控制舱设备，并验证水下机器人目标抓取视觉控制过程的可行性和系统的可靠性，具有测试准确可靠、仿真测试充分模拟试验等优点。

发明实现如下：

水下机器人-机械手系统视觉控制目标抓取的全实物仿真测试系统的组成包括水下机器人视觉控制目标抓取全实物仿真系统硬件和水下机器人视觉控制目标抓取的仿真测试软件。水下机器人视觉控制目标抓取的仿真测试软件在水面控制计算机内，用于进行水下机器人目标抓取的陆地仿真测试。

其中水下机器人-机械手系统视觉控制目标抓取全实物仿真系统硬件主要包括水面主控工控机，一组光端机、安装完成的水下机器人耐压舱和多关节水下机械手及其关节内自带机械手关节驱动器和编码器，水下视觉摄像机、多串口卡、CAN卡、AD、DA、IO卡、视频采集卡，嵌入式计算机、耐压舱供电所需的稳压电源、、光电复合脐带缆、水密线缆、三轴悬臂吊等。其中嵌入式计算机通过插槽和多串口卡、CAN卡、AD、DA、IO卡、视频采集卡相连，水下视觉摄像机通过水密线缆，舱内视频信号线和视频采集卡相连，多关节机械手通过水密线缆由CAN卡控制关节内驱动器并采集编码器的关节位置信号。多关节机械手的状态和水下视觉摄像机所采集到的图像由嵌入式计算机通过网络信号，经过耐压舱光端机、脐带缆和水面光端机，发送给水面工控机。水面工控机PCI插槽插接DA卡，通过DA卡驱动三轴悬臂吊带动水下机器人运动。水面工控机接收到耐压舱发送的信息，通过全实物仿真测试软件发送水下机器人和机械手的运动控制指令，同时模拟水下机器人的运动响应，控制三轴悬臂吊和机械手进行水下机器人对目标的地面抓取运动。

当水下视觉摄像机测量得到摄像机相对于目标的位置关系后，水下机器人-机械手系统视觉控制目标抓取的仿真测试软件利用下面的公式将摄像机相对于目标的距离转换为，机械手相对于目标的距离：

式中

代表某点在水下机械手末端坐标系下的坐标，ⁱT_i+1代表从机械手第i+1个关节到第i个关节的变换矩阵，^vT₁代表从水下机器人质心到机械手基关节的变换矩阵，/>

代表某点在摄像机坐标系下的坐标，/>

代表摄像机坐标系到水下机器人质心坐标系的旋转矩阵，/>

代表摄像机坐标系到水下机器人质心坐标系的三维平移向量。

其中，

x₁、y₁和z₁代表机械手基关节相对于水下机器人质心坐标系的位移，d_i代表关节两法线间的距离，a_i代表第i个机械手连杆的长度，α_i代表z_i和z_i+1轴之间的夹角,θ_i代表两连杆之夹角。

水下机器人目标抓取控制算法根据目标相对于机械手的距离进行轨迹规划，发送水下机器人和机械手的运动控制指令，水下机器人视觉控制目标抓取的仿真测试软件根据控制指令首先分析水下机器人在抓取过程中，机械手对机器人的受力影响，然后根据潜艇操纵性方程，将机械手对机器人的受力影响算作一项外界影响，就可以得到水下机器人的运动响应。机械手对机器人的受力影响X_effect按照下面的方法计算：

X_effect＝g(θ)+F(θ)+H(θ)

式中，g(θ)代表机械手在竖直方向所受的重力和浮力的合力，

代表机械手各臂杆所受重力，/>

代表机械手各臂杆所受浮力，/>

和/>

分别代表机械手各臂杆的重心和浮心位置，m_i代表机械手第i个臂杆的质量。

F(θ)代表机械手各关节运动给机器人载体带来的动量矩：

式中，ⁱI_i代表机械手第i个臂杆绕自身质心旋转的转动惯量，ⁱw_i代表机械手第i个臂杆绕自身质心旋转的角速度向量，ⁱv_ci代表机械手第i个臂杆在其质心的线速度向量。

H(θ)代表机械手在运动时受到的水动力。

其中，H_Mi代表机械手第i个臂杆运动带来的附加质量力，

ⁱV_ci＝[u_ci v_ci w_ci p_ci q_ci r_ci]代表机械手第i个臂杆在其质心的速度向量，包括线速度和角速度；

H_Ci＝C_Ai ⁱV_ci代表附加质量带来的哥氏力，

代表水动力系数。

代表粘性水动力ρ代表水的密度，D(x_i)代表机械手第i个臂杆的截面积；C_D(R_n)代表粘性水动力的拖曳系数R_n代表雷诺数，dl_i代表机械手第i个臂杆的长度微元。

根据所得到的水下机器人的运动响应，机械手的运动响应，视觉系统拍到的图像信息就可以在工控机内利用visual studio嵌入的Vega视景仿真软件表现水下机器人收到视觉控制指令后的运动和抓取过程，并同时驱动三轴悬臂吊带动水下机器人做空间运动，加上机械手的运动在陆地上和视景中分别完成目标的视觉控制全实物仿真抓取。

利用本发明完成了一种水下机器人-机械手系统视觉控制目标抓取的全实物仿真测试系统和测试方法。

该全实物仿真测试系统包括机器人水面控制工控机和包含加速度计、多普勒测速仪、高度计和视觉摄像机等传感器在内的水下机器人-机械手系统实物。目标识别和视觉跟踪程序、运动学解算程序、路径规划程序和Vega视景仿真程序集成于水面控制程序中，水面控制程序由工控机运行，工控机与机器人控制仓通过脐带缆内光纤进行数据交换。机器人通过悬挂在水池三轴悬臂吊上，模拟在水中各自由度的运动。

根据附图1-4，水下机器人-机械手系统视觉控制目标抓取的仿真测试的具体方法是：

将机器人固定在水池悬臂吊上，将目标物放置于视觉摄像机观察范围内，接通电源并运行机器人水面控制程序。建立工控机同水下机器人-机械手系统和水池悬臂吊的连接，并初始化各项参数。完毕后，选择自动控制选项，开始进行仿真测试。

首先，在接收到由视觉摄像机发送的图像信息后，由目标检测和视觉跟踪程序获得目标物与摄像机的相对位置信息，再经由运动学公式转化为目标物与机械手的相对位置信息。

然后，根据获得的相对位置信息，由水面控制程序实施路径规划，得到对水下机器人和机械手的控制响应，并将该响应转化为水下机器人各推进器和机械手各关节的输出响应。之后，工控机内嵌入的Vega视景仿真软件根据潜艇操纵性方程和水下机器人-机械手系统的各项水动力参数，载入水下机器人-机械手系统模型并计算水动力环境模型，将机械手的运动对机器人的水动力影响作为一项外界影响，即可模拟出水下机器人在水中的运动响应。

最后，根据模拟的水下机器人的运动响应，驱动悬臂吊带动机器人运动，同时根据机械手各关节的输出相应操纵机械手对目标实施抓取。通过对水下机器人-机械手系统在Vega视景仿真环境中的运动情况和在现实环境中对目标的抓取情况的分析，便可得到水下机器人-机械手系统在水中的真实运动情况，从而实现在陆地条件下，在不打开控制仓的情况下，完成对水下机器人-机械手系统视觉控制目标抓取的全实物仿真测试。

本发明可有效地进行水下机器人-机械手系统视觉控制目标抓取的全实物仿真测试，可实时地观察到水下机器人-机械手系统在对目标抓取过程中的各项真实响应。在水下机器人控制仓全封闭地情况下，结合陆地上和视景中的响应，完成对控制仓内的软硬件全面而真实的实物仿真测试，为水下机器人后续的水下实验做好充分的准备。

实施例2

本发明提供的是一种水下机器人-机械手系统视觉控制目标抓取的全实物仿真测试系统与测试方法，主要用于在陆地上，机器人控制仓全封闭的情况下，对控制仓内的软硬件进行全面而真实的实物测试，为水下机器人后续的水下试验做好充分的准备。水下机器人悬挂在悬臂吊上并通过脐带缆与水面控制程序相连，水面控制程序通过视觉摄像机获得目标位置并根据运动学原理解得机械手与目标的位置关系。之后，根据控制算法进行路径规划并向视景仿真界面输出控制响应，视景仿真界面根据控制响应以及已经设定好的水动力模型，得到水下机器人-机械手系统的运动响应，并驱动悬臂吊和机械手对目标实施自主仿真抓取。根据仿真情况和实际抓取结果，分析实际试验场景中可能出现的响应，从而在陆地环境下对系统软硬件进行接近于真实的测试。

1.根据水下机器人-机械手系统视觉控制目标抓取的全实物仿真测试系统，水下机器人-机械手系统视觉控制目标抓取的全实物仿真测试方法包含以下步骤：

步骤1.通过视觉摄像机对目标的实时跟踪，得出目标与视觉摄像机的位置关系。

步骤2.根据运动学方程将此关系转化为机械手与目标的位置关系。

步骤3.应用控制算法进行轨迹规划并获得控制响应，在工控机内将该控制响应转化为水下机器人-机械手系统的实际输出响应。

步骤3.根据潜艇操纵性方程以及各项水动力系数，在Vega视景仿真软件中加载水下机器人-机械手系统模型并计算得出水动力环境模型。

步骤4.利用Vega视景仿真软件将得到的输出响应结合水下机器人-机械手系统模型和水动力环境模型，计算分析得出水下机器人-机械手系统在水中的运动模拟结果及其响应。

步骤5.根据视景仿真软件的模拟结果和响应，以悬臂吊的运动代替水下机器人-机械手系统在水中的运动，并控制机械手对目标进行抓取，实现水下机器人-机械手系统在现实环境下的全实物抓取操作。

2.根据在Vega视景仿真环境中水下机器人-机械手系统的响应情况以及在现实环境中水下机器人-机械手系统对目标的抓取情况，便可分析得到水下机器人-机械手系统在实际试验场景中的响应。从而完成在陆地环境下，在控制仓全封闭的条件下，对基于视觉的水下机器人-机械手系统的控制仓和工控机内的软硬件的仿真测试。

Claims (2)

1.一种水下机器人视觉控制目标抓取测试方法，其特征在于，包括以下步骤：

步骤1：搭建测试系统，所述测试系统包括待测试水下机器人-机械手系统、抓取目标、三轴悬臂吊、水下视觉摄像机和水面主控工控机；将待测试水下机器人-机械手系统安装在三轴悬臂吊上，将抓取目标放置于水下视觉摄像机的观察范围内；接通电源并运行水下机器人-机械手系统的控制程序，建立水面主控工控机与水下机器人-机械手系统和三轴悬臂吊的连接，并初始化各项参数；

步骤2：通过水下视觉摄像机对抓取目标的实时跟踪，获取抓取目标与水下视觉摄像机的位置关系；

步骤3：根据运动学方程将目标与水下视觉摄像机的位置关系转化为多关节水下机械手与目标的位置关系；

步骤4：应用控制算法根据抓取目标相对于机械手的距离进行轨迹规划，并获得控制响应，在水面主控工控机内将控制响应转化为测试系统的实际输出响应；

步骤5：根据潜艇操纵性方程以及各项水动力系数，在Vega视景仿真软件中加载测试系统模型并计算得出水动力环境模型；

机械手对水下机器人的受力影响X_effect为：

X_effect＝g(θ)+F(θ)+H(θ)

其中，g(θ)表示机械手在竖直方向所受的重力和浮力的合力；F(θ)表示机械手各关节运动给水下机器人载体带来的动量矩；H(θ)表示机械手在运动时受到的水动力；

其中，

表示机械手各臂杆所受重力；/>

表示机械手各臂杆所受浮力；/>

和/>

分别表示机械手各臂杆的重心和浮心位置，m_i代表机械手第i个臂杆的质量；

其中，ⁱI_i表示机械手第i个臂杆绕自身质心旋转的转动惯量；ⁱw_i表示机械手第i个臂杆绕自身质心旋转的角速度向量；ⁱv_ci表示机械手第i个臂杆在其质心的线速度向量；

其中，H_Mi表示机械手第i个臂杆运动带来的附加质量力，

ⁱV_ci＝[u_ci v_ci w_ci p_ci q_ci r_ci]表示机械手第i个臂杆在其质心的速度向量；H_Ci＝C_Ai ⁱV_ci表示附加质量带来的哥氏力，/>

为水动力系数；/>

表示粘性水动力；ρ为水的密度；D(x_i)表示机械手第i个臂杆的截面积；C_D(R_n)为粘性水动力的拖曳系数；R_n表示雷诺数；dl_i表示机械手第i个臂杆的长度微元；

步骤6：利用Vega视景仿真软件将得到的输出响应结合测试系统模型和水动力环境模型，计算分析得出测试系统在水中的运动模拟结果及其响应；

步骤7：根据视景仿真软件的模拟结果和响应，以三轴悬臂吊的运动代替测试系统在水中的运动，并控制多关节水下机械手对目标进行抓取，实现测试系统在现实环境下的全实物抓取操作。

2.根据权利要求1所述的一种水下机器人视觉控制目标抓取测试方法，其特征在于：所述步骤3中的运动学方程为：

其中，

为某点在机械手末端坐标系下的坐标；/>

为某点水下视觉在摄像机坐标系下的坐标；/>

为水下视觉摄像机坐标系到水下机器人质心坐标系的旋转矩阵；/>

为水下视觉摄像机坐标系到水下机器人质心坐标系的三维平移向量；ⁱT_i+1为从机械手第i+1个关节到第i个关节的变换矩阵；^vT₁为从水下机器人质心到机械手基关节的变换矩阵；

其中，x₁、y₁和z₁为机械手基关节相对于水下机器人质心坐标系的位移；d_i为关节两法线间的距离；a_i为第i个机械手连杆的长度；α_i为z_i和z_i+1轴之间的夹角；θ_i为两连杆之夹角。

Images