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

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

Info

Publication number
CN112264989A
CN112264989A CN202010550353.4A CN202010550353A CN112264989A CN 112264989 A CN112264989 A CN 112264989A CN 202010550353 A CN202010550353 A CN 202010550353A CN 112264989 A CN112264989 A CN 112264989A
Authority
CN
China
Prior art keywords
mechanical arm
mechanical
obstacle
target point
arm
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.)
Granted
Application number
CN202010550353.4A
Other languages
English (en)
Other versions
CN112264989B (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 (8)

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

Cited By (7)

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

Citations (17)

* 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 浙江大学 仿人机器人机械臂避碰运动轨迹规划方法
KR20150142796A (ko) * 2014-06-11 2015-12-23 현대자동차주식회사 로봇의 팔꿈치 제어시스템 및 그 제어방법
CN105479490A (zh) * 2015-12-24 2016-04-13 华中科技大学 一种双机器人实时动态避障装置及其避障方法
CN106182019A (zh) * 2016-07-29 2016-12-07 中国科学技术大学 工业机器人抓取过程的动态避障***及其方法
CN108274465A (zh) * 2018-01-10 2018-07-13 上海理工大学 基于优化a*的人工势场机械臂三维避障路径规划方法
CN108555911A (zh) * 2018-04-22 2018-09-21 北京工业大学 基于虚拟推力的遥操作机械臂三维避障方法
CN109461185A (zh) * 2018-09-10 2019-03-12 西北工业大学 一种适用于复杂场景的机器人目标主动避障方法
CN109822554A (zh) * 2019-03-20 2019-05-31 华中科技大学 面向水下的双臂协同抓取、抱取及避碰一体化方法及***
US20190184560A1 (en) * 2017-01-19 2019-06-20 Beijing University Of Technology A Trajectory Planning Method For Six Degree-of-Freedom Robots Taking Into Account of End Effector Motion Error
CN109986564A (zh) * 2019-05-20 2019-07-09 上海应用技术大学 工业机械臂路径规划方法
CN110181515A (zh) * 2019-06-10 2019-08-30 浙江工业大学 一种双机械臂协同装配作业路径规划方法
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
CN110977982A (zh) * 2019-12-19 2020-04-10 南京理工大学 基于深度视觉的双机械臂控制方法

Patent Citations (17)

* 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 浙江大学 仿人机器人机械臂避碰运动轨迹规划方法
KR20150142796A (ko) * 2014-06-11 2015-12-23 현대자동차주식회사 로봇의 팔꿈치 제어시스템 및 그 제어방법
CN105479490A (zh) * 2015-12-24 2016-04-13 华中科技大学 一种双机器人实时动态避障装置及其避障方法
CN106182019A (zh) * 2016-07-29 2016-12-07 中国科学技术大学 工业机器人抓取过程的动态避障***及其方法
US20190184560A1 (en) * 2017-01-19 2019-06-20 Beijing University Of Technology A Trajectory Planning Method For Six Degree-of-Freedom Robots Taking Into Account of End Effector Motion Error
CN108274465A (zh) * 2018-01-10 2018-07-13 上海理工大学 基于优化a*的人工势场机械臂三维避障路径规划方法
CN108555911A (zh) * 2018-04-22 2018-09-21 北京工业大学 基于虚拟推力的遥操作机械臂三维避障方法
CN109461185A (zh) * 2018-09-10 2019-03-12 西北工业大学 一种适用于复杂场景的机器人目标主动避障方法
DE102019126465A1 (de) * 2018-10-01 2020-04-02 KBee AG Verfahren und Vorrichtung zur Trajektorienbestimmung für serielle Manipulatoren
CN109822554A (zh) * 2019-03-20 2019-05-31 华中科技大学 面向水下的双臂协同抓取、抱取及避碰一体化方法及***
CN109986564A (zh) * 2019-05-20 2019-07-09 上海应用技术大学 工业机械臂路径规划方法
CN110181515A (zh) * 2019-06-10 2019-08-30 浙江工业大学 一种双机械臂协同装配作业路径规划方法
CN110228069A (zh) * 2019-07-17 2019-09-13 东北大学 一种机械臂在线避障运动规划方法
CN110696000A (zh) * 2019-11-21 2020-01-17 河北工业大学 一种机械臂试探感知的避障方法
CN110977982A (zh) * 2019-12-19 2020-04-10 南京理工大学 基于深度视觉的双机械臂控制方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
祝敬,杨马英: "基于改进人工势场法的机械臂避障路径规划", 《计算机测量与控制》 *
祝敬,杨马英: "基于改进人工势场法的机械臂避障路径规划", 《计算机测量与控制》, vol. 26, no. 10, 23 October 2018 (2018-10-23), pages 205 - 210 *
薛阳等: "基于改进人工势场法的双机械臂避障路径规划", 《机械传动》 *
薛阳等: "基于改进人工势场法的双机械臂避障路径规划", 《机械传动》, vol. 44, no. 3, 15 March 2020 (2020-03-15), pages 39 - 44 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022160509A1 (zh) * 2021-01-27 2022-08-04 哈尔滨思哲睿智能医疗设备有限公司 一种腹腔镜微创手术机器人的碰撞检测方法及装置
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自由度双臂协作机器人防止自碰撞轨迹规划方法及***
CN113696187A (zh) * 2021-10-22 2021-11-26 成都飞机工业（集团）有限责任公司 一种适用于双机器人***的防碰撞方法
CN114454178A (zh) * 2022-03-15 2022-05-10 南京航空航天大学 面向异质整体制造的双机器人协同路径规划方法及***
CN114454178B (zh) * 2022-03-15 2023-11-24 南京航空航天大学 面向异质整体制造的双机器人协同路径规划方法及***
CN114700944A (zh) * 2022-04-06 2022-07-05 南京航空航天大学 一种面向异种任务的双机器人协同路径规划方法
CN114700944B (zh) * 2022-04-06 2023-11-24 南京航空航天大学 一种面向异种任务的双机器人协同路径规划方法

Also Published As

Publication number Publication date
CN112264989B (zh) 2023-06-06

Similar Documents

Publication Publication Date Title
CN112264989B (zh) 一种基于邻域遍历的双机械臂协同避障方法
US9862090B2 (en) Surrogate: a body-dexterous mobile manipulation robot with a tracked base
CN110262478B (zh) 基于改进人工势场法的人机安全避障路径规划方法
US9411335B2 (en) Method and apparatus to plan motion path of robot
US8825209B2 (en) Method and apparatus to plan motion path of robot
CN112068576A (zh) 一种基于任务分级时序优化的水下无人艇-双机械臂协同控制方法
CN110561419B (zh) 臂型线约束柔性机器人轨迹规划方法及装置
Chen et al. Path planning for obstacle avoidance of manipulators based on improved artificial potential field
CN116673963A (zh) 面向无序断路器零件的双机械臂协作柔性装配***及方法
CN111421540A (zh) 一种机械臂运动控制方法
Zieliński et al. Agent-based structures of robot systems
Tang et al. Coordinated motion planning of dual-arm space robot with deep reinforcement learning
Wang et al. Path planning optimization for teaching and playback welding robot
Xu et al. A cable-driven hyperredundant manipulator: Obstacle-avoidance path planning and tension optimization
CN115533920A (zh) 一种求解绳驱机械臂逆运动学的协同规划方法及***、计算机存储介质
Chen et al. Live working manipulator control model based on dppo-dqn combined algorithm
CN113084797B (zh) 一种基于任务分解的双臂冗余机械臂动态协同控制方法
Liu et al. Research on dynamic trajectory planning of collaborative robots base on RRT-RV algorithm
Yan et al. Integrated Drill Boom Hole-Seeking Control via Reinforcement Learning
Huang et al. Overview of trajectory planning methods for robot systems
Mettin et al. Analysis of human-operated motions and trajectory replanning for kinematically redundant manipulators
CN112936279B (zh) 一种移动作业机器人目标抓取作业最短时间求解方法
Li et al. Dynamic Planning for Sequential Whole-body Mobile Manipulation
Peng et al. Research on the Control Algorithm of Full Sea Heavy Load Proportional Seven-Function Hydraulic Manipulator Based on Load Sensing
Chen et al. An Autonomous Obstacle Avoidance Path Planning Method Involving PSO for Dual-Arm Surgical Robot

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