CN112264989B - 一种基于邻域遍历的双机械臂协同避障方法 - Google Patents

一种基于邻域遍历的双机械臂协同避障方法 Download PDF

Info

Publication number
CN112264989B
CN112264989B CN202010550353.4A CN202010550353A CN112264989B CN 112264989 B CN112264989 B CN 112264989B CN 202010550353 A CN202010550353 A CN 202010550353A CN 112264989 B CN112264989 B CN 112264989B
Authority
CN
China
Prior art keywords
mechanical arm
obstacle
target point
double
joint
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010550353.4A
Other languages
English (en)
Other versions
CN112264989A (zh
Inventor
向先波
熊昕飏
杨少龙
张琴
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Huazhong University of Science and Technology
Original Assignee
Huazhong University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Huazhong University of Science and Technology filed Critical Huazhong University of Science and Technology
Priority to CN202010550353.4A priority Critical patent/CN112264989B/zh
Publication of CN112264989A publication Critical patent/CN112264989A/zh
Application granted granted Critical
Publication of CN112264989B publication Critical patent/CN112264989B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
    • B25J9/023—Cartesian coordinate type
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

本发明针对在复杂的野外或太空环境及深海中作业的双臂机械手难以人工控制、容易与障碍物发生碰撞的问题，提出一种双机械臂协同避障算法，首先运用拉格朗日极值法和椭球体包围盒法建立机械臂与障碍物在工作空间中的模型，求解障碍物与双机械臂模型的数学表达式，其次运用本发明提出的邻域遍历避障方法，遍历机械臂当前位姿的相邻工作空间，通过碰撞检测算法筛选出无碰撞且朝向目标点运动距离最短的路径，引入虚拟目标点避免机械臂陷入局部极小值时的振荡运动问题，最后，机械臂末端执行器将沿该相对最优路径无碰撞地运动至目标点处。同时，双机械臂运动时，彼此之间视为椭球体包围盒法下的障碍物模型，可解决协同作业时双机械臂之间的避碰问题。

Description

一种基于邻域遍历的双机械臂协同避障方法
技术领域
本发明属于机械臂运动规划领域，更具体地，涉及一种基于邻域遍历的双机械臂协同避障方法。
背景技术
随着科技的发展和人类生产生活需求的不断提高，自主作业机器人在各个领域的应用越来越重要。在一些复杂的野外或太空环境及深海环境中，人类需要进行勘探、管道检测、设备维修等充满危险的工作，难以保证从业人员的生命安全。机器人可以在很大程度上替代人类进行上述工作，但由于所处工作环境复杂且危险，仅依靠操作员进行远程操控及其容易对设备造成损坏，并且专业操作员的培养也十分困难。从而，根据指定任务进行自主作业成为了此类机器人必须突破的难题。在机器人自主作业中，机械臂的自主避障控制是机器人安全执行任务的保障，因此，一种有效的机械臂协同避障自主算法是必要的。
此外，随着科考、维修、海底探缆等技术的发展，双机械臂对于在复杂环境工作的机器人的重要性也在逐步提升。双机械臂与单机械臂对比在以下方面更有优势：首先，双机械臂可以承受更大的负载；其次，双机械臂可完成单机械臂无法完成的任务，例如一只机械手通过抓取锚定，固定机器人基座，另一手完成作业，或者一只手对目标进行抓取或固定，另一只手对抓取的目标进行维修、取样等其他操作，提高作业效率和作业能力；最后，双机械臂对抓取目标物的形状适应性更强，可通过双手环抱的方式完成对球体或其他不规则物体的抓取任务。但是，在双机械臂的协同运动控制中一个需要解决的重要问题是如何避免两个机械臂之间“打架”的问题，即双机械臂之间的避碰问题。
目前，运用在机械臂路径规划中的传统机器人路径规划方法基本可达到避障的目的，例如人工势场法、快速扩展随机树(RRT)法等。但对于具有多连杆复杂结构的机械臂来说，人工势场法在势函数的选择、人工势能的计算上都相对复杂，计算量较大；而RRT算法由于随机搜索节点的特点，需要大量生成随机树节点的运算，使得运算量巨大，且生成的路径通常不是最优的。除此之外，还有基于生物启发式避障算法，包括神经网络、遗传算法、粒子群算法和蚁群算法等。这类路径规划的算法复杂度较大，只能应用于静态环境下的离线路径规划，在需要实时进行路径规划的工作场景中难以实现。
发明内容
本发明的目的在于提供一种双机械臂协同避障方法，以解决装备于在一些复杂的野外或太空环境及深海环境中工作的机器人的机械臂运动控制的技术问题。该方法基于邻域遍历算法，算法思路清晰简洁，生成机械臂运动路径相对较优，可实现双机械臂在复杂环境中的自主避障作业，同时满足双臂之间避碰要求。
为了实现上述目的，本发明采用的技术方案包括以下步骤：
(1)双机械臂在复杂环境中运动，已知机械臂的起始关节构型和目标点坐标，求解出机械臂的所有目标关节构型，并采用拉格朗日极值法和椭球体包围盒法建立环境中的障碍物模型，所述障碍物模型包括环境障碍物及机械臂连杆本身(双机械臂互视为障碍物的情况)；
(2)依次遍历各个机械臂连杆(即机械臂的各臂，包括大臂和小臂)的相邻工作空间(即邻域空间)，对各机械臂进行碰撞检测，选取与障碍物模型无干涉且机械臂末端执行器与目标点距离最短的中间关节构型，并控制各机械臂运动至相应的中间关节构型处；
(3)在机械臂运动过程中，若机械臂运动出现局部振荡，则视为机械臂陷入局部极小值点，通过引入虚拟目标点的方法使机械臂越过该局部极小值点；
(4)重复上述步骤(2)-(3)，直至机械臂执行器末端运动至目标点邻域空间，判定机械臂已完成避障，使机械臂运动至相应目标关节构型，完成避障运动。
所述步骤(1)采用逆运动学模型求解机械臂的所有目标关节构型，从而构造邻域遍历所需关节构型序列。
所述步骤(1)采用拉格朗日极值法和椭球体包围盒法建立障碍物模型，模型表达式的具体求法为：
首先给出椭球体包围盒的表达式：
Figure BDA0002542307070000031
式中，x0yoz0分别为椭球体中心点的坐标，abc为椭球体的三个轴长；将障碍物向笛卡尔坐标系的三个平面内投影，得到障碍物在xyz轴投影的最大最小值xminxmaxyminymaxzminzmax，可得椭球体中心点的坐标值：
Figure BDA0002542307070000032
Figure BDA0002542307070000033
Figure BDA0002542307070000034
选取(xmax，ymax，zmax)这一点，利用拉格朗日求极值的方法算出三个轴长abc，得到椭球的体积公式：
Figure BDA0002542307070000035
将选取的(xmax，ymax，zmax)代入椭球体的表达式：
Figure BDA0002542307070000041
拉格朗日函数：
L＝V+kF
对上式分别求出关于abc的偏导数，并令其为零，同时令F也为零，得到如下方程组：
Figure BDA0002542307070000042
由以上方程组求得椭球体的轴长abc，从而得到椭球体包围盒的表达式。
所述步骤(2)中机械臂邻域遍历方法为：取固定步长θ依次遍历各个关节当前状态附近±nθ的工作空间，n表示机械臂关节沿某一旋转方向遍历的步数，并记录在每个关节构型下机械臂末端执行器与目标点的距离Li；如果在第m(m＜n)个关节构型下机械臂与障碍物发生了干涉，则舍弃第m个关节构型，并且不再继续遍历剩下的工作空间，选择Li中的最小值作为机械臂执行器末端当前的运动目标，使机械臂运动至相应的中间关节构型。
在上述方法中，检测机械臂是否与障碍物发生干涉的方法为：
已知机械臂的各个关节角，通过机械臂正运动学模型解得机械臂每个关节的坐标，得到每个连杆线段的表达式：
Figure BDA0002542307070000043
将线段的表达式代入椭球体包围盒的方程式可得：
Figure BDA0002542307070000051
求解k值，对k值做如下分析以判断机械臂连杆是否与障碍物发生干涉：机械臂与障碍物发生干涉的情况有：(1)k有唯一解，0≤k≤1；(2)k有两解，0≤k1≤1或者0≤k2≤1；机械臂不与障碍物发生干涉的情况有：(1)k无解；(2)k有唯一解，k＜0或者k＞1；(3)k有两解，(k1＜0或者k1＞1)并且(k2＜0或者k2＞1)。
所述步骤(2)在对机械臂进行碰撞检测时，双机械臂视彼此为环境障碍物，该类“机械臂”环境障碍物的模型采用步骤(1)中所述的拉格朗日极值法和椭球体包围盒法建立，从而在机械臂避开障碍物的基础上还可基于邻域遍历方法实现双臂之间的互相避碰。
所述步骤(3)中局部极小值及虚拟目标点的详细说明为：当机械臂陷入局部极小值点时，机械臂的任意运动都会使末端执行器与目标点之间的距离L′增大，从而导致机械臂在局部空间内振荡；解决该问题的方法为：改变目标点与机械臂之间的位置关系；虽无法直接改变目标点位置，但可以在适当位置引入一个虚拟目标点，并使末端执行器运动至该虚拟目标点后再运动至真实目标点；通过引入一个虚拟目标，该过程间接改变了目标点与末端执行器之间的位置关系。
所述步骤(4)判断机械臂执行器末端运动至目标点邻域空间方法：设存在一个足够小的值ε(尺度远小于环境障碍物)，当末端执行器与目标点之间的距离L′＜ε时，认为末端执行器与目标点距离足够近，完成避障动作。
本发明的有益效果在于：
本发明针对机械臂的运动特性，设计出了一种基于邻域遍历的机械臂避障算法。该算法通过遍历机械臂当前关节构型的相邻工作空间，搜索出机械臂的下一较优关节构型，该较优关节构型满足：(1)与环境障碍物不发生干涉，且双臂之间不发生干涉；(2)末端执行器与目标点的距离在该次遍历中最短。本算法思路简单，便于实现，且相对于目前常用的基于快速扩展随机树的RRT算法，本算法无需随机生成大量无用节点，且生成的运动路径相对较优。在水下环境中，路径越短，对机器人造成的扰动就越小，对机器人位姿的影响也越小，需要对机器人系统做出的动力补偿就越小，或对机械臂运动路径的重规划越少，这有助于提高机器人系统在水下环境中运动的稳定性。
本发明针对在一些复杂的野外或太空环境及深海环境中机械臂难以操作的技术难题，提出的邻域遍历方法可实现双机械臂自主避障运动控制，避免了人为操作带来的危险。
本发明提出的算法在碰撞检测环节不仅检测机械臂与障碍物是否发生碰撞，同时检测双臂之间是否发生干涉。该检测方法可有效避免双机械臂在协同作业中相互碰撞的问题。
本发明针对机械臂运动过程中易陷入局部极小值点的问题，提出生成虚拟目标点的解决方法，间接改变了目标点与末端执行器之间的位置关系，从而使本避障算法跳出局部极小值点，完成机械臂避障运动。
附图说明
图1为本发明方法的流程示意图。
图2为实施例中双机械臂实物图。
图3为实施例中双机械臂结构示意图。
图4为实施例中双机械臂简化坐标系示意图。
图5为邻域遍历算法示意图。
图6为实施例中双机械臂在进行碰撞检测时双臂彼此视为障碍物的示意图。
图7为实施例中双机械臂自主避障运动仿真示意图。
具体实施方式
为了更好的理解本发明，下面结合附图和实施例对本发明的技术方案作详细的描述。
如图1所示为本发明算法的具体实施流程图。
如图2所示为实施例中双机械臂实物图。该双机械臂具体参数如下表所示。
表1双机械臂参数(双臂间距600mm)
Figure BDA0002542307070000071
如图3所示，可将该双机械臂划分为肩关节(肩部分)、大臂关节(大臂部分)、小臂关节(小臂部分)、手腕关节(腕部分)、爪关节(爪部分)五个部分。
所述肩部分包括：肩关节电机(115)、底座(135)、垫板(147)、轴承盖(145)、传动轴1(146)、电机盖(144)、隔垫；垫板(147)一端与水下无人潜航器(搭载物)连接，另一端与底座(135)连接，轴承盖(145)、电机盖(144)安装在底座(135)两侧，肩关节电机(115)安装于底座(135)一侧，与传动轴1(146)通过电机轴连接，传递电机转动。
所述大臂部分包括：大臂关节电机(119)、短臂(134)、轴承盖(145)、传动轴2(149)、电机盖(144)、大臂(84)、隔垫；短臂(134)与传动轴1(146)连接，大臂关节电机(119)和传动轴2(149)安装在短臂(134)和大臂(84)交界圆处，通过电机轴连接，轴承盖(145)和电机盖(144)安装在短臂(134)两侧，隔垫安装于轴承和大臂(84)之间。
所述小臂部分包括：小臂关节电机(126)、小臂(83)、轴承盖(145)、传动轴3(86)、电机盖(144)、隔垫；小臂关节电机(126)和传动轴3(86)安装在小臂(83)与大臂(84)交界圆处，通过电机轴连接，轴承盖(145)和电机盖(144)安装在小臂(83)两侧，隔垫安装于轴承和小臂(83)之间。
所述腕部分包括：腕关节电机(79)、轴套、法兰(12)，轴承座(38)、滑套(12)、传动盖(34)、小盖(37)、箱体(16)；腕关节电机(79)安装在小臂(83)内部，与法兰相连接；传动盖(34)与腕关节电机(79)轴相连，传递扭矩；传动轴与轴承座(38)相连，同时轴承座(38)与箱体(16)相连；使得电机转动能够带动箱体(16)及爪部(1)一并发生转动。
所述爪部分包括：爪关节电机(17)、滑套(12)、T型丝杆(33)、法兰、拨块(11)、小拨块(9)、推杆(8)、导套、支架(6)、隔套、销轴(3)、连杆(5)、爪部(1)；T型丝杆(33)与电机轴相连接，传递扭矩；爪关节电机(17)带动T型丝杆(33)旋转，带动拨块(11)沿螺纹向上下运动；小拨块(9)固定在拨块(11)一侧凹口处，另一侧凸出部分卡在推杆(8)凹口处，使得拨块(11)能够带动推杆(8)沿轴向运动；增加滑套(12)对推杆(8)进行限位，使其只能在支架(6)内部沿轴向进行运动；销轴(3)安装在推杆(8)另一侧，沿支架(6)开孔进行上下运动，带动其上连杆(5)转动；连杆(5)一端与爪部(1)相连，另一端同爪部(1)一角一同连接在销轴(3)上，连杆(5)发生运动时，自然带动爪部(1)产生张开和闭合运动。
由实物及相应参数可将该机械臂简化为如图4所示机械臂坐标系示意图，其中，z轴为各关节的旋转轴。
针对本实例使用MATLAB及其中的Robotics Toolbox对避障算法进行仿真实验验证。
首先，设定ε＝10mm，当末端执行器(即手爪部分)与目标点距离L′＜ε时跳出循环，遍历步进步长设置为2°，遍历范围设置为±10°。
由于手腕关节角度不会对末端执行器的位置产生影响，因此该实例只考虑前三个关节的运动。机械臂起始关节构型设定为qr0＝ql0＝[0°0°0°]，分别设置右臂、左臂目标点为(单位：mm)：
prgoal＝[649-970-471]
plgoal＝[649970-471]
通过双机械臂的逆运动学模型，分别求解出右臂、左臂的所有可能存在的目标构型：
qr1＝[-36.0°-31.3°-20.3°]，qr2＝[-36.0°-60.2°20.3°]
ql1＝[-36.0°60.2°-20.3°]，ql2＝[-36.0°31.3°20.3°]
分别设置右方、左方障碍物模型信息为(单位：mm)：
r＝(xmin，xmax，ymin，ymax，zmin，zmax)＝(700，900，-800，-600，-300，100)
l＝(xmin，xmax，ymin，ynax，zmin，zmax)＝(700，900，600，800，-300，100)
首先给出椭球体包围盒的表达式：
Figure BDA0002542307070000091
式中，x0yoz0分别为椭球体包围盒中心点的坐标，abc分别为椭球体包围盒的三个轴长。已知障碍物在xyz轴投影的最大值、最小值为xminxmaxyminymaxzminzmax，可得椭球体中心点的坐标值：
Figure BDA0002542307070000092
Figure BDA0002542307070000093
Figure BDA0002542307070000094
选取(xmax，ymax，zmax)这一点，利用拉格朗日求极值的方法算出三个轴长abc；椭球的体积公式：
Figure BDA0002542307070000101
将选取的(xmax，ymax，zmax)代入椭球体包围盒的表达式：
Figure BDA0002542307070000102
拉格朗日函数：
L＝V+kF
对上式分别求出关于a b c的偏导数，并令其为零，同时令F也为零，得到如下方程组：
Figure BDA0002542307070000103
由以上方程组求得椭球体的轴长a bc。
求解出右方、左方障碍物的椭球体包围盒模型参数为(单位：mm)：
xr＝800，yr＝-700，zr＝-100
xl＝800，yl＝700，zl＝-100
ar 2＝al 2＝30000，br 2＝bl 2＝30000，cr 2＝cl 2＝120000
由上述参数可得椭球体包围盒的表达式。
对机械臂当前关节构型进行邻域遍历的方法：以固定步长2°依次遍历各个关节当前状态附近±10°的工作空间，并记录在每个关节构型下机械臂末端执行器与目标点的距离Li。如果在第m(m＜5)个关节构型下机械臂与障碍物发生了干涉，则舍弃第m个关节构型，并且不再继续遍历剩下的工作空间，选择Li中的最小值作为机械臂执行器末端当前的运动目标，使机械臂运动至相应的中间关节构型。该算法示意图如图5所示。
在上述方法中，检测机械臂是否与障碍物发生干涉的方法为：
已知机械臂的各个关节角，通过机械臂正运动学模型解得机械臂每个关节的坐标，得到每个连杆线段的表达式：
Figure BDA0002542307070000111
将线段的表达式代入椭球体的方程式可得：
Figure BDA0002542307070000112
求解k值，对k值做如下分析以判断机械臂连杆是否与障碍物发生干涉：机械臂与障碍物发生干涉的情况有：(1)k有唯一解，0≤k≤1；(2)k有两解，0≤k1≤1或者0≤k2≤1。机械臂不与障碍物发生干涉的情况有：(1)k无解；(2)k有唯一解，k＜0或者k＞1；(3)k有两解，(k1＜0或者k1＞1)并且(k2＜0或者k2＞1)。
同时，在对机械臂进行碰撞检测时，双机械臂视彼此为环境障碍物，如图6所示，从而在机械臂避障的基础上实现双臂之间的避碰。
重复上述遍历步骤，直到末端执行器与目标点之间的距离L′＜ε时，认为末端执行器与目标点距离足够近，完成避障动作。将此状态下机械臂的关节构型与通过逆运动学解得的所有目标关节构型进行对比，并使机械臂运动至与当前状态相近的目标关节构型，完成避障运动。由MATLAB仿真的双机械臂避障过程如图7所示。在该仿真过程中，图(a)表示系统对障碍物及双机械臂使用椭球体包围盒法进行建模，图(b)至图(g)表示双机械臂邻域遍历避障过程，图(g)表示双机械臂末端执行器与目标点的距离足够近，认为机械臂已完成避障动作，图(h)表示双机械臂运动至目标关节构型，末端执行器到达目标点。在双机械臂进行邻域遍历避障的过程中，从图(b)和图(c)可看出双机械臂陷入局部极小值点，发生了局部振荡，因此在双机械臂的工作间内随机引入虚拟目标点，从图(d)可看出双机械臂成功脱离局部极小值点。从图(f)可看出双机械臂成功越过障碍物，完成避障，最终末端执行器顺利到达目标点。
本发明公开了一种面向多领域工作环境(陆上、太空中、水下)的双机械臂协同避障算法，属于自动控制领域。本发明针对在复杂的野外或太空环境及深海中作业的双臂机械手难以人工控制、容易与障碍物发生碰撞的问题，提出一种双机械臂协同避障算法，首先运用拉格朗日极值法和椭球体包围盒法建立机械臂与障碍物在工作空间中的模型，求解障碍物与双机械臂模型的数学表达式，其次运用本发明提出的邻域遍历避障方法，遍历机械臂当前位姿的相邻工作空间，通过碰撞检测算法筛选出无碰撞且朝向目标点运动距离最短的路径，引入虚拟目标点避免机械臂陷入局部极小值时的振荡运动问题，最后，机械臂末端执行器将沿该相对最优路径无碰撞地运动至目标点处。同时，双机械臂运动时，彼此之间视为椭球体包围盒法下的障碍物模型，可解决协同作业时双机械臂之间的避碰问题。本发明从理论上解决了在复杂的野外或太空环境及深海中作业的双臂机械手自主协同操控的难题，可实现双臂机械手自主躲避障碍物及双机械臂之间智能避碰，达到双臂机械手协同自主安全作业的目的。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于邻域遍历的双机械臂协同避障方法，其特征在于包含以下步骤：
(1)双机械臂在复杂环境中运动，已知机械臂的起始关节构型和目标点坐标，求解出机械臂的所有目标关节构型，并采用拉格朗日极值法和椭球体包围盒法建立环境中的障碍物模型，所述障碍物模型包括环境障碍物及机械臂连杆本身，即含将双机械臂互视为障碍物的情况；
(2)依次遍历各个机械臂连杆的相邻工作空间，即邻域空间，对各机械臂进行碰撞检测，选取与障碍物模型无干涉且机械臂末端执行器与目标点距离最短的中间关节构型，并控制各机械臂运动至相应的中间关节构型处；
(3)对于双机械臂之间的避碰规划，主机械臂将从机械臂视为障碍物，在完成单次邻域遍历循环后，立即运动至相应最优中间构型，随后从机械臂将主机械臂视为障碍物进行单次邻域遍历循环，同样立即运动至相应最优中间构型，重复上述过程实现实时的双机械臂之间的避碰运动规划；
(4)在机械臂运动过程中，若机械臂运动出现局部振荡，则视为机械臂陷入局部极小值点，通过引入虚拟目标点的方法使机械臂越过该局部极小值点；
(5)重复上述步骤(2)-(4)，直至机械臂执行器末端运动至目标点邻域空间，判定机械臂已完成避障，使机械臂运动至相应目标关节构型，完成避障运动；
所述步骤(1)采用逆运动学模型求解机械臂的所有目标关节构型，从而构造邻域遍历所需关节构型序列；
所述步骤(2)中机械臂邻域遍历方法为：取固定步长θ依次遍历各个关节当前状态附近±nθ的工作空间，n表示机械臂关节沿某一旋转方向遍历的步数，并记录在每个关节构型下机械臂末端执行器与目标点的距离Li；如果在第m个关节构型下机械臂与障碍物发生了干涉，m<n，则舍弃第m个关节构型，并且不再继续遍历剩下的工作空间，选择Li中的最小值作为机械臂执行器末端当前的运动目标，使机械臂运动至相应的中间关节构型；
所述步骤(4)中局部极小值及虚拟目标点的详细说明为：当机械臂陷入局部极小值点时，机械臂的任意运动都会使末端执行器与目标点之间的距离L′增大，从而导致机械臂在局部空间内振荡；解决该问题的方法为：改变目标点与机械臂之间的位置关系；虽无法直接改变目标点位置，但可以在适当位置引入一个虚拟目标点，并使末端执行器运动至该虚拟目标点后再运动至真实目标点；通过引入一个虚拟目标，该过程间接改变了目标点与末端执行器之间的位置关系；
所述步骤(5)判断机械臂执行器末端运动至目标点邻域空间方法：设存在一个足够小的值ε，即尺度远小于环境障碍物，当末端执行器与目标点之间的距离L′<ε时，认为末端执行器与目标点距离足够近，完成避障动作。
2.根据权利要求1所述的一种基于邻域遍历的双机械臂协同避障方法，其特征在于：所述步骤(1)采用拉格朗日极值法和椭球体包围盒法建立障碍物模型，模型表达式的具体求法为：
首先给出椭球体包围盒的表达式：
Figure FDA0004210344650000021
式中，x0y0z0分别为椭球体中心点的坐标，abc为椭球体的三个轴长；将障碍物向笛卡尔坐标系的三个平面内投影，得到障碍物在xyz轴投影的最大最小值xmin、xmax、ymin、ymax、zmin、zmax，可得椭球体中心点的坐标值：
Figure FDA0004210344650000031
Figure FDA0004210344650000032
Figure FDA0004210344650000033
选取(xmax,ymax,zmax)这一点，利用拉格朗日求极值的方法算出三个轴长abc，得到椭球的体积公式：
Figure FDA0004210344650000034
将选取的(xmax,ymax,zmax)代入椭球体的表达式：
Figure FDA0004210344650000035
拉格朗日函数：
L＝V+kF
对上式分别求出关于abc的偏导数，并令其为零，同时令F也为零，得到如下方程组：
Figure FDA0004210344650000036
由以上方程组求得椭球体的轴长abc，从而得到椭球体包围盒的表达式。
3.根据权利要求1所述的一种基于邻域遍历的双机械臂协同避障方法，其特征在于：检测机械臂是否与障碍物发生干涉的方法为：
已知机械臂的各个关节角，通过机械臂正运动学模型解得机械臂每个关节的坐标，得到每个连杆线段的表达式：
Figure FDA0004210344650000041
将线段的表达式代入椭球体包围盒的方程式可得：
Figure FDA0004210344650000042
求解k值，对k值做如下分析以判断机械臂连杆是否与障碍物发生干涉：机械臂与障碍物发生干涉的情况有：(1)k有唯一解，0≤k≤1；(2)k有两解，0≤k1≤1或者0≤k2≤1；机械臂不与障碍物发生干涉的情况有：(1)k无解；(2)k有唯一解，k<0或者k>1；(3)k有两解，k1<0或者k1>1并且k2<0或者k2>1。
CN202010550353.4A 2020-06-17 2020-06-17 一种基于邻域遍历的双机械臂协同避障方法 Active CN112264989B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010550353.4A CN112264989B (zh) 2020-06-17 2020-06-17 一种基于邻域遍历的双机械臂协同避障方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010550353.4A CN112264989B (zh) 2020-06-17 2020-06-17 一种基于邻域遍历的双机械臂协同避障方法

Publications (2)

Publication Number Publication Date
CN112264989A CN112264989A (zh) 2021-01-26
CN112264989B true CN112264989B (zh) 2023-06-06

Family

ID=74349062

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010550353.4A Active CN112264989B (zh) 2020-06-17 2020-06-17 一种基于邻域遍历的双机械臂协同避障方法

Country Status (1)

Country Link
CN (1) CN112264989B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112773506A (zh) * 2021-01-27 2021-05-11 哈尔滨思哲睿智能医疗设备有限公司 一种腹腔镜微创手术机器人的碰撞检测方法及装置
CN112906118A (zh) * 2021-03-12 2021-06-04 河北工业大学 一种虚实耦合环境下的建筑机器人远程作业方法
CN113741424A (zh) * 2021-08-04 2021-12-03 深圳市普渡科技有限公司 机器人协同避障系统、方法、机器人及存储介质
CN113715027A (zh) * 2021-09-13 2021-11-30 哈工大机器人（合肥）国际创新研究院 7自由度双臂协作机器人防止自碰撞轨迹规划方法及系统
CN113696187B (zh) * 2021-10-22 2022-01-25 成都飞机工业（集团）有限责任公司 一种适用于双机器人系统的防碰撞方法
CN114454178B (zh) * 2022-03-15 2023-11-24 南京航空航天大学 面向异质整体制造的双机器人协同路径规划方法及系统
CN114700944B (zh) * 2022-04-06 2023-11-24 南京航空航天大学 一种面向异种任务的双机器人协同路径规划方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003280710A (ja) * 2002-03-20 2003-10-02 Japan Atom Energy Res Inst ロボットハンドの作業軌道の生成と制御方法
CN102161198A (zh) * 2011-03-18 2011-08-24 浙江大学 用于三维空间中移动机械臂路径规划的主从式协进化方法
CN102646148A (zh) * 2012-04-25 2012-08-22 浙江大学 仿人机器人机械臂避碰运动轨迹规划方法
CN106182019A (zh) * 2016-07-29 2016-12-07 中国科学技术大学 工业机器人抓取过程的动态避障系统及其方法
CN108555911A (zh) * 2018-04-22 2018-09-21 北京工业大学 基于虚拟推力的遥操作机械臂三维避障方法
CN109986564A (zh) * 2019-05-20 2019-07-09 上海应用技术大学 工业机械臂路径规划方法
CN110228069A (zh) * 2019-07-17 2019-09-13 东北大学 一种机械臂在线避障运动规划方法
CN110696000A (zh) * 2019-11-21 2020-01-17 河北工业大学 一种机械臂试探感知的避障方法
DE102019126465A1 (de) * 2018-10-01 2020-04-02 KBee AG Verfahren und Vorrichtung zur Trajektorienbestimmung für serielle Manipulatoren

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102030141B1 (ko) * 2014-06-11 2019-10-10 현대자동차(주) 로봇의 팔꿈치 제어시스템 및 그 제어방법
CN105479490A (zh) * 2015-12-24 2016-04-13 华中科技大学 一种双机器人实时动态避障装置及其避障方法
CN106647282B (zh) * 2017-01-19 2020-01-03 北京工业大学 一种考虑末端运动误差的六自由度机器人轨迹规划方法
CN108274465A (zh) * 2018-01-10 2018-07-13 上海理工大学 基于优化a*的人工势场机械臂三维避障路径规划方法
CN109461185B (zh) * 2018-09-10 2021-08-17 西北工业大学 一种适用于复杂场景的机器人目标主动避障方法
CN109822554B (zh) * 2019-03-20 2020-09-18 华中科技大学 面向水下的双臂协同抓取、抱取及避碰一体化方法及系统
CN110181515B (zh) * 2019-06-10 2020-12-01 浙江工业大学 一种双机械臂协同装配作业路径规划方法
CN110977982A (zh) * 2019-12-19 2020-04-10 南京理工大学 基于深度视觉的双机械臂控制方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003280710A (ja) * 2002-03-20 2003-10-02 Japan Atom Energy Res Inst ロボットハンドの作業軌道の生成と制御方法
CN102161198A (zh) * 2011-03-18 2011-08-24 浙江大学 用于三维空间中移动机械臂路径规划的主从式协进化方法
CN102646148A (zh) * 2012-04-25 2012-08-22 浙江大学 仿人机器人机械臂避碰运动轨迹规划方法
CN106182019A (zh) * 2016-07-29 2016-12-07 中国科学技术大学 工业机器人抓取过程的动态避障系统及其方法
CN108555911A (zh) * 2018-04-22 2018-09-21 北京工业大学 基于虚拟推力的遥操作机械臂三维避障方法
DE102019126465A1 (de) * 2018-10-01 2020-04-02 KBee AG Verfahren und Vorrichtung zur Trajektorienbestimmung für serielle Manipulatoren
CN109986564A (zh) * 2019-05-20 2019-07-09 上海应用技术大学 工业机械臂路径规划方法
CN110228069A (zh) * 2019-07-17 2019-09-13 东北大学 一种机械臂在线避障运动规划方法
CN110696000A (zh) * 2019-11-21 2020-01-17 河北工业大学 一种机械臂试探感知的避障方法

Also Published As

Publication number Publication date
CN112264989A (zh) 2021-01-26

Similar Documents

Publication Publication Date Title
CN112264989B (zh) 一种基于邻域遍历的双机械臂协同避障方法
US9411335B2 (en) Method and apparatus to plan motion path of robot
Barbazza et al. Trajectory planning of a suspended cable driven parallel robot with reconfigurable end effector
CN112068576A (zh) 一种基于任务分级时序优化的水下无人艇-双机械臂协同控制方法
CN105922273A (zh) 一种用于钢构体保养维护的机器人
CN111300408B (zh) 一种结合形似与神似的仿人双臂机器人运动规划控制方法
CN109917793B (zh) 一种管片拼装机避障路径规划方法及装置
CN116673963A (zh) 面向无序断路器零件的双机械臂协作柔性装配系统及方法
Chen et al. Path planning for obstacle avoidance of manipulators based on improved artificial potential field
CN111421540A (zh) 一种机械臂运动控制方法
JPH0789286B2 (ja) 多関節マニピユレ−タの作業原点決定方法
Li et al. Path planning of the dual-arm robot based on VT-RRT algorithm
Li et al. Industrial robot optimal time trajectory planning based on genetic algorithm
Liu et al. Improved RRT path planning algorithm for humanoid robotic arm
Khalid et al. Study of operator behavior, learning, and performance using an input-shaped bridge crane
CN110695994A (zh) 一种面向双臂机械手协同重复运动的有限时间规划方法
Tang et al. Coordinated motion planning of dual-arm space robot with deep reinforcement learning
CN111515954B (zh) 一种机械臂高质量运动路径生成方法
Li et al. Stiffness-maximum trajectory planning of a hybrid kinematic-redundant robot machine
CN116872212A (zh) 一种基于A-Star算法和改进人工势场法的双机械臂避障规划方法
Xu et al. A cable-driven hyperredundant manipulator: Obstacle-avoidance path planning and tension optimization
CN113084797B (zh) 一种基于任务分解的双臂冗余机械臂动态协同控制方法
Pi et al. Multi-representation multi-heuristic A* motion planning for a dual-arm underwater vehicle manipulation system
Yan et al. Integrated Drill Boom Hole-Seeking Control via Reinforcement Learning
CN112936279B (zh) 一种移动作业机器人目标抓取作业最短时间求解方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant