基于多自由度机械臂的水下相机补光灯协同控制方法

1.本发明涉及水下监测技术领域，尤其涉及一种基于多自由度机械臂的水下相机补光灯协同控制方法。


背景技术：

2.基于机器视觉的水下监测方案是研究和开发海洋资源的重要手段之一。随着廉价的紧凑型相机传感器的出现，大规模部署相机传感器已经变为现实，摄像机数量不断增加，逐渐形成网络。传感器网络由一组传感设备组成，这些设备可通过无线通信来协调其动作，并执行诸如在感兴趣的区域上收集数据之类的任务。
3.相比单个摄像头，多个摄像头覆盖的场景有助于处理三个问题：扩大观测范围、处理物体遮挡和最优分辨率。相比于单个相机受限于自身视角范围，多个设置在不同位置的相机可以提供更丰富的视野。单个相机若想实现相同的广角视角拍摄，不仅成本会上升，而且受到视角畸变的影响很严重。此外，单个相机的工作模式大多是定点拍摄，而在合适的位置安装多个相机，能够获得相同目标的不同视角信息，算法可以将视图组合从而获得更完整的场景视图，也对遮挡问题提供了有效的解决途径。最后，多个相机也能让每个待观测对象都分配到一个较近的相机，减少目标到相机的平均距离，由此实现最优分辨率。
4.在水下感知系统中补光灯是非常必要的，因为光线的衰减与空气中相比更为严重。清澈水域中水的透明度只有空气透明度的千分之一，并且不同的波长光线有不同的影响。大量微生物与颗粒物的存在也将导致光在水中传播时发生散射。由于上述种种原因，水下光视觉系统的成像存在图像模糊、背景干扰和噪声等问题，并且在不同光照条件、悬浮物以及水流等因素下，捕获的水下图像颜色失真、整体对比度低。此外，图像采集系统在水下采集目标图像信息的难度会随着水深的增加而增加。
5.然而，目前传统的水下机器人平台上所部署的补光灯与相机通常采用固定安装模式，缺乏控制与调度自由度，难以适应动态多变的水下复杂场景。在一些复杂光线条件以及深海探测情景中，成像质量很大程度上依赖于补光灯对物体的覆盖效果。固定安装的设备很难覆盖到物体的各个方向。对此，本项目拟针对性地开展基于多自由度机械臂的相机-补光灯协同控制及水下应用研究，突破传统固定式视觉监测架构的限制，提升水下监测性能，为推动深海水下探测技术做出贡献。
6.经对现有文献检索发现，较为相似的实现方案为专利cn101739690a，一种多相机协同运动目标检测方法，用于解决现有技术多相机运动目标检测跟踪方法错误率高的技术问题。采用多层平面约束的多相机协同运动目标检测，用多视角冗余信息解决遮挡问题；在不增加相机数目的前提下，用多层平面的约束有效的去除多相机的冗余信息，达到去除虚警的目的，从而降低检测错误率。cn102072706b，一种多相机定位与跟踪方法及系统，多个相机从不同的方位实时连续采集被测物体的图像信号；将被测物在各个相机中不同时刻的成像位置二维坐标通过优化算法进行2d坐标插值计算；在所有相机成像面中的多个二维平面坐标所构成的多条空间异面直线，通过三维坐标定位算法计算被测物的空间三维坐标。
但是这类方案未经水下环境验证，没有考虑到动态配置的相机与补光灯系统。
7.10.27322/d.cnki.gsgyu.2021.001105，生物启发的多水下机器人协同跟踪方法研究，针对水下图像色彩对比度低、清晰度不高及光照不均匀的问题，对水下图像进行预处理并通过区域填充的方法去除倒影，搭建了水下图像采集系统,并通过相机标定矫正了水下相机参数。与此同时，基于yolov3深度学习算法及dsst相关滤波算法对水下目标实时视觉跟踪来感知目标的距离、方向等运动状态信息，并设计了基于模糊pd的水下机器人跟踪控制器。在对水下机器人进行运动学及动力学建模的基础上，基于鱼群行为的生物启发提出了一种多水下机器人协同跟踪编队策略。但是这类方法没有实现对于目标物体的补光、深度感知。
8.张利.基于自适应光学的水下成像技术研究，经过研究学者实验分析发现，提高水下成像图像衬度的一般原则：光源的发光强度要合适，过亮或暗都不利于衬度的提高；应均匀地照明被摄区域，设计合适的光束发散角，尽可能减少对其他区域的照明，减少照明光源的后向散射；应尽量增加人工辅助光源与摄像机的视线的夹角，减少进入摄像机的散射光，提高图像的衬度和成像光束的信噪比；采用多个辅助光源对称分布，可提供对目标物的均匀照明，提高图像的衬度和成像光束的信噪比。此外，除成像相关研究外，目前水下机器人平台上所部署的补光灯与相机通常采用固定安装模式，缺乏控制与调度自由度，难以适应动态多变的水下复杂环境。


技术实现要素：

9.针对上述现有技术中的不足，本发明提供一种基于多自由度机械臂的水下相机补光灯协同控制方法，可以有效地控制水下相机的拍摄角度以及补光灯的补光角度，更高效地控制水下相机补光灯观测系统的运行模式；在物体外形轮廓复杂、水下环境有遮挡、水体运动复杂的情况下都能灵活稳定地控制相机补光灯配置，以最大程度地降低噪声，捕获清晰的水下图像，提高下游任务的准确性。
10.为了实现上述目的，本发明提供一种基于多自由度机械臂的水下相机补光灯协同控制方法，包括步骤：
11.s1：深度测量，通过一水下相机补光灯协同控制装置的双目深度相机采集水下场景图像，使用一多层全连接神经网络学习，拟合水下深度测量结果的偏差，进行水下深度精确测量，实时获得场景精确深度信息；所述水下相机补光灯协同控制装置包括一计算模块以及连接所述计算模块的若干六轴机械臂、若干补光灯、若干三自由度云台和若干双目深度相机；所述补光灯安装于所述六轴机械臂上，所述双目深度相机安装于所述三自由度云台；所述计算模块、所述补光灯和所述双目深度相机外设置有防水结构；
12.s2：水下目标定位，通过实时得到的所述场景精确深度信息，依赖计算机视觉计数识别和分类采集图像中的物体，在多帧图像中跟踪所述物体，精确建模与计算水下所述物体的信息；
13.s3：相机补光灯协同控制，在获得所述物体在相机坐标系中的坐标后，考虑所述物体与所述双目深度相机的三维位姿，在虚拟环境中模拟各个位姿状态下补光灯效果并找到最优补光灯配置，协同控制所述补光灯的所述六轴机械臂。
14.优选地，所述s1步骤中：将水下场景的折射和水流特性刻画为三维空间到三维空
间的映射，并使用所述多层全连接神经网络学习一个r3
→
r3的映射关系，有效拟合水下深度测量的结果的偏差。
15.优选地，所述s1步骤进一步包括步骤：
16.s11：预先标定若干所述水下场景，在不同所述水下场景下，通过双目深度相机测量得到真实的深度值，将所述深度值作为数据集对所述多层全连接神经网络进行训练；
17.s12：使用所述多层全连接神经网络进行深度学习；
18.s13：在工作场景中，正向运行所述多层全连接神经网络，通过矫正的空间得到真实的深度空间，将水上视觉算法迁移到水下。
19.优选地，所述s2步骤中：采用一水下目标定位算法和一水下目标跟踪算法，所述水下目标定位算法依赖计算机视觉计数，通过识别和分类所述双目深度相机采集图像中的所述物体，以及在多帧图像中跟踪所述物体，来精确建模与计算水下所述物体的信息，所述水下目标跟踪算法考虑到了所述水下相机补光灯协同控制装置的实时运动状态和硬件控制裕度，实现控制稳定性的同时满足观测角度需求。
20.优选地，所述s3步骤中：在获得所述物体在相机坐标系中的坐标后，通过一协同控制算法考虑所述物体与所述双目深度相机的三维位姿，在虚拟环境中模拟各个位姿状态下补光灯效果并找到最优补光灯配置；通过加权物体的平均照明亮度、最亮与最暗部的光强比值和背景光线干扰等质量，定义所述物体的照明质量，将此问题转变成一个优化问题进行求解。
21.优选地，所述s3步骤进一步包括步骤：
22.s31：求取所述双目深度相机和所述补光灯的目标姿态；
23.s32：根据所述三自由度云台的机械属性计算所述三自由度云台的各关节角度；
24.s33：所述六轴机械臂协同控制依赖机械臂运动学和动力学求解；
25.s34：多所述六轴机械臂协同控制。
26.优选地，所述补光灯采用led补光灯。
27.本发明由于采用了以上技术方案，使其具有以下有益效果：
28.考虑到一般水体对红外光线的吸收以及对可见光的吸收峰，水下红外激光被吸收衰减较明显，所述深度相机选用双目深度相机。补光灯选用led补光灯，实现高抗压、高强度的水下补光，提高系统鲁棒性，并且有助于降低水下视频和图像的分析难度。计算模块需要能够满足水下实时多目标跟踪与建模的算力要求，同时实现一定的兼容性与可扩展性。六轴机械臂实现了高灵活性、高机动性，为协同控制提供运动平台。
29.与现有技术相比，本发明考虑水下深度测量特点，针对性地选择传感器并设计补偿修正算法；借助计算机视觉领域检测与识别框架，对水下目标识别设计数据集并进行fine-tune，提高算法对于水下物体的感知能力；同时考虑相机与补光灯底座机械臂动力学参数，跟踪控制目标实现相机补光灯联动控制。
附图说明
30.图1为本发明实施例的水下相机补光灯协同控制装置的结构示意图；
31.图2为本发明实施例的防水结构的结构示意图。
具体实施方式
32.下面根据附图图1和图2，给出本发明的较佳实施例，并予以详细描述，使能更好地理解本发明的功能、特点。
33.请参阅图1和图2，本发明实施例的一种基于多自由度机械臂的水下相机补光灯协同控制方法，包括步骤：
34.s1：深度测量，通过一水下相机补光灯协同控制装置的双目深度相机采集水下场景图像，使用一多层全连接神经网络学习，拟合水下深度测量结果的偏差，进行水下深度精确测量，实时获得场景精确深度信息；水下相机补光灯协同控制装置包括一计算模块1以及连接计算模块1的若干六轴机械臂2、若干补光灯3、若干三自由度云台4和若干双目深度相机5；补光灯3安装于六轴机械臂2上，双目深度相机5安装于三自由度云台4；计算模块1、补光灯3和双目深度相机5外设置有防水结构；补光灯3采用led补光灯。
35.s1步骤中：将水下场景的折射和水流特性刻画为三维空间到三维空间的映射，并使用多层全连接神经网络学习一个r3
→
r3的映射关系，有效拟合水下深度测量的结果的偏差。
36.为此，需要特制的水下实验平台以及预先标定的实验设备场景，以采集在不同场景下双目相机测量的深度值，将其作为数据集进行训练，而在工作场景中，正向运行神经网络，通过矫正的空间得到真实的深度空间，就可以将水上视觉算法迁移到水下。
37.s1步骤进一步包括步骤：
38.s11：预先标定若干水下场景，在不同水下场景下，通过双目深度相机5测量得到真实的深度值，将深度值作为数据集对多层全连接神经网络进行训练；
39.s12：使用多层全连接神经网络进行深度学习；假设目标物体的坐标为r'(x',y',z')，经过深度学习算法的修正，其真实坐标为r(x,y,z)。设神经网络的参数为θ，则可以用以下公式表示目标物体坐标经过神经网络映射到真实坐标的过程：r＝nn(r',θ)。其中，nn表示深度学习算法的神经网络模型，θ表示神经网络的参数。通过输入目标物体的坐标r'，神经网络可以输出其真实的三维坐标r。最后，利用采集的数据通过反向传播算法不断更新网络参数，使得误差逐渐减小，从而拟合出一个适用于水下环境的深度学习模型。
40.s13：在工作场景中，正向运行多层全连接神经网络，通过矫正的空间得到真实的深度空间，将水上视觉算法迁移到水下。该深度学习模型能够根据双目相机拍摄的图像，快速准确地计算出水下目标物体的深度信息，从而提高了水下观测和水下作业的精度。
41.s2：水下目标定位，通过实时得到的场景精确深度信息，依赖计算机视觉计数识别和分类采集图像中的物体，在多帧图像中跟踪物体，精确建模与计算水下物体的信息；
42.采用一水下目标定位算法和一水下目标跟踪算法，水下目标定位算法依赖计算机视觉计数，通过识别和分类双目深度相机5采集图像中的物体，以及在多帧图像中跟踪物体，来精确建模与计算水下物体的信息，水下目标跟踪算法考虑到了水下相机补光灯协同控制装置的实时运动状态和硬件控制裕度，实现控制稳定性的同时满足观测角度需求。
43.本步骤中，得到了场景精确深度信息后，应用计算机视觉计数通过识别和分类双目深度相机5采集图像中的物体，以及在多帧图像中跟踪这些物体，来精确建模与计算水下物体的信息。目标跟踪算法则需要考虑到水下机器人的实时运动状态、硬件控制裕度，实现控制稳定性的同时满足各类观测角度需求。可以利用实现稳定精确的水下目标跟踪目标，
解算出目标在像素坐标系中的位置。本发明基于yolo端到端视觉检测算法，对图像中的物体进行实时识别和跟踪。其计算效率高，时间效率高，可以在低功耗硬件上运行。技术方案核心是在一个神经网络上训练检测框，检测框可以将图像中的物体进行分类，并且可以根据物体的大小和位置进行精确定位，依赖双目深度相机5信息可以获得目标物体在三维空间下的位置与姿态。在水下物体识别与跟踪中，本发明算法可以有效地识别水下物体，并且可以对其进行精确定位和跟踪。
[0044][0045]
网络方面主要采用googlenet，卷积层主要用来提取特征，全连接层主要用来预测类别概率和坐标。并且将googlenet中的inception模块换成了1x1卷积后接3x3卷积，最终网络结构由24个卷积层和4个最大池化层和2个全连接层组成。对位置误差，confidence误差，分类误差均使用了均方差作为损失函数，具体如公式所示，第一行表示对坐标中心的预测，第二行表示对宽和高的预测，第三四行表示boundingbox的损失，第五行表示对预测类别的误差。使用水下数据集进行针对性训练，同时进行相应的性能优化，可以得到更优的检测性能。
[0046]
s3：相机补光灯协同控制，在获得物体在相机坐标系中的坐标后，考虑物体与双目深度相机5的三维位姿，在虚拟环境中模拟各个位姿状态下补光灯3效果并找到最优补光灯3配置，协同控制补光灯3的六轴机械臂2。
[0047]
s3步骤中：在获得物体在相机坐标系中的坐标后，通过一协同控制算法考虑物体与双目深度相机5的三维位姿，在虚拟环境中模拟各个位姿状态下补光灯3效果并找到最优补光灯3配置；通过加权物体的平均照明亮度、最亮与最暗部的光强比值和背景光线干扰等质量，定义物体的照明质量，将此问题转变成一个优化问题进行求解。
[0048]
s3步骤进一步包括步骤：
[0049]
s31：求取双目深度相机5和补光灯3的目标姿态；通过加权物体的平均照明亮度、最亮与最暗部的光强比值、背景光线干扰等质量，可以定义目标物体的照明质量，由此将确定最优补光灯3配置的问题转变成一个优化问题进行求解，求取双目深度相机5、补光灯3的目标姿态；
[0050]
s32：根据三自由度云台4的机械属性计算三自由度云台4的各关节角度；此时需要将云台转变成多连杆结构，使用机械臂d-h坐标系方法对其进行刻画。设双目深度相机5和
机械臂坐标系之间的变换矩阵为t，则可以用以下公式将目标物体从相机坐标系转换到机械臂坐标系：
[0051]
pm＝t*pc
[0052]
其中，pm表示目标物体在机械臂坐标系中的坐标。
[0053]
s33：六轴机械臂2协同控制依赖机械臂运动学和动力学求解；
[0054]
s34：多六轴机械臂2协同控制。同时控制多个机械臂，包括双目深度相机5与补光灯3，这不仅要求云台硬件的带负载能力、响应速度，还对控制算法稳定性提出了要求。
[0055]
在获得双目深度相机5、补光灯3的目标姿态后，需要根据各云台的机械属性计算各关节角度。此时需要将云台转变成多连杆结构，使用机械臂d-h坐标系方法对其进行刻画。设双目深度相机5和机械臂坐标系之间的变换矩阵为t，则可以用以下公式将目标物体从相机坐标系转换到机械臂坐标系：
[0056]
pm＝t*pc
[0057]
其中，pm表示目标物体在机械臂坐标系中的坐标。
[0058]
补光机械臂协同控制依赖机械臂运动学、动力学求解。求解机器人运动学的方法有许多种，如闭环矢量和d-h法等。其中d-h是一种流行且通用的方法，适用于操作臂各连杆之间位移关系的研究。由运动学的观点来看，杆件的作用仅在于它能保持其两端关节间的形态不变。这种形态由两个参数决定，一是杆件的长度，一个是杆件的扭转角。机械臂的正向运动学的解是唯一确定的，通过d-h坐标法就可以比较容易得到正向运动学方程，但是逆运动学却往往具有多重解，也可能不存在解。对于各种计算方法的计算效率，计算精度均有较多要求，主要分为封闭解与数值解法两大类。由于一般机械手臂的几何构性不满足封闭解要求，本项目拟采用数值解进行逆运动学计算。设机械臂的d-h变换矩阵为a，则可以用以下公式计算机械臂的运动学逆解：
[0059]
q＝inv(a)*pm
[0060]
其中，q表示机械臂各个关节的角度，inv(a)表示a矩阵的逆矩阵。机械臂的d-h变换矩阵描述了机械臂各个关节的运动学特性，它与各个关节的角度有关。
[0061]
在已知机械臂各个关节的角度、负载质量和惯量的基础上，可以通过求解机械臂的动力学方程来计算关节电机的扭矩。机械臂的动力学方程可以表示为：
[0062]
τ＝m(q)*q”+c(q,q')+g(q)+f(q,q')
[0063]
其中，τ表示关节电机的扭矩，q表示机械臂各个关节的角度，q'和q”分别表示关节角速度和角加速度，m(q)表示关节质量矩阵，c(q,q')表示离心力矩和科里奥利力矩，g(q)表示重力矩，f(q,q')表示摩擦力矩。通过求解机械臂的动力学方程，可以得到关节电机的扭矩，进而实现对机械臂的精确控制。求解动力学方程的过程中，这些参数可以通过机械臂的结构和各部件的参数计算得到。
[0064]
最后，还需要同时控制多个机械臂，包括双目深度相机5与补光灯3，这不仅要求云台硬件的带负载能力、响应速度，还对控制算法稳定性提出了要求。项目将分别采用状态反馈与输出反馈两种控制策略来对多机械臂系统模型进行控制，采用障碍李雅普诺夫函数来保证输出受限，由此实现多机械臂协同控制。障碍李雅普诺夫函数是一种能够保证系统输出受限的控制方法。其基本思想是将系统输出的限制条件作为障碍，设计合适的李雅普诺夫函数来实现输出受限控制。在多机械臂协同控制问题中，需要保证各个机械臂的运动不
会发生冲突，同时要保证系统输出的范围不超过设定的限制条件。为此，可以采用如下障碍李雅普诺夫函数：
[0065]
h(q,q',y)＝1/2*(y-y_max)^2+1/2*a(q,q')^2
[0066]
其中，y表示系统输出，y_max表示输出的最大限制值，a(q,q')表示系统状态的一阶导数。障碍李雅普诺夫函数的控制策略是将李雅普诺夫函数作为系统控制器的目标函数，通过对李雅普诺夫函数进行最小化来实现控制。在实际应用中，可以采用优化算法(如梯度下降算法或牛顿迭代算法)来求解最优控制。
[0067]
以上结合附图实施例对本发明进行了详细说明，本领域中普通技术人员可根据上述说明对本发明做出种种变化例。因而，实施例中的某些细节不应构成对本发明的限定，本发明将以所附权利要求书界定的范围作为本发明的保护范围。

技术特征：
1.一种基于多自由度机械臂的水下相机补光灯协同控制方法，包括步骤：s1：深度测量，通过一水下相机补光灯协同控制装置的双目深度相机采集水下场景图像，使用一多层全连接神经网络学习，拟合水下深度测量结果的偏差，进行水下深度精确测量，实时获得场景精确深度信息；所述水下相机补光灯协同控制装置包括一计算模块以及连接所述计算模块的若干六轴机械臂、若干补光灯、若干三自由度云台和若干双目深度相机；所述补光灯安装于所述六轴机械臂上，所述双目深度相机安装于所述三自由度云台；所述计算模块、所述补光灯和所述双目深度相机外设置有防水结构；s2：水下目标定位，通过实时得到的所述场景精确深度信息，依赖计算机视觉计数识别和分类采集图像中的物体，在多帧图像中跟踪所述物体，精确建模与计算水下所述物体的信息；s3：相机补光灯协同控制，在获得所述物体在相机坐标系中的坐标后，考虑所述物体与所述双目深度相机的三维位姿，在虚拟环境中模拟各个位姿状态下补光灯效果并找到最优补光灯配置，协同控制所述补光灯的所述六轴机械臂。2.根据权利要求1所述的基于多自由度机械臂的水下相机补光灯协同控制方法，其特征在于，所述s1步骤中：将水下场景的折射和水流特性刻画为三维空间到三维空间的映射，并使用所述多层全连接神经网络学习一个r3
→
r3的映射关系，有效拟合水下深度测量的结果的偏差。3.根据权利要求2所述的基于多自由度机械臂的水下相机补光灯协同控制方法，其特征在于，所述s1步骤进一步包括步骤：s11：预先标定若干所述水下场景，在不同所述水下场景下，通过双目深度相机测量得到真实的深度值，将所述深度值作为数据集对所述多层全连接神经网络进行训练；s12：使用所述多层全连接神经网络进行深度学习；s13：在工作场景中，正向运行所述多层全连接神经网络，通过矫正的空间得到真实的深度空间，将水上视觉算法迁移到水下。4.根据权利要求1所述的基于多自由度机械臂的水下相机补光灯协同控制方法，其特征在于，所述s2步骤中：采用一水下目标定位算法和一水下目标跟踪算法，所述水下目标定位算法依赖计算机视觉计数，通过识别和分类所述双目深度相机采集图像中的所述物体，以及在多帧图像中跟踪所述物体，来精确建模与计算水下所述物体的信息，所述水下目标跟踪算法考虑到了所述水下相机补光灯协同控制装置的实时运动状态和硬件控制裕度，实现控制稳定性的同时满足观测角度需求。5.根据权利要求1所述的基于多自由度机械臂的水下相机补光灯协同控制方法，其特征在于，所述s3步骤中：在获得所述物体在相机坐标系中的坐标后，通过一协同控制算法考虑所述物体与所述双目深度相机的三维位姿，在虚拟环境中模拟各个位姿状态下补光灯效果并找到最优补光灯配置；通过加权物体的平均照明亮度、最亮与最暗部的光强比值和背景光线干扰等质量，定义所述物体的照明质量，将此问题转变成一个优化问题进行求解。6.根据权利要求5所述的基于多自由度机械臂的水下相机补光灯协同控制方法，其特征在于，所述s3步骤进一步包括步骤：s31：求取所述双目深度相机和所述补光灯的目标姿态；s32：根据所述三自由度云台的机械属性计算所述三自由度云台的各关节角度；
s33：所述六轴机械臂协同控制依赖机械臂运动学和动力学求解；s34：多所述六轴机械臂协同控制。7.根据权利要求1所述的基于多自由度机械臂的水下相机补光灯协同控制方法，其特征在于，所述补光灯采用led补光灯。

技术总结
本发明提供一种基于多自由度机械臂的水下相机补光灯协同控制方法，包括步骤：S1：通过双目深度相机采集水下场景图像，使用多层全连接神经网络学习，进行水下深度精确测量；S2：通过实时得到的场景精确深度信息，依赖计算机视觉计数识别和分类采集图像中的物体，精确建模与计算水下物体的信息；S3：在获得物体在相机坐标系中的坐标后，协同控制补光灯的六轴机械臂。本发明的一种基于多自由度机械臂的水下相机补光灯协同控制方法，在物体外形轮廓复杂、水下环境有遮挡、水体运动复杂的情况下都能灵活稳定地控制相机补光灯配置，以最大程度地降低噪声，捕获清晰的水下图像，提高下游任务的准确性。准确性。准确性。


技术研发人员：林志良 张学超 黄侃 郑尧坤 徐继晟 何建平
受保护的技术使用者：上海交通大学
技术研发日：2023.04.13
技术公布日：2023/7/12