CN117773911B - 一种复杂环境下工业机器人避障方法 - Google Patents
一种复杂环境下工业机器人避障方法 Download PDFInfo
- Publication number
- CN117773911B CN117773911B CN202311463044.3A CN202311463044A CN117773911B CN 117773911 B CN117773911 B CN 117773911B CN 202311463044 A CN202311463044 A CN 202311463044A CN 117773911 B CN117773911 B CN 117773911B
- Authority
- CN
- China
- Prior art keywords
- joint
- industrial robot
- particle
- track
- time
- 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 74
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 60
- 230000033001 locomotion Effects 0.000 claims abstract description 23
- 238000004364 calculation method Methods 0.000 claims abstract description 11
- 239000002245 particle Substances 0.000 claims description 157
- 230000008569 process Effects 0.000 claims description 27
- 230000001133 acceleration Effects 0.000 claims description 22
- 230000000739 chaotic effect Effects 0.000 claims description 22
- 238000013507 mapping Methods 0.000 claims description 20
- 239000013598 vector Substances 0.000 claims description 18
- 238000005070 sampling Methods 0.000 claims description 15
- 238000005457 optimization Methods 0.000 claims description 9
- 238000001514 detection method Methods 0.000 claims description 8
- 230000006978 adaptation Effects 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 5
- 238000012804 iterative process Methods 0.000 claims description 5
- 230000003247 decreasing effect Effects 0.000 claims description 4
- 238000012952 Resampling Methods 0.000 claims description 3
- 230000001149 cognitive effect Effects 0.000 claims description 3
- 239000012636 effector Substances 0.000 claims description 3
- 230000001172 regenerating effect Effects 0.000 claims description 3
- 230000001360 synchronised effect Effects 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 2
- 230000014509 gene expression Effects 0.000 claims 1
- 238000007689 inspection Methods 0.000 claims 1
- 230000007246 mechanism Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 6
- 230000009471 action Effects 0.000 description 5
- 230000008901 benefit Effects 0.000 description 5
- 230000006872 improvement Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种复杂环境下工业机器人避障方法,包括以下步骤:S1、对工作空间内的工业机器人进行模型简化以及将障碍物简化为障碍物点云;S2、采用“眼在手外”的布置方式将深度相机放置在工业机器人的运动空间之外;S3、在工业机器人进行运动前采用G‑APF‑RRT*算法在工作空间中规划一条可行路径。本发明中,工业机器人与障碍物之间距离的计算方式简单,所需时间较短,通过使用虚拟关节插补而后在关节处用球状包围盒完全包络工业机器人以简化模型,只需计算有限的球状包围盒球心与障碍物点云中各点之间的距离,即可完成距离计算。
Description
技术领域
本发明涉及工业机器人技术领域,具体为一种复杂环境下工业机器人避障方法。
背景技术
随着工业机器人的应用领域不断扩展,人们对于工业机器人的协调性、平稳性、高效性等要求日益提升,其中,关于工业机器人的避障能力与工作效率受到人们的广泛关注。
目前,工业机器人如何在复杂的环境中进行避障运动的同时要求尽可能缩短运行时间以提升工作效率尚未形成一套成熟的、适用性强、能够真正应用到工业生产中的技术方案,对于工业机器人如何在避障的同时尽可能缩短运行时间主要存在的问题包括:由于工业机器人关节较多,灵活性强,姿态形式多样,不能够简单视为一个质点来考虑避障,因此在完成工业机器人末端避障的同时还需考虑工业机器人关节不与障碍物发生碰撞;工业机器人的运动规划过程多且复杂,使得规划时间长;轨迹规划的优劣对完成动作所需的运动时间有很大影响。
发明内容
本发明的目的在于提供一种复杂环境下工业机器人避障方法,以解决上述背景技术提出的对于由于工业机器人关节较多,灵活性强,姿态形式多样,不能够简单视为一个质点来考虑避障,因此在完成工业机器人末端避障的同时还需考虑工业机器人关节不与障碍物发生碰撞;工业机器人的运动规划过程多且复杂,使得规划时间长;轨迹规划的优劣对完成动作所需的运动时间有很大影响的问题。
为实现上述目的,本发明提供如下技术方案:一种复杂环境下工业机器人避障方法,包括以下步骤:
S1、对工作空间内的工业机器人进行模型简化以及将障碍物简化为障碍物点云;
S2、采用“眼在手外”的布置方式将深度相机放置在工业机器人的运动空间之外;
S3、在工业机器人进行运动前采用G-APF-RRT*算法在工作空间中规划一条可行路径;
S4、为避免工业机器人末端沿路径运动时工业机器人关节与障碍物发生碰撞,对新生成的树节点qnew进行关节碰撞检测,获得工业机器人从起始点运动到目标点的无碰撞完整路径;
S5、将获得的无碰撞完整路径进行逆运动学计算,得出工业机器人各关节在关节空间下的关节位置,即插值位置,用于后续的轨迹插值,通过3-5-3多项式插值方法对工业机器人进行关节空间轨迹规划,得到工业机器人的关节空间位置—时间轨迹曲线、角速度—时间轨迹曲线和角加速度—时间轨迹曲线;
S6、根据关节空间轨迹方程建立优化目标函数和轨迹约束条件,为后续进行时间最优轨迹规划做准备;
S7、使用改进的粒子群算法,对3-5-3多项式插值轨迹中3段轨迹的运行时间tj1、tj2、tj3进行寻优,使工业机器人各关节沿轨迹运行的时间最短,且满足运行过程中关节角速度vji、角加速度aji不超过约束值vmax和amax;
S8、完成所有关节的轨迹优化,进行关节同步处理;
S9、使用同步处理后的插值时间t1,t2,t3对各关节进行3-5-3多项式插值,得到各关节的时间最优轨迹;
S10、将关节空间轨迹发送给控制器,控制工业机器人沿轨迹运动,完成躲避障碍物并到达目标点。
优选的,在步骤S1中,所述工业机器人模型简化方法为:确定工业机器人的真实关节点,在相邻真实关节点之间的连杆上以一定距离插补虚拟关节点,而后以关节点为球心采用球形包围盒完全包络工业机器人本体,确保工业机器人处于任何位姿时,都能完整代替工业机器人。
优选的,在步骤S2中,在进行路径规划前获取工作空间中工业机器人和障碍物的点云信息,使用点云滤波器滤除工业机器人本体点云,仅保留工作空间下的障碍物点云。
优选的,在步骤S3中,所述G-APF-RRT*算法是一种改进型的快速扩展随机树算法,快速扩展随机树算法简称RRT算法;所述快速扩展随机树算法是在工作空间内基于随机采样方法对随机采样点进行连接以形成一条可行路径,其连接过程类似树的生长。
优选的,在步骤S4中,所述关节碰撞检测的流程为:通过逆运动学求解工业机器人末端位于新生成的树节点处时各关节的位置信息,在基坐标系下,计算工业机器人各包围球球心与障碍物点云之间的欧式距离Dji,获取最短距离与安全距离进行比较,若所述最短距离大于设定安全距离时,保留该新生成的树节点,若所述最短距离小于设定安全距离时,舍弃该新生成的树节点并进行重新采样,直至获得工业机器人从起始点运动到目标点的无碰撞完整路径,所述欧氏距离计算公式如下:
式中,Dji代表第j个包围球球心与第i个障碍物点云之间的欧氏距离,(Psphejx,Psphejy,Psphejz)为工业机器人上第j个包围球在基坐标系下的坐标,(Pobstix,Pobstiy,Pobstiz)为第i个障碍物点云在基坐标系下的坐标。
优选的,在步骤S5中,所述轨迹曲线的方程如下:
式中,θj1、θj2、θj3、分别代表工业机器人第j个关节的第1、2、3段运动轨迹,其分别采用3次多项式插值、5次多项式插值、3次多项式插值即3-5-3多项式插值,j=1,2,…,n,n为工业机器人的真实关节数目;t1、t2、t3分别代表工业机器人沿第1、2、3段轨迹运动的插值时间,aj=[aj10aj11 aj12 aj13 aj20 aj21 aj22 aj23 aj24 aj25 aj30 aj31aj32 aj33]为多项式系数;
根据位置-时间轨迹曲线方程可得到关节j的角速度-时间轨迹曲线和角加速度-时间轨迹曲线公式如下:
使用3-5-3多项式插值规划轨迹时,需要设置约束条件:Xji代表第j个关节已知的插值位置,i=0,1,2,3;轨迹起始点为Xj0、中间路径点为Xj1、Xj2、轨迹末端点为Xj3,设置初始点和末端点的速度、加速度为0,路径点之间的速度和加速度连续;
由以上约束结合上式可推出多项式系数、插值位置、插值时间之间的关系式如下:
b=[000000Xj300 Xj000 Xj2 Xj1]T;
a=A-1·b=[aj13 aj12 aj11 aj10 aj25 aj24 aj23 aj22 aj21 aj20 aj33 aj32 aj31 aj30]T;
由上式可知,当已知4个插值位置和3段插值时间,可得到多项式系数,进而得到位置-时间轨迹曲线、速度-时间轨迹曲线、加速度-时间轨迹曲线的具体方程,即关节空间的轨迹方程。
优选的,在步骤S6中,所述目标函数如下所示:
所述约束条件如下所示:
式中,f(tj)代表第j个关节在3-5-3插值轨迹上运动的最短时间,vji为第j个关节在第i段轨迹上的角速度,aji为第j个关节在第i段轨迹上的角加速度;vmax为各段轨迹约束的最大角速度,amax为各段轨迹约束的最大角加速度。
优选的,在步骤S7中,所述改进粒子群算法是在基础粒子群算法的基础上使用PWLCM混沌映射初始化种群,使用可调非线性递减惯性权重使算法快速收敛到全局最优;所述改进粒子群算法具体过程为:
S71、确定迭代次数N、种群数量M、最大角速度vmax、最大角加速度amax;
S72、采用PWLCM混沌映射初始化粒子种群,粒子维度为3,其中包含粒子位置与粒子飞行速度信息,粒子位置为关节j在三段轨迹上运行的时间tj1、tj2、tj3,tj1、tj2、tj3为M维向量,粒子飞行速度为vj1、vj2、vj3,vj1、vj2、vj3为M维向量,粒子速度上限为vjimax;通过混沌映射能够使初始粒子均匀分布在解空间内,即保证解的随机性与均匀性,以提升粒子群算法的全局搜索能力,所述PWLCM混沌映射如下所示:
式中j为工业机器人第j个关节,i代表第i段轨迹,m代表粒子的序号,即第m个解,p为控制参数,p取值在区间(0,0.5)范围内;
S73、对混沌映射得到的粒子进行检验是否符合要求,将混沌映射得到的粒子位置tj1、tj2、tj3,轨迹起始点Xj0、中间路径点为Xj1、Xj2,末端点为Xj3代入多项式系数、插值位置、插值时间之间的关系式中得到多项式系数,进而得到关节j的位置-时间轨迹曲线、角速度-时间轨迹曲线和角加速度-时间轨迹曲线,判断各段轨迹的速度、加速度大小是否满足约束值vmax和amax,若满足则保存为初始粒子,并计算该粒子的适应度f,若不满足则重新生成粒子进行检验,直至生成M个初始粒子,形成初始种群;
S74、对粒子种群中的各个粒子进行迭代寻优,首先迭代过程中对粒子的位置、速度进行更新,更新公式如下所示:
式中,为关节j第m个粒子第k次迭代时飞行速度的第d维分量,/>为关节j第m个粒子第k次迭代时位置的第d维分量,r1和r2为[0,1]之间的随机数,pjmd为关节j第m个粒子的最优位置的第d维分量,pjgd为关节j粒子种群最优位置的第d维分量,c1、c2为认知权重因子,w为惯性权重,kl为可调斜率系数,可以调节w在迭代过程的变化方式,影响粒子群算法的收敛性和全局最优性;
接着判断粒子速度是否满足上限vjimax,若满足则设为该粒子的当前速度,若不满足则该粒子的当前速度设为vjimax,而后计算该粒子的适应f以及检验该粒子对应的轨迹曲线是否满足约束条件,若该粒子满足约束条件且适应度f小于上一代粒子则更新为新一代粒子,否则将上一代粒子作为新一代粒子;将该新一代粒子的适应度f与当前种群最优适应度fg对比,若新一代粒子的适应度f小于当前种群最优适应度fg,则将该新一代粒子作为种群最优粒子,该新一代粒子的适应度作为种群最优适应度fg,该新一代粒子的位置作为种群最优位置tj1g、tj2g、tj3g。重复迭代直至迭代次数达到N,获取种群最优粒子的位置数据tj1g、tj2g、tj3g作为关节j在三段轨迹上的运行时间。
优选的,在步骤S8中,由于工业机器人在点到点关节规划中,各关节的规划时间不同,为确保关节运动的同步性,需要对各关节的运行时间进行同步处理,以确保在各关节的配合下,工业机器人末端执行器到达预定位置;分别对第1、2、3段轨迹取各关节在该轨迹段上运行时间中的最大值,并作为所有关节在该轨迹段上的插值时间,即t1=max{tj1g},t2=max{tj2g},t3=max{tj3g}(j=1,2,……,n)。
与现有技术相比,本发明的有益效果是:
1、本方法中,工业机器人与障碍物之间距离的计算方式简单,所需时间较短,通过使用虚拟关节插补而后在关节处用球状包围盒完全包络工业机器人以简化模型,只需计算有限的球状包围盒球心与障碍物点云中各点之间的距离,即可完成距离计算。
2、本方法中,工业机器人末端路径的规划更加快速,所得路径短且平滑。加入人工势场法机制和目标偏置机制影响树节点的生长方向,使得随机树有方向地朝着目标点生长;RRT*算法的相邻空间搜索和树重连机制能够使得减少路径的拐点,使得路径更加短且平滑。
3、碰撞检测流程简洁,避免生成的完整路径不可用。基于G-APF-RRT*算法采样点较少的优势,采取采点——检测同时进行的方式,简化了流程,缩短了路径规划的时间,且避免了规划的完整路径不可用。
4、提升了粒子群算法的全局搜索能力,使得找到最优解的可能性更大。通过采用PWLCM混沌映射初始化粒子种群,使初始粒子均匀分布在解空间内,保证解的随机性与均匀性,进而提升粒子群算法的全局搜索能力。
5、改进的粒子群算法能够快速地收敛到全局最优解。通过使用可调非线性递减惯性权重,既能够调整惯性权重w在迭代过程的变化方式,同时保证了粒子群算法在初期的惯性权重w较大,以提升全局搜索能力,后期的惯性权重w较小,加快了收敛速度。
附图说明
图1为本发明一种复杂环境下工业机器人避障方法的流程图;
图2为本发明一种复杂环境下工业机器人避障方法的工业机器人简化与障碍物点云化图;
图3为本发明一种复杂环境下工业机器人避障方法的相机布置方式图;
图4为本发明一种复杂环境下工业机器人避障方法的加入人工势场法机制的RRT*算法的树节点生成过程图;
图5为本发明一种复杂环境下工业机器人避障方法的欧式距离Dji示意图;
图6为本发明一种复杂环境下工业机器人避障方法的PWLCM混沌映射分布图;
图7为本发明一种复杂环境下工业机器人避障方法的不同kl值时w的变化曲线图;
图8为本发明一种复杂环境下工业机器人避障方法的ROS工业机器人初、末位姿仿真图;
图9为本发明一种复杂环境下工业机器人避障方法的ROS工业机器人避障运动轨迹仿真图;
图10为本发明一种复杂环境下工业机器人避障方法的G-APF-RRT*算法路径规划图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施条例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
请参阅图1-图10,本发明提供一种技术方案:一种复杂环境下工业机器人避障方法,包括以下步骤:
步骤一、对工作空间内的工业机器人进行模型简化以及将障碍物简化为障碍物点云,见图2;其中,工业机器人模型简化方法为:确定工业机器人的真实关节点,在相邻真实关节点之间的连杆上以一定距离插补虚拟关节点,而后以关节点为球心采用球形包围盒完全包络工业机器人本体,确保工业机器人处于任何位姿时,都能完整代替工业机器人;
步骤二、采用“眼在手外”的布置方式将深度相机放置在工业机器人的运动空间之外,见图3;其中,在进行路径规划前获取工作空间中工业机器人和障碍物的点云信息,使用点云滤波器滤除工业机器人本体点云,仅保留工作空间下的障碍物点云;
步骤三、在工业机器人进行运动前采用G-APF-RRT*算法在工作空间中规划一条可行路径;其中,G-APF-RRT*算法是一种改进型的快速扩展随机树算法,快速扩展随机树算法简称RRT算法;快速扩展随机树算法是在工作空间内基于随机采样方法对随机采样点进行连接以形成一条可行路径,其连接过程类似树的生长;
具体的,改进一是以RRT算法为基本框架,在此基础上引入了相邻空间搜索和树重连机制,称为RRT*算法,其中,相邻空间搜索是指当随机节点qrand选定好父节点后,以新生成的树节点qnew作为圆心,将半径为r的圆形区域内的其他节点作为待选父节点并分别计算以这些待选父节点作为树节点qnew的父节点时路径的代价值,如果存在某一待选父节点与树节点qnew形成的新路径的代价值小于原路径的代价值,且该新路径不与障碍物相交,则将该待选父节点设定为树节点qnew的父节点,删除原路径。树重连机制是指在相邻空间确定新的父节点后,以新的树节点qnew作为父节点连接相邻空间内的其他节点,若相邻空间内存在某一节点以树节点qnew为父节点时的新路径的代价值小于原路径的代价值,且该新路径未与障碍物相交,则采用新路径的连接方式。
改进二是在RRT*的随机采样过程中引入目标偏置机制来加快路径的搜索效率;通过设置目标偏置阈值p,程序在进行随机采点前会生成一个[0,1]以内的随机数Nrand,当Nrand≤p时,以目标点qgoal作为随机节点qrand进行新节点的选取,使得随机树以一定概率朝着目标点方向生长;当Nrand>p时,在状态空间进行随机采点以保证算法的完备性。
改进三是将人工势场法思想引入RRT*算法中,即由目标点产生一个引力和由障碍物产生一个斥力来影响新的树节点的选择,见图4,在新的树节点的生成过程中,随机采样点qrand对距离其最近的树节点qnearest产生一个由树节点qnearest指向随机点qrand的单位向量Qrand,同时由目标点qgoal的引力产生一个由树节点qnearest指向目标点qgoal的单位向量Qrand,由障碍物产生一个由树节点qnearest指向与障碍物相反方向的一个单位向量Qrep。通过调节生长步长s、引力增益系数g、斥力增益系数k,对上述三个向量进行相加可获得一个合向量Qnew,在合向量Qrand方向以生长步长s取新的树节点qnew,可以使得新树节点稳定地朝着目标点的方向生长;合向量Qnew公式为:
式中,式中,Qrepi为第i个障碍物所产生的斥力单位向量,n为障碍物的个数,是随机采样点qrand的位置矢量,/>是树节点qnearest的位置矢量,/>是目标点qgoal的位置矢量,/>是第i个障碍物的位置矢量,s为生长步长,g为引力增益系数,通过改变g的大小可调节新的树节点qnew偏向目标点qgoal方向的程度,k为斥力增益系数,通过改变k的大小可调节新的树节点qnew朝远离障碍物方向生长的程度;rri为随机采样点qrand与第i个障碍物的最短距离,r0为障碍物产生斥力场的影响距离;
步骤四、为避免工业机器人末端沿路径运动时工业机器人关节与障碍物发生碰撞,对新生成的树节点qnew进行关节碰撞检测,获得工业机器人从起始点运动到目标点的无碰撞完整路径;其中,关节碰撞检测的流程为:通过逆运动学求解工业机器人末端位于新生成的树节点处时各关节的位置信息,在基坐标系下,计算工业机器人各包围球球心与障碍物点云之间的欧式距离Dji,见图5,获取最短距离与安全距离进行比较,若最短距离大于设定安全距离时,保留该新生成的树节点,若最短距离小于设定安全距离时,舍弃该新生成的树节点并进行重新采样,直至获得工业机器人从起始点运动到目标点的无碰撞完整路径,欧氏距离计算公式如下:
式中,Dji代表第j个包围球球心与第i个障碍物点云之间的欧氏距离,(Psphejx,Psphejy,Psphejz)为工业机器人上第j个包围球在基坐标系下的坐标,(Pobstix,Pobstiy,Pobstiz)为第i个障碍物点云在基坐标系下的坐标;
步骤五、将获得的无碰撞完整路径进行逆运动学计算,得出工业机器人各关节在关节空间下的关节位置,即插值位置,用于后续的轨迹插值,通过3-5-3多项式插值方法对工业机器人进行关节空间轨迹规划,得到工业机器人的关节空间位置—时间轨迹曲线、角速度—时间轨迹曲线和角加速度—时间轨迹曲线;其中,轨迹曲线的方程如下:
式中,θj1、θj2、θj3、分别代表工业机器人第j个关节的第1、2、3段运动轨迹,其分别采用3次多项式插值、5次多项式插值、3次多项式插值即3-5-3多项式插值,j=1,2,…,n,n为工业机器人的真实关节数目;t1、t2、t3分别代表工业机器人沿第1、2、3段轨迹运动的插值时间,aj=[aj10aj11 aj12 aj13 aj20 aj21 aj22 aj23 aj24 aj25 aj30 aj31aj32 aj33]为多项式系数;
根据位置-时间轨迹曲线方程可得到关节j的角速度-时间轨迹曲线和角加速度-时间轨迹曲线公式如下:
使用3-5-3多项式插值规划轨迹时,需要设置约束条件:Xji代表第j个关节已知的插值位置,i=0,1,2,3;轨迹起始点为Xj0、中间路径点为Xj1、Xj2、轨迹末端点为Xj3,设置初始点和末端点的速度、加速度为0,路径点之间的速度和加速度连续;
由以上约束结合上式可推出多项式系数、插值位置、插值时间之间的关系式如下:
b=[000000Xj300 Xj000 Xj2 Xj1]T;
a=A-1·b=[aj13 aj12 aj11 aj10 aj25 aj24 aj23 aj22 aj21 aj20 aj33 aj32 aj31 aj30]T;
由上式可知,当已知4个插值位置和3段插值时间,可得到多项式系数,进而得到位置-时间轨迹曲线、速度-时间轨迹曲线、加速度-时间轨迹曲线的具体方程,即关节空间的轨迹方程;
步骤六、根据关节空间轨迹方程建立优化目标函数和轨迹约束条件,为后续进行时间最优轨迹规划做准备;其中,目标函数如下所示:
约束条件如下所示:
式中,f(tj)代表第j个关节在3-5-3插值轨迹上运动的最短时间,vji为第j个关节在第i段轨迹上的角速度,aji为第j个关节在第i段轨迹上的角加速度;vmax为各段轨迹约束的最大角速度,amax为各段轨迹约束的最大角加速度;
步骤七、使用改进的粒子群算法,对3-5-3多项式插值轨迹中3段轨迹的运行时间tj1、tj2、tj3进行寻优,使工业机器人各关节沿轨迹运行的时间最短,且满足运行过程中关节角速度vji、角加速度aji不超过约束值vmax和amax;
具体的,改进粒子群算法是在基础粒子群算法的基础上使用PWLCM混沌映射初始化种群,使用可调非线性递减惯性权重使算法快速收敛到全局最优;改进粒子群算法具体过程为:
71)确定迭代次数N、种群数量M、最大角速度vmax、最大角加速度amax;
72)采用PWLCM混沌映射初始化粒子种群,粒子维度为3,其中包含粒子位置与粒子飞行速度信息,粒子位置为关节j在三段轨迹上运行的时间tj1、tj2、tj3,tj1、tj2、tj3为M维向量,粒子飞行速度为vj1、vj2、vj3,vj1、vj2、vj3为M维向量,粒子速度上限为vjimax;通过混沌映射能够使初始粒子均匀分布在解空间内,即保证解的随机性与均匀性,以提升粒子群算法的全局搜索能力,混沌映射布图见图6,PWLCM混沌映射如下所示:
式中j为工业机器人第j个关节,i代表第i段轨迹,m代表粒子的序号,即第m个解,p为控制参数,p取值在区间(0,0.5)范围内;
73)对混沌映射得到的粒子进行检验是否符合要求,将混沌映射得到的粒子位置tj1、tj2、tj3,轨迹起始点Xj0、中间路径点为Xj1、Xj2,末端点为Xj3代入多项式系数、插值位置、插值时间之间的关系式中得到多项式系数,进而得到关节j的位置-时间轨迹曲线、角速度-时间轨迹曲线和角加速度-时间轨迹曲线,判断各段轨迹的速度、加速度大小是否满足约束值vmax和amax,若满足则保存为初始粒子,并计算该粒子的适应度f,若不满足则重新生成粒子进行检验,直至生成M个初始粒子,形成初始种群;
74)对粒子种群中的各个粒子进行迭代寻优,首先迭代过程中对粒子的位置、速度进行更新,更新公式如下所示:
/>
式中,为关节j第m个粒子第k次迭代时飞行速度的第d维分量,/>为关节j第m个粒子第k次迭代时位置的第d维分量,r1和r2为[0,1]之间的随机数,pjmd为关节j第m个粒子的最优位置的第d维分量,pjgd为关节j粒子种群最优位置的第d维分量,c1、c2为认知权重因子,w为惯性权重,kl为可调斜率系数,可以调节w在迭代过程的变化方式,如图7所示,影响粒子群算法的收敛性和全局最优性;接着判断粒子速度是否满足上限vjimax,若满足则设为该粒子的当前速度,若不满足则该粒子的当前速度设为vjimax,而后计算该粒子的适应f以及检验该粒子对应的轨迹曲线是否满足约束条件,若该粒子满足约束条件且适应度f小于上一代粒子则更新为新一代粒子,否则将上一代粒子作为新一代粒子;将该新一代粒子的适应度f与当前种群最优适应度fg对比,若新一代粒子的适应度f小于当前种群最优适应度fg,则将该新一代粒子作为种群最优粒子,该新一代粒子的适应度作为种群最优适应度fg,该新一代粒子的位置作为种群最优位置tj1g、tj2g、tj3g。重复迭代直至迭代次数达到N,获取种群最优粒子的位置数据tj1g、tj2g、tj3g作为关节j在三段轨迹上的运行时间;
步骤八、完成所有关节的轨迹优化,进行关节同步处理;由于工业机器人在点到点关节规划中,各关节的规划时间不同,为确保关节运动的同步性,需要对各关节的运行时间进行同步处理,以确保在各关节的配合下,工业机器人末端执行器到达预定位置;分别对第1、2、3段轨迹取各关节在该轨迹段上运行时间中的最大值,并作为所有关节在该轨迹段上的插值时间,即t1=max{tj1g},t2=max{tj2g},t3=max{tj3g}(j=1,2,……,n);
步骤九、使用同步处理后的插值时间t1,t2,t3对各关节进行3-5-3多项式插值,得到各关节的时间最优轨迹;
步骤十、将关节空间轨迹发送给控制器,控制工业机器人沿轨迹运动,完成躲避障碍物并到达目标点,仿真效果如图8和图9所示。
本方法中,从两方面缩短了工业机器人在复杂环境中完成动作所需的时间,一是从规划时间方面:在较短时间内规划出一条可行路径,缩短了工业机器人进行路径规划的时间,以及加快粒子群算法的收敛速度,缩短了工业机器人进行轨迹规划的时间;二是从运动时间方面:采用改进粒子群算法进行时间最优轨迹规划,缩短了工业机器人完成动作所需的运动时间。
1、工业机器人进行避障过程中,计算工业机器人本体与障碍物之间的距离是一个复杂而耗时的操作,通过对工业机器人连杆进行虚拟关节点插补,而后在关节点处使用球状包围盒对工业机器人进行包络以简化工业机器人模型,结合将障碍物进行点云化的方式,使得后续进行工业机器人本体与障碍物距离计算时,只需计算虚拟关节点与障碍物点云中各点之间的距离即可,极大简化了距离计算的复杂程度进而缩短了工业机器人进行路径规划的时间。
2、基础RRT算法是通过随机采样和逐步扩展树结构的方式进行路径搜索,其能够快速搜索并生成可行路径,即具备快速收敛的性质;RRT*算法通过相邻空间搜索和树重连机制减少了路径的转折,使得路径更加平滑、减短了路径的长度,有利于缩短工业机器人运动的时间;通过加入人工势场机制与目标偏置机制对采样过程进行干预,可以使随机树有方向地朝着目标点生长,进一步加快了搜索路径的速度。G-APF-RRT*算法具备上述所有优点,能够极大缩短工业机器人进行路径规划的时间以及缩短工业机器人完成动作所需的运动时间。
3、常规的关节避障是在规划完整路径后,对每个路径点进行关节碰撞检测,若某一路径点处关节发生碰撞,则需剔除该路径点进行重新采点并连接上原路径,该过程复杂且可能无法生成可行路径,而本发明基于G-APF-RRT*算法树节点较少的优势,见图10,采用生成树节点后立即进行碰撞检测的方式,简化了关节避障的流程,缩短了工业机器人进行路径规划的时间,且避免了规划的完整路径不可用。
4、采用PWLCM混沌映射初始化粒子种群,提升了粒子群算法的全局搜索能力,使得经过优化后的关节运行时间tj1g,tj2g,tj3g更趋近于最优解,即关节在各段轨迹上运行的时间更短,缩短了工业机器人完成动作所需的运动时间。
5、对传统粒子群算法惯性权重进行修改,采用可调非线性递减惯性权重影响粒子位置的更新,使算法快速收敛到全局最优,缩短了工业机器人进行轨迹规划的时间。
尽管参照前述实施例对本发明进行了详细的说明,对于本领域的技术人员来说,其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (3)
1.一种复杂环境下工业机器人避障方法,其特征在于,包括以下步骤:
S1、对工作空间内的工业机器人进行模型简化以及将障碍物简化为障碍物点云;
所述工业机器人模型简化方法为:确定工业机器人的真实关节点,在相邻真实关节点之间的连杆上以一定距离插补虚拟关节点,而后以关节点为球心采用球形包围盒完全包络工业机器人本体,确保工业机器人处于任何位姿时,都能完整代替工业机器人;
S2、采用“眼在手外”的布置方式将深度相机放置在工业机器人的运动空间之外;在进行路径规划前获取工作空间中工业机器人和障碍物的点云信息,使用点云滤波器滤除工业机器人本体点云,仅保留工作空间下的障碍物点云;
S3、在工业机器人进行运动前采用G-APF-RRT*算法在工作空间中规划一条可行路径;
S4、为避免工业机器人末端沿路径运动时工业机器人关节与障碍物发生碰撞,对新生成的树节点进行关节碰撞检测,获得工业机器人从起始点运动到目标点的无碰撞完整路径;
所述关节碰撞检测的流程为:通过逆运动学求解工业机器人末端位于新生成的树节点处时各关节的位置信息,在基坐标系下,计算工业机器人各包围球球心与障碍物点云之间的欧式距离,获取最短距离与安全距离进行比较,若所述最短距离大于设定安全距离时,保留该新生成的树节点,若所述最短距离小于设定安全距离时,舍弃该新生成的树节点并进行重新采样,直至获得工业机器人从起始点运动到目标点的无碰撞完整路径,欧氏距离计算公式如下:
;
式中,代表第j个包围球球心与第i个障碍物点云之间的欧氏距离,(/>,/>,)为工业机器人上第j个包围球在基坐标系下的坐标,(/>,/>,/>)为第i个障碍物点云在基坐标系下的坐标;
S5、将获得的无碰撞完整路径进行逆运动学计算,得出工业机器人各关节在关节空间下的关节位置,即插值位置,用于后续的轨迹插值,通过3-5-3多项式插值方法对工业机器人进行关节空间轨迹规划,得到工业机器人的关节空间位置—时间轨迹曲线、角速度—时间轨迹曲线和角加速度—时间轨迹曲线;
所述轨迹曲线的方程如下:
;
;
;
式中,、/>、/>、分别代表工业机器人第j个关节的第1、2、3段运动轨迹,其分别采用3次多项式插值、5次多项式插值、3次多项式插值即3-5-3多项式插值,j=1,2,…,n,n为工业机器人的真实关节数目;t1、t2、t3分别代表工业机器人沿第1、2、3段轨迹运动的插值时间,/>=[/> ]为多项式系数;
根据位置-时间轨迹曲线方程可得到关节j的角速度-时间轨迹曲线和角加速度-时间轨迹曲线公式如下:
;
;
;
;
;
;
使用3-5-3多项式插值规划轨迹时,需要设置约束条件:代表第j个关节已知的插值位置,i = 0,1,2,3;轨迹起始点为/>、中间路径点为/>、/>、轨迹末端点为/>,设置初始点和末端点的速度、加速度为0,路径点之间的速度和加速度连续;
由以上约束结合上式可推出多项式系数、插值位置、插值时间之间的关系式如下:
;
;
;
由上式可知,当已知4个插值位置和3段插值时间,可得到多项式系数,进而得到位置-时间轨迹曲线、速度-时间轨迹曲线、加速度-时间轨迹曲线的具体方程,即关节空间的轨迹方程;
S6、根据关节空间轨迹方程建立优化目标函数和轨迹约束条件,为后续进行时间最优轨迹规划做准备;
所述目标函数如下所示:
;
所述约束条件如下所示:
;
式中,代表第j个关节在3-5-3插值轨迹上运动的最短时间,/>为第j个关节在第i段轨迹上的角速度,/>为第j个关节在第i段轨迹上的角加速度;/>为各段轨迹约束的最大角速度,/>为各段轨迹约束的最大角加速度;
S7、使用改进的粒子群算法,对3-5-3多项式插值轨迹中3段轨迹的运行时间、/>、进行寻优,使工业机器人各关节沿轨迹运行的时间最短,且满足运行过程中关节角速度、角加速度/>不超过约束值/>和/>;
改进粒子群算法是在基础粒子群算法的基础上使用PWLCM混沌映射初始化种群,使用可调非线性递减惯性权重使算法快速收敛到全局最优;所述改进粒子群算法具体过程为:
S71、确定迭代次数N、种群数量M、最大角速度、最大角加速度/>;
S72、采用PWLCM混沌映射初始化粒子种群,粒子维度为3,其中包含粒子位置与粒子飞行速度信息,粒子位置为关节j在三段轨迹上运行的时间、/>、/>,/>、/>、/>为M维向量,粒子飞行速度为/>、/>、/>,/>、/>、/>为M维向量,粒子速度上限为;通过混沌映射能够使初始粒子均匀分布在解空间内,即保证解的随机性与均匀性,以提升粒子群算法的全局搜索能力,所述PWLCM混沌映射如下所示:
;
式中j为工业机器人第j个关节,i代表第i段轨迹,m代表粒子的序号,即第m个解,p为控制参数,p取值在区间(0,0.5)范围内;
S73、对混沌映射得到的粒子进行检验是否符合要求,将混沌映射得到的粒子位置、、/>,轨迹起始点/>、中间路径点为/>、/>,末端点为/>代入多项式系数、插值位置、插值时间之间的关系式中得到多项式系数,进而得到关节j的位置-时间轨迹曲线、角速度-时间轨迹曲线和角加速度-时间轨迹曲线,判断各段轨迹的速度、加速度大小是否满足约束值/>和/>,若满足则保存为初始粒子,并计算该粒子的适应度/>,若不满足则重新生成粒子进行检验,直至生成M个初始粒子,形成初始种群;
S74、对粒子种群中的各个粒子进行迭代寻优,首先迭代过程中对粒子的位置、速度进行更新,更新公式如下所示:
;
;
;
;
;
式中,为关节j第m个粒子第k次迭代时飞行速度的第d维分量,/>为关节j第m个粒子第k次迭代时位置的第d维分量,r1和r2为[0,1]之间的随机数,/>为关节j第m个粒子的最优位置的第d维分量,/>为关节j粒子种群最优位置的第d维分量,c1、c2为认知权重因子,w为惯性权重,kl为可调斜率系数,可以调节w在迭代过程的变化方式,影响粒子群算法的收敛性和全局最优性;
接着判断粒子速度是否满足上限,若满足则设为该粒子的当前速度,若不满足则该粒子的当前速度设为/>,而后计算该粒子的适应/>以及检验该粒子对应的轨迹曲线是否满足约束条件,若该粒子满足约束条件且适应度/>小于上一代粒子则更新为新一代粒子,否则将上一代粒子作为新一代粒子;将该新一代粒子的适应度/>与当前种群最优适应度/>对比,若新一代粒子的适应度/>小于当前种群最优适应度/>,则将该新一代粒子作为种群最优粒子,该新一代粒子的适应度作为种群最优适应度/>,该新一代粒子的位置作为种群最优位置/>、/>、/>,重复迭代直至迭代次数达到N,获取种群最优粒子的位置数据/>、/>、/>作为关节j在三段轨迹上的运行时间;
S8、完成所有关节的轨迹优化,进行关节同步处理;
S9、使用同步处理后的插值时间t1,t2,t3对各关节进行3-5-3多项式插值,得到各关节的时间最优轨迹;
S10、将关节空间轨迹发送给控制器,控制工业机器人沿轨迹运动,完成躲避障碍物并到达目标点。
2.根据权利要求1所述的一种复杂环境下工业机器人避障方法,其特征在于,在步骤S3中,所述G-APF-RRT*算法是一种改进型的快速扩展随机树算法,快速扩展随机树算法简称RRT算法;所述快速扩展随机树算法是在工作空间内基于随机采样方法对随机采样点进行连接以形成一条可行路径,其连接过程类似树的生长。
3.根据权利要求1所述的一种复杂环境下工业机器人避障方法,其特征在于,在步骤S8中,由于工业机器人在点到点关节规划中,各关节的规划时间不同,为确保关节运动的同步性,需要对各关节的运行时间进行同步处理,以确保在各关节的配合下,工业机器人末端执行器到达预定位置;分别对第1、2、3段轨迹取各关节在该轨迹上运行时间中的最大值,并作为所有关节在该轨迹上的插值时间,即,/>,(j=1,2,……,n)。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311463044.3A CN117773911B (zh) | 2023-11-03 | 2023-11-03 | 一种复杂环境下工业机器人避障方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311463044.3A CN117773911B (zh) | 2023-11-03 | 2023-11-03 | 一种复杂环境下工业机器人避障方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN117773911A CN117773911A (zh) | 2024-03-29 |
CN117773911B true CN117773911B (zh) | 2024-05-17 |
Family
ID=90400617
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311463044.3A Active CN117773911B (zh) | 2023-11-03 | 2023-11-03 | 一种复杂环境下工业机器人避障方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117773911B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN118024262A (zh) * | 2024-04-11 | 2024-05-14 | 深圳市普渡科技有限公司 | 运行状态检测方法、装置、设备和存储介质 |
Citations (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109676610A (zh) * | 2019-01-25 | 2019-04-26 | 温州大学 | 一种断路器装配机器人及其实现工作轨迹优化的方法 |
CN111515503A (zh) * | 2020-04-30 | 2020-08-11 | 华东理工大学 | 一种弧焊机器人无碰撞路径规划方法 |
CN112601641A (zh) * | 2018-08-23 | 2021-04-02 | 实时机器人有限公司 | 用于机器人运动规划的碰撞检测 |
CN112809665A (zh) * | 2020-12-16 | 2021-05-18 | 安徽工业大学 | 一种基于改进rrt算法的机械臂运动规划方法 |
CN114147708A (zh) * | 2021-11-26 | 2022-03-08 | 山东大学 | 基于改进的天牛须搜索算法的机械臂避障路径规划方法 |
CN114161416A (zh) * | 2021-12-06 | 2022-03-11 | 贵州大学 | 基于势函数的机器人路径规划方法 |
CN114407030A (zh) * | 2021-11-12 | 2022-04-29 | 山东大学 | 一种自主导航配网带电作业机器人及其工作方法 |
CN115008475A (zh) * | 2022-07-21 | 2022-09-06 | 北京工业大学 | 一种基于混合几何表征的双机械臂协同避障运动规划优化方法 |
CN115284293A (zh) * | 2022-08-19 | 2022-11-04 | 北京空间飞行器总体设计部 | 适应复杂任务的空间站机械臂多模式路径规划系统和方法 |
CN115309161A (zh) * | 2022-08-23 | 2022-11-08 | 南京信息工程大学 | 一种移动机器人路径规划方法、电子设备及存储介质 |
CN116117822A (zh) * | 2023-03-08 | 2023-05-16 | 福州大学 | 基于非障碍物空间概率势场采样的rrt机械臂轨迹规划方法 |
CN116352714A (zh) * | 2023-04-11 | 2023-06-30 | 广东工业大学 | 一种机械臂避障路径规划方法 |
CN116619379A (zh) * | 2023-06-12 | 2023-08-22 | 北京熠视科技有限公司 | 一种基于机械臂的改进rrt路径规划算法 |
WO2023155923A1 (zh) * | 2022-02-21 | 2023-08-24 | 中兴通讯股份有限公司 | 路径规划方法及装置、可读存储介质 |
CN116945177A (zh) * | 2023-08-03 | 2023-10-27 | 江苏今世缘酒业股份有限公司 | 上甑机器人补料路径规划方法、装置、电子设备及介质 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20190184561A1 (en) * | 2017-12-15 | 2019-06-20 | The Regents Of The University Of California | Machine Learning based Fixed-Time Optimal Path Generation |
US20230048578A1 (en) * | 2022-09-27 | 2023-02-16 | Intel Corporation | Real-time predictor of human movement in shared workspaces |
-
2023
- 2023-11-03 CN CN202311463044.3A patent/CN117773911B/zh active Active
Patent Citations (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112601641A (zh) * | 2018-08-23 | 2021-04-02 | 实时机器人有限公司 | 用于机器人运动规划的碰撞检测 |
CN109676610A (zh) * | 2019-01-25 | 2019-04-26 | 温州大学 | 一种断路器装配机器人及其实现工作轨迹优化的方法 |
CN111515503A (zh) * | 2020-04-30 | 2020-08-11 | 华东理工大学 | 一种弧焊机器人无碰撞路径规划方法 |
CN112809665A (zh) * | 2020-12-16 | 2021-05-18 | 安徽工业大学 | 一种基于改进rrt算法的机械臂运动规划方法 |
CN114407030A (zh) * | 2021-11-12 | 2022-04-29 | 山东大学 | 一种自主导航配网带电作业机器人及其工作方法 |
CN114147708A (zh) * | 2021-11-26 | 2022-03-08 | 山东大学 | 基于改进的天牛须搜索算法的机械臂避障路径规划方法 |
CN114161416A (zh) * | 2021-12-06 | 2022-03-11 | 贵州大学 | 基于势函数的机器人路径规划方法 |
WO2023155923A1 (zh) * | 2022-02-21 | 2023-08-24 | 中兴通讯股份有限公司 | 路径规划方法及装置、可读存储介质 |
CN115008475A (zh) * | 2022-07-21 | 2022-09-06 | 北京工业大学 | 一种基于混合几何表征的双机械臂协同避障运动规划优化方法 |
CN115284293A (zh) * | 2022-08-19 | 2022-11-04 | 北京空间飞行器总体设计部 | 适应复杂任务的空间站机械臂多模式路径规划系统和方法 |
CN115309161A (zh) * | 2022-08-23 | 2022-11-08 | 南京信息工程大学 | 一种移动机器人路径规划方法、电子设备及存储介质 |
CN116117822A (zh) * | 2023-03-08 | 2023-05-16 | 福州大学 | 基于非障碍物空间概率势场采样的rrt机械臂轨迹规划方法 |
CN116352714A (zh) * | 2023-04-11 | 2023-06-30 | 广东工业大学 | 一种机械臂避障路径规划方法 |
CN116619379A (zh) * | 2023-06-12 | 2023-08-22 | 北京熠视科技有限公司 | 一种基于机械臂的改进rrt路径规划算法 |
CN116945177A (zh) * | 2023-08-03 | 2023-10-27 | 江苏今世缘酒业股份有限公司 | 上甑机器人补料路径规划方法、装置、电子设备及介质 |
Non-Patent Citations (1)
Title |
---|
陈鹏飞.六自由度工业搬运机器人运动规划研究.硕士学位论文全文库.1-78页. * |
Also Published As
Publication number | Publication date |
---|---|
CN117773911A (zh) | 2024-03-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN117773911B (zh) | 一种复杂环境下工业机器人避障方法 | |
CN111546347B (zh) | 一种适用于动态环境下的机械臂路径规划方法 | |
CN112440281A (zh) | 一种基于数字孪生的机器人轨迹规划方法 | |
CN112809665B (zh) | 一种基于改进rrt算法的机械臂运动规划方法 | |
CN109240091B (zh) | 一种基于强化学习的水下机器人控制方法及其进行跟踪的控制方法 | |
CN113103236B (zh) | 一种快速渐进最优的机械臂避障路径规划方法 | |
CN109976347B (zh) | 一种基于快速扩展随机树和势场法的视觉伺服路径规划方法 | |
CN112549016A (zh) | 一种机械臂运动规划方法 | |
CN111251306B (zh) | 一种带有底盘误差的机械臂路径规划方法 | |
CN115416016A (zh) | 一种基于改进人工势场法的机械臂避障路径规划方法 | |
CN109108978A (zh) | 基于学习泛化机制的三自由度空间机械臂运动规划方法 | |
CN109032128B (zh) | 离散多agv非质点系统的三角编队控制方法 | |
CN114115271A (zh) | 一种改进rrt的机器人路径规划方法和系统 | |
CN116117822A (zh) | 基于非障碍物空间概率势场采样的rrt机械臂轨迹规划方法 | |
JP7210201B2 (ja) | 情報処理方法、プログラム、記録媒体、情報処理装置、ロボットシステム、物品の製造方法 | |
CN116852367A (zh) | 一种基于改进RRTstar的六轴机械臂避障路径规划方法 | |
CN116690557A (zh) | 基于点云的仿人三维扫描运动控制方法及装置 | |
CN113829351B (zh) | 一种基于强化学习的移动机械臂的协同控制方法 | |
CN115542921A (zh) | 多机器人的自主路径规划方法 | |
CN115008460A (zh) | 基于目标偏置和障碍物因子的rrt机械臂避障规划方法 | |
CN115922716A (zh) | 一种融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划 | |
CN111857054B (zh) | 一种基于神经网络的数控系统运动轨迹控制方法 | |
CN113448336B (zh) | 3d避障路径规划方法 | |
CN113848725A (zh) | 一种基于自适应遗传算法的机械臂时间最优轨迹规划优化方法 | |
CN117182932B (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 |