面向水下的双臂协同抓取、抱取及避碰一体化方法及系统与流程

本发明属于水下作业自动控制领域，涉及一种面向水下的双臂协同抓取、抱取及避碰一体化方法及系统，更具体地，涉及一种面向水下作业的双机械臂协同抓取、协同抱取及其避碰一体化方法及系统。

背景技术：

目前海洋技术得到了世界各海洋大国的高度重视和空前发展，海洋作业深度越来越大，且作业环境也越来越恶劣。水下机械手是一种能很好的代替人去完成这些危险任务的工具。人类海洋勘探活动的增加和自动化技术的成熟，水下机械手逐渐被投放到市场中，辅助人类进行海洋工程作业。由于可以节省大量人力物力，水下机械手在水下作业方面崭露头角。

伴随载人潜水器技术的发展、下潜深度的增加，近年来水下机械手技术发展迅速，例如“蛟龙”号载人潜水器在马里亚纳海沟下潜抵达7062米深度，打破了世界上同类作业型航行器下潜的最大深度。但是，随之而来的就是机械手的作业深度也相应增加，如何在深海恶劣环境下进行机械手避碰成为当前最棘手的问题之一。

目前，应用于水下的机械手绝大多数都是单臂的，且轨迹规划比较单一。水下单机械臂在工作过程中只需考虑环境的避碰，属于静态避碰，多通过传感器进行反馈，完全无需考虑自身“打架”的问题。

但是，随着潜水深度增加，水下环境愈加复杂且悬浮作业模式难度大、精度极低。水下悬浮作业困难，载体时时刻刻都在晃动，手爪与目标物无法精准对接，并且，手爪抓取受手爪开度尺寸限制，对于尺寸超过手爪开度的物体无法抓取，而抱取动作能抓物体的范围更广。在此基础上，为提高作业精度，可以采用双臂协同拟人抱取的方法。抱取动作容错率高，且对不同大小的物体适应度好。若遇复杂海况，抓取无法达到目的时，采用双臂协同拟人抱取的方法，以提高作业效率，且双臂抱取对于不同被抓目标物的尺寸适应度较好。

以auv搭载双机械臂在深海环境下进行抱取作业为例，通过双机械臂的配合将物体抱在auv表面，这种情况下规划的双机械臂末端轨迹很可能发生交叉，双机械臂的连杆也很有可能在运动过程中发生干涉，因此需要对其避碰方案进行设计。

正是由于双臂抱取的水下作业模式，双臂存在交叉与协作，作业的过程则较单机械臂复杂的多，不仅要考虑与环境的避碰，更重要的是防止自身双臂之间发生碰撞，这样可以有效的减少作业出错率，确保机械臂自身安全。

但是，现有的避碰方式以传感器反馈为主，其特点在于工作方式为实时检测，这就导致控制效果对传感器的精度和稳定性依赖较高；陆地上的机械臂作业，也有通过视觉传感进行预判的技术，但是由于深海环境恶劣，光照不足，视觉传感设备的安装及成像均存在较大困难，不适于水下作业的避碰检测。

因此，迫切需要一种适用于双机械臂在水下作业时的避碰方法。

技术实现要素：

针对现有技术的以上缺陷或改进需求，本发明提供了一种面向水下的双臂协同抓取、抱取及避碰一体化方法及系统，其目的在于，通过对双臂的工作空间进行划分，结合插值法进行路径分解，进而进行轨迹交叉、机械臂干涉判断，并根据判断结果依据划分的工作空间重新规划轨迹，从而在机械臂原本的任务轨迹规划基础上，对碰撞进行预分析并提前更新轨迹进行规避，由此实现机械臂任务执行及避碰的一体化控制，减少对传统传感器反馈技术的依赖。

为了实现上述目的，按照本发明的一个方面，提供了一种面向水下的双臂协同抓取、抱取及避碰一体化方法，在双机械臂的双臂工作空间基础上，划分单臂运动可行区、单臂运动警示区以及双臂合作工作区，先判断是否存在双臂合作工作区：

若不存在双臂合作工作区，则直接按规划的末端轨迹执行双臂动作；

若存在双臂合作工作区，则同时对双机械臂的末端轨迹进行插值，根据插值点将每个单臂的末端轨迹分解为若干小段路径，以小段路径为单位，进行如下避碰步骤：在经过每小段路径之前先进行机械臂末端轨迹交叉及机械臂连杆干涉判定，若不存在交叉及干涉，则机械臂执行当前小段路径的动作；否则，重新规划该小段路径的动作并重新进行交叉及干涉判定；

每小段路径执行之前循环该避碰步骤，直至双机械臂末端运动到轨迹终点；

其中，

双臂合作工作区，是指两个单臂的运动空间的交叉区域；单臂运动可行区，是指每个单臂的运动空间去除双臂合作工作区后的区域；单臂运动警示区，是指以工作空间的中轴线为界将工作空间分为两个半区，其中一个机械臂所在的半区，是另一机械臂的单臂运动警示区；

以满足工作任务要求为前提，各机械臂优先在自身的单臂运动可行区进行末端轨迹及运动规划，单臂运动可行区无法完成规划时，再考虑双臂合作工作区，最后考虑单臂运动警示区。

进一步地，每小段路径执行动作之前，先判断各机械臂的该小段路径是否位于对应的单臂运动可行区内，若是，则直接执行该小段路径的规划动作；否则，进行避碰步骤。

进一步地，将一个机械臂设定为主动手臂，另一个机械臂设定为从动手臂，在主动手和从动手的末端轨迹上同时进行插值，避碰步骤中，若判断存在交叉或干涉，则保持主动手臂末端轨迹规划不变，重新规划从动手臂的该小段路径，以使从动手臂避开主动手臂。

进一步地，包括如下步骤：

s1、判断是否存在双臂合作工作区，是则转入步骤s2；否则直接按照原规划的末端轨迹执行；

s2、划分单臂运动可行区和单臂运动警示区，进入步骤s3；

s3、对两个机械臂的末端轨迹同时进行插值，按照插值点将末端轨迹分成若干小段路径，进入步骤s4；

s4、当双臂动作执行至某一小段路径时，先进行该小段路径的轨迹判定：

各机械臂的该小段路径均在对应的单臂运动可行区，则直接执行该小段路径的动作；

至少一机械臂的该小段路径不在对应的单臂运动可行区，则进入步骤s5；

s5、判断该小段路径下是否存在双臂末端运动轨迹交叉：

是则将其中一个机械臂作为主动手臂，另一个机械臂作为从动手臂，保持主动手臂该小段路径的轨迹不变，调节从动手臂的关节角度，使从动手臂与主动手臂错开相应角度，此时得到的从动手臂的末端位置为新的轨迹的插值点，返回步骤s3并更新从动手臂的末端轨迹及插值点；

否则，进入步骤s6；

s6、判断该小段路径下两个机械臂的连杆是否干涉：

若不干涉则直接执行该小段路径的动作；

若发生干涉，则保持主动手臂该小段路径的运动规划不变，由从动手臂的该小段路径的逆运动学解中，选择一组满足机械臂连杆无干涉且加权机械臂关节运动距离最优的解，作为从动手臂在当前小段路径运动时的控制参数，对从动手臂重新进行运动规划，然后，执行该小段路径的动作。

进一步地，步骤s5中采用最小尺寸避碰法计算从动手臂的错开角度，方法如下：

假设从动手臂末端连接的连杆到肩关节的距离为l，该连杆最粗段的直径为d，肩关节只需向当前运动方向反向运动角度θ即可保证双臂末端不发生碰撞，则肩关节避碰运动角度为：

将θ代入机械臂的正运动学方程中，计算从动手臂末端的空间坐标(x2，y2，z2)，并与主动手末端坐标(x1，y1，z1)进行比较，若x1≠x2或y1≠y2或z1≠z2，则表示双臂末端该位型下不发生碰撞，此时从动手末端的坐标点(x2，y2，z2)为新的轨迹插值点。

进一步地，步骤s6中获得满足机械臂连杆无干涉且加权机械臂关节运动距离最优的解的方法如下：

设从动手臂有n个关节，首先将从动手臂的各个关节按照运动规划上的优先运动顺序规定一个权值ωi，优先运动的关节权值较高；假设第i个关节在当前小段的初始角度为θ0i，通过逆解得到当前小段的目标插值点对应的多组关节角度的解，去除会导致连杆干涉的解后，剩余m组解，其中第k组关节角度的解对应的加权机械臂关节运动距离ak为：

计算出的ak中的最小值所对应的一组关节角度，即为满足机械臂连杆无干涉且加权机械臂关节运动距离最优的解，将该组关节角度作为从动手臂的运动控制参数并执行。

计算出的ak中的最小值所对应的一组关节角度，即为满足机械臂连杆无干涉且加权机械臂关节运动距离最优的解，将该组关节角度作为从动手臂的运动控制参数并执行。

为了实现上述目的，按照本发明的另一方面，提供了一种面向水下的双臂协同抓取、抱取及避碰一体化系统，包括控制器及避碰程序模块，所述避碰程序模块在被所述控制器调用时，实现如前所述的双机械臂协同抓取、协同抱取及其避碰一体化方法。总体而言，本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

1、按照本发明的方法，可在执行双臂协同工作任务规划的同时，提前对可能发生的碰撞进行预测，并对原规划轨迹进行更新，从而在不干扰任务正常执行的情况下，有效的防止双臂机械手在作业过程中自身“打架”，确保了水下双臂机械手设备的安全，提高水下作业的可靠性。

2、本发明提供的避碰方法是直接在机械臂执行动作之前就先进行判定与规划，做到预防性控制。这种方法更加智能化，也可以实现精准动作。是在已有的机械手运动控制基础上，进一步将其改进为具备自主分析能力的运动规划，具有一定实用价值和发展潜力。

3、通过单臂运动可行区、单臂运动警示区以及双臂合作工作区的划分，可以预先判断是否存在碰撞可能性，不存在碰撞可能性的轨迹部分直接正常执行原规划动作，能够大大减轻运算负担，提高工作效率；而在避碰时按照分区的优先级进行轨迹规划，则可以进一步降低碰撞概率实现运动避碰。

4、本发明能够在规划作业轨迹的同时进行机械臂的运动避碰，是将水下双机械臂协同作业及运动避碰相互统一的一体化方法，兼容性好，适用范围广。

5、本发明提出了在一般简要任务如双臂协同抓取作业情况下，各手臂的运动尽可能在各自所划分的可行区运动；若存在复杂任务如双臂协同抱取、交叉协作抓取等双臂之间可能存在自身碰撞情况下，则采用基于运动轨迹插值的双臂末端执行器轨迹目标点干涉判定法，将各机械臂可行轨迹分解为若干小段，每小段进行双臂干涉判定，其中包括轨迹交叉判定与连杆干涉判定。若不满足避碰条件，则提出最小尺寸避碰法计算得到解决碰撞问题的机械臂各关节角度，对机械臂各连杆关节进行调整，并采用加权机械臂关节运动距离最优法选择出不发生手臂碰撞的一组运动学反解值，直至再次通过双臂干涉判定法及机械臂正运动学验证确定双机械臂不发生碰撞后再执行各关节动作。该一体化方法有效解决了双机械臂协同抓取或协同抱取时可能存在的自身碰撞问题。

附图说明

图1是对双机械臂的工作空间进行划分示意图：虚线半圆是右臂工作空间，实线半圆是左臂工作空间；两个工作空间交叉的阴影区域为双臂合作工作区，左臂工作空间去除双臂合作工作区后的部分为左臂运动可行区，右臂工作空间去除双臂合作工作区后的部分为右臂运动可行区，虚线方框中的区域为左臂警示区，实线方框中的区域为右臂警示区。

图2是双臂协同抓取下左右臂在各自的可行区内进行轨迹规划的示意图：在轨迹上进行插值，机械臂末端每运动到一个插值点时，计算从该插值点至下一插值点之间的轨迹是否存在碰撞可能。

图3是左右臂运动至双臂合作工作区的示意图：判断运动至下一插值点过程中可能碰撞，则在向下一个插值点运动之前，在下一个插值点的规划上，根据主动手(左臂)的运动轨迹与手臂的尺寸，令从动手错开相应的距离，计算出从动手关节需要调整的角度，并用该角度通过正运动学计算与验证确定出新的轨迹插值点。

图4是机械臂末端避碰示意图：根据上一步的规划，从动手臂(右臂)运动至新的插值点上，避开了主动手臂(左臂)。

图5是双臂末端运动至轨迹重点的示意图：从动手臂绕开主动手臂后，双臂按照原规划的路径正常运动，最终实现预期功能，整个过程有效的实现了双机械臂避碰。

图6是图4避碰时的肩关节错开局部细节图：通过肩关节电机的转动，使主动手臂与从动手臂原先可能发生干涉的连杆在侧视投影面上不相交，因此在空间上使两手臂错开，避免干涉。

图7是抽取双臂协同抱取时末端避碰的数学模型；

图8是本发明优选实施例的流程示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本实施例的双机械臂以auv(autonomousunderwatervehicle，自主式水下潜器，又称无缆水下机器人)为载体，根据双机械臂的结构，在双臂工作空间基础上，将该空间划分为单臂运动可行区、单臂运动警示区以及双臂合作工作区，如图1所示。单臂运动可行区即左臂(或右臂)在各关节可达角度范围内以任意角度运动都无法接触右臂(或左臂)工作空间的区域，换言之，左臂、右臂各自在单臂运动可行区内无论如何运动均不会与对方发生碰撞。

如图1，虚线半圆区域中去除阴影区域后的部分为右臂运动可行区。单臂运动警示区以左臂为例，以工作空间中轴线为界的右半部份，即图1中虚线方框中的区域即为左臂的单臂运动警示区。双臂工作空间的重合区域划分为双臂合作工作区，即图1中虚线半圆与实线半圆重叠的阴影区域。

在本实施例中，对于右臂而言，存在右臂运动可行区与右臂警示区；对于左臂而言，存在左臂运动可行区与左臂警示区；同时对于双臂而言，存在双臂合作工作区。在其他实施例中，如果不存在双臂合作工作区，则也意味着每个单臂均不会运动至警示区内，即，无论如何都不会与对方发生碰撞，这种情况下直接按照任务规划的轨迹运动即可，也可以通过插值方式逐步运动，减缓运动速度。

在进行机械臂轨迹规划时，将双臂中的一只设定为主动手，另一只设定为从动手。双臂同时在轨迹上进行插值，如图2所示，根据插值点将轨迹分解为若干小段。机械臂在最初始位置，首先对第一小段轨迹进行轨迹交叉判定，轨迹不发生交叉，则继续判定连杆是否存在干涉，连杆也不存在干涉，则执行动作末端执行器从初始位置运动至第一个插值点，此后重复该步骤。当双机械臂进行双臂抱取动作时，机械臂末端轨迹显然需要越过警示区到达双臂合作工作区。当双机械臂轨迹判定出存在交叉时，如图3所示机械臂位型，此时主动手的运动规划不变，采用最小尺寸避碰法，计算出从动手肩关节电机转动角度。通过图7抽取出的模型，假设手臂长度为l，手臂最粗段的直径为d，该距离为手臂错开最小距离，肩关节只需错开角度θ即可保证双臂不发生碰撞。由该模型得：

因此

故肩关节避碰运动角度为：

此时得到的机械臂角度代入机械臂的正运动学方程中，计算从动手末端执行器的空间坐标(x2，y2，z2)，并与主动手末端坐标(x1，y1，z1)进行比较。

若x1≠x2或y1≠y2或z1≠z2，则表示双臂末端执行器在该位型下不发生重合。此时从动手的坐标点为新的轨迹插值点。

从动手肩关节电机向相反方向转动θ角，如图4与图7，从侧视投影面上可看到从动手臂与主动手臂已错开，机械臂在空间上错开后，可根据任务再进行下一步轨迹规划，完成机械臂运动。

若连杆干涉判定存在碰撞可能，则重新规划机械臂运动。机械臂的逆运动学运算存在多组解，在其余的解中再筛选出满足无干涉且为“加权机械臂关节运动距离最优”的解，将该解输入控制器，执行机械臂动作。

下面，以图1～7所示的四关节机械臂的协同抱取工作为例对本发明的方法进行进一步说明，实际上本发明的方法适用于常见的多关节机械臂，不同之处仅在于n、m的取值不同而已，并不以四关节为限。

本实施例的四关节机械臂包括：肩关节、大臂旋转关节、肘关节、腕关节、肩部连杆、大臂连杆、小臂连杆和末端执行器。肩部连杆一端连接肩关节，另一端通过大臂旋转关节连接大臂连杆上端，大臂连杆下端和小臂连杆上端通过肘关节连接，小臂连杆下端通过腕关节连接末端执行器。该四关节机械臂的运动顺序为：肩关节→大臂旋转关节→肘关节→腕关节。

如图8所示，协同抱取及避碰一体化操作方法如下：

s1、如图1所示，经判断，双臂工作空间内存在双臂合作工作区，转入步骤s2；

s2、划分单臂运动可行区和单臂运动警示区，进入步骤s3；

s3、如图2所示，对两个机械臂的末端轨迹同时进行插值，按照插值点将末端轨迹分成若干小段路径，进入步骤s4；

s4、当双臂动作执行至某一小段路径时，先进行该小段路径的轨迹判定：

如图2所示，各机械臂的该小段路径均在对应的单臂运动可行区内，说明当前小段路径不会发生碰撞，则直接执行该小段路径的动作；

如图3所示，当运动到图3的位置时，判断出左臂和右臂即将执行的的该小段路径不在对应的单臂运动可行区，进入了双臂合作工作区，说明有可能发生碰撞，则进入步骤s5；

s5、判断该小段路径下是否存在双臂末端运动轨迹交叉：

经判断，即将执行的该小段路径下，双臂末端的运动轨迹出现交叉，会在交叉点发生碰撞。此时，将左臂作为主动手臂，右臂作为从动手臂，保持主动手臂该小段路径的轨迹不变，调节从动手臂的关节角度，使从动手臂与主动手臂错开相应角度，此时得到的从动手臂的末端位置为新的轨迹的插值点，返回步骤s3并更新从动手臂的末端轨迹及插值点。

具体地，如图4、7所示，采用最小尺寸避碰法计算得到从动手臂的错开角度将θ代入机械臂的正运动学方程中，计算从动手臂末端的空间坐标(x2，y2，z2)，并与主动手末端坐标(x1，y1，z1)进行比较，此时x1≠x2，表示双臂末端执行器在该位型下不发生碰撞，此时从动手末端的坐标点(x2，y2，z2)为新的轨迹插值点；

根据更新后的插值点(x2，y2，z2)重新规划该小段路径的轨迹之后，判断左臂和右臂在该小段路径的轨迹不交叉，即双臂末端执行器不会发生碰撞，进入步骤s6；

s6、判断该小段路径下两个机械臂的连杆是否干涉：

经判断，虽然根据新的插值点重新规划该小段路径的轨迹后，两个机械臂的末端执行器不会发生碰撞，但是当前解出的控制参数，即四个关节的运动角度，会使左臂和右臂的小臂连杆发生干涉。

此时，保持主动手臂(即左臂)该小段路径的运动规划不变，由从动手臂(即右臂)的该小段路径的逆运动学解中，选择一组满足机械臂连杆无干涉且加权机械臂关节运动距离最优的解，作为从动手臂在当前小段路径运动时的控制参数，对从动手臂重新进行运动规划，然后，执行该小段路径的动作。

具体地，由于右臂的关节数量n＝4，根据关节运动的优先顺序，设置每个关节运动角度的权重依次为四个关节运动角度的权重依次设置为肩关节ω1＞大臂旋转关节ω2＞肘关节ω3＞腕关节ω4。此时(x2，y2，z2)的运动学反解有多组，其中一部分解同样会导致机械臂连杆干涉，还有一部分解会导致右臂进入右臂警示区，剔除这两部分解之后，剩余4组解既不会导致机械臂连杆干涉，也不会导致进入右臂警示区，即满足机械臂连杆无干涉且不进入右臂警示区的解有m＝4组。

设各关节的初始角度依次为θ01、θ02、θ03、θ04(即i＝1,2,3,4)，该角度是原轨迹规划中，右臂的末端执行器运动至(x1，y1，z1)时的角度。则需要从4组无干涉的解中，选出加权机械臂关节运动距离最优的解，以使整体运行距离最短，提高工作效率。具体地，

当目标点发生变化，即插值点由(x1，y1，z1)更新为(x2，y2，z2)时，通过机械臂的运动学方程进行逆解得到的4组角度为θk1、θk2、θk3、θk4，其中，k＝1,2,3,4。

则依照加权机械臂关节运动距离最优法计算第k组解的加权运动距离得：

计算出四个ak权和后，再进行选小，即取minak为最优解的运动距离权和，其对应的四个关节角度，即为满足机械臂连杆无干涉且加权机械臂关节运动距离最优的解。

简而言之，除去发生连杆干涉的从动手臂运动角度的解外，在其余不发生连杆干涉的运动角度的解中选取最小权和所对应的那组关节角度，即为最终关节角度。将该组关节角度输入机械臂的控制器，更新并执行机械臂运动即可。以此方法可以解决机械臂连杆干涉问题。

一般情况下，只要关节角度存在既不会导致机械臂连杆干涉，也不会导致进入右臂警示区的解，就从这些解中筛选最优解。但是，在其他实施例中，有可能只有导致机械臂连杆干涉的解和不干涉但会导致进入右臂警示区的解，此情况下则只能从不干涉但会导致进入右臂警示区的解中进行运动距离加权评估选取最优解。设置警示区但却优先排除经过警示区的解，其意义在于，尽量避免机械臂经过警示区，可以最大限度降低接下来剩余轨迹的规划及运动中发生碰撞的概率。

按照上述步骤完成一小段路径的避碰之后，对下一小段路径重复上述避碰过程，直至机械臂运动至目标点，完成作业任务，由此实现在机械臂任务轨迹规划的同时进行避碰的一体化控制。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。