一种面向工业应用的机器人回位方法

摘 要：针对工业机器人作业过程中非正常停机引起位姿处于未知状态而导致机器人无法从停机位置自动返回初始位置的问题，提出一种工业机器人的回位方法。设计机器人回位路径，结合k-近邻算法、构造回位空间以及空间分类阈值对机器人的位姿信息进行分类化处理；重构机器人特征数据进行决策树模型的训练；使用训练完成的模型决策回位路径后执行机器人回位程序，实现工业机器人自动返回初始位。回位试验实验结果表明：在随机位姿和携带多样工具的状态下机器人回位成功率为93.3%。

关键词：工业机器人；回位方法；空间构造；路径设计；决策树算法；k-近邻算法

中图分类号：TP242; TP391 文献标志码：B 文章编号：1671-5276（2024）04-0235-06

A Robot Homing Method for Industrial Applications

HOU Longxiao

（Henan Machinery Design amp; Research Institute Co.， Ltd.， Zhengzhou 450003，China）

Abstract：For the abnormal halt of industrial robots in the working process resulting in their postures in an unknown state and the failure of automatic return to the initial position from the halt position， a homing method for industrial robot is proposed， which includes designing a robot return path， classifying the robot's pose information by combining the k-nearest neighbour algorithm and constructing return space and spatial classification threshold， reconstructing the robot feature data for decision tree model training andexcuting the robot regression program after using the trained model decision return path.Thus theautomatic return of industrial robots to the initial position being realized. The experimental results show that the success rate of robot return is 93.3% in random posture and with various tools.

Keywords：industrial robot；homing method；spatial structure；path design；decision tree algorithm；k-nearest neighbour algorithm

0 引言

随着工业机器人的应用范围越来越广泛，工业机器人技术的发展已成为衡量一个国家科技和制造业水平的重要标志^[1]。为降低人工劳动强度，提高生产效率，降低生产成本，企业广泛引进工业机器人代替人工进行生产作业^[2]。然而企业共同面临的问题是生产人员未经过系统的操作培训，难以熟练地使用工业机器人，一旦机器人因为故障或不当操作等原因停机于某个随机位置，操作人员难以使机器人回到至初始位置重新投入运行。

工业机器人的回位问题属于避障路径规划的范畴。现阶段，面对较为复杂的工作场景时，常采用搜索算法，如A^*算法^[3-4]、人工势场法^[5-6]、遗传算法等^[7-8]，这些方法在多关节工业机器人的应用中存在算法复杂度较高、实现较复杂、搜索缓慢等问题；另外还有利用传感器获取障碍物信息，通过网格表示机器人所处的环境，根据蚁群算法、粒子群算法等^[9]引导机器人进行避障路径规划。这些方法通过建立目标吸引函数，利用自适应学习机制，提高最优路径的选择概率，其实现相对容易，但往往需要较高的硬件成本。

1 回位问题描述

1.1 机器人位姿及TCP概述

基坐标系是为确定空间中某一点的位置和姿态而在空间上进行定义的笛卡儿坐标系，表示为{A}。基坐标系中的任一点p的位置信息可由一个矢量^AP在基坐标系下的坐标分量进行描述：

式中：p_x、p_y、p_z是点p在基坐标系中各轴向的坐标分量；^AP上标表示基坐标系{A}。

除了位置信息外，还需要描述点p在空间的姿态，建立一个以p点为原点的坐标系{B}，并给出{B}坐标系相对于基坐标系{A}的方向余弦矩阵来表示点p相对于基坐标系的姿态信息^[10]：

式中：^A_BR表示p点的坐标{B}相对于基坐标{A}的旋转矩阵；cos（·，·）表示坐标{B}在各轴矢量上与基坐标{A}各轴矢量间夹角的余弦。

综合位置信息和姿态信息，空间中的一点p的位姿描述为{P_B，^A_BR}，P_B为p点位置信息相对于基坐标系{A}的位置矢量^[11-12]。

为了描述工具在坐标系中的位置和姿态，需要建立一个工具坐标系TCS（tool coordinate system），工具坐标系的原点即工具中心点TCP（tool center point） ^[13]。工业机器人的TCP位姿信息{P_B，^A_BR}一般情况下转换为一个一维数组表示：[x，y，z，r_x，r_y，r_z]，其中数组的前3项[x，y，z]表示TCP在基坐标系中各轴向的坐标分量；后3项[r_x，r_y，r_z]表示各轴矢量上与基坐标各轴矢量间的夹角角度值。

1.2 回位问题分析

在工业应用中，TCP存在一个初始位姿，机器人运行中因故停机使TCP停止于另一个位姿，结束后续的作业步骤，从当前位姿移动至初始位姿的这一过程即是机器人回位过程。在实际情况中，由于作业环境狭小且不规则；两个位姿路径之间存在障碍物阻隔；TCP因携带多种类工具，需要以不同方式移动至初始位姿；机器人本身多连杆机构的机械约束等原因导致TCP往往不能够直接由停止位姿移动至初始位姿。因此，回位问题需要解决在机器人多连杆机构的约束下，不产生碰撞地将可能携带多种类工具的TCP从一个起始位姿移动至初始位姿。

2 方法概述

本文回位方法的原理是将随机的位姿及状态通过模型分类判定为若干个状态分类，通过各分类状态对应的回位路径以尽可能小幅度的位移将机器人移动至该状态分类下的示教点，再以此示教点移动至初始位置。

本文方法的总体流程为：首先构造用于判定TCP所处位置及状态的空间；然后根据不同的空间类型设计回位路径；之后通过收集机器人处于各空间的位姿数据训练区域判定模型，匹配与之对应的回位路径；最后将作业中机器人位姿信息导入训练完成的模型，判定TCP所处的空间及位姿状态，根据模型输出的回位路径退回至确定的示教点，最终返回初始位置，流程如图1所示。

3 回位路径设计

3.1 构造位置判定空间

解决工业机器人回位问题的关键是正确判定机器人的起始位姿，其中TCP的位置数据在回位决策中占有最大的权重。本节提出3种空间的划分方法用于TCP位置的判定。

1）以示教点为中心构造正方体空间

工业机器人最常用的作业模式是依赖于示教点按步骤完成不同的工序。在这种作业模式下，机器人回位时的起始位置可能正好处于示教点位置或只移动了极小的一段距离。构建以示教点为中心的小立方体空间用于判定TCP是否处于某个示教点邻近的位置。示教点^TP在基坐标系下的矢量如式（3）所示。

[^Tp_x， ^Tp_y， ^Tp_z]为该示教点在基坐标系各轴的分量，在p点各轴分量上各加减一个较小的数值n即可得到由8个分量值坐标组成的立方体空间C_t，如图2所示。

2）以作业区域中心构造立方体空间

工业机器人另一种较为常用的作业模式是基于一个作业区域的示教点^TP进行位置运算后得到^Wp₁、^Wp₂，…，^Wp_n点，TCP在^Wp₁、^Wp₂，…，^Wp_n点完成工作后返回p点结束一次作业。在这种作业模式下，机器人回位时的起始位置很大概率位于作业区域内的某个位置。假设由^WP点计算得到了n个作业点，则^WP_2/n点为作业区域的中心点。作业区域起始点^WP在基坐标系下的矢量描述为

作业区域中心点p_2/n基坐标系下的矢量描述为

式中：[^Wp_x， ^Wp_y， ^Wp_z]为点p在基坐标系各轴的分量；[^Wp′_x， ^Wp′_y， ^Wp′_z]为点^WP_2/n在基坐标系各轴的分量。点p_2/n各轴向分量到作业区域边缘的距离为[|^Wp_x-^Wp′_x|， | ^Wp_y-^Wp′_y|， | p_z-p′_z|]，以作业区域中心点的坐标分量值沿各轴向各加减一个到作业区域边缘的距离即可得8个分量值坐标组成的立方体空间C_w，如图3所示。

3）全区域构造象限空间

为了使得机器人无论处于任何起始位置都能回位初始位置，在两类立方体空间之外还需要对机器人所有可达区域进行象限划分。以基坐标系原点O为象限区域的分界点，点O在基坐标系下的矢量描述为

以分界点O的各轴向将整体空间划分为8个区域，各个区域的范围表示为Q_i，如图4所示。

3.2 回位路径设计

本文的目标是使工业机器人由停机位置移动至初始位置，因此需要依据TCP回位时的起始位姿设计合适的回位路径使机器人完成回位动作，并建立起机器人位置信息、姿态信息等特征和回位路径之间的对应关系，为回位模型的样本提供模板。

1）示教点立方体类回位路径设计

机器人回位的起始位置处于某个示教点为中心的小立方体内时，回位方式应先移动至对应此立方体的示教点位，如图5所示。

2）作业区域立方体类回位路径设计

机器人回位的起始位置处于某个作业区域立方体内时，考虑到机器人在同一作业区域可能会有多种姿态，所以回位方式应首先结合机器人的当前姿态来判断是否需要变换至过渡点相近的姿态之后再移动至此作业区域内的过渡点，然后移动至与作业立方体内过渡点邻近的示教点，如图6所示。

3）象限区域类回位路径设计

机器人回位的起始位置处于某个象限区域内时，说明机器人停机于作业区域或示教点之间的路径上。此状态下机器人可能存在多种工具和姿态，回位方式应首先结合机器人的当前工具种类选择目标过渡点；然后通过当前姿态来判断是否需要变换至过渡点相近的姿态之后再移动至此作业区域内的过渡点；最后移动至与象限区域过渡点邻近的示教点，如图7所示。

4 回位路径决策

4.1 准备数据样本

回位路径的判定基于决策树算法，对此模型进行训练，需要对机器人起始状态数据进行统计，作为训练样本。数据包括TCP的位置数据、姿态数据、工具种类编号和机器人回位路径编号，其中位置数据、姿态数据、工具种类编号作为样本的特征值，将在此状态下机器人的回位路径编号作为样本的目标值，如表1所示。

4.2 原始数据分类化

由于决策树算法对离散数值类型的特征值响应较差，所以需要使用k-近邻算法将离散的TCP位置数据和姿态数据转换为类别数据。由于机器人会以不同的位姿进行作业，所以对于同一个立方体空间除了空间编号，还需要对其所有的作业姿态进行编号。在作业区域立方体中记录数据时需要将机器人TCP以作业时的各个姿态移动至作业区域立方体空间中，以不同的作业姿态移动至不同的位置若干次，记录TCP的姿态数据和所处空间中姿态编号。

使用sklearn模块中的KNeighborsClassfier类进行k-近邻算法建模。将训练集和训练集对应的标签输入KNeighborsClassfier.fit（）函数进行模型训练，得出的结果即是将TCP的离散位置数据和姿态数据转换为空间编号和姿态编号的分类。以分类后的数据样本中TCP位置分类编号、TCP姿态分类编号、工具种类编号为特征值，回位路径编号为目标值，如表2所示。

4.3 决策树模型训练

1）决策树算法简介

决策树（decision tree，DT）算法是一种有监督的学习模型，它是一个由内节点和叶节点构成的树型结构^[14]。采用决策树中的C4.5算法，其样本特征的划分依据为信息增益比。设样本中的某个特征A，其特征熵为

式中：i表示特征A的n种取值；|D|表示划分前的样本总数；|D_i|表示划分的样本数目。

特征A的信息增益为

式中：H（D）表示样本的总熵值；H_A（D）表示特征A的特征熵。

信息增益比为

2）模型训练

使用sklearn模块中的DecisionTreeClassifier类进行决策树算法建模。决策树的模型存在3个超参数：限制树的最大深度（max_depth）；1个节点在分支后的每个子节点都必须至少包含的样本数（min_samples_leaf）；1个节点至少要包含的样本数（min_samples_split）^[15]。为了在回位路径决策中取得较好剪枝效果，将训练集进行10等分用于交叉验证，对预先设定max_depth=[3，5]，min_samples_leaf=[5，8，10]，min_samples_split=[1，2，3]进行的网格搜索，得出模型在该数据样本下的最优超参数。

将训练集和训练集对应的标签输入DecisionTreeClassifier.fit（）函数进行模型训练，此模型即为判定在拥有某种位置编号、姿态编号和工具种类的特征下所对应的回位路径编号。

5 试验

5.1 系统硬件构成

采用FANUC R-2000iC-210L型机器人进行回位试验和分析；Raspberry Pi-4B型嵌入式计算机通过k-近邻算法对输入的机器人离散位姿数据进行分类化，加载决策树模型对机器人回位路径进行决策；机器人与计算机通过Modbus-TCP协议进行数据交换。

5.2 试验流程及结果统计

在试验区域中，选取输送带抓取区域、左侧码放区域、右侧码放区域、1号工具台区域、2号工具台区域和各区域间部分为试验区域，如图8—图9所示。

将TCP随机停止于各区域内，采集机器人的位姿信息，即包含位置信息和姿态信息的一维数组；将采集的一维数组进行拆分，分离位置信息[x，y，z]和姿态信息[r_x，r_y，r_z]；将位置信息和姿态信息通过k-近邻算法进行分类处理；采集机器人当前携带的工具编号等特征信息；将位置分类编号、姿态分类编号和工具编号组装成待回位机器人的特征信息；将特征信息导入训练完成的决策树模型获取回位路径编号；按照路径编号执行分类设计的机器人回位程序；触发回位信号观察回位执行结果。

回位试验时将机器人运行速度设置为10%，以各区域内随机位置为回位起点试验30次。TCP处于输送带区域时，理论路径为起始点直线运行至初始位置，观察实际路径与理论路径全部符合；TCP处于左侧码放区域和右侧码放区域时，理论路径为起始点直线移动到各自码放区域内的过渡点后再移动至初始位置，观察实际路径中左侧码放区域有2次未经过渡点直接移动至初始位置，右侧码放区域有1次未经过渡点直接移动至初始位置；TCP处于1号工具台和2号工具台时，理论路径分为带有工具和没有工具两种情况，带工具状态时为直接移动至更换工具位置，无工具状态为先移动至更换工具位置后再移动至初始位置，观察实际路径1号工具台区域有1次带有工具状态回位至初始位置；TCP处于区域间部分时，理论路径是根据所处邻近区域及所带工具状态共同决定的，观察实际路径有2次不符合理论路径，有8次因位姿状态导致路径决策失败而无法成功回位。回位试验结果统计如表3所示。

5.3 结果分析

分析试验结果可知，本文方法在解决不同区域类型的回位问题上存在性能差异。输送带区域大多是示教点且姿态单一，通过本文方法可以快速准确地定位TCP所处的示教点立方体空间，进而通过事先设计的回位路径回位；在左、右码放区域多为以作业区域立方体回位的类型，姿态较为单一，但有不同的工具类型，通过本文方法可以较为准确地定位TCP所处的作业空间，进而移动至过渡点后回位至初始位置；工具台区域与输送带区域类似，多为示教点但有不同工具类型，本文方法针对此类型的回位具有良好的表现。各空间区域间部分的情况最为复杂，位姿多样且可能携带不同的工具类型，理论上处于此类空间中的TCP应该判定为象限空间分类进行回位，通过对决策失败的观察，失败原因多是由于TCP未决策为象限空间分类而是判定为邻近的作业区域空间分类，从而导致姿态不匹配而无法完成回位任务，还有少部分原因是因为姿态原因导致无法移动至过渡点而导致回位失败。

6 结语

本文针对生产应用中，工业机器人非正常停机后难以回位的问题提出了一种回位方法。本方法相较于传统的搜索算法较容易实施，同时依赖的硬件成本较低，便于应用于工业环境中。通过一例机器人回位试验验证，可以满足工业应用中机器人回位需求。本文提出的方法针对处于各划分空间领域之间的象限区域回位有待优化，在后续的研究中应设置各类空间的阈值以及增加象限空间，以处理TCP不同姿态的机制。

参考文献：

[1] 徐博凡. 基于视觉引导的机器人识别及柔性抓取系统研究[D]. 郑州：郑州大学，2018.

[2] 孙中国. 基于机器视觉的面粉袋码垛机器人研究[D]. 济南：山东建筑大学，2015.

[3] HART P E，NILSSON N J，RAPHAEL B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE Transactions on Systems Science and Cybernetics，1968，4（2）：100-107.

[4] 贾庆轩，陈钢，孙汉旭，等. 基于A^*算法的空间机械臂避障路径规划[J]. 机械工程学报，2010，46（13） ：109-115.

[5] 何兆楚，何元烈，曾碧. RRT与人工势场法结合的机械臂避障规划[J]. 工业工程，2017，20（2）：56-63.

[6] 王俊龙，张国良，羊帆，等. 改进人工势场法的机械臂避障路径规划[J]. 计算机工程与应用，2013，49（21）：266-270.

[7] 王雷，李明. 改进自适应遗传算法在移动机器人路径规划中的应用[J]. 南京理工大学学报（自然科学版），2017，41（5） ：627-633.

[8] FANG N，ZHOU J Z，ZHANG R，et al. A hybrid of real coded genetic algorithm and artificial fish swarm algorithm for short-term optimal hydrothermal scheduling[J].International Journal of Electrical Power amp;Energy Systems，2014，62： 617-629.

[9] 熊俊涛，李中行，陈淑绵，等.基于深度强化学习的虚拟机器人采摘路径避障规划[J].农业机械学报，2020，51（增刊2）：1-10.

[10] 贾亚洲.基于虚拟样机的特种机器人关键机构仿真研究[D].洛阳：河南科技大学，2019.

[11] 李虎.六自由度喷涂机器人结构方案设计与位姿误差分析[D].武汉：华中科技大学，2012.

[12] 黄奎.多关节式坐标测量系统的关键技术研究[D].武汉：华中科技大学，2010.

[13] 段海峰，江丽珍，刘建光，等. 微型电机外壳冲压送料机器人的编程及应用[J]. 现代制造工程，2020（7）：6-10.

[14] 孙向阳. 基于XGBoost算法的窃电用户行为研究[D].兰州：兰州交通大学，2018.

[15] 任强，桑世叶，刘长宁. 重楼属植物在线分类鉴定系统的设计与实现[J]. 计算机时代，2020（9）：72-75.

收稿日期：2023-02-20