面向水下的双臂协同抓取、抱取及避碰一体化方法及系统技术方案

本发明专利技术公开了一种面向水下的双臂协同抓取、抱取及避碰一体化方法及系统，属于水下作业自动控制领域。本发明专利技术基于双臂协同工作的原有任务轨迹规划，在双机械臂工作空间基础上，提出划分三个碰撞概率不同的区域，并优先在低碰撞概率的区域进行轨迹规划，而在必须经过高概率区域时通过插值法将轨迹分割为多小段路径，并在执行每小段的运动之前进行碰撞判定和避碰规划，根据避碰规划更新小段路径的运动，从而在不干扰原有任务正常执行的前提下，对碰撞风险进行预防性控制。本发明专利技术能够在规划作业轨迹的同时进行机械臂的预防性运动避碰，是将水下双机械臂协同作业及运动避碰相互统一的一体化方法，具有一定实用价值和发展潜力。

An Integrated Method and System for Underwater-Oriented Two-Arm Collaborative Grabbing, Holding and Collision Avoidance

The invention discloses an integrated method and system for underwater dual-arm cooperative grasping, holding and collision avoidance, which belongs to the field of underwater operation automatic control. The invention is based on the original task trajectory planning of two-arm cooperative work. On the basis of the workspace of two-arm, it proposes to divide three regions with different collision probability, and to plan the trajectory in the region with low collision probability first. When the region with high collision probability has to be passed, the trajectory is divided into multi-segment paths by interpolation method, and the collision decision is made before the movement of each segment is executed. And collision avoidance planning, according to the collision avoidance planning to update the movement of the small path, so as to prevent the collision risk without interfering with the normal execution of the original task. The invention can plan the operation trajectory and prevent the collision of the manipulator by preventive motion. It is an integrated method for the cooperation of the underwater two manipulators and the collision avoidance by motion. It has certain practical value and development potential.

【技术实现步骤摘要】
面向水下的双臂协同抓取、抱取及避碰一体化方法及系统
本专利技术属于水下作业自动控制领域，涉及一种面向水下的双臂协同抓取、抱取及避碰一体化方法及系统，更具体地，涉及一种面向水下作业的双机械臂协同抓取、协同抱取及其避碰一体化方法及系统。
技术介绍
目前海洋技术得到了世界各海洋大国的高度重视和空前发展，海洋作业深度越来越大，且作业环境也越来越恶劣。水下机械手是一种能很好的代替人去完成这些危险任务的工具。人类海洋勘探活动的增加和自动化技术的成熟，水下机械手逐渐被投放到市场中，辅助人类进行海洋工程作业。由于可以节省大量人力物力，水下机械手在水下作业方面崭露头角。伴随载人潜水器技术的发展、下潜深度的增加，近年来水下机械手技术发展迅速，例如“蛟龙”号载人潜水器在马里亚纳海沟下潜抵达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，通过逆解得到当前小段的目标插值点对应的多组关节角度的解，去除会导致连杆干涉本文档来自技高网...

【技术保护点】
1.一种面向水下的双臂协同抓取、抱取及避碰一体化方法，用于水下复杂环境中作业时的作业任务规划及避碰一体化控制，其特征在于，初步确定作业任务规划，并将双机械臂的双臂工作空间划分为单臂运动可行区、单臂运动警示区以及双臂合作工作区，先判断是否存在双臂合作工作区：若不存在双臂合作工作区，则直接按规划的末端轨迹执行双臂动作；若存在双臂合作工作区，则同时对双机械臂的末端轨迹进行插值，根据插值点将每个单臂的末端轨迹分解为若干小段路径，以小段路径为单位，进行如下避碰步骤：在经过每小段路径之前先进行机械臂末端轨迹交叉及机械臂连杆干涉判定，若不存在交叉及干涉，则机械臂执行当前小段路径的动作；否则，重新规划该小段路径的动作并重新进行交叉及干涉判定；每小段路径执行之前循环该避碰步骤，直至双机械臂末端运动到轨迹终点；其中，双臂合作工作区，是指两个单臂的运动空间的交叉区域；单臂运动可行区，是指每个单臂的运动空间去除双臂合作工作区后的区域；单臂运动警示区，是指以双臂工作空间的中轴线为界将工作空间分为两个半区，其中一个机械臂所在的半区，是另一机械臂的单臂运动警示区；以满足工作任务要求为前提，各机械臂优先在自身的单臂运动可行区进行末端轨迹及运动规划，单臂运动可行区无法完成规划时，再考虑双臂合作工作区，最后考虑单臂运动警示区。...

【技术特征摘要】
1.一种面向水下的双臂协同抓取、抱取及避碰一体化方法，用于水下复杂环境中作业时的作业任务规划及避碰一体化控制，其特征在于，初步确定作业任务规划，并将双机械臂的双臂工作空间划分为单臂运动可行区、单臂运动警示区以及双臂合作工作区，先判断是否存在双臂合作工作区：若不存在双臂合作工作区，则直接按规划的末端轨迹执行双臂动作；若存在双臂合作工作区，则同时对双机械臂的末端轨迹进行插值，根据插值点将每个单臂的末端轨迹分解为若干小段路径，以小段路径为单位，进行如下避碰步骤：在经过每小段路径之前先进行机械臂末端轨迹交叉及机械臂连杆干涉判定，若不存在交叉及干涉，则机械臂执行当前小段路径的动作；否则，重新规划该小段路径的动作并重新进行交叉及干涉判定；每小段路径执行之前循环该避碰步骤，直至双机械臂末端运动到轨迹终点；其中，双臂合作工作区，是指两个单臂的运动空间的交叉区域；单臂运动可行区，是指每个单臂的运动空间去除双臂合作工作区后的区域；单臂运动警示区，是指以双臂工作空间的中轴线为界将工作空间分为两个半区，其中一个机械臂所在的半区，是另一机械臂的单臂运动警示区；以满足工作任务要求为前提，各机械臂优先在自身的单臂运动可行区进行末端轨迹及运动规划，单臂运动可行区无法完成规划时，再考虑双臂合作工作区，最后考虑单臂运动警示区。2.根据权利要求1所述的一种面向水下的双臂协同抓取、抱取及避碰一体化方法，其特征在于，每小段路径执行动作之前，先判断各机械臂的该小段路径是否位于对应的单臂运动可行区内，若是，则直接执行该小段路径的规划动作；否则，进行避碰步骤。3.根据权利要求1或2所述的一种面向水下的双臂协同抓取、抱取及避碰一体化方法，其特征在于，将一个机械臂设定为主动手臂，另一个机械臂设定为从动手臂，在主动手和从动手的末端轨迹上同时进行插值，避碰步骤中，若判断存在交叉或干涉，则保持主动手臂末端轨迹规划不变，重新规划从动手臂的该小段路径，以使从动手臂避开主动手臂。4.根据权利要求1所述的一种面向水下的双臂协同抓取、抱取及避碰一体化方法，其特征在于，包括如下步骤：S1、判断是否存在双臂合作工作区，是则转入步骤S2；否则直接按照原规划的末端轨迹执行；S2、划分单臂运动可行区和单臂运动警示区，进入步骤S3；S3、对两个机械臂的末端轨迹同时进行插值，按照插值点将末端轨迹分成若干小段路径，进入步骤S4；S4、当双臂动作执行至某一小段路径时，先进行该小段路径的轨迹判定：各机械臂的该小段路径均在对应的单臂运动可行区，则直接执行该小段路径的动作；至少...

【专利技术属性】
技术研发人员：向先波，张琴，刘宇豪，徐国华，唐国元，甘帅奇，王广航，董东磊，
申请(专利权)人：华中科技大学，
类型：发明
国别省市：湖北,42