双工业机器人同步作业的无碰撞路径规划方法

2022-10-03张立香孟欣佳王晓敏丁志杰

制造业自动化 2022年9期

张立香，孟欣佳，王晓敏，丁志杰

（河北工程大学机械与装备工程学院，邯郸 056038）

0 引言

工业机器人是现代智能制造系统的重要组成部分，是衡量一个国家科技创新实力和高端制造业水平的重要标志[1,2]。工业领域机器人化的加速发展，使得机器人系统涵盖的作业任务愈加复杂和繁重，单机器人独立作业已难以满足协同装配[3]、复杂焊接[4]、大尺寸重物搬运[5]等复杂工况的需求。多机器人系统[6]通过机器人间的有机协作完成各种复杂任务，从而满足不同作业需要，成为了解决复杂作业问题的有效途径。多机器人的协同作业给机器人技术的发展带来了新的挑战，而路径规划作为机器人发展的核心技术之一，也成为多机器人领域研究的热点[7,8]。

张瑞星等[9]对双机器人协同焊接的路径规划进行研究，结合蚁群算法和粒子群算法，提出一种基于蚁群粒子群的融合算法。李洋等[10]对快速随机扩展树（Rapidly exploring-random tree，RRT）方法进行改进，提出了随机树被动生长方法，实现了双机器人在各自构型空间中的协同路径规划。徐金雄等[11]提出一种双蚁群路径规划方法，利用不同种群蚁群之间信息素的负反馈作用，降低两个机器人间的冲突，进而找到最优移动路径。Das等[12]提出改进的粒子群算法来计算多机器人在已知环境中的最优无碰撞路径。Nazarahari等[13]提出一种改进的遗传算法，首先采用人工势场法找到初始的可行路径，然后利用改进的遗传算法改进初始方案，并加入碰撞移除操作，从而保证多机器人获得最优的无碰撞路径。彭鹏等[14]对6自由度双机械臂的工作空间进行分析，分别利用改进的人工势场法（Artificial potential field method，APF）和RRT算法对主、从机械臂的路径进行规划，有效解决了从机械臂路径规划时陷入局部极小值的问题。

目前，针对多工业机器人的路径规划主要集中在多个机器人完成不同任务时，各机器人的最优路径规划。但在实际工程中，存在多个机器人协同完成同一任务的情况，如，多个机器人需要共同合作装配或搬运同一个零件，即多个机器人处于同步作业，此时机器人的执行末端在行进过程中始终保持固定距离。本文针对多个机器人同步作业时的避障路径规划问题，在距离约束下，将粒子群优化（Particle Swarm Optimization，PSO）算法与随机抽样技术相结合，提出一种用于双工业机器人同步作业时的无碰撞路径规划方法。

1 双机器人无碰撞路径规划建模

如图1所示，机器人R1与机器人R2同步作业时，R1的执行末端从起点Q1沿路径L1运行到目标点G1，而R2的执行末端从起点Q2沿路径L2运行到目标点G2。在此过程中，R1与R2的执行末端之间的距离始终保持为两机器人抓取点之间的距离d不变，且在行进过程中路径L1、L2和作业物体W均不能与障碍物发生碰撞。因此，双工业机器人的避障路径规划实际上是在有约束的情况下，进行双路径优化求解问题。

图1 双机器人同步作业示意图

由此，建立双工业机器人同步作业的无碰撞路径规划模型为：

式(1)中，f为双路径总距离；f1和f2分别为R1和R2行走的路径L1和L2的距离；i为机器人走过的路径点数；表示求模数；d为两条路径之间的固定距离；W为作业物体所在区域集合；Ψ为障碍物所在区域集合；∉表示不属于。

2 基于PSO算法和距离约束的双路径避障规划方法

2.1 双路径避障规划方法原理分析

针对式(1)所示的优化问题，实际上是一个无碰撞的双路径优化问题，可将其拆解成两个嵌套的优化模型，具体见式(2)和式(3)。

式(2)所示的优化问题实际上就是单机器人的避障路径规划问题，现有的RRT算法、人工势场法、PSO算法等方法均可对其进行求解。其中，PSO算法因其结构简单，易于实现，在机器人的路径规划中得到了广泛应用[15,16]。利用PSO算法进行路径求解时，每个粒子都视为一条路径，通过不断迭代求解最终获得距离最短的粒子，即最优路径。

在对式(3)所示的路径优化问题求解时，需要在距离约束下，求解式(3)所示的路径规划问题时，其原理如图2所示。

图2 距离约束下的路径求解原理图

如图2所示，L1(i)为路径L1的第i个路径点，与之对应的路径L2的第i个路径点为L2(i)。为保证路径点L1(i)与路径点L1(i)之间的距离为d，点L2(i)只能在以为L1(i)圆心，以d为半径的球面上。因此，求点L2(i)时，可以点L1(i)为原点，构建球坐标系，球坐标系的参数为：ρ=d，θ∈[0,π]，φ∈[0,2π]，d为已知。参数θ和φ可利用抽样原理在其取值范围内进行随机抽样，获得球坐标θ和φ的不同组合，利用坐标变换获得直角坐标系下点L2(i)的不同取值。再依据最优原则和避障约束，取距离L2的上一个点L2(i-1)最近的点为L1(i)，且要保证L2(i)到L1(i)的连线不能经过障碍物所在区域。

通过上述分析，本文以PSO算法求解路径L1的基本流程为主体，在每一代求解的最优粒子下，基于距离约束下的路径求解原理，嵌套进行路径L2的求解，通过循环求解，最终获得L1和L2的最优路径，以此构建一种基于PSO算法和距离约束的双路径避障规划方法。

2.2 双路径避障规划方法流程

基于PSO算法和距离约束的双路径避障规划方法是在PSO优化求解的流程上嵌入距离约束下的路径规划，其流程如图3所示。

图3 基于PSO算法和距离约束的双路径避障规划方法流程

基于PSO算法和距离约束的双路径避障规划方法的具体步骤如下：

Step1：PSO算法初始化

PSO算法初始化主要包括以下几个方面：

1）分别确立两个机器人的起点Q1、Q2和对应的目标点G1、G2。

2）设初始迭代次数为k1=0，最大迭代次数为N。

3）设置粒子群总数为M，每个粒子的位置控制点的总个数为n。

4）设惯性权重为ω，粒子学习因子为c1、c2。

5）确定速度界限[Vmin,Vmax]和位置界限[Xmin,Xmax]，初始速度为V0。

6）粒子群初始化，在起点Q1和对应的目标点G1之间随机生成n个随机数，以此获得第i个粒子的初始位置点

Step2：B样条插值

对当前迭代中，每个粒子进行B样条插值获得M条光滑路径。每个粒子的B样条曲线方程为：

Step3：碰撞检测

依据障碍物的外形特点，建立碰撞模型。判断是否碰撞的准则为：

式(6)中，Pi为生成路径离散的第i个路径点。当生成的路径点Pi位于障碍物所在区域时，发生碰撞，此路径不可行；当生成的路径点Pi不位于障碍物所在区域时，不发生碰撞，路径可行。

Step4：适应度计算

适应度函数是评价路径质量的重要指标。通过适应度函数求解各粒子的路径长度，以便寻找当代粒子群中的最优粒子。将Step3获得的可行路径进行点与点之间的路径求解，再整体求和获得整个粒子的路径长度，具体为：

式(7)中，f1,j为第j个粒子的路径长度，下角标1表示路径L1；m为路径离散点的个数；Pi为第i个路径点；P0为起点；Lm+1为目标点。

Step5：最优粒子获取

对第k1次迭代的可行路径的适应度值进行对比，适应度值最小的即为路径L1在第k1次迭代的最优粒子记为L1,k1，其适应度值为f1,k1。将适应度值传递到Step7，并将L1,k1的路径点传递到Step6，进行路径2的求解。

Step6：距离约束下的路径求解

在第一条路径L1,k1已知大前提下，求解与其对应的第二条路径L2,k1。

Step6-1：初始化设置

设路径L1,k1的路径点总个数为N1，初始迭代次数为k2=1，路径L2,k1的初始距离f2,k1=0。

Step6-2：坐标系建立

提取路径L1,k1的第k2个路径点P1,k2，以该点为原点建立局部坐标系x'y'z'，坐标轴的方向与全局坐标系xyz相同，如图4所示。

图4 路径点局部坐标系

全局坐标系xyz与局部坐标系x'y'z'的坐标变换公式为：

式(8)中，x、y、z为直角坐标系下的全局坐标，x'y'z'为直角坐标系下的局部坐标，(x1,k2,y1,k2,z1,k2)为点P1,k2的全局直角坐标值。

以参数d、θ、φ，以x'、y'、z'为坐标轴建立球坐标系，与点P1,k2相对应的点P2,k2在x'y'z'坐标系的直角坐标为：

Step6-3：随机抽样

对球坐标系的参数θ和φ进行随机抽样，抽样范围为：θ∈[0,π]，φ∈[0,π]，抽样个数为N2。为避免取样点集中的问题，在此采用拉丁超立方(LHS)抽样法。依据式(8)和式(9)，将抽样获得球坐标转换到全局直角坐标，从而获得N2组样本。

Step6-4：碰撞检测

依据Step3对Step6-3获得的所有抽样点进行碰撞检测，同时还需要对点P2,k2和P2,k2的连线进行碰撞检测。以此将会产生碰撞的点去除，最终获得可行的抽样点。

Step6-5：距离求解

依据点到点的距离公式，求解所有可行的抽样点到上一个点P2,k2-1的距离，以及到目标点的距离。选取值最小的抽样点作为点P2,k2，得

Step6-5：收敛判断

距离约束下路径求解的收敛准则，路径L2,k1的所有路径点都求解完，即：

当满足收敛准则时，Step 6求解结束，由此获得第k1次的路径L2,k1和路径长度f2,k1，并将f2,k1值传递到Step7，进行整体收敛判断；否则，令k2=k2+1，继续进行下一个路径点的求解。

Step7：收敛判断

将Step5和Step6获得f1,k1和f2,k1进行求和，先计算整体路径长度fk1=f1,k1+f2,k1，再做收敛判断，其收敛准则为：

式(12)中，fk1和fk1-1分别为前后两次迭代求解整体路径值；Nε为满足收敛精度的次数；[Nε]为满足收敛精度的允许最小次数，以保证求解的稳定性。

当满足收敛准则时，给出粒子群求解的最优路径；当不满则收敛准则时，令k1=k1+1，转到Step8继续求解。

Step8：粒子速度更新

每次迭代完成后，需要更新粒子速度，以便获得新的粒子群。粒子速度更新公式为：

Step9：粒子群更新

利用Step8获得粒子速度更新粒子位置，即获得新的粒子群。粒子位置更新公式为：

当获得新的粒子群后，转到Step2，继续进行迭代求解，直至满足Step7的收敛准则。

3 仿真验证

3.1 仿真设计

本文是在障碍物形状，机器人的起点和目标点、抓取物体形状均已知的情况下，对所提方法进行的验证。作业物体形状为圆柱体，简化成一条线段。机器人抓取位置为物体两端，简化为点，两机器人抓取点之间的距离d=5。机器人R1与R2的起点和目标点在全局坐标系下的坐标分别为Q1(1,1,1)、G1(100,100,100)和Q2(4,5,1)、G2(103,104,100)。从起点到目标点设置4个椭球形状的障碍物，其参数如表1所示。

表1 障碍物参数

在采用本方法进行路径规划时，Step3和Step6-4的碰撞检测包括路径点碰撞检测和物体碰撞检测。鉴于障碍物的形状，路径点碰撞检测是判断路径点是否在椭球面内，具体为将路径点坐标代入椭球公式，椭球公式为：

式(17)中，(x,y,z)为路径点的全局直角坐标，(xc,yc,zc)为椭球中心坐标，(xr,yr,zr)为椭球半轴。当QT≤1时，将与障碍物发生碰撞；否则不发生碰撞。

作业物体碰撞检测方法，先求解椭球中心Ac(xc,yc,zc)到两路径点连线的垂直距离l和垂足坐标A0(x0,y0,z0)，为：

通过判断垂足所在位置，及椭球中心到垂足的距离来判断作业物体是否与障碍物碰撞。当垂足A0位于两路径点P1,k2和P2,k2之间时，将垂足坐标代入式(17)，判断原则与路径点相同；当垂足A0位于两路径点P1,k2和P2,k2之外时，只要两路径点P1,k2和P2,k2不碰撞，整体作业物体即不与障碍物发生碰撞。

3.2 仿真分析

本方法涉及到PSO算法和随机抽样，在距离约束下求解路径L2时，还需要将路径L1离散成多个路径点。PSO算法的种群大小、随机抽样的个数以及路径点的个数对求解结果有较大的影响，采用MATLAB在上述不同参数下进行仿真，对三种系统参数的灵敏度进行分析。

表2为随机抽样个数和路径点相同时，在不同种群大小下的仿真结果。由表2可知，种群大小为90时，三次仿真求解的最优值差距最小，求解结果相对稳定，且路径长度的平均值最小。

表2 不同种群大小的仿真结果

表3为种群大小和路径点相同时，在不同抽样个数下的仿真结果，表中差值表示与前一组优化解的差，“－”表示减少。由表3可知，随着抽样个数的增加，最优值越来越小，求解时间越来越长。抽样个数每增加100，仿真获得的最优值与前一组的差值逐渐减小。此外，抽样个数为400时，迭代次数最少，仿真时间与抽样个数为300时，差距不多。

表3 不同抽样个数的仿真结果

表4为种群大小和抽样个数相同时，在不同路径点下的仿真结果。由表4可知，路径点越多，最优值越大。这是由于路径的长度是由点到点的直线距离叠加而成的，路径离散的点越少，越接近直线，求解的长度就越小，路径也就越失真。此外，由表4还可看出，路径点个数为50时，迭代次数最大为53，但其所用的仿真时间仍然是最少的。这表明仿真时间受路径点个数的影响较大，路径点越多，仿真时间越长。综合考虑路径的保真性和求解时间，可选取路径点个数为100。