基于视觉的平面关节机器人避障路径规划

，

(哈尔滨工业大学(深圳) 机电工程及自动化学院，广东 深圳 518000)

0 引言

工业机器人在工业生产中的广泛应用极大地提高了生产效率。在2015年，我国提出“中国制造2025”更是将机器人作为十大重点发展领域之一。机械臂作为目前工业上应用最为广泛的工业机器人，在汽车制造、焊接和码垛等领域大范围普及。在焊接、码垛等应用场景中，机械臂的工作空间中一般没有障碍物存在，通过示教方式就可以实现点到点的运动。随着智能制造概念的普及，工业机器人被用在越来越多的场景中，机器人的工作环境越来越复杂。在锂电池制造行业，有一道工艺是将薄片电池叠在一起，通常这一工艺是由叠片机完成，平面关节机器人(SCARA)可以在叠片机内完成叠放薄片电池的步骤。然而叠片机内部还有其他机构，且空间紧凑，因此要想顺利将平面关节机器人应用到叠片机内，还需要具备避障路径规划的功能。

避障路径规划的目的是在给定起始点和目标点以及障碍物信息的情况下，规划出一条无碰撞到达目标点位的路径。关于避障路径规划，目前已经提出了许多方法，主要分为传统算法和智能算法2类[1]。传统算法包括C空间法、人工势场法和A*算法等，智能算法包括遗传算法和粒子群算法等[2-6]。

平面关节机器人由于其在运动过程中执行机构一般与XOY平面保持垂直，因此空间路径规划问题可以简化为平面路径规划问题。除此之外，平面关节机器人的工作空间范围有限，地图信息构建较为容易。针对平面关节机器人的应用场景，结合机器视觉技术，从相机中获取机器人工作平面的信息并构建地图；利用图像分割技术实现工作平面上的障碍物轮廓提取，做为路径规划问题的约束条件；同时利用改进的A*避障路径算法，实现可控安全距离的避障路径搜索；最后在样机上完成了算法的验证。

1 样机模型

实验采用的平面关节机器人为吊装型SCARA，如图1所示。

图1 实验用吊装SCARA

样机的D-H参数如表1所示。

表1 样机的D-H参数

2 机器人工作空间地图信息的建立

实现机器人避障的前提是获取工作空间的地图信息，构建地图信息的方式是将地图栅格化，栅格地图中的栅格分为自由栅格和障碍物栅格，避障路径规划的目的就是寻找到一条由自由栅格连接而成的从起始点位到目标点位的路径。一般构建栅格地图的方法是对环境进行数学建模，然而障碍物的轮廓往往不是规则的矩形，为了简化模型，需要将障碍物的轮廓用矩形组合表示。障碍物轮廓的矩形组合表示存在将自由栅格标记为障碍物栅格的情况，为了更精确地构建地图信息，利用相机获取机器人工作空间的环境信息，再通过图像处理技术，提取障碍物的位置信息，进而实现地图信息的建立。

2.1 视觉系统构建

根据相机的安装位置，可以将视觉系统分为ETH(eye to hand)形式和EIH(eye in hand)形式。ETH形式的视觉系统是将相机安装在固定的支架上，在SCARA运动的过程中，相机的位置不发生变化，这种安装方式的相机坐标系到世界坐标系的映射矩阵是固定的；EIH形式的视觉系统是将相机安装在SCARA上，因此在SCARA运动过程中，相机的位姿也相对SCARA的基座不断变化，这种安装方式需要动态更新相机坐标系到世界坐标系的映射矩阵。

为方便相机安装及提高路径规划的速度，采用ETH形式安装相机，将相机安装在机架上，图2所示为硬件系统的组成。

图2 硬件系统示意

2.2 手眼标定[7

手眼标定的目的是确定世界坐标系和像素坐标系的转换矩阵，是将障碍物位置信息从像素坐标系转换到世界坐标系的前提。手眼标定涉及到的坐标系主要有3个，分别是世界坐标系、相机坐标系和像素坐标系。其中世界坐标系通常是以基座为原点，SCARA工作平面为XOY平面构建的坐标系；相机坐标系是以相机光轴为Z方向，以光心为原点构建的坐标系；像素坐标系是光轴与成像平面交点为圆心，成像平面为XOY平面，以像素为单位构成的二维坐标系。相机坐标系和像素坐标系的关系如图3所示。

图3 相机坐标系与像素坐标系

世界坐标系到相机坐标系的变换为刚体变换，可通过旋转和平移表示[8-9]。

(1)

其中，R为旋转矩阵;T为平移向量。旋转和平移共同构成外参矩阵。

相机坐标系到像素坐标系的转换主要是缩放变换，根据相机的针孔模型可得到相机坐标系到像素坐标系的转换关系。

(2)

Mc为相机的内参矩阵。

结合式(1)和式(2)，可以得到世界坐标到像素坐标的转换关系。

(3)

将内参矩阵和外参矩阵融合为一个3×4的矩阵后，将式(3)展开得到

(4)

矩阵包含12个未知参数，求解需要至少12个方程，但是注意到由于SCARA的工作平面一般是固定的，在世界坐标系建立的过程中，可以将工作平面的Z坐标设为零；除此之外，在相机安装过程中，保证相机光轴与工作平面尽量垂直，则工作平面上的点在相机坐标系下的Z坐标也为一常数。根据这2个假设条件，可以将式(4)简化，得到世界坐标系下XOY平面的点到像素坐标系的映射关系。

(5)

矩阵的未知参数有9个，需要至少9个方程来求解。M为世界坐标系中XOY平面上的点到像素坐标系的映射矩阵，M-1则为像素坐标系到世界坐标系XOY平面上点的映射矩阵。标定工具选择棋盘格，通过OpenCV获取相机图片中的棋盘格角点坐标和棋盘格角点在世界坐标系下的坐标值，可以得到一组矩阵方程，最终求解映射矩阵。

2.3 障碍物轮廓提取

手眼标定实现了像素坐标系和世界坐标系之间的相互转换，相机能够获取SCARA工作平面的环境信息，图片中的障碍物是由一系列像素点组成的，可以通过手眼标定矩阵将图像中的障碍物像素点位转换为世界坐标系下的坐标。

障碍物的像素点位在不考虑障碍物重叠的情况下，是由互相独立的区域组成的，对于避障路径规划任务而言，障碍物的轮廓本身已经属于障碍物栅格的点位，轮廓为封闭曲线，因此轮廓内部的点位也不可能到达。根据这一特点，只需要将障碍物的轮廓从背景中提取出来，就能得到完整的工作平面环境信息。

障碍物轮廓提取最常用的方式就是对图像进行边缘检测[10-11]，但直接进行边缘检测的方式要求图片的质量较高，在常规的应用中，图片的质量往往达不到能直接提取边缘的要求。为了能够适应更多的应用场景，决定先对图像进行分割，预先将物体和轮廓区分开，增强物体边缘，再提取物体轮廓。根据这一处理步骤，可以从图4a所示工作平面原图得到图4b所示轮廓。

图4 图片提取障碍物轮廓

物体的轮廓被较好地从背景中提取出来，轮廓的像素点位即障碍物点位，在路径规划过程中，这些点位不可到达。避障路径规划可以将像素点位的环境信息通过手眼映射矩阵转化为工作平面上的世界坐标信息，进而在世界坐标系下进行规划；或是直接在像素坐标系下进行规划，再映射到世界坐标系中。本文采用在像素坐标系下进行规划的方案。

3 避障路径规划

3.1 改进A*算法的避障路径规划

A*算法属于启发式搜索算法，实现启发式搜索路径的关键是构造每个节点的估价函数。

f(n)=g(n)+h(n)

(6)

f(n)为节点n的估价函数；g(n)为初始节点到节点n的实际代价；h(n)是节点n到目标节点的最佳路径代价。A*算法搜索路径的过程中采用贪心策略，即每次都选择估价函数最小的节点作为下一节点。在路径搜索前，障碍物节点的估价函数被初始化为无穷大，因此障碍物节点不会被选择。估价函数的构造形式直接影响A*算法的搜索效率，实际代价的构造与路径距离的计算方式有关。在栅格化地图中，路径距离的计算方式可以分为Manhattan距离和Euler距离，Manhattan距离是移动起点和移动终点的X和Y方向坐标差值的绝对值之和，即

gM=|x2-x1|+|y2-y1|

(7)

Euler距离是移动起点和移动终点的连线距离，即

(8)

(x1,y1)为移动的起点；(x2,y2)为移动的终点。

最佳路径代价可表示为目标节点到当前节点的Manhattan距离，即

h(n)=|xd-xn|+|yd-yn|

(9)

根据A*算法搜索到的路径是由一系列的节点连接而成的折线，在图片中表现为一系列的像素点连线，图5是以图4a为基本环境信息得到的避障路径。

一系列的路径节点能准确避开障碍物前进，但是对于SCARA的轨迹规划而言，过多的路径节点会极大提升速度规划的难度，因此需要对A*算法进行改进。注意到规划出的路径节点中有相当一部分节点属于冗余节点，冗余节点是指剔除后对避障功能无实际影响的节点。判断冗余节点的方法是通过判断某一节点前后节点的连线是否会与障碍物干涉来实现的，若节点前后节点的连线不与障碍物干涉，则说明节点剔除后不影响路径的避障功能，为冗余节点；反之，说明为非冗余节点[12]。改进后的A*避障路径如图6所示。从图6中可以看到，原先一系列的节点最终被优化为4个关键节点，4个关键节点连接而成的折线依然能够满足避障的要求。

图5 A*算法的避障路径

图6 改进A*算法的避障路径

3.2 可控安全距离的避障路径规划

根据以上改进A*算法路径规划得到的是一系列节点，虽然能够实现避障功能，但是节点到障碍物的距离是不可控的。由于算法中估价函数只包含实际代价函数和最佳路径函数，采用贪心策略选取得到的节点一般是贴着障碍物轮廓的节点，在实际的应用中，SCARA的末端执行器的尺寸还需要考虑进去，因此需要控制路径上节点到障碍物的安全距离。

在构建工作平面地图信息时，直接将障碍物轮廓作为障碍物节点的情况下，若避障路径需要绕开障碍物，则节点将沿着障碍物轮廓分布。根据A*算法的这一特点，要实现安全距离的可控性，可以将障碍物轮廓沿曲线法向向外部扩展，则扩展的像素距离就能映射成为工作平面上的安全距离。图7所示为原始轮廓与扩展轮廓，从图7可以看出，扩展轮廓的形状与原始轮廓一致，能保证节点到障碍物的最近距离为扩展的安全距离。

图7 原始轮廓与扩展轮廓

将扩展后的轮廓点位作为障碍物点位进行避障路径规划，最终得到的路径如图8所示。

图8 安全距离为20个像素距离的避障路径

避障路径规划得到的一系列点位为路径节点，如果直接将节点相连得到的将是一条折线，对于速度规划而言难度较大，因此对折线进行平滑处理，转折处采用圆角过渡，最终得到的避障路径如图9所示。

图9 采用圆角过渡的避障路径

4 避障算法对比

为对比不同算法在避障路径规划上的性能，随机选取10对起点和终点进行避障路径规划。在都能实现避障功能的前提下，将避障路径的长度作为避障算法性能比较的主要依据，得到的结果如图10所示。

图10 不同避障算法对比

通过路径长度对比可以发现，A*算法规划的路径长度最长，改进A*算法可以减少路径中的冗余点，进而缩短路径长度。圆角过渡对路径长度的影响几乎可以忽略。10次随机路径规划中，改进A*算法平均缩短3.47%的长度，圆角过渡后平均缩短3.55%的长度。

5 结束语

针对平面关节机器人在工作平面作业时需要进行避障路径规划的情况，利用视觉信息构建工作平面的环境信息，并采用图像处理技术，将安全距离映射为安全像素距离，扩展障碍物轮廓，最后利用改进的A*算法实现了避障路径规划。

利用视觉信息，将工作平面上的障碍物表示为图片中的轮廓像素点，完成了栅格地图的创建；利用图像处理技术，首先将安全距离映射为安全像素距离，再将障碍物轮廓向外扩展安全像素距离个单位，保证在路径规划过程中节点到障碍物的最小距离不大于安全像素距离；改进的A*算法剔除了冗余节点，降低了机器人速度规划的复杂度。