基于采样的工业机器人半约束路径规划

孙 昱，孟祥群，郁苗成，唐厚君

(1. 上海交通大学电气工程系，上海 200240；2. 上海方菱计算机软件有限公司，上海 200240)

0 引言

随着人力成本的上升，机器人进入工厂以提高工厂自动化水平、提高生产力、降低生产成本的需求日益提高。目前，对工业机器人任务路径规划的研究主要集中在点到点的运动规划，即在机器人的初始点与终点位姿确定的情况下,对运动路径约束较少甚至无约束，在此条件下由算法自动完成对运动路径中间点的搜索，在该类规划中有效配置空间大，如何从诸多可行解中选择最优解成为研究的重点。常用的点到点路径规划算法有A*算法[1]、 蚁群算法[2]、 遗传因子算法[3]、 随机树法[4-5]等。

在切割、焊接、打磨和喷涂等六轴工业机器人的典型应用中，需要机器人的末端执行器始终遵循固定的几何路径，同时还需要在工作过程中保持一定的空间姿态。目前对此类具有过程约束的路径规划研究较少，文献[6-7]针对经过空间中多个路径点的固定路径规划问题，通过设计评价路径的目标函数，将多点路径规划问题转化为旅行商问题利用智能优化算法对路径进行求解，得到了较好的规划效果。但所采用智能算法收敛速度慢，且规划结果经过路径点的顺序不确定，机器人在运动过程中不能始终满足任务约束。文献[8-10]提出了基于采样的路径规划算法，在关节空间对满足任务约束的关节配置进行采样，并在采样结果的基础上进行路径规划，这样能保证机器人运动过程中始终满足任务约束，但缺点是对中间状态的随机采样搜索导致算法不适用于末端执行器路径固定的情况。

在上述工业应用场景中，机器人任务路径通常是半约束的(如机器人进行打磨或喷涂时，末端执行器可绕工具坐标系Zt轴[11]进行任意角度旋转)，此时机器人的自由度大于路径的自由度。本文在文献[8-10]等提出的采样算法的基础上，利用此类应用场景中的半约束路径特性，通过在笛卡尔空间对有冗余自由度的路径点进行离散采样，避免了文献[6-7]中提出的智能算法搜索维度大导致的收敛速度慢，耗时过长的问题，避免了文献[8-10]中末端执行器路径不固定的问题。利用六轴工业机器人的结构特点，采用解析法进行逆运动学求解，该方法比文献[4]采用的基于牛顿-拉夫逊的逆运动学求解方法速度快，且求解一致性更高。最后在关节空间进行最优路径搜索,解决了文献[6-7]采用的智能算法收敛速度慢的问题。在保证路径规划效果的同时，大幅减少规划所用时间，有效解决了具有任务约束的固定路径规划问题。并以ABB IRB1200型号六轴工业机器人为对象进行了仿真和实验测试，验证了方案的可行性和有效性。

1 基于采样的半约束路径规划

针对六轴工业机器人典型应用场景中机器人自由度大于满足任务约束的运动路径自由度的特点，本文提出基于采样的路径规划算法，具体实现和评估如下。

1.1 基于采样的路径规划算法

算法实现分为以下4个阶段：

(1)生成笛卡尔空间半约束路径

根据需要执行的任务，我们可以得到机器人在工作空间(笛卡尔空间)中运动路径的描述。与传统人工示教方式不同，本文采取编程的方式精确产生笛卡尔空间路径曲线的参数描述,并对曲线进行等间隔插值获得一组插值点。路径的参数描述是一个六维函数，路径上每个插值点都是一个六维向量，用以指示工具的位置和方向，见式(1)：

p=[x,y,z,Rx,Ry,Rz]T

(1)

式中，x、y、z代表工具的位置，Rx、Ry、Rz代表工具的方向。这种基于多个中间点表示的路径根据任务约束的不同可分为如下两类[12]。

1)插值路径：路径曲线与定点相交，如图1a中的黑点所示。

2)近似值路径：路径曲线可在指定允差范围(一定的位置误差或方向误差)内不精确地通过这些点，如图1b所示。

(a) 插值路径 (b) 近似值路径图1 经过一组空间点的路径曲线

在切割、焊接和喷涂等典型六轴工业机器人应用场景中，机器人末端执行器工作过程中所要遵循的路径通常是第二种，例如机器人在进行切割或喷涂作业时，末端执行器绕工具坐标系Zt轴[11]的旋转角度可取任意值。此时机器人的自由度(6DOF)大于路径的自由度(5DOF)。故称此类有冗余自由度的路径为半约束路径，对路径插值点设置满足任务约束的允差，获得的笛卡尔空间半约束路径点可表示为式(2)：

p=[x,y,z,Rx,Ry,Rz,xtol,ytol,ztol,Rxtol,Rytol,Rztol]T

(2)

式中,xtol,ytol,ztol代表工具的位置允差,Rxtol,Rytol,Rztol代表工具的方向允差。

(2)根据约束条件进行离散采样

在获得有冗余自由度的笛卡尔空间半约束路径后。根据每个路径点的约束条件，在允差范围内进行等间隔均匀离散采样。每个有一定允差的半约束路径点，采样结果对应一组六维笛卡尔空间点(位置，方向确定)。如图2上半部分所示。

图2 基于采样的路径规划算法示意图

(3)对采样结果进行逆运动学求解

根据所有笛卡尔空间路径点的采样结果对六轴工业机器人进行逆运动学求解，并对所获得逆运动学解进行约束检测，去除与障碍物发生碰撞或超出机器人关节运动范围的解。每个逆运动学解对应一个机器人关节空间六维向量，向量值代表六轴机器人每个对应关节的角度。

(4)在关节空间搜索最优路径

获得每个笛卡尔空间路径点对应的所有合法关节空间路径点后，在关节空间构建路径搜索图G=(V,E)，如图2下半部分所示。其中V为关节空间路径点集，对应逆运动学求解得到的机器人位形；边集E中的元素则表示路径点之间的连线，对应于机器人位形之间满足动态约束(速度，加速度约束)的局部可行路径。生成搜索图的算法见表1。

表1 算法1

续表

为在关节空间获得一条连续平滑的最优路径，需要机器人在运动过程中关节位移量最小且尽量避免机器人进入奇异位形。故将搜索图中各边的长度设置为两个机器人位形之间每个关节角度变化的绝对值之和，以代表机器人从一关节点运动到另一关节点时动作幅度的大小。假设posem,posen∈V,距离函数可以表示为：

(3)

表2 算法2

续表

上述路径搜索算法时间复杂度为O(V+E)，优于常见的Dijkstra，O(ElogV)和Bellman-Ford，O(VE)最短路径搜索算法，在搜索图较大时可以获得更好的表现。

1.2 算法评估

1.2.1 完备性

为充分利用工业机器人在执行半约束路径任务时的冗余自由度，提出基于采样的路径规划。这种基于采样的路径规划是概率完备的，如果在指定任务约束下有路径解存在，只要采样密度足够大，就一定能确保找到一条路径解；采样密度及逆运动学求解的效率很大程度上决定了路径规划能否成功以及所花费的时间。目前对机器人进行逆运动学求解的方法可分为两类，数值法和解析法。数值法中比较常用的是牛顿-拉夫逊迭代法。数值方法的优点是通用性高，适用于所有串联机械臂。但是，求解速度较慢(约1 ms)[13]且不保证会收敛。同时数值解法不能应对由关节空间限制产生的非平滑搜索空间，而且每次求解只返回一个解决方案。为了求得各种可能的姿势对应的逆运动学解，必须使用不同的初始条件多次求解。而解析法是特定的代数方程，不需要迭代求解，速度较快(约6 μs)[13]，可以一次求得所有逆解，且保证每次求解的一致性。但是解析法通常通用性较差，需要根据机器人的结构来判断，是否有解析解。研究表明，如果满足以下两个条件中的任何一个，那么串联机械手逆运动学就可以以解析法求解[14]。

① 3个相邻的关节轴在一个点上相交；

② 3个相邻的关节轴是平行的(在技术上这是一个特殊情况，因为平行线在无穷远处相交)。

目前在工业中使用的六自由度串联机器人中的大多数都满足上述条件之一。例如，本文仿真及实验部分所使用的ABB IRB1200型号工业机器人就满足条件①，对应的几何模型及示意图如图3所示。

(a) 几何模型 (b) 构型示意图图3 ABB IRB1200型号工业机器人

可以看出，ABB IRB1200机器人的最后3个关节轴交于一点。这意味着机器人末端执行器在空间中的位置与方向分量可以解耦求解，由前3个关节轴确定末端执行器的位置，后3个关节轴确定末端执行器的方向，使得要求解的最复杂的多项式是二次多项式，大大简化了逆运动学求解难度。本文实验部分比较了不同的采样密度及不同逆运动学求解方法对基于采样的路径规划结果及成功率的影响。

1.2.2 最优性

在关节空间通过使用基于动态规划的最短路径搜索算法，能保证在搜索图中有解的情况下得到对应采样密度下满足机器人任务约束条件的一条关节位移量最小的最优路径。并且可以通过对路径点之间的距离函数进行改写，如增加与目标位姿的距离或工作过程中与位姿有关的损耗，使得最优性评价更加满足实际应用场景。

2 实验

本文基于机器人操作系统(Robot Operating System)[15]，利用其提供的开发环境和rqt-plot(数据可视化)，RViz(三维可视化)等软件工具，完成了工业机器人控制软件的原型开发，以ABB IRB1200型机器人为控制对象。对半约束路径规划算法的可行性进行了评估和验证。仿真及实验平台系统框图如图4所示。

图4 仿真及实验平台系统框图

2.1 任务约束

为了测试半约束路径规划算法在实际工业机器人应用中的可行性，以及各种采样参数与逆运动学求解方法对路径规划效果的影响。本文设计了两种典型六轴工业机器人任务约束场景进行仿真及实验。

(1)约束Ⅰ：空间圆路径(喷涂空间圈)

一些工业机器人典型应用场景的任务约束是需要机器人在末端执行器指向保持固定的条件下经过连续的路径。例如，在喷涂机器人进行喷漆作业时，需要保持末端执行器方向与作业面垂直。任务约束Ⅰ假设喷涂机器人在进行空间圆轨迹的喷涂作业，作业过程中保持末端执行器方向与水平面垂直，即末端执行器的俯仰角与横滚角均为0(相对于绝对坐标系)。在三维空间中运动路径确定，俯仰、偏航自由度也被限制的情况下，工作路径的自由度为1, 机器人可以绕工具坐标系Zt轴进行任意角度的旋转，如图5所示。

(a) 起始位姿 (b) 运动过程中位姿

(2)约束Ⅱ：空间直线路径(切割H型钢)

在一些工业机器人应用场景中，机器人末端执行器的方向与被加工工件表面之间的夹角可以在允许范围内变化。例如在对钢材进行激光/等离子切割时，机器人在切割路径上可以绕工具Xt轴[11](与末端执行器运动平面垂直的轴)，进行一定角度的旋转。任务约束Ⅱ假设切割机器人在进行多段空间直线轨迹组成的H型钢切割作业，如图6所示。

(a) 起始位姿 (b) 运动过程中位姿

2.2 实验环境设置

为验证基于采样的半约束路径规划算法的性能并与传统的路径规划算法做比较，针对上述每种任务约束，以规划成功率、计算时间、路径花费为指标，在不同笛卡尔空间路径点数量，不同采样密度，不同逆运动学求解方式的条件下进行仿真测试，所使用的计算机配置为Intel i5-7300HQ CPU(2.5 GHz)和6 GB RAM。

实验测试环境，如图7所示。利用硬件通讯接口通过套接字实现上位机与ABB IRC 5机器人控制器相互通讯，并将路径规划得到的机器人关节路径传输到运动控制器，由运动控制器驱动ABB IRB1200机器人执行相应任务。

(a) 任务约束Ⅰ对应位姿 (b) 任务约束Ⅱ对应位姿

2.3 实验结果

2.3.1 仿真计算

仿真实验结果见下表，表3、表5、表7对应任务约束Ⅰ，表4、表6对应任务约束Ⅱ。每个表的第二列表示路径规划算法在关节空间搜索图的大小：“笛卡尔空间路径点的数量/在每个笛卡尔路径点的采样数量”，采样点数量为1代表不利用任务路径的冗余自由度进行采样的常规算法。每个表的第二行表示路径规划算法所采用的逆运动学求解方法。

表3 任务约束Ⅰ条件下，不同参数配置对轨迹规划成功率的影响

表4 任务约束Ⅱ条件下，不同参数配置对轨迹规划成功率的影响

从表3、表4可以看出，对于路径规划成功率这项性能指标，在使用数值法进行逆运动学求解的情况下，当采样数量较少时路径规划成功率较低。这是因为，数值法逆运动学求解是基于牛顿-拉普森迭代法实现的，每次迭代只能得到一个解，在关节限制影响下有可能找不到解。在采样数量较少时可能发生逆运动学求解失败，无法获得对应的关节配置，导致路径规划失败。而通过解析法逆运动学求解，在每次求解时可以求得笛卡尔空间点对应的所有逆运动学解，而且保证每次求解的一致性。故在笛卡尔空间路径点未超过机器人运动范围的情况下使用解析法一定能得到对应的逆运动学解，保证路径规划的成功。表3、表4后两列仿真数据体现了两种逆运动学求解方法的求解效率差距，与解析法相比，数值法的求解效率较低。进行仿真实验时，针对每个笛卡尔空间采样点数值法逆运动学求解将会执行10次迭代求解，花费更多时间但是得到的有效解更少。

表5 任务约束Ⅰ条件下，不同参数配置对规划时间及规划效果的影响

表6 任务约束Ⅱ条件下，不同参数配置对规划时间及规划效果的影响

从表5、表6可以看出，对于逆运动学求解时间这项性能指标，使用数值法与解析法得到的结果之间也存在显著差异(约两个数量级)。因为基于牛顿-拉普森迭代法的数值法在迭代求解时收敛速度较慢，同时为提高求解成功率需增加迭代求解次数，从而导致求解花费时间进一步增加。而解析法逆运动学求解，使用封闭形式解，要求解的最复杂的多项式是二次多项式，求解速度明显快于数值法。随着在每个笛卡尔路径点的采样数增加，使用两种方法进行逆运动学求解所花费时间均线性增加。而在关节空间进行路径规划所花费的时间则会发生平方级的增长，因为采样点数量增加将导致进行路径规划的关节空间搜索图中顶点的数量与边的数量同时增长。关节位移量这项性能指标代表了机器人运动轨迹的光滑程度，关节位移量越小代表机器人运动过程中抖动越小轨迹越平滑。可以看到随着采样数量的增加关节位移量之和逐渐减小。

表7 任务约束Ⅰ条件下，不同笛卡尔路径点数量 对规划时间及规划效果的影响

从表7可以看出，随着笛卡尔路径点数量的增加，逆运动学求解时间与关节空间路径搜索时间均线性增加，这是由于所需的计算量线性增加导致。而且对于不同数量笛卡尔路径点的路径，由于它们都代表相同的任务约束，在采样数量足够多时关节位移量趋于一致，表示规划得到了对应参数下满足任务约束的最优路径。

2.3.2 实验验证

基于ROS控制ABB IRB1200型号工业机器人进行对算法进行实验验证，并利用ROS提供的rqt_plot工具记录机器人控制器实时上传的各个关节变化数据。在任务约束Ⅰ条件下几组不同参数配置的实验结果如图8所示。

(a) 采样数5,数值法求解

(b) 采样数5,解析法求解

(c) 采样数30,数值法求解

(d) 采样数30,解析法求解

(e) 采样数180,数值法求解

(f) 采样数180,解析法求解

可以看到在使用数值法逆运动学求解，而且采样数量较少时，规划得到的路径不连续且抖动非常大，完全无法在实际生产中使用。而采样点数量足够多，或者使用解析法进行逆运动学求解时可以得到连续光滑的最优路径。在其它任务约束及参数配置条件下得到的实验结果相似。实验所得数据与仿真分析结果基本一致，证明了算法的可行性。

2.4 实验结果分析

由于本文介绍的基于采样的路径规划算法依赖于连续空间的离散化，因此算法是概率完备的，在使用数值法逆运动学求解且采样点密度低时有一定机率失败。通过解析法进行逆运动学求解或增加采样密度，便可将规划失败概率降到极低。在机器人执行冗余自由度较高的任务约束，或者采样密度较大时，基于数值法进行轨迹规划所花费时间较长，需要根据具体应用场景在规划时间与规划效果之间做一定权衡。而如算法完备性分析部分所述，根据六轴工业机器人的结构特点，使用基于解析法的半约束路径规划，求解速度快，一致性高，适合在工业场景中应用。

3 结论

本文针对六轴工业机器人应用场景中的半约束路径规划问题，利用机器人的冗余自由度，提出基于采样的规划算法进行路径规划。结果表明，通过选择合理的采样点数及逆运动学求解方式，算法可以在较短的规划时间内得到满足任务约束的连续光滑且不与空间障碍物发生碰撞的最优路径。实验证明所提出的方法是实用、有效的。