CN115125923B - 一种小型水域垃圾清理机器人 - Google Patents

Abstract

本发明公开了一种小型水域垃圾清理机器人，包括船体、设置在船体后方的推进器、设置在船体正前方的激光雷达、设置在船体上部中央位置的垃圾收集箱、设置在船体头部位置的机舱、固定在机舱上端的云台以及固定在机舱外侧的至少一个机械臂；机舱的内部设有机器人控制器和电源模块，云台上设有摄像头；机器人控制器包含船体子系统、图像识别子系统和机械臂子系统；船体子系统用于实现船体在水域中自主巡航的功能；图像识别子系统用于获取机械臂坐标系下目标垃圾的3D坐标信息；机械臂子系统用于实时控制机械臂运动，夹持网兜捞取水面垃圾至垃圾收集箱。利用本发明，节约了垃圾清理成本，且不会产生噪声，解决了在小型水域中水面垃圾清理的问题。

Description

一种小型水域垃圾清理机器人

技术领域

本发明属于水上垃圾清理技术领域，尤其是涉及一种小型水域垃圾清理机器人。

背景技术

小型水域分布在城市的很多角落，是城市市容市貌的重要部分。随着社会经济的快速发展，水资源受到的威胁日益严重，越来越多的水面垃圾是污染问题的重要体现。水面垃圾不仅会对水体生态造成严重威胁，破坏生态景观，而且由于其往往大量聚集，难以清除，形成阻塞以至于影响河道、城市小型水坝的正常运行，给居民正常工作与生活带来困扰。

对于水面垃圾的打捞，国内外目前普遍采用人力方式。人力打捞虽然操作灵活到位，但是劳动强度大、效率低、成本高，还有一定的安全隐患。针对人力打捞的种种缺陷，近年来各种自动化清理船被制造并投入使用，它们通常以双体船作船体，体型较大，以格栅滚筒作为清理机构，船体的收集、存储、卸载系统全部采用液压驱动，同时采用全局路径优化和局部路径追踪技术，具备在广阔河道中的自主巡航能力。这些自动化清理船虽然具有高效的水面清理效果，但会产生一定程度的空气和噪音污染，主要应用于远离居民区的大型水域。

如公开号为CN104986300A的中国专利文献公开了一种水面垃圾清理船，包括船体，所述船体上固定设置有垃圾收集装置和驱动装置，所述船体下方两侧固定设置有两个浮箱，两个浮箱之间形成水流通过区域；所述垃圾收集装置包括从动轴、主动轴和垃圾输送滤带，所述从动轴通过轴承活动设置船体下方，所述主动轴通过轴承设置在船体上，且所述主动轴与驱动装置连接，所述垃圾输送滤带通过主动轴和从动轴倾斜设置在船体上

公开号为CN111038654A的中国专利文献公开了一种垃圾清理船，包括双体船船体，所述双体船船体的船底两侧设有浮体，两所述浮体之间形成清理区；所述双体船船体内设有安装腔和置料腔，所述安装腔内设有输送装置和粉碎装置，所述输送装置包括输送带，所述安装腔的底部设有开口，所述输送带穿过所述开口并延伸至所述清理区内，所述粉碎装置安装在所述输送带的上端出料端，所述粉碎装置的下端出料端设有螺旋输送机，所述螺旋输送机的出料口位于所述置料腔内，所述安装腔的顶部开设有检修孔，所述检修孔上设有检修门，所述置料腔的顶部设有排料口，所述排料口上设有门。

与大型水域不同，小型水域通常深度浅、面积小，靠近居民区。更有一些景观水域为了美观设计的需要，专门规划成外观不规则的形状。在这些小型水域中，大型自动化清理船难以灵活移动和转向，降低了工作效率，巨大的噪声也会影响周边居民的生活。因此，一款专门用于小型水域的垃圾清理机器人，具有很广阔的应用前景。

发明内容

本发明提供了一种小型水域垃圾清理机器人，能够实现在小型水域自主巡航，在利用机器视觉识别到水面垃圾后，近侧船载机械臂夹持末端网兜实时捞取目标垃圾，并置于船载的垃圾箱中。

一种小型水域垃圾清理机器人，包括带有姿态传感器IMU的船体、设置在船体后方的推进器、设置在船体正前方的激光雷达、设置在船体上部中央位置的垃圾收集箱、设置在船体头部位置的机舱、固定在机舱上端的云台以及固定在机舱外侧的至少一个机械臂；

所述机舱的内部设有机器人控制器和电源模块，所述云台上设有摄像头，所述机械臂的末端设有用于夹持网兜的夹具；

所述的机器人控制器包含船体子系统、图像识别子系统和机械臂子系统；所述的船体子系统融合激光雷达和IMU收集的数据，实现船体在水域中自主巡航的功能；所述的图像识别子系统用于获取机械臂坐标系下目标垃圾的3D坐标信息；所述的机械臂子系统用于根据图像识别子系统反馈的坐标信息，实时控制机械臂运动，夹持网兜捞取水面垃圾至垃圾收集箱。

可选择地，机械臂可以为一个或两个，当机械臂设有两个时，通过机械臂底座分别对称地固定在机舱两侧。

优选地，所述的机械臂采用具有六个自由度的六轴机械臂，所述六轴机械臂的六个关节上分别设有一个电机。

优选地，所述船体周围安装有四组对称提手，便于进行人工搬运；所述船体底部安装四组支架，用于机器人在陆地上的放置。

所述图像识别子系统获取机械臂坐标系下目标垃圾的3D坐标信息过程如下：

收集足量的垃圾数据集，选择YOLOv5作为水面目标检测算法，并利用垃圾数据集对算法进行预训练以提高算法对环境的抗干扰能力；在识别目标垃圾过程中，用2D+1D的方式获取目标垃圾在相机坐标系下的3D坐标和包含目标表面信息的点云信息，并将目标垃圾的3D坐标转移至机械臂坐标系下。

所述的机械臂子系统使用基于改进GWLN的控制算法和姿态定向算法来对机械臂进行控制，在实现水面垃圾打捞的主任务前提下，满足包含关节角度限位、空间避障在内的多个子任务。

使用基于改进GWLN的控制算法对机械臂进行控制时，具体为：

机械臂的典型次任务约束包括关节角度限位、空间避障，机械臂的一般次任务约束则描述为

H_i(q)≥h_i (1)

其中，H_i(q)∈R是次任务的性能函数，h_i对应阈值；为使一般次任务约束具有和关节角度限位相同的处理方式，引入变换矩阵

式中，I_n表示n维单位矩阵，H_g是任意非实关节限位约束，是其梯度向量，由此定义虚拟关节

由式(3)、(4)得到，映射后关节速度分为两部分，其中为次任务约束关于时间的导数，对应虚拟关节，另一部分/>为实际关节速度，对应实际关节；/>表示关节速度，由于变换矩阵T非方阵，故定义其矩阵的逆为

T^#＝(T^TWT)^-1T^TW (4)

其中，W为对角权重矩阵，对角线元素对应各个关节；设第i个关节的性能函数为H_i，阈值为h_i，安全裕量为∈_i，则定义归一化距离为

而对于同时具有上下限约束的次任务，同样定义阈值上限h_g+、下限h_g-，及对应的安全裕量∈_i+、∈_i-，相应d_i定义为

基于式(5)和(6)，进一步构造权值因子w_i，考虑到次任务约束分为两类：虚拟关节约束和实际关节约束，实际关节约束是各个实关节的角度限制，而虚拟关节约束是其他次任务映射为虚拟关节后的等效角度限制，由式(6)映射知，对于实际关节约束，当关节角度满足安全裕量时，应取常值1，而对于虚拟关节约束，则应取常值0，故构造权值因子为

其中，s是预设参数，当控制周期较长时，取较大s值，以免由于离散指令而不满足阈值约束；式中f(i)用于区分虚拟关节约束和实际关节约束，定义为

由式(3)、(4)得虚拟关节对应的雅各比矩阵为

J_v＝JT^#(q) (9)结合PD控制和GWLN方法，最终得到逆运动学控制律为

W_v＝T^#·W^-1·(T^#)^T (10)

在机械臂运动过程中，当次任务性能函数H_i接近阈值时，对应式(5)或(6)的归一化距离趋向于0，式(7)对应对角线位置的权重因子快速变大，迫使虚拟关节速度下降，即对应次任务性能函数导数/>趋向于0，从而保证次任务约束；而当H_i远离阈值时，权重因子不起作用，避免机械臂的无效自运动。

使用姿态定向算法来对机械臂进行控制时，网兜平面关于法向量的旋转不影响捞取结果，不需要对机械臂末端的姿态进行完全控制，仅对空间姿态的某一轴方向进行控制，具体为：

从旋转矩阵角度来看，任意三维空间姿态均可表示为

其中，^Ax_B、^Ay_B、^Az_B列矢量分别表示坐标系{B}的单位坐标轴矢量在坐标系{A}中的投影；

设当前姿态旋转矩阵为期望姿态旋转矩阵为/>则期望姿态在当前姿态相应坐标系{0}下表示为

式中，三个列矢量是期望姿态相应坐标系{d}中的X、Y、Z轴单位向量，在当前姿态相应坐标系中{c}的投影；对于X轴定向、Y轴定向、Z轴定向，分别对应要求⁰x_c ^T·⁰x_d＝1、⁰y_c ^T·⁰y_d＝1、⁰z_c ^T·⁰z_d＝1；

进一步考虑用四元数表示定向姿态间的偏差，以关于X轴定向⁰x_c ^T·⁰x_d＝1为例，设则由四元数与旋转矩阵对应关系，得到联立方程

解得

即X轴定向仅要求四元数偏差中/>类似地，分别带入旋转矩阵Y、Z轴定向条件，对应解得Y轴定向对应四元数条件/> Z轴定向对应四元数条件/>

除姿态偏差项外，定向任务的运动学控制方程也需对应修改；以X轴定向为例，则对应机械臂运动控制描述为

其中J_p为雅各比矩阵前三行，J_oy、J_oz为雅各比矩阵第五、六个行矢量；由于六自由度机械臂应用于五维任务，故等效为冗余机械臂，即使在单个关节陷入关节限位时，机械臂仍能保证主任务的精度。

与现有技术相比，本发明具有以下有益效果：

1.本发明的小型水域垃圾清理机器人整体体积小、质量轻、运动灵活，非常适于在小型水域进行垃圾清理工作且不会产生巨大噪声，同时方便人工搬运和陆上放置。

2、本发明把机械臂作为垃圾清理机构，打捞过程更加灵活多变，可在复杂环境中排除干扰完成打捞任务。

3、本发明装有多种传感器配合控制系统可以自主调整巡航路径，检测水面垃圾并进行捞取，更趋近于智能化和无人化，可以减少人工劳动量，节约人工成本。

4、本发明的机械臂控制采用了基于GWLN的机械臂实时控制算法和姿态定向算法，将机械臂转化至冗余机械臂，在完成水面垃圾打捞主任务的前提下，还能满足关节角度限位、空间避障等次任务。

附图说明

图1为本发明一种小型水域垃圾清理机器人的正视结构图；

图2为本发明一种小型水域垃圾清理机器人的俯视结构图；

图3为本发明一种小型水域垃圾清理机器人的侧视结构图；

图4为本发明中机械臂的结构示意图；

图5为本发明中采用一个机械臂的小型水域垃圾清理机器人示意图；

图6为本发明的控制框图。

图中：1、船体；2、推进器；3、框体；4、机械臂；5、网兜；6、激光雷达；7、摄像头；8、云台；9、提手；10、支架；11、机舱；12、第一连接轴；13、第二连接轴；14、机械臂底座；15、一轴电机；16、二轴电机；17、三轴电机；18、四轴电机；19、五轴电机；20、六轴电机；21、机械臂夹具。

具体实施方式

下面结合附图和实施例对本发明做进一步详细描述，需要指出的是，以下所述实施例旨在便于对本发明的理解，而对其不起任何限定作用。

如图1～4所示，一种小型水域垃圾清理机器人，包括船体1、推进器2、框体3、机械臂4、网兜5、激光雷达6、摄像头7、云台8、提手9、支架10、机舱11、第一连接轴12、第二连接轴13、机械臂底座14、一轴电机15、二轴电机16、三轴电机17、四轴电机18、五轴电机19、六轴电机20、机械臂夹具21。机器人的身体整体近似人形，呈现模拟人工打捞的姿态。船体1为单体船，体积约为90cm*130cm*40cm，和常见的自动化清理船相比体积较小，适于在小型水域灵活运动，便于进行人工搬运，船身形状采用流线型的边界，非常美观。两个推进器2对称地位于船体后方作为船体的动力装置；机舱11通过第一连接轴12与船体固连，机舱内置机器人控制器和供电设备，机舱后部设有机舱门，机舱门为单开门式结构，便于技术人员在所述机器人的调试和使用时完成控制器和供电设备等机舱内设备的拆卸与安装；框体3为放置垃圾收集箱的装置，其空间大小约为70cm*70cm*30cm，位于船体中央，垃圾收集箱可以拆卸和替换，以便于对打捞的垃圾进行后续处理。

机械臂4是六轴机械臂，具有六个自由度。机械臂4通过机械臂底座14分别对称地固定在机舱两侧，机械臂的六个关节为六个电机，均选用步进电机驱动，具有力矩大、精度高的优点，同时在复杂环境下能够保证机械臂的位置和跟踪精度，六台电机相互配合，通过设定不同的关节角度，机械臂4的末端可以处在工作空间内的任意位置，呈现任意姿态。机械臂末端安置机械臂夹具21可以夹取网兜5打捞水面垃圾，并将目标垃圾置于位于船体中央框体3位置的垃圾收集箱中。网兜5可以根据实际工作的需要选择不同的尺寸，轻松完成目标垃圾的打捞任务并且进行过滤。

机械臂使用姿态定向算法，减少了末端所需的一个姿态方向上的自由度，此时机械臂关节数大于工作空间维数，此机械臂成为冗余机械臂，冗余机械臂对于带约束的运动控制具备更好的性能；除此之外，选择了基于改进GWLN的控制算法和姿态定向算法来对所述机械臂进行控制，以上控制算法能够在实现水面垃圾打捞的主任务的前提下，满足包括关节角度限位、空间避障等在内的多个子任务。

摄像头7选择双目摄像头，位于机器人头部，安装在云台8上，检测机器人周边的水面垃圾；云台8可以在偏航角的方向上自由转动，扩大了所述摄像头检测的范围，云台通过第二连接轴13与机舱固连。机器人的视觉方案包含以下部分：一、收集足量的垃圾数据集，数据集中的图片来自所述机器人在实际作业中收集所得和网络收集；二、选择YOLOv5作为水面目标检测算法，并利用垃圾数据集对算法进行预训练以提高算法对环境的抗干扰能力；三、用2D+1D的方式获取目标物在相机坐标系下的3D坐标和包含目标表面信息的点云信息；四、将目标物的3D坐标转移至机械臂坐标系下。经过上述步骤，机器人可以获得机械臂坐标系下目标垃圾的3D坐标信息，继而通过图像伺服技术和机械臂配合，实时调整机械臂规划的运动路径，完成水面垃圾的打捞。

激光雷达6位于船体正前方，用于检测机器人的周围环境信息来调整自主巡航的轨迹。IMU是所述机器人的姿态传感器，可以实时反馈所述机器人当前姿态给控制器。

船体1外侧安装有四组对称提手9，便于进行人工搬运，船体底部安装四组放置支架10，用于机器人在陆地上的放置并保持其平衡与稳定。

机舱11内部包含控制组件，控制组件用于控制小型水域垃圾清理机器人的运行，控制组件包括船体控制子系统、机械臂控制子系统、图像识别子系统。控制组件分别连接各自移动组件，可以分别控制机器人船体在水中的运动、进行机械臂的运动规划和控制、检测水面垃圾并反馈其位置信息。

机舱11内部还包含供电组件，供电组件连接控制组件、推进器、机械臂等用电设备。供电组件包括电池和电压转换模块，在本实施例中，使用24V锂电池进行供电，为机器人的正常运行提供动力。

如图5所示，在具体应用过程中，机械臂4也可以只设置一个，即只在机舱11的其中一侧安装机械臂4。

如图6所示，本发明的工作原理如下：首先，预先在规划器中设定机器人在水域中的巡航轨迹，在自主巡航过程中，船体控制子系统融合激光雷达和IMU收集的传感数据，利用无人船轨迹跟踪控制算法实时调整巡航轨迹，并结合扰动观测器观测的信号，控制船尾的推进器2转动，以实现机器人在水中的前进、后退、转向、调速等运动目标，实现机器人在水域中自主巡航的功能。此外，在机器人巡航过程中，当摄像头检测到水面垃圾时，船体运动至适当位置，此时机械臂子系统根据摄像头反馈的图像信息，利用图像伺服技术于规划器中实时生成机械臂运动轨迹，并控制靠近目标垃圾一侧的六轴机械臂运动，夹持网兜捞取水面垃圾并置于船载垃圾箱中。完成垃圾捞取任务后机械臂恢复至初始姿态，船体继续前进。

机械臂的控制使用了基于改进GWLN的机械臂运动控制算法和姿态定向算法。前者可以在完成轨迹跟踪的主任务的前提下，满足给定的多个任务约束；后者减少了工作空间上的一个自由度，机械臂关节数大于工作空间维度时，使机械臂成为冗余机械臂，使带约束的运动控制具备更好的性能。接下来对两种算法进行详述。

(一)基于改进GWLN的机械臂运动控制

机械臂的运动学控制，通常要求在以笛卡尔空间轨迹跟踪为主任务的同时，满足给定次任务约束。典型的次任务约束包括关节角度限位、空间避障等，对于一般的机械臂次任务，通常可描述为

H_i(q)≥h_i (1)

其中H_i(q)∈R是次任务的性能函数，h_i对应阈值。为使一般次任务约束具有和关节角度限位相同的处理方式，引入变换矩阵

式中H_g是任意非实关节限位约束，是其梯度向量，由此定义虚拟关节

由式(3)、(4)易得，映射后关节速度可分为两部分，其中为次任务约束关于时间的导数，对应“虚拟关节”，另一部分/>为实际关节速度，对应“实际关节”。由于变换矩阵T非方阵，故定义其逆为

T^#＝(T^TWT)^-1T^TW (4)

其中W为对角权重矩阵，对角线元素为下述各个关节。设第i个关节的性能函数为H_i，阈值为h_i，安全裕量为∈_i，则定义归一化距离为

而对于同时具有上下限约束的次任务，同样可定义阈值上限h_g+、下限h_g-、及对应的安全裕量∈_i+、∈_i-，相应d_i定义为

基于式(5)和(6)，进一步构造权值因子w_i，考虑到次任务约束可分为两类：虚拟关节约束和实际关节约束，实际关节约束是各个实关节的角度限制，而虚拟关节约束是其他次任务映射为虚拟关节后的等效角度限制，由式(6)映射可知，对于实际关节约束，当关节角度满足安全裕量时，应取常值1，而对于虚拟关节约束，则应取常值0，故构造权值因子为

其中s是预设参数，当控制周期较长时，可取较大s值，以免由于离散指令而不满足阈值约束。式中f(i)用于区分虚拟关节约束和实际关节约束，定义为

由式(3)、(4)可得虚拟关节对应的雅各比矩阵为

J_v＝JT^#(q) (9)结合PD控制和GWLN方法，最终可得逆运动学控制律为

W_v＝T^#·W^-1·(T^#)^T (10)

在机械臂运动过程中，当次任务性能函数H_i接近阈值时，对应式(5)或(6)的归一化距离趋向于0，式(7)对应对角线位置的权重因子快速变大，迫使虚拟关节速度下降，即对应次任务性能函数导数/>趋向于0，从而保证了次任务约束。而当H_i远离阈值时，权重因子不起作用，避免了机械臂的无效自运动。

(二)姿态定向算法

三维空间姿态的完全确定通常需要三个独立参数，但机械臂夹持网杆捞取水面垃圾时，网兜平面关于法向量的旋转并不影响捞取结果，并不需要对机械臂末端的姿态进行完全控制。故引入姿态定向概念，仅对空间姿态的某一轴方向进行控制。

从旋转矩阵角度来看，任意三维空间姿态均可表示为

其中^Ax_B、^Ay_B、^Az_B列矢量分别表示坐标系{B}的单位坐标轴矢量在坐标系{A}中的投影，由特殊正交群SO(3)定义可得，任意两个列矢量的确定均可得到唯一旋转矩阵。而姿态定向，是指仅对某个轴的指向，即式(12)中等式右侧某个列矢量进行控制，而不进行指定该轴旋转。以关于X轴定向为例，则仅需对列矢量^Ax_B即X轴方向进行控制，如图6所示。

设当前姿态旋转矩阵为期望姿态旋转矩阵为/>则期望姿态在当前姿态相应坐标系{0}下可表示为

式中三个列矢量是期望姿态相应坐标系{d}中的X、Y、Z轴单位向量，在当前姿态相应坐标系中{c}的投影。对于常见的姿态定位，要求当前位姿与期望位姿的坐标轴对应重合，即式(13)所得为单位阵，而对于姿态定向，则仅需使设定的定向轴与期望重合。具体而言，对于X轴定向、Y轴定向、Z轴定向，分别对应要求⁰x_c ^T·⁰x_d＝1、⁰y_c ^T·⁰y_d＝1、⁰z_c ^T·⁰z_d＝1。进一步考虑用四元数表示定向姿态间的偏差，以关于X轴定向⁰x_c ^T·⁰x_d＝1为例，设则由四元数与旋转矩阵对应关系，可得联立方程

易解得

即X轴定向仅要求四元数偏差中/>类似地，分别带入旋转矩阵Y、Z轴定向条件，可对应解得Y轴定向对应四元数条件/> Z轴定向对应四元数条件/>

除姿态偏差项外，定向任务的运动学控制方程也需对应修改。一方面，由于定向任务通常是针对机械臂末端或夹持的工件，因而机械臂运动学方程中姿态相关部分需转换至末端坐标系{e}中，另一方面，关于定向轴的旋转不进行控制，则任务维度相应减少一维。以X轴定向为例，则对应机械臂运动控制可描述为

其中J_p为雅各比矩阵前三行，J_oy、J_oz为雅各比矩阵第五、六个行矢量。由式(16)可知，姿态定向使机械臂主任务维度减少1维，从而使机械臂带约束运动控制具备更好的性能。对于本方案，由于六自由度机械臂应用于五维任务，故可等效为冗余机械臂，即使在单个关节陷入关节限位时，机械臂仍能保证主任务的精度。

以上所述的实施例对本发明的技术方案和有益效果进行了详细说明，应理解的是以上所述仅为本发明的具体实施例，并不用于限制本发明，凡在本发明的原则范围内所做的任何修改、补充和等同替换，均应包含在本发明的保护范围之内。

Claims (6)

1.一种小型水域垃圾清理机器人，其特征在于，包括带有姿态传感器IMU的船体、设置在船体后方的推进器、设置在船体正前方的激光雷达、设置在船体上部中央位置的垃圾收集箱、设置在船体头部位置的机舱、固定在机舱上端的云台以及固定在机舱外侧的至少一个机械臂；

所述机舱的内部设有机器人控制器和电源模块，所述云台上设有摄像头，所述机械臂的末端设有用于夹持网兜的夹具；

所述的机器人控制器包含船体子系统、图像识别子系统和机械臂子系统；所述的船体子系统融合激光雷达和IMU收集的数据，实现船体在水域中自主巡航的功能；所述的图像识别子系统用于获取机械臂坐标系下目标垃圾的3D坐标信息；所述的机械臂子系统用于根据图像识别子系统反馈的坐标信息，实时控制机械臂运动，夹持网兜捞取水面垃圾至垃圾收集箱；

所述的机械臂子系统使用基于改进GWLN的控制算法和姿态定向算法来对机械臂进行控制，在实现水面垃圾打捞的主任务前提下，满足包含关节角度限位、空间避障在内的多个子任务；

使用基于改进GWLN的控制算法对机械臂进行控制时，具体为：

机械臂的典型次任务约束包括关节角度限位、空间避障，机械臂的一般次任务约束则描述为H_i(q)≥h_i(1)

其中，H_i(q)∈R是次任务的性能函数，h_i对应阈值；为使一般次任务约束具有和关节角度限位相同的处理方式，引入变换矩阵

式中，I_n表示n维单位矩阵，H_g是任意非实关节限位约束，是其梯度向量，由此定义虚拟关节

由式(3)、(4)得到，映射后关节速度分为两部分，其中为次任务约束关于时间的导数，对应虚拟关节，另一部分/>为实际关节速度，对应实际关节；/>表示关节速度，由于变换矩阵T非方阵，故定义其矩阵的逆为

T^#＝(T^TWT)^-1T^TW (4)

其中，W为对角权重矩阵，对角线元素对应各个关节；设第i个关节的性能函数为H_i，阈值为h_i，安全裕量为∈_i，则定义归一化距离为

而对于同时具有上下限约束的次任务，同样定义阈值上限h_g+、下限h_g-，及对应的安全裕量∈_i+、∈_i-，相应d_i定义为

基于式(5)和(6)，进一步构造权值因子w_i，考虑到次任务约束分为两类：虚拟关节约束和实际关节约束，实际关节约束是各个实关节的角度限制，而虚拟关节约束是其他次任务映射为虚拟关节后的等效角度限制，由式(6)映射知，对于实际关节约束，当关节角度满足安全裕量时，应取常值1，而对于虚拟关节约束，则应取常值0，故构造权值因子为

其中，s是预设参数，当控制周期较长时，取较大s值，以免由于离散指令而不满足阈值约束；式中f(i)用于区分虚拟关节约束和实际关节约束，定义为

由式(3)、(4)得虚拟关节对应的雅各比矩阵为J_v＝JT^#(q)(9)

结合PD控制和GWLN方法，最终得到逆运动学控制律为W_v＝T^#·W^-1·(T^#)^T(10)

在机械臂运动过程中，当次任务性能函数H_i接近阈值时，对应式(5)或(6)的归一化距离趋向于0，式(7)对应对角线位置的权重因子快速变大，迫使虚拟关节速度下降，即对应次任务性能函数导数/>趋向于0，从而保证次任务约束；而当H_i远离阈值时，权重因子不起作用，避免机械臂的无效自运动。

2.根据权利要求1所述的小型水域垃圾清理机器人，其特征在于，所述的机械臂设有两个，通过机械臂底座分别对称地固定在机舱两侧。

3.根据权利要求1所述的小型水域垃圾清理机器人，其特征在于，所述的机械臂采用具有六个自由度的六轴机械臂，所述六轴机械臂的六个关节上分别设有一个电机。

4.根据权利要求1所述的小型水域垃圾清理机器人，其特征在于，所述船体周围安装有四组对称提手，所述船体底部安装四组支架。

5.根据权利要求1所述的小型水域垃圾清理机器人，其特征在于，所述图像识别子系统获取机械臂坐标系下目标垃圾的3D坐标信息过程如下：

收集足量的垃圾数据集，选择YOLOv5作为水面目标检测算法，并利用垃圾数据集对算法进行预训练以提高算法对环境的抗干扰能力；在识别目标垃圾过程中，用2D+1D的方式获取目标垃圾在相机坐标系下的3D坐标和包含目标表面信息的点云信息，并将目标垃圾的3D坐标转移至机械臂坐标系下。

6.根据权利要求1所述的小型水域垃圾清理机器人，其特征在于，使用姿态定向算法来对机械臂进行控制时，网兜平面关于法向量的旋转不影响捞取结果，不需要对机械臂末端的姿态进行完全控制，仅对空间姿态的某一轴方向进行控制，具体为：

从旋转矩阵角度来看，任意三维空间姿态均可表示为

其中，^Ax_B、^Ay_B、^Az_B列矢量分别表示坐标系{B}的单位坐标轴矢量在坐标系{A}中的投影；

设当前姿态旋转矩阵为期望姿态旋转矩阵为/>则期望姿态在当前姿态相应坐标系{0}下表示为

式中，三个列矢量是期望姿态相应坐标系{d}中的X、Y、Z轴单位向量，在当前姿态相应坐标系中{c}的投影；对于X轴定向、Y轴定向、Z轴定向，分别对应要求⁰x_c ^T·⁰x_d＝1、⁰y_c ^T·⁰y_d＝1、⁰z_c ^T·⁰z_d＝1；

进一步考虑用四元数表示定向姿态间的偏差，以关于X轴定向⁰x_c ^T·⁰x_d＝1为例，设则由四元数与旋转矩阵对应关系，得到联立方程

解得

即X轴定向仅要求四元数偏差中/>类似地，分别带入旋转矩阵Y、Z轴定向条件，对应解得Y轴定向对应四元数条件/> Z轴定向对应四元数条件

除姿态偏差项外，定向任务的运动学控制方程也需对应修改；以X轴定向为例，则对应机械臂运动控制描述为

其中J_p为雅各比矩阵前三行，J_oy、J_oz为雅各比矩阵第五、六个行矢量；由于六自由度机械臂应用于五维任务，故等效为冗余机械臂，即使在单个关节陷入关节限位时，机械臂仍能保证主任务的精度。