魏志強(qiáng)，李 兵，屈玉豐，沙 群

（1.北京航空航天大學(xué)機(jī)器人研究所，北京 100191；2.新奧特（北京）視頻技術(shù)有限公司，北京 100185）

0 引言

目前，大型智能化倉(cāng)儲(chǔ)系統(tǒng)對(duì)機(jī)器人拆碼垛系統(tǒng)的工作效率及自動(dòng)化程度提出了越來(lái)越高的要求。激光雷達(dá)以其測(cè)距精度高、環(huán)境適應(yīng)性強(qiáng)和實(shí)時(shí)性好等優(yōu)點(diǎn)，已在機(jī)器人運(yùn)動(dòng)及工程領(lǐng)域得到廣泛應(yīng)用［1－4］。為此，利用激光雷達(dá)技術(shù)，設(shè)計(jì)了機(jī)器人拆碼垛控制系統(tǒng)，詳細(xì)介紹了機(jī)器人拆碼垛系統(tǒng)總體功能、目標(biāo)提取原理及控制實(shí)現(xiàn)方法。

1 系統(tǒng)的組成及功能

系統(tǒng)總體功能如圖1所示。采用ABB IRB6640機(jī)器人，并配以專門的手爪夾具，機(jī)器人手臂上裝有德國(guó)SICK公司LMS400激光雷達(dá)。其工作過程如下:將打包好的裝有產(chǎn)品的周轉(zhuǎn)箱以“品”字型布局方式運(yùn)輸?shù)酵斜P上（托盤上每層3摞箱子，每摞10層或?qū)訑?shù)不定，箱子實(shí)物如圖2所示），當(dāng)機(jī)器人運(yùn)動(dòng)至系統(tǒng)掃描起始位置，啟動(dòng)機(jī)器人并啟動(dòng)放置于機(jī)器人手臂上的激光雷達(dá)，以一定速度在水平方向作勻速直線運(yùn)動(dòng)，從而實(shí)現(xiàn)對(duì)待拆周轉(zhuǎn)箱的數(shù)據(jù)信息掃描；掃描完畢，由上位機(jī)對(duì)所保存的三維數(shù)據(jù)文件進(jìn)行分析及處理，計(jì)算獲得3個(gè)周轉(zhuǎn)箱在機(jī)器人用戶坐標(biāo)中的中心位置及偏轉(zhuǎn)角信息，隨后，將數(shù)據(jù)信息以規(guī)定的協(xié)議通過以太網(wǎng)通訊方式傳遞給機(jī)器人下位機(jī)控制程序，從而進(jìn)行后續(xù)的機(jī)器人定位及拆垛操作。

圖1 基于激光雷達(dá)的機(jī)器人拆碼垛系統(tǒng)

圖2 “品”字形擺放的周轉(zhuǎn)箱及內(nèi)部物料實(shí)物

2 激光雷達(dá)三維成像原理

LMS400高速高精度基于相位差原理的測(cè)距激光雷達(dá)在系統(tǒng)中的主要參數(shù)為:掃描距離，0.7～3 m；角度分辨率，0.25°；掃描頻率，370Hz；掃描角度及起始角，70°，55°；單次掃描數(shù)據(jù)數(shù)，280組。

激光掃描示意如圖3所示。在XYZ坐標(biāo)系中，Y軸方向?yàn)榇怪奔埫嫦蛲?，即激光雷達(dá)掃描時(shí)的運(yùn)動(dòng)方向。激光雷達(dá)通過內(nèi)部擺鏡控制光束由起始55°角開始從左至右掃描目標(biāo)，得出單個(gè)截面上的280個(gè)距離信息；同時(shí)，機(jī)器人以一定運(yùn)行速度沿Y軸方向勻速移動(dòng)激光雷達(dá)，從而，通過對(duì)多個(gè)掃描截面的積分，得出周轉(zhuǎn)箱的三維數(shù)據(jù)。

圖3 掃描示意

3 三維圖像建模與顯示

設(shè)Diskk為被測(cè)對(duì)象上任意點(diǎn)P到激光雷達(dá)發(fā)射原點(diǎn)的距離，由圖3可知，對(duì)一次單線掃描來(lái)說，第k個(gè)點(diǎn)的Z及X向坐標(biāo)值為:

為方便計(jì)算，采用圖3中X′Y′Z′坐標(biāo)系（Y′軸正方向?yàn)榇怪奔埫嫦蚶铮瑘D中未標(biāo)出）。設(shè)此時(shí)的坐標(biāo)原點(diǎn)為被測(cè)對(duì)象的最低點(diǎn)，并位于激光雷達(dá)發(fā)射點(diǎn)的最下方的托盤上，即此點(diǎn)測(cè)出的箱體高度值為最大，并將高度值進(jìn)行如下變換:

Z為由原始數(shù)據(jù)計(jì)算得到的原始Z向坐標(biāo)值矩陣；Zmax為Z向坐標(biāo)矩陣元素中的最大值；Z′為坐標(biāo)變換后的Z向坐標(biāo)值矩陣。

利用從現(xiàn)場(chǎng)實(shí)際采集的三維數(shù)據(jù)，通過Matlab建立了周轉(zhuǎn)箱三維圖像，如圖4所示。

圖4 三維圖像

4 噪聲濾除及目標(biāo)提取原理

由于數(shù)據(jù)范圍較寬，同時(shí)也為方便后續(xù)圖像處理，將圖像從三維轉(zhuǎn)向二維顯示，用灰度值表示相應(yīng)的高度，并通過線性變換增加圖像的顯示細(xì)節(jié)，線性變換公式為:

Z″為Z向坐標(biāo)值矩陣；Z′ max 為Z′矩陣元素中的最大值；Z′ min 為Z′矩陣元素中的最小值。

圖5a為目標(biāo)高度灰度圖像，可見噪聲比較嚴(yán)重。為獲取較好的去噪效果，采用參考文獻(xiàn)［5］自適應(yīng)中值濾波方法濾除噪聲，去噪后的高度圖像如圖5b所示。由圖5b去噪后的二維圖像可見，相對(duì)于圖像其他部分，箱體的邊緣細(xì)節(jié)較為清晰，具有較強(qiáng)的對(duì)比度。因此，通過已知的先驗(yàn)知識(shí)搜索圖像中的箱體邊緣位置信息，進(jìn)而獲取箱體在圖像中的準(zhǔn)確位置參數(shù)，具體實(shí)現(xiàn)步驟如下:

a.利用OTSU大津法對(duì)去噪后的圖像進(jìn)行分割，獲取分割后的二值圖像，如圖5c所示。

b.利用形態(tài)學(xué)膨脹算法對(duì)箱體邊緣進(jìn)行增強(qiáng)，增強(qiáng)后的圖像如圖5d所示。

c.利用種子填充算法獲取圖像各連通域，通過連通域的長(zhǎng)寬比、對(duì)稱性等先驗(yàn)知識(shí)獲取箱體的邊緣位置信息，如圖5e中的4個(gè)小方框所示。

d.對(duì)箱體邊緣信息進(jìn)行分析，獲取箱體在圖像中的準(zhǔn)確位置信息，將箱體在圖像中的位置信息映射到箱體三維數(shù)據(jù)，獲取最終的高度信息。

圖5 基于二維圖像的目標(biāo)提取

5 坐標(biāo)變換及位置參數(shù)計(jì)算

在高度圖像上建立XimgYimgZimg過渡坐標(biāo)系，如圖6所示。此處的X軸同激光雷達(dá)起始位置掃描線相重合，原點(diǎn)位于激光雷達(dá)光束垂直出射點(diǎn)落下的位于托盤表面的投影點(diǎn)位置。Y軸同機(jī)器人掃描直線軌跡重合，其正向指向機(jī)器人的運(yùn)動(dòng)方向，Z軸由右手定則確定。

圖6 坐標(biāo)變換

系統(tǒng)給出用戶坐標(biāo)系及過渡坐標(biāo)系建立方法，并給出由計(jì)算位置信息的過渡坐標(biāo)系到用戶坐標(biāo)系的變換方法。

首先，確定機(jī)器人ABB IRB6640工具中心點(diǎn)TCP，以圖6中Orobot為固定參考點(diǎn)，并測(cè)量專用手爪夾具中心位置到TCP的距離，采用4個(gè)以上位置點(diǎn)或六點(diǎn)法計(jì)算TCP的位置數(shù)據(jù)，而后保存在機(jī)器人Tooldata程序數(shù)據(jù)中。

其次，要將基于圖像坐標(biāo)系解算的圖像數(shù)據(jù)轉(zhuǎn)換到工件坐標(biāo)系中，以方便機(jī)器人定位及拆垛操作?；谟沂肿鴺?biāo)系，并規(guī)定旋轉(zhuǎn)的正方向?yàn)橛沂致菪较?，即從該軸正半軸向原點(diǎn)看是逆時(shí)針方向，根據(jù)坐標(biāo)變換原則，圖像坐標(biāo)系（以Ximg，Yimg，Zimg表示）向工件坐標(biāo)系（以Xrobot，Yrobot，Zrobot表示）的轉(zhuǎn)換共分為2步:第1步為繞Z軸逆時(shí)針轉(zhuǎn)動(dòng)90°得到轉(zhuǎn)換矩陣k1；第2步為繞X軸逆時(shí)針轉(zhuǎn)動(dòng)180°得轉(zhuǎn)換矩陣k2。

則總的轉(zhuǎn)換矩陣為:

可得坐標(biāo)之間的關(guān)系為:

ΔX，ΔY及ΔZ及分別為圖像坐標(biāo)系到用戶坐標(biāo)系的平移距離，其值由多次實(shí)際測(cè)量、試驗(yàn)并經(jīng)多次標(biāo)定獲得。

根據(jù)上述過渡坐標(biāo)系、前述方法及已知的箱體尺寸大小等相關(guān)信息，提取3個(gè)周轉(zhuǎn)箱在整個(gè)掃描區(qū)域內(nèi)的位置信息，從而可計(jì)算出面積區(qū)域的質(zhì)心坐標(biāo)，進(jìn)而換算出用戶坐標(biāo)系下的X與Y坐標(biāo)值。Z坐標(biāo)的數(shù)值非常重要，它影響到機(jī)器人手爪開始抓取時(shí)下降的高度，高度值解算誤差大，則可能損害機(jī)器人或周轉(zhuǎn)箱及其內(nèi)部產(chǎn)品。因此，這里只給出計(jì)算Z坐標(biāo)的詳細(xì)過程，并約定Z坐標(biāo)為周轉(zhuǎn)箱外沿處的平均高度，如圖7所示。

圖7 Z坐標(biāo)計(jì)算流程

對(duì)于各箱體偏轉(zhuǎn)角的計(jì)算，可首先通過中值濾波對(duì)周轉(zhuǎn)箱的二值圖像進(jìn)行邊緣輪廓的平滑，而后，通過求取平滑后目標(biāo)圖像的最小外接矩形，則相應(yīng)的偏轉(zhuǎn)角便惟一確定了。機(jī)器人控制系統(tǒng)總體工作流程如圖8所示。

圖8 機(jī)器人工作流程

6 基于VB與VC混合編程的控制軟件系統(tǒng)

首先，機(jī)器人上位機(jī)控制程序基于Visual Studio 2008開發(fā)語(yǔ)言，并依賴于ABB Robotics IRC5 PC SDK，利用工控機(jī)IPC建立與機(jī)器人的通訊與控制關(guān)系。充分利用了VB語(yǔ)言編程簡(jiǎn)單、界面開發(fā)快捷的特點(diǎn)。

其次，數(shù)據(jù)采集、分析、解算與圖像顯示功能基于可視化編程語(yǔ)言Visual C＋＋6.0及視覺算法庫(kù)OpenCV，利用其運(yùn)行速度快、編程靈活和高效的特點(diǎn)，完成激光雷達(dá)大量數(shù)據(jù)的實(shí)時(shí)采集、數(shù)據(jù)處理、解算與圖像顯示；三維激光雷達(dá)掃描數(shù)據(jù)的獲取基于多線程編程技術(shù)，并通過以太網(wǎng)與激光雷達(dá)進(jìn)行通訊，程序具有數(shù)據(jù)包完整性判斷機(jī)制，保證了數(shù)據(jù)采集的快速性、連續(xù)性及準(zhǔn)確性。

采用虛擬串口技術(shù)實(shí)現(xiàn)VB與VC的混合編程，基于一定串行通訊協(xié)議，相互傳遞模塊之間的控制信號(hào)、報(bào)警及運(yùn)行狀態(tài)等信息。

另外，下位機(jī)機(jī)器人控制程序基于離線編程軟件RobotStudio或機(jī)器人示教器，采用RAPID編程指令編制運(yùn)動(dòng)程序，并配備IRC5機(jī)器人控制器來(lái)控制機(jī)器人運(yùn)行。

7 結(jié)束語(yǔ)

目前，激光雷達(dá)在機(jī)器人領(lǐng)域的應(yīng)用越來(lái)越多，但將其應(yīng)用于高精度機(jī)器人拆碼垛定位系統(tǒng)的案例鮮有報(bào)道。而本文所設(shè)計(jì)的機(jī)器人拆碼垛控制系統(tǒng)，已成功應(yīng)用于某大型智能化倉(cāng)儲(chǔ)系統(tǒng)自動(dòng)拆碼垛控制系統(tǒng)中。系統(tǒng)運(yùn)行良好，滿足了智能化倉(cāng)儲(chǔ)系統(tǒng)的實(shí)際需要，避免了工作過程中對(duì)周轉(zhuǎn)箱位置的人工校正與干預(yù)，減少了人力及維護(hù)成本，實(shí)現(xiàn)了大批量連續(xù)生產(chǎn)的需求，從而提高了工作效率。

［1］于金霞，菜自興，段琢華.基于激光雷達(dá)的移動(dòng)機(jī)器人運(yùn)動(dòng)目標(biāo)檢測(cè)與跟蹤［J］.電子器件，2007，30（6）:2301－2306.

［2］楊 明，王 宏，何克忠，等.基于激光雷達(dá)的移動(dòng)機(jī)器人環(huán)境建模與避障［J］.清華大學(xué)學(xué)報(bào)（自然科學(xué)版），2000，40（7）:112－116.

［3］王曉東，劉文耀，金月寒，等.嵌入式DSP激光雷達(dá)距離圖像處理系統(tǒng)［J］.儀器儀表學(xué)報(bào)，2005，26（10）:1044－1051.

［4］郭裕蘭，萬(wàn)建偉，魯 敏，等.激光雷達(dá)目標(biāo)三維姿態(tài)估計(jì)［J］.光學(xué)精密工程，2012，20（4）:843－849.

［5］張旭明，徐濱士，董世運(yùn).用于圖像處理的自適應(yīng)中值濾波［J］.計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào)，2005，17（2）:295－299.