基于激光传感器的工业机器人动态抓取系统设计*

杨 永，杨进兴，黄伟龙，李 俊

（1.中北大学 电气与控制工程学院，山西 太原 030051；2.中国科学院海西研究院泉州装备制造研究中心，福建 泉州 362216）

0 引 言

分拣作业是工业机器人的应用之一［1］。在物体的定位方面，基于图像的物体定位方式［2］，易受到环境光线、相机拍摄角度等因素的干扰。基于激光雷达物体定位方式［3］，由于其精度高、响应快等优点得到了广泛应用。

点云聚类是将大量的点云数据划分成若干子类，从而实现物体点云数据的分割。基于层次划分的聚类算法如K-Means［4，5］、K-Medoids［6］等通过迭代的方式将类中心不断地向数据中心移动；基于密度的算法如DBSCAN［7］、OPTICS［8］等通过定义一个距离半径，将所有可到达的点聚为一类。Campello R 等人使用HDBSCAN［9］通过稀疏度变换使噪声点远离核心点，引入了层次聚类的思想，实现不同密度点的聚类。

上述聚类算法，需要获取整体的点云数据，再进行数据处理。在实际的工程应用过程中，由于空间范围的限制，需要在扫描完整箱体之后及时地获取结束信号，快速实时地对数据进行处理，否则会造成完整箱体被分割，如果加大视觉区域，虽然会避免该现象，但会压缩后续抓取的空间。

本文设计了一种基于激光传感器的机器人动态抓取系统。在箱体的定位方面，利用LMS511激光传感器作为视觉设备，采用帧内和帧间密度聚类算法和最小外接矩形实现箱体实时定位与尺寸测量。在箱体的抓取方面，利用KUKA工业机器人作为抓取设备，采用二分法实现抓取位置与时间的近似求解。经实验结果证明，该系统能够实现大、重箱体的实时定位与动态抓取。

1 系统总体设计

系统上位机控制器为工控机，用于处理点云数据、求解近似抓取位置和提供整套系统的人机交互界面，使用C#设计开发程序。下位机控制器用于实现工控机与机器人控制器之间的数据传输，抓手开合和吸盘吸气放气的指令控制，系统状态的实时监控。工业机器人控制器实现根据抓取位置自动生成机械臂的运动轨迹及机械臂的运动控制。

工控机与激光传感器通过TCP／IP通信，工控机与PLC通过Modbus／TCP通信，PLC 与机器人控制器通过Profinet总线连接。基于单线激光传感器的机器人动态抓取系统架构如图1所示。

图1 系统架构

2 点云数据建模

2.1 点云数据获取

点云数据获取示意如图2所示。单线激光雷达固定于传送带上方，并对传送带区域进行扫描。单点位置精度为±12 mm，扫描频率为100 Hz。箱体放置于传送带上，匀速通过扫描区域。扫描数据为扫描位置点相对雷达的距离与角度。

图2 箱体点云数据获取示意

针对扫描过程每一帧数据，根据单线激光雷达与输送带之间的坐标转换关系，由式（1）计算箱体在传送带坐标系中三维点云数据，从而建立箱体三维点云模型

式中 （x，y，z）为点云数据的三维坐标；θ1，θ2为单线激光雷达到扫描点角度；d1，d2为单线激光雷达到扫描点的长度；v为传送带的运行速度；t为扫描时间。

2.2 点云数据实时分割

在箱体的三维扫描过程中，针对每一帧的数据，定义了3种点云数据类型，如图3 所示。类型A，在多个箱体同时到达时，位于箱体中间噪声点n 会造成不同的箱体数据连接在一起，从而使系统误判为一个箱体。类型B，在扫描过程中，单线激光雷达对扇形区域进行扫描，较高的箱体距离单线激光雷达较近，因而该部分点云数据较为密集。反之，点云数据较为稀疏。类型C，2 个高度相同的箱体相互接触，2个箱体的点云数据没有明显的区分界限。

图3 帧内和帧间密度聚类算法

为了实现点云数据的实时分割，针对每一帧的数据，采取帧内和帧间密度聚类算法。

1）帧内和帧间密度聚类算法

a.帧内聚类

为了防止位于2 个类中间的噪声点将2 个类连接，在进行聚类之前，利用稀疏度变换将噪声点远离核心点。设待聚类的数据集X ＝｛x1，x2，…，xn｝，对于其中每一个数据点计算计算其核心距离corek（xi）与互达距离dmreach-k（xi）。

样本与第k 个最近邻样本点的距离称为核心距离corek（xi），即

式中 Nk（x）为x的第k个最近邻点；d（x，N）为2点之间的距离。式（2）可以看出，核心距离corek（xi）取决于点的密度及最近邻点的个数k。

相邻2个样本点间的互达距离dmreach-k（xi）为

由式（3）可得到数据点的稀疏度变换，稀疏点与核心点之间的距离将会被扩大。

由于点云数据密度不固定，需要根据不同的点云密度自动的调整阈值

式中 T（i）为阈值，λ为阈值系数，该系数由激光传感器单点精度决定，精度越低，λ 值越大。在点云数据密集区域，点云数据的核心距离较小，从而使得T（i）较小；在点云数据稀疏区域，点云数据的核心距离较大，从而使得T（i）较大；在噪声点位置，T（i）达到一个极大值。

b.帧间聚类

帧间聚类主要根据上一帧数据的聚类情况对当前帧数据的分割，然后按聚类中心距离最小进行匹配。

2）算法测试

如图3所示，根据实际情况，选择6 帧点云数据进行测试。图3（a）中第1～6 帧数据类型分别为：一类数据、类间含有噪声点的数据、两类密度一样且相连的数据、两类密度不一样且相连的数据、两类不相连的数据、一类数据。图3（b）为聚类结果。图3（c）表明阈值随点云密度而变化。图中，横坐标为点云数据序列，纵坐标为与上一点的距离（cm），第2、4、5 帧数据点云数据密度发生变化，阈值也随之变化，在不同密度交界处，阈值小于可达距离，从而将不同密度的点云数据分割。

2.3 最小外接矩形提取

采用旋转法以实现箱体最小外接矩形的快速提取。首先，将点云数据以固定的角度由0°逐步旋转至90°，分别提取其x方向和y方向最值，组合为矩形的4 个顶点，选择面积最小的矩形为最小外接矩形；然后，根据旋转角度和顶点位置计算箱体的位姿和尺寸；最后，根据坐标标定算法［10～12］将计算得到的位姿转换为机械人坐标系中的位姿。

3 动态抓取

3.1 路径规划

动态抓取运动规划目的是机器人在满足避障要求的前提下，能够精准地抓取箱体并平稳地将箱体运送至指定位置。将箱体从传送带上抓取并放置到装载车中是典型的点到点（point-to-point，PTP）过程，关键点如图4 所示。Home点为抓取起始点，P1—P2 为抓取逼近段，P2—P3 为抓取跟随段，P9为码垛点。为防止拐角处轨迹突变引起的机器人振动，同时缩短运动时间，采用样条曲线过渡的方式［13］，使得关节空间输出速度、加速度和转矩曲线更加平滑，有效减小对机器人冲击，提高机器手动态性能［14］。

图4 机器人末端运动路径规划示意

3.2 抓取位置求解

拦截式抓取是根据目标箱体当前位置、速度和加速度来计算出其将来任意时刻的位置，然后规划机器人末端的动作，使其能与目标物体相遇，虽然行李箱的运动轨迹为匀速直线运动，但是机械臂末端的运动轨迹为空间曲线非匀速运动，为了实现快速的求解抓取位置，采用二分法求近似解。在抓取范围为1 000 mm的条件下，迭代6次之后，最大误差范围小于20 mm。

箱体的线性匀速运动方程为

机器人末端的运动为非线性非匀速运动方程为

根据式（5）和式（7），利用二分法求解Ctg＝Btg即可得出抓取位置和抓取时间。

4 实验验证

为了测试系统有效性，搭建了实验平台，如图5所示。

图5 系统布局

图6（a）和图6（b）为2 种随机摆放的箱体点云数据处理结果，在扫描完一个完整的箱体后，自动结束扫描。图6（c）显示点云数据处理中几种典型点云数据帧的阈值设定。获取点云数据之后，提取箱体的位姿，计算抓取位置，最后控制工业机器人对箱体进行抓取。在20 次抓取实验中，尺寸识别误差为±14 mm，角度偏差为±5°，拣选位置平均误差为±16 mm，拣选效率为20 s／件。

图6 箱体点云数据处理测试

5 结 论

在实际工程应用中，由于设备空间结构的限制，需要快速提取箱体的尺寸和计算抓取位置。在箱体定位过程中，利用帧内和帧间密度聚类算法，根据箱体的密度自适应选择阈值，实现对点云数据的逐帧聚类与分割，解决了单一阈值无法满足不同点云密度的问题。在抓取过程中，采用PTP的路径规划满足避障要求，在拐角处采用样条曲线过渡满足运动过程中的平稳性要求，采用二分法求抓取位置与抓取时间的近似解。最后搭建了实验平台对算法的可行性和精确度进行了测试，实验结果表明，该系统能够实现箱体在随输送带运输过程中的实时点云数据分割和自动分拣。