CN114643581B - 基于改进人工势场法的双机械臂避碰轨迹规划方法及*** - Google Patents
基于改进人工势场法的双机械臂避碰轨迹规划方法及*** Download PDFInfo
- Publication number
- CN114643581B CN114643581B CN202210415041.1A CN202210415041A CN114643581B CN 114643581 B CN114643581 B CN 114643581B CN 202210415041 A CN202210415041 A CN 202210415041A CN 114643581 B CN114643581 B CN 114643581B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- mechanical
- potential field
- field
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 113
- 238000001514 detection method Methods 0.000 claims description 13
- 238000011217 control strategy Methods 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 3
- 230000004048 modification Effects 0.000 abstract description 5
- 238000012986 modification Methods 0.000 abstract description 5
- 230000010355 oscillation Effects 0.000 abstract description 5
- 238000004422 calculation algorithm Methods 0.000 abstract description 2
- 230000009471 action Effects 0.000 description 12
- 239000012636 effector Substances 0.000 description 11
- 230000001133 acceleration Effects 0.000 description 6
- 239000000463 material Substances 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 238000003466 welding Methods 0.000 description 1
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/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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种基于改进人工势场法的双机械臂避碰轨迹规划方法及***,本发明对人工势场法进行改进:在改进人工势场法中,将势场力与机械臂关节速度直接相关。这一改动不仅减少了算法的计算量,且解决了机械臂在目标的振荡问题。在人工势场法中的引力场函数中设置一阈值距离,当机械臂到目标点之间的距离大于此阈值距离后,引力不再增加,而是等于一常数。这一改动避免了机械臂在距离目标点较远时受到过大的引力。
Description
技术领域
本发明涉及轨迹规划技术领域,涉及一种使用改进人工势场法的双机械臂避碰轨迹规划方法及***。
背景技术
双机械臂***具有比单机械臂更高的灵活性,具有更高的工作效率。当前双机械臂***已应用于多种工业场合,如搬运、装车、装配、焊接等。但双机械臂***往往共享同一工作空间,因此需要对双机械臂进行协调控制,以避免机械臂之间发生碰撞。双机械臂***的协调控制策略主要有主/从协调控制、混合位置/力控制、动态协调控制等。其中主/从协调控制最适合与双机械臂***的避碰轨迹规划结合。其原理为将两台机械臂分别设定为主机械臂和从机械臂,从机械臂通过延时动作、变更速度、限制工作范围等方式避免与主机械臂发生碰撞。也有学者提出对双机械臂采用时间调度、B样条插值局部改变机械臂轨迹、采用启发式规则生成机械臂轨迹等方法。但已有的很多方法会影响机械臂的工作效率,有些计算复杂,求解时间长,不利于双机械臂的协调运动与避碰轨迹规划。
传统人工势场法(TAPF)的原理是在双机械臂的运动空间中建立虚拟人工势场,虚拟人工势场由引力场和斥力场组成。引力场设置在目标点处,斥力场设置在障碍物处。机械臂在斥力场的作用下避开障碍物,并在引力场的作用下到达其轨迹的目标点。但该方法存在一些问题,传统人工势场法中的势场力与机械臂的关节加速度相关,这会导致机械臂在到达目标点时加速度为0,但仍有速度,从而造成机械臂在目标点附近振荡。而且传统人工势场法中的引力大小与机械臂到引力场的距离成正比,当机械臂距离目标点较远时,势场引力会很大,这可能导致两台机械臂碰撞。
发明内容
针对现有技术存在的不足,本发明的目的是提供一种基于改进人工势场法的双机械臂避碰轨迹规划方法,此方法计算简便,求解速度快,且通过离线规划提前生成双机械臂***的运动轨迹,安全性更高。
为了实现上述目的,本发明是通过如下的技术方案来实现:
第一方面,本发明提出了一种基于改进人工势场法的双机械臂避碰轨迹规划方法,如下:
步骤1:制定双机械臂的控制策略,设定两台机械臂的优先级;
步骤2:获取机械臂的尺寸数据,依据获得的尺寸数据建立两台机械臂的三维模型和运动学模型;
步骤3:依据三维模型和运动学模型建立双机械臂的碰撞检测模型;
步骤4:确定双机械臂工作路径的所有途经点,包括起始点、第一中间点、第二中间点、目标点,完成所有途经点间的插值拟合得到两台机械臂完整的初始轨迹;
步骤5:使用碰撞模型离线检测初始轨迹,当检测到两台机械臂之间有碰撞风险时,改进人工势场法开始作用并修改从机械臂的初始轨迹,改进人工势场法在双机械臂***的工作空间内生成虚拟引力场和斥力场,引力场设置在从机械臂目标点处且在引力场函数内设置阀值距离,斥力场设置在主机械臂的各个关节处;通过改进人工势场法得到修改后的轨迹。
作为进一步的技术方案,将主/从协调控制作为双机械臂的控制策略。
作为进一步的技术方案,步骤2中,测量机械臂的尺寸数据,依据获得的尺寸数据使用软件建立机械臂三维模型;计算尺寸数据得到机械臂D-H参数表,依据标准D-H参数法建立双机械臂***的运动学模型和URDF文件。
作为进一步的技术方案,步骤3中所述的碰撞检测模型可通过离线计算从机械臂到主机械臂的距离来判断两台机械臂是否有碰撞风险。
作为进一步的技术方案,步骤4中采用直线插值函数拟合起点到第一中间点,以及第二中间点到终点的距离,采用三次多项式插值函数拟合第一中间点到第二中间点的轨迹。
第二方面,本发明还提出了一种基于改进人工势场法的双机械臂避碰轨迹规划***,包括:
模块一:被配置为制定双机械臂的控制策略,设定两台机械臂的优先级;
模块二:被配置为获取机械臂的尺寸数据,依据获得的尺寸数据建立两台机械臂的三维模型和运动学模型;
模块三:被配置为依据三维模型和运动学模型建立双机械臂的碰撞检测模型;
模块四:被配置为确定双机械臂工作路径的所有途经点,包括起始点、第一中间点、第二中间点、目标点,完成所有途经点间的插值拟合得到两台机械臂完整的初始轨迹;
模块五:被配置为使用碰撞模型离线检测初始轨迹,当检测到两台机械臂之间有碰撞风险时,改进人工势场法开始作用并修改从机械臂的初始轨迹,改进人工势场法在双机械臂***的工作空间内生成虚拟引力场和斥力场,引力场设置在从机械臂目标点处且在引力场函数内设置阀值距离,斥力场设置在主机械臂的各个关节处;通过改进人工势场法得到修改后的轨迹。
作为进一步的技术方案,所述的模块一中将主/从协调控制作为双机械臂的控制策略。
作为进一步的技术方案,所述的模块二被配置为测量机械臂的尺寸数据,依据获得的尺寸数据使用软件建立机械臂三维模型;计算尺寸数据得到机械臂D-H参数表,依据标准D-H参数法建立双机械臂***的运动学模型和URDF文件。
作为进一步的技术方案,所述的碰撞检测模型被配置为通过离线计算从机械臂到主机械臂的距离来判断两台机械臂是否有碰撞风险。
作为进一步的技术方案,所述的模块四本配置为采用直线插值函数拟合起点到第一中间点,以及第二中间点到终点的距离,采用三次多项式插值函数拟合第一中间点到第二中间点的轨迹。
上述本发明的实施例的有益效果如下:
本发明提出了一种基于改进人工势场法的双机械臂避碰轨迹规划方法,该方法避碰效果好,结构简单,易于实现。首先,该方法提出了一种碰撞检测模型,检测到碰撞后局部修改机械臂的轨迹以实现避碰,相对于全局修改轨迹的避碰方法,降低了对双机械臂工作效率的影响。其次,该方法对人工势场法进行改进,解决了人工势场法中引力过大和末端振荡的问题,并降低了计算量。最后,对该方法进行仿真测试,结果表明该方法收敛和求解速度快,在该方法的作用下,两台机械臂在运动过程中始终保持安全距离。
本发明在改进人工势场法中,将势场力与机械臂关节速度直接相关。这一改动不仅减少了算法的计算量,且解决了机械臂在目标的振荡问题;在人工势场法中的引力场函数中设置一阈值距离,当机械臂到引力场之间的距离大于此阈值距离后,引力不再增加,而是等于一常数。
附图说明
构成本发明的一部分的说明书附图用来提供对本发明的进一步理解,本发明的示意性实施例及其说明用于解释本发明,并不构成对本发明的不当限定。
图1为本实施例的实施对象-龙门架双机械臂装车***示意图;
图2为本实施例的机械臂的三维模型示意图;
图3为本实施例的机械臂的D-H参数表;
图4、图5、图6为本实施例的碰撞检测模型中几何体的位置关系;
图7为本实施例的双机械臂***避碰轨迹;
图8为本实施例的机械臂R1与机械臂R2末端执行器之间的距离。
图中:11主机该械臂、12从机械臂、13机械抓手、14传送带、15待装车辆、16龙门架、21机械臂底座、22机械臂旋转轴,23机械臂旋转轴,24机械臂旋转轴,25机械臂旋转轴,26机械臂旋转轴,27机械臂旋转轴。
具体实施方式
应该指出,以下详细说明都是例示性的,旨在对本发明提供进一步的说明。除非另有指明,本发明使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是,这里所使用的术语仅是为了描述具体实施方式,而非意图限制根据本发明的示例性实施方式。如在这里所使用的,除非本发明另外明确指出,否则单数形式也意图包括复数形式,此外,还应当理解的是,当在本说明书中使用术语“包含”和/或“包括”时,其指明存在特征、步骤、操作、器件、组件和/或它们的组合;
正如背景技术所介绍的,现有技术中存在的不足,为了解决如上的技术问题,本发明提出了一种基于改进人工势场法的双机械臂避碰轨迹规划方法及***。
实施例1
请参阅图1,本实施例选取图1所示的龙门架双机械臂装车***作为本发明方法的实施对象,龙门架双机械臂装车***的关键装置包括主机该械臂11、从机械臂12、机械抓手13、传送带14、待装车辆15和龙门架16;本发明提供的基于改进人工势场法的双机械臂避碰轨迹规划方法,包括以下步骤:
包括以下步骤:
步骤1:根据主/从协调控制原理将两台机械臂分别设定为主机械臂R1和从机械臂R2,设定主机械臂的优先级高于从机械臂。检测到双机械臂之间有碰撞风险时,不改变高优先级的主机械臂的初始轨迹,通过改变低优先级的从机械臂的轨迹来避免两台机械臂之间的碰撞。
步骤2:本实例中选用的机械臂型号为KUKA KR 270R3100 ultra K。测量机械臂的尺寸数据,包括大臂、小臂、旋转轴等的形状尺寸。依据测量得到的尺寸数据在Solidworks软件中建立机械臂的三维模型,机械臂的三维模型请参阅图2;依据机械臂的尺寸计算机械臂的D-H参数,D-H参数表请参阅图3。图3中αi是连杆扭角,pi是连杆长度,di是连杆扭矩,θi是关节变量,i=1,2,…,6。依据D-H参数表可建立机械臂的运动学模型和URDF文件。
步骤3:为了方便地分析碰撞问题,将双机械臂三维模型简化为几何模型。将主机械臂R2的末端执行器视为半径为ra的球形区域,将主机械臂R1的关节视为半径为rb的球体,将主机械臂R1的连杆视为长度为lb的圆柱体,圆柱体的半径为rc。主机械臂R2末端执行器与主机械臂R1关节和连杆之间的距离分别为[d1,…,di]和[d1,…,dj],主机械臂R1的末端执行器和主机械臂R2的末端执行器之间的距离为dk。碰撞检测模型中几何体之间的位置关系请参阅图4、图5、图6,图中R1J、R1L、R1E分别代表机械臂R1的关节区域、连杆区域、末端执行器区域,R2E代表机械臂R2的末端执行器区域。依据运动学模型计算两台机械臂在轨迹中同时刻的距离,当满足公式(1)中的下列条件之一时,视为两个机械臂发生碰撞。
式中ds1,ds2,ds3分别是机械臂R2末端执行器到机械臂R1关节、连杆、末端执行器的最小安全距离。本实例中ds1,ds2,ds3的值分别为0.8m,0.8m,1.28m。
步骤4:确定双机械臂工作路径的所有途经点,包括起始点、第一中间点、第二中间点、目标点。其中起始点在传送带末端的物料抓取处,第一中间点物料抓去处上方,第二中间点在待装车辆的物料码放处上方,目标点在待装车辆的物料码放处。采用直线插值函数拟合起点到第一中间点,以及第二中间点到终点的距离。采用三次多项式插值函数拟合第一中间点到第二中间点的轨迹。
三次多项式的插值函数见公式(2)。
式中θ(t),分别代表关节角度,关节角速度,关节角加速度,a0,a1,a2,a3分别代表插值函数的系数。完成所有途经点间的插值拟合即可得到两台机械臂完整的初始轨迹。
步骤5:使用碰撞模型离线检测初始轨迹,当检测到两台机械臂之间有碰撞风险时,改进人工势场法开始作用并修改从机械臂的初始轨迹。在双机械臂***的关节空间内设置虚拟力场,该虚拟力场只对机械臂R2起作用以改变其轨迹。改进人工势场法的引力场设置在机械臂R2目标点处,其引力场函数如公式(3)所示:
式中,Katt是引力系数,ρ(q,qg)是一向量,其值是机械臂当前位置q到目标点qg的欧氏距离,其方向是从机械臂指向目标点位置,dq是机械臂到目标点之间的距离,rq是引力场的临界作用距离,Uk是一个常数。引力场的势场力函数如公式(4)所示:
式中Fk是一个常数。斥力场设置在机械臂R1的6个关节处。按照传统人工势场法,机械臂的受到的势场力为Kattρ(q,qg)。由于刚检测到碰撞风险时,机械臂R2距离目标点较远,因此会受到一个较大的势场力,这要求机械臂R2在瞬间产生较大的加速度,可能会对机械臂关节造成损伤。但改进人工势场法在引力场函数内设置了阈值距离rq,当机械臂R2与目标点距离大于此阈值距离时,机械臂受到的势场力为常数Fk,保证了机械臂R2的安全运行。
改进人工势场法的斥力场函数如公式(5)所示:
式中Krep是斥力系数,ρ(q,qo)是一向量,其值是机械臂当前位置q到障碍物qo的欧氏距离,其方向是从机械臂指向障碍物位置,di是机械臂R2到机械臂R1关节之间的距离,rs是斥力场的临界作用距离。斥力场的势场力函数如公式(6)所示:
全局势场由双机械臂***工作空间内所有虚拟引力场和斥力场合成,其势场函数如公式(7)所示:
其中U代表全局势场,n是机械臂的连杆数量。全局势场力的函数如公式(8)所示:
全局势场力与机械臂的关节速度相关,机械臂的关节速度可由公式(9-11)得到:
M=JF T(θ1,θ2,…,θ6)F(9
式中M是关节力矩,JF是雅可比矩阵,是机械臂的关节速度,BJ是机械臂关节的转动惯量。如果按照传统人工势场法,机械臂由全局势场力得到的是关节加速度/>这会导致机械臂到达目标点时,关节加速度为0,但关节速度不为0,机械臂由于这种惯性速度继续运动,而不停在目标点。而改进人工势场法将势场力与关节速度直接相关,机械臂运动到目标点时,关节速度也减为0,机械臂停止在目标点,解决了这种振荡现象。
经由前五个步骤,就获得了双机械臂的避碰轨迹。具体的,可以借助Matlab中的Roboticssystem toolbox工具将机械臂URDF文件可视化。已知机械臂末端执行器的轨迹,通过求机械臂运动学逆解获得机械臂关节空间的轨迹,依据机械臂关节空间的轨迹规划机械臂的运动。将机械臂的避碰过程通过仿真实现:
两台机械臂在Y轴相距2.25m,他们的底座中心坐标分别为[0,0,0]和[0,2.25,0]。机械臂R1从初始点[1.5,-1.2,-0.5]运动到目标点[2,0.5,-1],机械臂R2从初始点[1.5,3.45,-0.5]运动到目标点[2,1,-1]。两台机械臂在t=2.9s时检测到碰撞风险,改进人工势场法开始作用,机械臂R2在虚拟斥力场的作用下避开R1,R1完成码放任务后返回初始点,R2在虚拟引力场的作用下到达目标点。机械臂R1和R2的避碰轨迹请参阅图7。机械臂R1和R2末端执行之间的距离请参阅图8。由图8可见,改进人工势场法先于传统人工势场法收敛,且在避碰过程中机械臂R2与机械臂R1末端执行器的距离始终大于最小安全距离ds3。改进人工势场法作用下的机械臂R2平稳运动到目标点,未发生在目标点振荡的现象。
实施例2
本实施例提供了一种基于改进人工势场法的双机械臂避碰轨迹规划***,该***包括:
模块一:被配置为制定双机械臂的控制策略,设定两台机械臂的优先级;
模块二:被配置为获取机械臂的尺寸数据,依据获得的尺寸数据建立两台机械臂的三维模型和运动学模型;
模块三:被配置为依据三维模型和运动学模型建立双机械臂的碰撞检测模型;
模块四:被配置为确定双机械臂工作路径的所有途经点,包括起始点、第一中间点、第二中间点、目标点,完成所有途经点间的插值拟合得到两台机械臂完整的初始轨迹;
模块五:被配置为使用碰撞模型离线检测初始轨迹,当检测到两台机械臂之间有碰撞风险时,改进人工势场法开始作用并修改从机械臂的初始轨迹,改进人工势场法在双机械臂***的工作空间内生成虚拟引力场和斥力场,引力场设置在从机械臂目标点处且在引力场函数内设置阀值距离,斥力场设置在主机械臂的各个关节处;通过改进人工势场法得到修改后的轨迹。
上述的每个模块的具体步骤可以参阅实施例1,模块一对应实施例1中的步骤1,模块二对应实施例1中的步骤2,模块三对应实施例1中的步骤3,模块四对应实施例1中的步骤4,模块五对应实施例1中的步骤5,在此不进行赘述了。
最后还需要说明的是,诸如第一和第二之类的关系术语仅仅用来将一个实体或者操作与另一实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (10)
1.基于改进人工势场法的双机械臂避碰轨迹规划方法,其特征在于,如下:
步骤1:制定双机械臂的控制策略,设定两台机械臂的优先级;
步骤2:获取机械臂的尺寸数据,依据获得的尺寸数据建立两台机械臂的三维模型和运动学模型;
步骤3:依据三维模型和运动学模型建立双机械臂的碰撞检测模型;
步骤4:确定双机械臂工作路径的所有途经点,包括起始点、第一中间点、第二中间点、目标点,完成所有途经点间的插值拟合得到两台机械臂完整的初始轨迹;
步骤5:使用碰撞检测模型离线检测初始轨迹,当检测到两台机械臂之间有碰撞风险时,改进人工势场法开始作用并修改从机械臂的初始轨迹,改进人工势场法在双机械臂***的工作空间内生成虚拟引力场和斥力场,引力场设置在从机械臂目标点处且在引力场函数内设置阀值距离,斥力场设置在主机械臂的各个关节处;通过改进人工势场法得到修改后的轨迹;
改进人工势场法的引力场设置在从机械臂目标点处,其引力场函数公式如下:
式中,K att是引力系数,ρ(q,q g)是一向量,其值是机械臂当前位置q到目标点q g的欧氏距离,其方向是从机械臂指向目标点位置,d q是机械臂到目标点之间的距离,r q是引力场的临界作用距离,U k是一个常数;
引力场的势场力函数公式如下:
式中F k是一个常数;
改进人工势场法的斥力场函数公式如下:
式中K rep是斥力系数,ρ(q,q o)是一向量,其值是机械臂当前位置q到障碍物q o的欧氏距离,其方向是从机械臂指向障碍物位置,d i是从机械臂到主机械臂关节之间的距离,r s是斥力场的临界作用距离,斥力场的势场力函数公式如下:
全局势场由双机械臂***工作空间内所有虚拟引力场和斥力场合成,其势场函数公式如下:
其中U代表全局势场,n是机械臂的连杆数量,全局势场力的函数公式如下:
全局势场力与机械臂的关节速度相关,机械臂的关节速度可由如下公式得到:
式中M是关节力矩,J F是雅可比矩阵,是机械臂的关节速度,B J是机械臂关节的转动惯量,θ i 是关节变量, i=1, 2, …, 6。
2.如权利要求1所述的基于改进人工势场法的双机械臂避碰轨迹规划方法,其特征在于,步骤1中,将主/从协调控制作为双机械臂的控制策略。
3.如权利要求1所述的基于改进人工势场法的双机械臂避碰轨迹规划方法,其特征在于,步骤2中,测量机械臂的尺寸数据,依据获得的尺寸数据使用软件建立机械臂三维模型;计算尺寸数据得到机械臂D-H参数表,依据标准D-H参数法建立双机械臂***的运动学模型和URDF文件。
4.如权利要求1所述的基于改进人工势场法的双机械臂避碰轨迹规划方法,其特征在于,步骤3中所述的碰撞检测模型可通过离线计算从机械臂到主机械臂的距离来判断两台机械臂是否有碰撞风险。
5.如权利要求1所述的基于改进人工势场法的双机械臂避碰轨迹规划方法,其特征在于,步骤4中采用直线插值函数拟合起始点到第一中间点,以及第二中间点到目标点的距离,采用三次多项式插值函数拟合第一中间点到第二中间点的轨迹。
6.一种基于改进人工势场法的双机械臂避碰轨迹规划***,其特征在于,包括:
模块一:被配置为制定双机械臂的控制策略,设定两台机械臂的优先级;
模块二:被配置为获取机械臂的尺寸数据,依据获得的尺寸数据建立两台机械臂的三维模型和运动学模型;
模块三:被配置为依据三维模型和运动学模型建立双机械臂的碰撞检测模型;
模块四:被配置为确定双机械臂工作路径的所有途经点,包括起始点、第一中间点、第二中间点、目标点,完成所有途经点间的插值拟合得到两台机械臂完整的初始轨迹;
模块五:被配置为使用碰撞检测模型离线检测初始轨迹,当检测到两台机械臂之间有碰撞风险时,改进人工势场法开始作用并修改从机械臂的初始轨迹,改进人工势场法在双机械臂***的工作空间内生成虚拟引力场和斥力场,引力场设置在从机械臂目标点处且在引力场函数内设置阀值距离,斥力场设置在主机械臂的各个关节处;通过改进人工势场法得到修改后的轨迹;
改进人工势场法的引力场设置在从机械臂目标点处,其引力场函数公式如下:
式中,K att是引力系数,ρ(q,q g)是一向量,其值是机械臂当前位置q到目标点q g的欧氏距离,其方向是从机械臂指向目标点位置,d q是机械臂到目标点之间的距离,r q是引力场的临界作用距离,U k是一个常数;
引力场的势场力函数公式如下:
式中F k是一个常数;
改进人工势场法的斥力场函数公式如下:
式中K rep是斥力系数,ρ(q,q o)是一向量,其值是机械臂当前位置q到障碍物q o的欧氏距离,其方向是从机械臂指向障碍物位置,d i是从机械臂到主机械臂关节之间的距离,r s是斥力场的临界作用距离,斥力场的势场力函数公式如下:
全局势场由双机械臂***工作空间内所有虚拟引力场和斥力场合成,其势场函数公式如下:
其中U代表全局势场,n是机械臂的连杆数量,全局势场力的函数公式如下:
全局势场力与机械臂的关节速度相关,机械臂的关节速度可由如下公式得到:
式中M是关节力矩,J F是雅可比矩阵,是机械臂的关节速度,B J是机械臂关节的转动惯量,θ i 是关节变量, i=1, 2, …, 6。
7.如权利要求6所述的基于改进人工势场法的双机械臂避碰轨迹规划方法,其特征在于,模块一中将主/从协调控制作为双机械臂的控制策略。
8.如权利要求6所述的基于改进人工势场法的双机械臂避碰轨迹规划方法,其特征在于,所述的模块二被配置为测量机械臂的尺寸数据,依据获得的尺寸数据使用软件建立机械臂三维模型;计算尺寸数据得到机械臂D-H参数表,依据标准D-H参数法建立双机械臂***的运动学模型和URDF文件。
9.如权利要求6所述的基于改进人工势场法的双机械臂避碰轨迹规划方法,其特征在于,所述的碰撞检测模型被配置为通过离线计算从机械臂到主机械臂的距离来判断两台机械臂是否有碰撞风险。
10.如权利要求6所述的基于改进人工势场法的双机械臂避碰轨迹规划方法,其特征在于,所述的模块四被配置为采用直线插值函数拟合起始点到第一中间点,以及第二中间点到目标点的距离,采用三次多项式插值函数拟合第一中间点到第二中间点的轨迹。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210415041.1A CN114643581B (zh) | 2022-04-20 | 2022-04-20 | 基于改进人工势场法的双机械臂避碰轨迹规划方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210415041.1A CN114643581B (zh) | 2022-04-20 | 2022-04-20 | 基于改进人工势场法的双机械臂避碰轨迹规划方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114643581A CN114643581A (zh) | 2022-06-21 |
CN114643581B true CN114643581B (zh) | 2024-01-19 |
Family
ID=81997203
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210415041.1A Active CN114643581B (zh) | 2022-04-20 | 2022-04-20 | 基于改进人工势场法的双机械臂避碰轨迹规划方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114643581B (zh) |
Citations (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5483630A (en) * | 1990-07-12 | 1996-01-09 | Hitachi, Ltd. | Method and apparatus for representing motion of multiple-jointed object, computer graphic apparatus, and robot controller |
JP2003241836A (ja) * | 2002-02-19 | 2003-08-29 | Keio Gijuku | 自走移動体の制御方法および装置 |
JP2012011498A (ja) * | 2010-06-30 | 2012-01-19 | Toshiba Corp | ロボットアーム操作システムおよびその操作方法 |
CN104029203A (zh) * | 2014-06-18 | 2014-09-10 | 大连大学 | 实现空间机械臂避障的路径规划方法 |
CN108153298A (zh) * | 2017-04-19 | 2018-06-12 | 中国北方车辆研究所 | 一种基于改进人工势场的足式机器人牵引控制方法及*** |
CN110471420A (zh) * | 2019-08-23 | 2019-11-19 | 苏州浪潮智能科技有限公司 | 一种基于人工势场法的机器避障方法及装置 |
EP3634695A1 (en) * | 2017-05-29 | 2020-04-15 | Franka Emika GmbH | Collision handling by a robot |
CN111546343A (zh) * | 2020-05-13 | 2020-08-18 | 浙江工业大学 | 一种基于改进人工势场法的消防移动机器人路径规划方法及*** |
CN112327831A (zh) * | 2020-10-20 | 2021-02-05 | 大连理工大学 | 一种基于改进人工势场法的工厂agv轨迹规划方法 |
CN112596525A (zh) * | 2020-12-16 | 2021-04-02 | 中国地质大学(武汉) | 基于遗传算法和改进人工势场法的机器人路径规划方法 |
CN113334379A (zh) * | 2021-05-30 | 2021-09-03 | 南京理工大学 | 一种基于虚拟力的主从跟随与避碰方法 |
CN113858210A (zh) * | 2021-11-01 | 2021-12-31 | 贵州大学 | 基于混合算法的机械臂路径规划方法 |
CN113997291A (zh) * | 2021-11-23 | 2022-02-01 | 南方电网电力科技股份有限公司 | 一种机器人力引导自治控制方法及相关装置 |
CN114028153A (zh) * | 2021-09-18 | 2022-02-11 | 深圳华鹊景医疗科技有限公司 | 一种康复机器人及其控制方法 |
CN114355937A (zh) * | 2021-12-31 | 2022-04-15 | 江苏理工学院 | 一种基于改进型人工势场法的无人车局部路径规划方法 |
-
2022
- 2022-04-20 CN CN202210415041.1A patent/CN114643581B/zh active Active
Patent Citations (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5483630A (en) * | 1990-07-12 | 1996-01-09 | Hitachi, Ltd. | Method and apparatus for representing motion of multiple-jointed object, computer graphic apparatus, and robot controller |
JP2003241836A (ja) * | 2002-02-19 | 2003-08-29 | Keio Gijuku | 自走移動体の制御方法および装置 |
JP2012011498A (ja) * | 2010-06-30 | 2012-01-19 | Toshiba Corp | ロボットアーム操作システムおよびその操作方法 |
CN104029203A (zh) * | 2014-06-18 | 2014-09-10 | 大连大学 | 实现空间机械臂避障的路径规划方法 |
CN108153298A (zh) * | 2017-04-19 | 2018-06-12 | 中国北方车辆研究所 | 一种基于改进人工势场的足式机器人牵引控制方法及*** |
EP3634695A1 (en) * | 2017-05-29 | 2020-04-15 | Franka Emika GmbH | Collision handling by a robot |
CN110471420A (zh) * | 2019-08-23 | 2019-11-19 | 苏州浪潮智能科技有限公司 | 一种基于人工势场法的机器避障方法及装置 |
CN111546343A (zh) * | 2020-05-13 | 2020-08-18 | 浙江工业大学 | 一种基于改进人工势场法的消防移动机器人路径规划方法及*** |
CN112327831A (zh) * | 2020-10-20 | 2021-02-05 | 大连理工大学 | 一种基于改进人工势场法的工厂agv轨迹规划方法 |
CN112596525A (zh) * | 2020-12-16 | 2021-04-02 | 中国地质大学(武汉) | 基于遗传算法和改进人工势场法的机器人路径规划方法 |
CN113334379A (zh) * | 2021-05-30 | 2021-09-03 | 南京理工大学 | 一种基于虚拟力的主从跟随与避碰方法 |
CN114028153A (zh) * | 2021-09-18 | 2022-02-11 | 深圳华鹊景医疗科技有限公司 | 一种康复机器人及其控制方法 |
CN113858210A (zh) * | 2021-11-01 | 2021-12-31 | 贵州大学 | 基于混合算法的机械臂路径规划方法 |
CN113997291A (zh) * | 2021-11-23 | 2022-02-01 | 南方电网电力科技股份有限公司 | 一种机器人力引导自治控制方法及相关装置 |
CN114355937A (zh) * | 2021-12-31 | 2022-04-15 | 江苏理工学院 | 一种基于改进型人工势场法的无人车局部路径规划方法 |
Non-Patent Citations (2)
Title |
---|
基于改进人工势场法的双机械臂避障路径规划;薛阳等;机械传动;第44卷(第3期);39-44 * |
基于速度场的人工势场法机械臂动态避障研究;史亚飞;张力;刘子煊;吕一平;;机械传动(第04期);44-50 * |
Also Published As
Publication number | Publication date |
---|---|
CN114643581A (zh) | 2022-06-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US8600554B2 (en) | System and method for robot trajectory generation with continuous accelerations | |
US8560122B2 (en) | Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same | |
EP2660013B1 (en) | Method of controlling seven-shaft multi-joint robot, control program, and robot control device | |
WO2020010626A1 (zh) | 机器人的运动控制方法、机器人及机器人运动控制*** | |
Korayem et al. | Dynamic load-carrying capacity of mobile-base flexible joint manipulators | |
US20200189101A1 (en) | Trajectory generation system and trajectory generating method | |
WO2023069292A1 (en) | Nonlinear trajectory optimization for robotic devices | |
CN115476356A (zh) | 一种无需力传感器的空间机械臂自适应混合阻抗控制方法 | |
CN114310914A (zh) | 多自由度机械臂模糊自适应迭代轨迹跟踪控制方法及*** | |
CN114643581B (zh) | 基于改进人工势场法的双机械臂避碰轨迹规划方法及*** | |
CN113263496A (zh) | 一种六自由度机械臂优化路径的方法和计算机设备 | |
Zhang et al. | Kuka youBot arm shortest path planning based on geodesics | |
Muller-Karger et al. | Hyperbolic trajectories for pick-and-place operations to elude obstacles | |
JP2020110884A (ja) | ロボット制御装置、ロボット制御方法、及びロボット制御プログラム | |
CN114888812A (zh) | 一种基于刚度性能优化的移动机械臂站位规划方法 | |
Bae et al. | A dynamic visual servoing of robot manipulator with eye-in-hand camera | |
Kim | Design of a Novel 4-DOF High-Speed Parallel Robot | |
Yang et al. | Collision avoidance trajectory planning for a dual-robot system: using a modified APF method | |
Zhang et al. | Varying inertial parameters model based robust control for an aerial manipulator | |
Chávez et al. | Kinematic and dynamic modeling of the phantomx ax metal hexapod mark iii robot using quaternions | |
Ren et al. | Performance improvement of tracking control for a planar parallel robot using synchronized control | |
Omrčen et al. | Autonomous motion of a mobile manipulator using a combined torque and velocity control | |
Li et al. | Dynamic modeling and analysis for 6-DOF industrial robots | |
Kuppan Chetty et al. | A heuristic approach towards path planning and obstacle avoidance control of planar manipulator | |
KR102643316B1 (ko) | 로봇 매니퓰레이터의 충격량을 산출하는 검출장치 및 방법 |
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 |