一种使用双机械臂的受限环境柔性线缆操作方法和系统

1.本发明涉及柔性线缆操作
技术领域：
：，特别涉及一种使用双机械臂的受限环境柔性线缆操作方法和系统。
背景技术：
：：2.柔性线缆广泛存在于人类的生产生活中，对柔性线缆的操作场景也十分广泛，例如，工业生产中的线缆装配、日常生活中的打结、医疗手术中的缝线等。对柔性线缆的操作一般指对其位形进行控制，即通过控制机械臂，将柔性线缆操作至期望位形，但柔性线缆的变形大、自由度高、模型未知等特性使机械臂对其进行操作面临种种困难，并且，实际生产生活中的柔性线缆操作往往需要大量人工参与，难以实现自动化、自主化。3.现有技术中，柔性线缆操作方法通常假设了一个理想环境，即机械臂末端可以在空间中自由运动，且不考虑柔性线缆与环境之间、柔性线缆与机械臂之间、机械臂与环境之间、以及柔性线缆本身可能发生的碰撞。但是在实际的操作任务中，一定会有障碍物的存在，且机械臂本身的碰撞也是不能忽略的。因此，现有的柔性线缆操作方法难以直接应用于有障碍物的约束环境实际任务中。技术实现要素：4.鉴于上述问题，本发明实施例提供了一种使用双机械臂的受限环境柔性线缆操作方法和系统，以便克服上述问题或者至少部分地解决上述问题。5.本发明实施例的第一方面，公开了一种使用双机械臂的受限环境柔性线缆操作方法，应用于局部控制器，所述方法包括：6.接收全局规划器发送的全局规划路径，所述全局规划路径是所述全局规划器基于约束双向快速探索随机树方法，根据柔性线缆的初始位形和目标期望位形进行全局规划，得到的全局规划路径；7.将所述全局规划路径作为引导路径，通过实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。8.可选地，所述将所述全局规划路径作为引导路径，通过实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形，包括：9.将所述全局规划路径中的每个节点作为中间期望位形；10.以所述双机械臂的实时位形和所述柔性线缆的实时位形作为反馈，控制所述双机械臂迭代地沿每个节点移动，直到将所述柔性线缆移动到目标期望位形。11.可选地，所述控制所述双机械臂迭代地沿每个节点移动，直到将所述柔性线缆移动到目标期望位形，包括：12.针对所述全局规划路径中的每个节点，所述局部控制器控制所述双机械臂依次从上一个节点移动到当前节点；13.在所述柔性线缆的实际位形与当前节点的期望位形的距离小于设定阈值的情况下，控制所述双机械臂移动到下一个节点。14.可选地，所述局部控制器通过以下方式控制所述双机械臂移动到下一个节点：15.根据引导柔性线缆向下一个节点的柔性线缆期望位形移动的吸引势场、双机械臂向下一个节点的双机械臂期望位形的吸引势场、柔性线缆的空间排斥势场、双机械臂的空间排斥势场，构建控制代价模型；16.对所述控制代价模型进行求解，得到局部控制最优解，将所述局部最优解作为所述双机械臂的控制输入，以控制所述双机械臂移动到下一个节点。17.可选地，所述方法还包括：18.在所述柔性线缆未达到目标期望位形，但所述局部控制器输入接近零的情况下，向所述全局规划器发送重新规划请求，以使全局规划器根据柔性线缆的当前位形和目标期望位形进行全局规划，得到新的全局规划路径；19.接收所述全局规划器发送的新的全局规划路径，并将所述新的全局规划路径作为引导路径，通实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。20.本发明实施例的第二方面，公开了一种使用双机械臂的受限环境柔性线缆操作方法，应用于全局规划器，所述方法包括：21.基于约束双向快速探索随机树方法，根据柔性线缆的初始位形和目标期望位形进行全局规划，得到全局规划路径，所述全局规划路径由多个节点组成；22.将所述全局规划路径发送至局部控制器，以使所述局部控制器将所述全局规划路径作为引导路径，通实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。23.可选地，所述基于约束双向快速探索随机树方法，根据柔性线缆的初始位形和目标期望位形进行全局规划，得到全局规划路径，包括：24.两棵随机树分别从开始节点和目标节点出发，向彼此迭代生长，直到所述两棵随机树相连时停止迭代生长，其中，所述两棵随机树上的每个节点包括：柔性线缆的位形和双机械臂的位形；25.从所述两棵随机树中提取出从所述开始节点到所述目标节点的路径，并对所述路径进行平滑处理，得到全局规划路径。26.可选地，所述两棵随机树分别从开始节点和目标节点出发，向彼此迭代生长，直到所述两棵随机树相连时停止迭代生长，包括：27.针对每次迭代，通过随机采样得到随机采样节点；28.从第一棵随机树中找出离所述随机采样节点最近的第一节点，由所述第一节点向所述随机采样节点扩展生长，得到所述第一棵随机树扩展节点，并从所述第二棵随机树中找出离所述第一棵随机树扩展节点最近的第二节点，由所述第二节点向所述第一棵随机树扩展节点方向扩展生长，得到所述第二棵随机树扩展节点，并判断所述第一棵随机树扩展节点与所述第二棵随机树扩展节点之间是否能够相连；29.在相连的情况下，结束迭代生长；30.在不相连的情况下，从所述第二棵随机树开始进行下一次的迭代生长。31.可选地，随机树中的节点通过以下方式实现扩展生长：32.将上一次迭代生长到达的柔性线缆位形向目标扩展位形的方向进行插值，得到新的柔性线缆位形，所述上一次迭代生长到达的柔性线缆位形是指所述第一节点或所述第二节点对应的柔性线缆位形，所述目标扩展位形是指所述随机采样点或所述第一棵随机树扩展节点对应的柔性线缆位形；33.基于势能的投影方法将所述新的柔性线缆位形投影成一个稳定的柔性线缆位形；34.利用机械臂逆运动学求解所述柔性线缆在所述稳定的柔性线缆位形的末端位姿对应的双机械臂的位形，所述双机械臂的位形表征双机械臂的关节角；35.在逆运动学求解成功且碰撞检测结果为无碰撞的情况下，基于所述稳定的柔性线缆位形和所述双机械臂的位形生成随机树的扩展节点，所述随机树的扩展节点包括：第一棵随机树扩展节点和第二棵随机树扩展节点。36.可选地，所述方法还包括：37.接收并响应于所述局部控制器发送的重新规划请求，根据柔性线缆的当前位形和目标期望位形进行全局规划，得到新的全局规划路径；38.将所述新的全局规划路径发送至所述局部控制器，以使所述局部控制器将所述新的全局规划路径作为引导路径，通实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。39.可选地，所述全局规划路径是基于柔性线缆能量模型进行规划得到的，所述柔性线缆能量模型通过以下方式构建得到的：40.以所述柔性线缆上均匀分布的m个特征点的位置来表征所述柔性线缆的位形，根据弹簧质点模型对所述柔性线缆进行建模，得到柔性线缆能量模型，所述柔性线缆能量模型表示为：[0041][0042]其中，e为柔性线缆能量模型的弹性势能，λ1为1型弹簧刚度，λ2为2型弹簧的刚度，xk表示第k个特征点，xk+1表示第k+1个特征点，xk+2表示第k+2个特征点，l为柔性线缆的长度。[0043]本发明实施例的第三方面，公开了一种使用双机械臂的受限环境柔性线缆操作系统，用于执行本发明实施例第一方面和第二方面所述的使用双机械臂的受限环境柔性线缆操作方法，所述系统包括：[0044]全局规划器，所述全局规划器用于生成全局规划路径，并将所述全局规划路径发送至局部控制器；接收并响应于所述局部控制器发送的重新规划请求，规划新的全局规划路径，将所述新的全局规划路径发送至所述局部控制器；[0045]局部控制器，所述局部控制器用于接收所述全局规划器发送的全局规划路径，将所述全局规划路径作为引导路径，通过实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形；向所述全局规划器发送重新规划请求，接收所述全局规划器发送的新的全局规划路径，并将所述新的全局规划路径作为引导路径，通实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。[0046]本发明实施例包括以下优点：[0047]在本发明实施例中，为了实现双机械臂在有障碍受限环境中将柔性线缆鲁棒地操作至期望位形。首先，接收全局规划器基于约束双向快速探索随机树方法，根据柔性线缆的初始位形和目标期望位形进行全局规划，得到的全局规划路径；然后将全局规划路径作为引导路径，通过实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。与现有技术相比，本发明实施例考虑了实际应用场景中柔性线缆和机械臂本身之间的可能碰撞并实现避障；有效结合了离线全局规划与局部反馈控制，实现了保障整体路径可靠的同时极大地减小了执行误差；能够直接应用于不同长度、粗细、材质的柔性线缆操作任务，对柔性线缆建模误差鲁棒。附图说明[0048]为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例的描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。[0049]图1是本发明实施例提供的一种应用于局部控制器的使用双机械臂的受限环境柔性线缆操作方法的步骤流程图；[0050]图2是本发明实施例提供的一种柔性线缆从初始位形到目标期望位形的操作过程示意图；[0051]图3是本发明实施例提供的一种应用于全局规划器的使用双机械臂的受限环境柔性线缆操作方法的步骤流程图；[0052]图4是本发明实施例提供的一种柔性线缆能量模型示意图；[0053]图5是本发明实施例提供的一种基于约束双向快速探索随机树方法的路径规划示意图；[0054]图6是本发明实施例提供的一种柔性线缆原始随机采样位形与投影后的位形对比图；[0055]图7是本发明实施例提供的一种使用双机械臂的受限环境柔性线缆操作方法的整体方案示意图；[0056]图8是本发明实施例提供的一种使用双机械臂的受限环境柔性线缆操作系统的结构框图。具体实施方式[0057]为使本发明的上述目的、特征和优点能够更加明显易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。[0058]参照图1所示，图1示出了本发明实施例提供的一种应用于局部控制器的使用双机械臂的受限环境柔性线缆操作方法的步骤流程图。如图1所示，本发明实施例提供的一种应用于局部控制器的使用双机械臂的受限环境柔性线缆操作方法具体步骤可以包括步骤s101至步骤s102：[0059]步骤s101：接收全局规划器发送的全局规划路径，所述全局规划路径是所述全局规划器基于约束双向快速探索随机树方法，根据柔性线缆的初始位形和目标期望位形进行全局规划，得到的全局规划路径。[0060]本实施例中，在柔性线缆的操作过程中，双机械臂抓住柔性线缆的两端，操作柔性线缆从初始位形到目标期望位形，在此过程中柔性线缆和双机械臂都不能与障碍物碰撞。其中，柔性线缆位形是指柔性线缆的形状，即柔性线缆的初始位形是指柔性线缆的初始形状，柔性线缆的目标期望位形是指柔性线缆的期望形状。以柔性线缆上均匀分布的m(m为正整数)个特征点的位置表示柔性线缆的位形x，其中，第k(k≤m)个特征点位置记为则柔性线缆的位形向量表示为[0061]本实施例中，全局规划路径是指双机械臂控制柔性线缆从初始位形到目标期望位形所经过的路径，该全局规划路径是全局规划器离线规划的一条相对粗略的无碰撞的全局路径，以保证柔性线缆从初始位形到目标期望位形的操作的可解性。全局规划路径由多个节点组成，每个节点包括柔性线缆的位形和双机械臂的位形。在局部控制器控制双机械臂操作柔性线缆时，需要接收全局规划器发送的全局规划路径，以在后续步骤中基于该全局规划路径进行局部控制。[0062]步骤s102：将所述全局规划路径作为引导路径，通过实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。[0063]本实施例中，由于全局规划路径是一条离线规划路径，在规划过程中并不能像闭环控制那样进行误差补偿，因此受规划误差的影响，全局规划的路径并不能完全无碰撞的使双机械臂操作柔性线缆达到目标期望位形。[0064]因此，在步骤s102中局部控制器将全局规划器离线规划的全局规划路径作为引导路径进行跟踪，并同时利用实时反馈来闭环地调整双机械臂运动，从而保证柔性线缆和双机械臂的实际运动都是无碰撞的，并且保证可以准确到达柔性线缆的目标期望位形。[0065]在一种可选的实施例中，所述将所述全局规划路径作为引导路径，通过实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形，包括：[0066]将所述全局规划路径中的每个节点作为中间期望位形；[0067]以所述双机械臂的实时位形和所述柔性线缆的实时位形作为反馈，控制所述双机械臂迭代地沿每个节点移动，直到将所述柔性线缆移动到目标期望位形。[0068]本实施例中，局部控制器采用单步控制的方式，以全局规划路径中的每个节点作为中间期望位形，迭代的移动到每个节点。具体地，针对所述全局规划路径中的每个节点，所述局部控制器控制所述双机械臂依次从上一个节点移动到当前节点；在所述柔性线缆的实际位形与当前节点的期望位形的距离小于设定阈值的情况下，控制所述双机械臂移动到下一个节点。[0069]示例地，如图2所示，柔性线缆需要从上半圆位形(即初始位形)操作为下半圆位形(即目标期望位形)，局部控制器控制双机械臂通过三次迭代，依次第一个节点移动到第二个节点、再从第二个节点到第三个节点、最后从第三个节点到第四个节点，使柔性线缆达到目标期望位形。[0070]在一种可选的实施例中，所述局部控制器通过以下方式控制所述双机械臂移动到下一个节点：[0071]根据引导柔性线缆向下一个节点的柔性线缆期望位形移动的吸引势场、双机械臂向下一个节点的双机械臂期望位形的吸引势场、柔性线缆的空间排斥势场、双机械臂的空间排斥势场，构建控制代价模型；[0072]对所述控制代价模型进行求解，得到局部控制最优解，将所述局部最优解作为所述双机械臂的控制输入，以控制所述双机械臂移动到下一个节点。[0073]具体地，引导柔性线缆向下一个节点的柔性线缆期望位形移动的吸引势场表示为：[0074][0075]其中，x表示柔性线缆当前的位形，xd表示下一个节点的柔性线缆的期望位形。[0076]双机械臂向下一个节点的双机械臂期望位形的吸引势场表示为：[0077][0078]其中，q表示双机械臂当前的位形(即双机械臂的关节角度)，qd表示下一个节点的双机械臂的期望位形。[0079]为实现柔性线缆和双机械臂的局部避障，设计了排斥势场。定义在笛卡尔空间中放置单点p和障碍物co碰撞的排斥势场ur表示为：[0080][0081]其中，c(p)为该单点p与最近障碍物之间的距离，εc为排斥势场影响的最大距离。[0082]进而柔性线缆的空间排斥势场表示为：[0083][0084]其中，xk表示第k个特征点位置，m表示特征点的个数。[0085]双机械臂上y个控制点在笛卡尔空间中的位置ξ(q)＝[ξ1(q)；…；ξy(q)]，其中ξi(·)表示第i个点的正运动学，从每个机械臂中选择一个控制点，则双机械臂的空间排斥势场表示为：[0086][0087]进而构建出控制代价模型，该控制代价模型包括：控制代价函数和约束条件，其中，控制代价函数以最小化控制损失为目标。具体地，所述控制代价模型表示为：[0088][0089]s.t.cdofjarmu＝0[0090][0091]其中，分别为引导柔性线缆向下一个节点的柔性线缆期望位形移动的吸引势场、双机械臂向下一个节点的双机械臂期望位形的吸引势场、柔性线缆的空间排斥势场、双机械臂的空间排斥势场的权重系数，δt为时间步长，ku为控制输入的权重系数，jarm为机械臂的jacobian矩阵，u为给定的控制输入，给定的控制输入是指双机械臂的关节速度，cdof表示表示对机械臂末端自由度的约束矩阵，umax表示双机械臂的最大关节速度。[0092]本实施例中，以控制代价模型的局部控制最优解作为双机械臂单步的控制输入，即作为双机械臂的控制指令，进而双机械臂即可实现在受限环境中将柔性线缆不断移动至期望位形。[0093]在一种可选的实施例中，考虑到柔性线缆自由度高，闭环系统为欠驱动系统，在大范围、大变形的操作过程中，局部控制器可能会陷入局部最优，导致柔性线缆操作停滞。因此，为了避免局部控制器陷入局部最优，所述方法还包括步骤a1和步骤a2：[0094]步骤a1：在所述柔性线缆未达到目标期望位形，但所述局部控制器输入接近零的情况下，向所述全局规划器发送重新规划请求，以使全局规划器根据柔性线缆的当前位形和目标期望位形进行全局规划，得到新的全局规划路径；[0095]步骤a2：接收所述全局规划器发送的新的全局规划路径，并将所述新的全局规划路径作为引导路径，通实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。[0096]本实施例中，当柔性线缆未达到目标期望位形，但此时局部控制器输入接近零，说明局部控制器陷入局部最优。因此，为了继续完成柔性线缆操作，此时需要向全局规划器发送重新规划请求，具体地，重新规划请求中包含：柔性线缆的当前位形和目标期望位形、双机械臂的当前位形和目标位形，进而使得全局规划器在接收到重新规划请求后，根据柔性线缆的当前位形和目标期望位形、双机械臂的当前位形和目标位形进行全局规划，以得到新的全局规划路径并发送至局部控制器。进而局部控制器基于新的全局规划路径操作柔性线缆，以使柔性线缆达到目标期望位形。[0097]基于上述方法，通过对局部最优检测和全局路径重规划，克服了局部控制器限于局部最优的问题，确保了柔性线缆操作任务顺利完成。[0098]本实施例中，为了实现双机械臂在有障碍受限环境中将柔性线缆鲁棒地操作至期望位形，有效结合了离线全局规划与局部反馈控制，实现了保障整体路径可靠的同时极大地减小了执行误差；能够直接应用于不同长度、粗细、材质的柔性线缆操作任务，对柔性线缆建模误差鲁棒。[0099]参照图3所示，图3示出了本发明实施例提供的一种应用于全局规划器的使用双机械臂的受限环境柔性线缆操作方法的步骤流程图。如图3所示，本发明实施例提供的一种应用于全局规划器的使用双机械臂的受限环境柔性线缆操作方法具体步骤可以包括步骤s301和步骤s302：[0100]步骤s301：基于约束双向快速探索随机树方法，根据柔性线缆的初始位形和目标期望位形进行全局规划，得到全局规划路径，所述全局规划路径由多个节点组成。[0101]本实施例中，在局部控制器操作柔性线缆之前，先利用全局规划器进行离线的全局规划，得到一条相对粗略的无碰撞的全局规划路径，以保证柔性线缆从初始位形到目标期望位形是可行的。[0102]为了对柔性线缆操作过程进行全局规划，现有方法为了保证规划路径的可靠性，需要精确的柔性线缆并付出极大的计算时间成本，但由于离线规划不能像闭环控制一样使用实时反馈进行补偿，因此模型误差的影响更大，极易导致柔性线缆未按照期望路径移动的情况发生。[0103]因此，在本实施例中，所述全局规划路径是基于柔性线缆能量模型进行规划得到的，所述柔性线缆能量模型通过以下方式构建得到的：[0104]以所述柔性线缆上均匀分布的m个特征点的位置来表征所述柔性线缆的位形，根据弹簧质点模型对所述柔性线缆进行建模，得到柔性线缆能量模型，所述柔性线缆能量模型表示为：[0105][0106]其中，e为柔性线缆能量模型的弹性势能，λ1为1型弹簧刚度，λ2为2型弹簧的刚度，xk表示第k个特征点，xk+1表示第k+1个特征点，xk+2表示第k+2个特征点，l为柔性线缆的长度。[0107]在实际应用中，根据操作的柔性线缆类型选择相应的弹簧刚度值，来表征不用的柔性线缆。如图4所示，最终将柔性线缆建模为由弹簧连接的一系列质量，第i个质量通过第1类弹簧连接到第(i±1)个质量，通过第2类弹簧连接到第(i±2)个质量。[0108]本实施例中，基于柔性线缆能量模型来进行全局规划，与现有技术相比，无需对精确的柔性线缆进行计算，极大的减少了计算成本，实现高效的规划出全局规划路径，保证了柔性线缆操作的可行性。[0109]步骤s302：将所述全局规划路径发送至局部控制器，以使所述局部控制器将所述全局规划路径作为引导路径，通实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。[0110]本实施例中，全局规划器在完成全局规划后，将全局规划路径发送到局部控制器，以使局部控制器将全局规划路径作为引导路径进行跟踪，同时根据双机械臂的实时位形和柔性线缆的实时位形作为反馈，闭环地补偿步骤301中进行全局路径规划时，由于柔性线缆能量模型的建模误差导致的全局规划路径误差，并实现局部避障，来保证柔性线缆操作的准确性。[0111]在一种可选的实施例中，步骤s301中的所述基于约束双向快速探索随机树方法，根据柔性线缆的初始位形和目标期望位形进行全局规划，得到全局规划路径，包括步骤b1和步骤b2：[0112]步骤b1：两棵随机树分别从开始节点和目标节点出发，向彼此迭代生长，直到所述两棵随机树相连时停止迭代生长，其中，所述两棵随机树上的每个节点包括：柔性线缆的位形和双机械臂的位形。[0113]在本实施例中，开始节点是柔性线缆初始位形和双机械臂的初始位形而生成的节点，目标节点是柔性线缆的目标期望位形和双机械臂的目标位形而生成的节点。对于开始节点，柔性线缆初始位形和双机械臂的初始位形都是已知的，但对于目标节点，只知道柔性线缆的目标期望位形，因此，为了确定双机械臂的目标位形，将柔性线缆目标期望位形的末端姿态的逆运动学解对应的双机械臂的关节角作为双机械臂的目标位形。[0114]本实施例中，基于约束双向快速探索随机树方法作为路径规划框架，进行迭代生长，直到两棵随机树相连时停止迭代生长。此外，当发现新节点生成失败或新节点离目标节点的距离反而大于初始节点离目标节点的距离时，也停止迭代生长。由于是从开始节点和目标节点进行双向生长，提高了路径规划的效率。[0115]具体地，所述两棵随机树分别从开始节点和目标节点出发，向彼此迭代生长，直到所述两棵随机树相连时停止迭代生长，包括：[0116]针对每次迭代，通过随机采样得到随机采样节点；[0117]从第一棵随机树中找出离所述随机采样节点最近的第一节点，由所述第一节点向所述随机采样节点扩展生长，得到所述第一棵随机树扩展节点，并从所述第二棵随机树中找出离所述第一棵随机树扩展节点最近的第二节点，由所述第二节点向所述第一棵随机树扩展节点方向扩展生长，得到所述第二棵随机树扩展节点，并判断所述第一棵随机树扩展节点与所述第二棵随机树扩展节点之间是否能够相连；[0118]在相连的情况下，结束迭代生长；[0119]在不相连的情况下，从所述第二棵随机树开始进行下一次的迭代生长。[0120]示例地，如图5所示，随机树a和随机树b分别表示开始节点和目标节点，当前迭代是从随机树a开始生长，首先随机生成一个随机采样点1，从随机树a中离随机采样点1最近的点1向随机采样点1扩展生长，得到节点3，然后从随机树b中找出离节点3最近的点2，由点2向点3方向扩展生长，得到点4，此时发现点3和点4之间存在障碍不能相连，开始下一次迭代；下一次迭代生长从随机树b开始生长，随机生成一个随机采样点2，从随机树b最近的点4向随机采样点2扩展生长，得到点5，然后从随机树a中找出离点5最近的点3，由点3向点5方向扩展生长，得到点6，此时发现点5和点6可以相连，则结束迭代生长。[0121]步骤b2：从所述两棵随机树中提取出从所述开始节点到所述目标节点的路径，并对所述路径进行平滑处理，得到全局规划路径。[0122]在本实施例中，当两棵随机树相连时，说明规划出了从开始节点到目标节点之间的路径，进而提取出相连路径上的所有节点作为开始节点到目标节点的路径，如图5所示，依次提取出“开始节点”、“点1”、“点3”、“点6”、“点5”、“点4”、“点2”、“目标节点”作为路径。由于路径中的各节点是随机生长的，路径中存在大量的折线，路径的平滑性较差，因此需要对提取出的路径进行平滑处理，将平滑处理后的路径作为全局规划路径。其中，路径平滑处理可通过路径平滑算法(例如，short-cutting算法)对路径进行处理。[0123]在一种可选的实施例中，在每次扩展生长过程中，随机树中的节点通过以下步骤c1至步骤c4的方式实现扩展生长：[0124]步骤c1：将上一次迭代生长到达的柔性线缆位形向目标扩展位形的方向进行插值，得到新的柔性线缆位形，所述上一次迭代生长到达的柔性线缆位形是指所述第一节点或所述第二节点对应的柔性线缆位形，所述目标扩展位形是指所述随机采样点或所述第一棵随机树扩展节点对应的柔性线缆位形。[0125]本实施例中，根据实际需要预设步长，将上一次迭代生长到达的柔性线缆位形向目标期望位形的方向按照预设步长进行插值，以得到新的柔性线缆位形。在进行插值时，对柔性线缆形心位置使用线性插值，对柔性线缆中与平移无关的相对变形采用球面插值，以此来保持线状柔性体的形状，防止柔性线缆被过度压缩。[0126]步骤c2：基于势能的投影方法将所述新的柔性线缆位形投影成一个稳定的柔性线缆位形。[0127]本实施例中，由柔性线缆的位形向量表示为可知，柔性线缆的位形空间为3m维；然而，只有一个子空间包含稳定位形，将该稳定的位形称为流形。因此，在全局规划器在进行全局规划时，柔性线缆的位形需要被限制在这个流形上，即各节点的柔性线缆的位形应保持稳定。在步骤c1中插值得到的新的柔性线缆位形是一个不稳定的位形，进而需要将新的柔性线缆位形投影成一个稳定的柔性线缆位形。[0128]其中，所述势能的投影方法具体是指：将柔性线缆能量模型的弹性势能e假设其由柔性线缆的位形x完全决定，两端位姿固定的柔性线缆的一个稳定位形需要满足e局部极小，即：[0129][0130]其中，柔性线缆最左侧两个特征点和最右侧两个特征点的位置固定表示柔性线缆的末端位姿固定。将步骤c1中插值得到的新的柔性线缆位形x0作为变量初值，局部极小化势能e将其投影至稳定位形的流形上势能e将其投影至稳定位形的流形上进而将xstable作为一个稳定的位形。[0131]因此，步骤c2中根据上述的势能的投影方法，将步骤c1中得到的新的柔性线缆位形投影为稳定的柔性线缆位形。[0132]步骤c3：利用机械臂逆运动学求解所述柔性线缆在所述稳定的柔性线缆位形的末端位姿对应的双机械臂的位形，所述双机械臂的位形表征双机械臂的关节角。[0133]步骤c4：在逆运动学求解成功且碰撞检测结果为无碰撞的情况下，基于所述稳定的柔性线缆位形和所述双机械臂的位形生成随机树的扩展节点，所述随机树的扩展节点包括：第一棵随机树扩展节点和第二棵随机树扩展节点。[0134]本实施例中，利用机械臂逆运动学求解柔性线缆在稳定的柔性线缆位形的末端位姿对应的双机械臂的位形，如果存在多个解，则选择最接近上一次迭代生长机械臂位形的解作为双机械臂的位形。然后在步骤c4中根据稳定的柔性线缆位形和双机械臂的位形进行碰撞检测，以判断生成的稳定的柔性线缆位形和双机械臂的位形是否可行。[0135]在一种可选的实施例中，通过随机采样得到随机采样节点，包括：在原空间中对柔性线缆进行随机采样，得到柔性线缆的原始随机采样位形。柔性线缆的原始随机采样位形不是一个稳定的位形，因此基于局部极小化势能将其投影至稳定位形的流形上，得到一个稳定的随机采样位形。如图6所示，图6示意出了柔性线缆原始随机采样位形和投影后的位形对比图。[0136]在本实施例中，基于约束双向快速探索随机树方法，得到一条由初始位形到目标期望位形之间的一条路径，并且在路径规划过程中通过抛弃其中的有碰撞随机采样点实现该路径的避障，保证了柔性线缆操作的可行性。[0137]在一种可选的实施例中，当局部控制器的柔性线缆操作陷入局部最优时，所述方法还包括：[0138]步骤d1：接收并响应于所述局部控制器发送的重新规划请求，根据柔性线缆的当前位形和目标期望位形进行全局规划，得到新的全局规划路径。[0139]步骤d2：将所述新的全局规划路径发送至所述局部控制器，以使所述局部控制器将所述新的全局规划路径作为引导路径，通实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。[0140]本实施例中，接收局部控制器发送的重新规划请求，其中，该重新规划请求中包含：柔性线缆的当前的位形和目标期望位形、双机械臂当前的位形和目标位形，进而全局规划器根据柔性线缆的当前位形和目标期望位形、双机械臂当前位形和目标位形进行全局规划，得到新的全局规划路径。[0141]图7示意出了本实施例中的一种使用双机械臂的受限环境柔性线缆操作方法的整体方案示意图。首先，全局规划器基于柔性线缆能量模型，根据柔性线缆的初始位形和目标期望位形，高效地规划出一条相对粗略的全局规划路径，进而保证柔性线缆操作的可行性。之后，局部控制器将全局规划路径作为引导路径进行跟踪，以双机械臂的实时位形和柔性线缆的实时位形作为反馈，闭环地补偿柔性线缆能量模型建模误差导致的全局规划路径误差，以控制双机械臂移动，直到柔性线缆达到目标期望位形。[0142]在本发实施例中，为了实现双机械臂在有障碍受限环境中将柔性线缆鲁棒地操作至期望位形。首先，接收全局规划器基于约束双向快速探索随机树方法，根据柔性线缆的初始位形和目标期望位形进行全局规划，得到的全局规划路径；然后将全局规划路径作为引导路径，通过实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。与现有技术相比，本发明实施例考虑了实际应用场景中柔性线缆和机械臂本身之间的可能碰撞并实现避障；有效结合了离线全局规划与局部反馈控制，实现了保障整体路径可靠的同时极大地减小了执行误差；能够直接应用于不同长度、粗细、材质的柔性线缆操作任务，对柔性线缆建模误差鲁棒。[0143]参照图8所示，示出了本发明实施例的一种使用双机械臂的受限环境柔性线缆操作系统的结构框图，该使用双机械臂的受限环境柔性线缆操作系统用户执行上述实施例所述的使用双机械臂的受限环境柔性线缆操作方法，如图8所示，所述系统具体可以包括：全局规划器和局部控制器，具体地：[0144]全局规划器，所述全局规划器用于生成全局规划路径，并将所述全局规划路径发送至局部控制器；接收并响应于所述局部控制器发送的重新规划请求，规划新的全局规划路径，将所述新的全局规划路径发送至所述局部控制器；[0145]局部控制器，所述局部控制器用于接收所述全局规划器发送的全局规划路径，将所述全局规划路径作为引导路径，通实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形；向所述全局规划器发送重新规划请求，接收所述全局规划器发送的新的全局规划路径，并将所述新的全局规划路径作为引导路径，通实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。[0146]本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。[0147]本发明实施例是参照根据本发明实施例的方法和系统的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理终端设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理终端设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。[0148]这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。[0149]这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上，使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。[0150]尽管已描述了本发明实施例的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。[0151]最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。[0152]以上对本发明所提供的一种使用双机械臂的受限环境柔性线缆操作方法和系统进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。当前第1页12当前第1页12
技术特征：
1.一种使用双机械臂的受限环境柔性线缆操作方法，其特征在于，应用于局部控制器，所述方法包括：接收全局规划器发送的全局规划路径，所述全局规划路径是所述全局规划器基于约束双向快速探索随机树方法，根据柔性线缆的初始位形和目标期望位形进行全局规划，得到的全局规划路径；将所述全局规划路径作为引导路径，通过实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。2.根据权利要求1所述的方法，其特征在于，所述将所述全局规划路径作为引导路径，通过实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形，包括：将所述全局规划路径中的每个节点作为中间期望位形；以所述双机械臂的实时位形和所述柔性线缆的实时位形作为反馈，控制所述双机械臂迭代地沿每个节点移动，直到将所述柔性线缆移动到目标期望位形。3.根据权利要求2所述的方法，其特征在于，所述控制所述双机械臂迭代地沿每个节点移动，直到将所述柔性线缆移动到目标期望位形，包括：针对所述全局规划路径中的每个节点，所述局部控制器控制所述双机械臂依次从上一个节点移动到当前节点；在所述柔性线缆的实际位形与当前节点的期望位形的距离小于设定阈值的情况下，控制所述双机械臂移动到下一个节点。4.根据权利要求3所述的方法，其特征在于，所述局部控制器通过以下方式控制所述双机械臂移动到下一个节点：根据引导柔性线缆向下一个节点的柔性线缆期望位形移动的吸引势场、双机械臂向下一个节点的双机械臂期望位形的吸引势场、柔性线缆的空间排斥势场、双机械臂的空间排斥势场，构建控制代价模型；对所述控制代价模型进行求解，得到局部控制最优解，将所述局部最优解作为所述双机械臂的控制输入，以控制所述双机械臂移动到下一个节点。5.根据权利要求1所述的方法，其特征在于，所述方法还包括：在所述柔性线缆未达到目标期望位形，但所述局部控制器输入接近零的情况下，向所述全局规划器发送重新规划请求，以使全局规划器根据柔性线缆的当前位形和目标期望位形进行全局规划，得到新的全局规划路径；接收所述全局规划器发送的新的全局规划路径，并将所述新的全局规划路径作为引导路径，通过实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。6.一种使用双机械臂的受限环境柔性线缆操作方法，其特征在于，应用于全局规划器，所述方法包括：基于约束双向快速探索随机树方法，根据柔性线缆的初始位形和目标期望位形进行全局规划，得到全局规划路径，所述全局规划路径由多个节点组成；将所述全局规划路径发送至局部控制器，以使所述局部控制器将所述全局规划路径作为引导路径，通实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标
期望位形。7.根据权利要求6所述的方法，其特征在于，所述基于约束双向快速探索随机树方法，根据柔性线缆的初始位形和目标期望位形进行全局规划，得到全局规划路径，包括：两棵随机树分别从开始节点和目标节点出发，向彼此迭代生长，直到所述两棵随机树相连时停止迭代生长，其中，所述两棵随机树上的每个节点包括：柔性线缆的位形和双机械臂的位形；从所述两棵随机树中提取出从所述开始节点到所述目标节点的路径，并对所述路径进行平滑处理，得到全局规划路径。8.根据权利要求7所述的方法，其特征在于，所述两棵随机树分别从开始节点和目标节点出发，向彼此迭代生长，直到所述两棵随机树相连时停止迭代生长，包括：针对每次迭代，通过随机采样得到随机采样节点；从第一棵随机树中找出离所述随机采样节点最近的第一节点，由所述第一节点向所述随机采样节点扩展生长，得到所述第一棵随机树扩展节点，并从所述第二棵随机树中找出离所述第一棵随机树扩展节点最近的第二节点，由所述第二节点向所述第一棵随机树扩展节点方向扩展生长，得到所述第二棵随机树扩展节点，并判断所述第一棵随机树扩展节点与所述第二棵随机树扩展节点之间是否能够相连；在相连的情况下，结束迭代生长；在不相连的情况下，从所述第二棵随机树开始进行下一次的迭代生长。9.根据权利要求8所述的方法，其特征在于，随机树中的节点通过以下方式实现扩展生长：将上一次迭代生长到达的柔性线缆位形向目标扩展位形的方向进行插值，得到新的柔性线缆位形，所述上一次迭代生长到达的柔性线缆位形是指所述第一节点或所述第二节点对应的柔性线缆位形，所述目标扩展位形是指所述随机采样点或所述第一棵随机树扩展节点对应的柔性线缆位形；基于势能的投影方法将所述新的柔性线缆位形投影成一个稳定的柔性线缆位形；利用机械臂逆运动学求解所述柔性线缆在所述稳定的柔性线缆位形的末端位姿对应的双机械臂的位形，所述双机械臂的位形表征双机械臂的关节角；在逆运动学求解成功且碰撞检测结果为无碰撞的情况下，基于所述稳定的柔性线缆位形和所述双机械臂的位形生成随机树的扩展节点，所述随机树的扩展节点包括：第一棵随机树扩展节点和第二棵随机树扩展节点。10.根据权利要求6所述的方法，其特征在于，所述方法还包括：接收并响应于所述局部控制器发送的重新规划请求，根据柔性线缆的当前位形和目标期望位形进行全局规划，得到新的全局规划路径；将所述新的全局规划路径发送至所述局部控制器，以使所述局部控制器将所述新的全局规划路径作为引导路径，通实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。11.根据权利要求6-10任一所述的方法，其特征在于，所述全局规划路径是基于柔性线缆能量模型进行规划得到的，所述柔性线缆能量模型通过以下方式构建得到的：以所述柔性线缆上均匀分布的m个特征点的位置来表征所述柔性线缆的位形，根据弹
簧质点模型对所述柔性线缆进行建模，得到柔性线缆能量模型，所述柔性线缆能量模型表示为：其中，e为柔性线缆能量模型的弹性势能，λ1为1型弹簧刚度，λ2为2型弹簧的刚度，x
k
表示第k个特征点，x
k+1
表示第k+1个特征点，x
k+2
表示第k+2个特征点，l为柔性线缆的长度。12.一种使用双机械臂的受限环境柔性线缆操作系统，其特征在于，用于执行权利要求1-11任一所述的使用双机械臂的受限环境柔性线缆操作方法，所述系统包括：全局规划器，所述全局规划器用于生成全局规划路径，并将所述全局规划路径发送至局部控制器；接收并响应于所述局部控制器发送的重新规划请求，规划新的全局规划路径，将所述新的全局规划路径发送至所述局部控制器；局部控制器，所述局部控制器用于接收所述全局规划器发送的全局规划路径，将所述全局规划路径作为引导路径，通过实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形；向所述全局规划器发送重新规划请求，接收所述全局规划器发送的新的全局规划路径，并将所述新的全局规划路径作为引导路径，通过实时反馈闭环控制的方式控制双机械臂运动，直到所述柔性线缆达到目标期望位形。

技术总结
本发明实施例提供了一种使用双机械臂的受限环境柔性线缆操作方法和系统，所述方法包括：接收全局规划器发送的全局规划路径，所述全局规划路径是所述全局规划器基于约束双向快速探索随机树方法，根据柔性线缆的初始位形和目标期望位形进行全局规划，得到的全局规划路径；将全局规划路径作为引导路径，通实时反馈闭环控制的方式控制双机械臂运动，直到柔性线缆达到目标期望位形。本发明实施例中，考虑了实际应用场景中柔性线缆和双机械臂本身之间的可能碰撞并实现避障；有效结合了离线全局规划与局部反馈控制，实现了保障整体路径可靠的同时极大地减小了执行误差；能够直接应用于不同长度、粗细、材质的柔性线缆操作任务。材质的柔性线缆操作任务。材质的柔性线缆操作任务。


技术研发人员：李翔 于铭瑞 吕康晨 宋士吉
受保护的技术使用者：清华大学
技术研发日：2023.06.09
技术公布日：2023/8/30