基于激光雷达三维图像建模的机器人拆码垛控制系统

魏志强，李 兵，屈玉丰，沙 群

（1.北京航空航天大学机器人研究所，北京 100191；2.新奥特（北京）视频技术有限公司，北京 100185）

0 引言

目前，大型智能化仓储系统对机器人拆码垛系统的工作效率及自动化程度提出了越来越高的要求。激光雷达以其测距精度高、环境适应性强和实时性好等优点，已在机器人运动及工程领域得到广泛应用［1－4］。为此，利用激光雷达技术，设计了机器人拆码垛控制系统，详细介绍了机器人拆码垛系统总体功能、目标提取原理及控制实现方法。

1 系统的组成及功能

系统总体功能如图1所示。采用ABB IRB6640机器人，并配以专门的手爪夹具，机器人手臂上装有德国SICK公司LMS400激光雷达。其工作过程如下:将打包好的装有产品的周转箱以“品”字型布局方式运输到托盘上（托盘上每层3摞箱子，每摞10层或层数不定，箱子实物如图2所示），当机器人运动至系统扫描起始位置，启动机器人并启动放置于机器人手臂上的激光雷达，以一定速度在水平方向作匀速直线运动，从而实现对待拆周转箱的数据信息扫描；扫描完毕，由上位机对所保存的三维数据文件进行分析及处理，计算获得3个周转箱在机器人用户坐标中的中心位置及偏转角信息，随后，将数据信息以规定的协议通过以太网通讯方式传递给机器人下位机控制程序，从而进行后续的机器人定位及拆垛操作。

图1 基于激光雷达的机器人拆码垛系统

图2 “品”字形摆放的周转箱及内部物料实物

2 激光雷达三维成像原理

LMS400高速高精度基于相位差原理的测距激光雷达在系统中的主要参数为:扫描距离，0.7～3 m；角度分辨率，0.25°；扫描频率，370Hz；扫描角度及起始角，70°，55°；单次扫描数据数，280组。

激光扫描示意如图3所示。在XYZ坐标系中，Y轴方向为垂直纸面向外，即激光雷达扫描时的运动方向。激光雷达通过内部摆镜控制光束由起始55°角开始从左至右扫描目标，得出单个截面上的280个距离信息；同时，机器人以一定运行速度沿Y轴方向匀速移动激光雷达，从而，通过对多个扫描截面的积分，得出周转箱的三维数据。

图3 扫描示意

3 三维图像建模与显示

设Diskk为被测对象上任意点P到激光雷达发射原点的距离，由图3可知，对一次单线扫描来说，第k个点的Z及X向坐标值为:

为方便计算，采用图3中X′Y′Z′坐标系（Y′轴正方向为垂直纸面向里，图中未标出）。设此时的坐标原点为被测对象的最低点，并位于激光雷达发射点的最下方的托盘上，即此点测出的箱体高度值为最大，并将高度值进行如下变换:

Z为由原始数据计算得到的原始Z向坐标值矩阵；Zmax为Z向坐标矩阵元素中的最大值；Z′为坐标变换后的Z向坐标值矩阵。

利用从现场实际采集的三维数据，通过Matlab建立了周转箱三维图像，如图4所示。

图4 三维图像

4 噪声滤除及目标提取原理

由于数据范围较宽，同时也为方便后续图像处理，将图像从三维转向二维显示，用灰度值表示相应的高度，并通过线性变换增加图像的显示细节，线性变换公式为:

Z″为Z向坐标值矩阵；Z′ max 为Z′矩阵元素中的最大值；Z′ min 为Z′矩阵元素中的最小值。

图5a为目标高度灰度图像，可见噪声比较严重。为获取较好的去噪效果，采用参考文献［5］自适应中值滤波方法滤除噪声，去噪后的高度图像如图5b所示。由图5b去噪后的二维图像可见，相对于图像其他部分，箱体的边缘细节较为清晰，具有较强的对比度。因此，通过已知的先验知识搜索图像中的箱体边缘位置信息，进而获取箱体在图像中的准确位置参数，具体实现步骤如下:

a.利用OTSU大津法对去噪后的图像进行分割，获取分割后的二值图像，如图5c所示。

b.利用形态学膨胀算法对箱体边缘进行增强，增强后的图像如图5d所示。

c.利用种子填充算法获取图像各连通域，通过连通域的长宽比、对称性等先验知识获取箱体的边缘位置信息，如图5e中的4个小方框所示。

d.对箱体边缘信息进行分析，获取箱体在图像中的准确位置信息，将箱体在图像中的位置信息映射到箱体三维数据，获取最终的高度信息。

图5 基于二维图像的目标提取

5 坐标变换及位置参数计算

在高度图像上建立XimgYimgZimg过渡坐标系，如图6所示。此处的X轴同激光雷达起始位置扫描线相重合，原点位于激光雷达光束垂直出射点落下的位于托盘表面的投影点位置。Y轴同机器人扫描直线轨迹重合，其正向指向机器人的运动方向，Z轴由右手定则确定。

图6 坐标变换

系统给出用户坐标系及过渡坐标系建立方法，并给出由计算位置信息的过渡坐标系到用户坐标系的变换方法。

首先，确定机器人ABB IRB6640工具中心点TCP，以图6中Orobot为固定参考点，并测量专用手爪夹具中心位置到TCP的距离，采用4个以上位置点或六点法计算TCP的位置数据，而后保存在机器人Tooldata程序数据中。

其次，要将基于图像坐标系解算的图像数据转换到工件坐标系中，以方便机器人定位及拆垛操作。基于右手坐标系，并规定旋转的正方向为右手螺旋方向，即从该轴正半轴向原点看是逆时针方向，根据坐标变换原则，图像坐标系（以Ximg，Yimg，Zimg表示）向工件坐标系（以Xrobot，Yrobot，Zrobot表示）的转换共分为2步:第1步为绕Z轴逆时针转动90°得到转换矩阵k1；第2步为绕X轴逆时针转动180°得转换矩阵k2。

则总的转换矩阵为:

可得坐标之间的关系为:

ΔX，ΔY及ΔZ及分别为图像坐标系到用户坐标系的平移距离，其值由多次实际测量、试验并经多次标定获得。

根据上述过渡坐标系、前述方法及已知的箱体尺寸大小等相关信息，提取3个周转箱在整个扫描区域内的位置信息，从而可计算出面积区域的质心坐标，进而换算出用户坐标系下的X与Y坐标值。Z坐标的数值非常重要，它影响到机器人手爪开始抓取时下降的高度，高度值解算误差大，则可能损害机器人或周转箱及其内部产品。因此，这里只给出计算Z坐标的详细过程，并约定Z坐标为周转箱外沿处的平均高度，如图7所示。

图7 Z坐标计算流程

对于各箱体偏转角的计算，可首先通过中值滤波对周转箱的二值图像进行边缘轮廓的平滑，而后，通过求取平滑后目标图像的最小外接矩形，则相应的偏转角便惟一确定了。机器人控制系统总体工作流程如图8所示。

图8 机器人工作流程

6 基于VB与VC混合编程的控制软件系统

首先，机器人上位机控制程序基于Visual Studio 2008开发语言，并依赖于ABB Robotics IRC5 PC SDK，利用工控机IPC建立与机器人的通讯与控制关系。充分利用了VB语言编程简单、界面开发快捷的特点。

其次，数据采集、分析、解算与图像显示功能基于可视化编程语言Visual C＋＋6.0及视觉算法库OpenCV，利用其运行速度快、编程灵活和高效的特点，完成激光雷达大量数据的实时采集、数据处理、解算与图像显示；三维激光雷达扫描数据的获取基于多线程编程技术，并通过以太网与激光雷达进行通讯，程序具有数据包完整性判断机制，保证了数据采集的快速性、连续性及准确性。

采用虚拟串口技术实现VB与VC的混合编程，基于一定串行通讯协议，相互传递模块之间的控制信号、报警及运行状态等信息。

另外，下位机机器人控制程序基于离线编程软件RobotStudio或机器人示教器，采用RAPID编程指令编制运动程序，并配备IRC5机器人控制器来控制机器人运行。

7 结束语

目前，激光雷达在机器人领域的应用越来越多，但将其应用于高精度机器人拆码垛定位系统的案例鲜有报道。而本文所设计的机器人拆码垛控制系统，已成功应用于某大型智能化仓储系统自动拆码垛控制系统中。系统运行良好，满足了智能化仓储系统的实际需要，避免了工作过程中对周转箱位置的人工校正与干预，减少了人力及维护成本，实现了大批量连续生产的需求，从而提高了工作效率。

［1］于金霞，菜自兴，段琢华.基于激光雷达的移动机器人运动目标检测与跟踪［J］.电子器件，2007，30（6）:2301－2306.

［2］杨 明，王 宏，何克忠，等.基于激光雷达的移动机器人环境建模与避障［J］.清华大学学报（自然科学版），2000，40（7）:112－116.

［3］王晓东，刘文耀，金月寒，等.嵌入式DSP激光雷达距离图像处理系统［J］.仪器仪表学报，2005，26（10）:1044－1051.

［4］郭裕兰，万建伟，鲁 敏，等.激光雷达目标三维姿态估计［J］.光学精密工程，2012，20（4）:843－849.

［5］张旭明，徐滨士，董世运.用于图像处理的自适应中值滤波［J］.计算机辅助设计与图形学学报，2005，17（2）:295－299.