本發(fā)明屬于人機(jī)協(xié)作交互系統(tǒng)以及工業(yè)機(jī)器人對工作物件位姿感知的技術(shù)領(lǐng)域，涉及到Kinect傳感器與三維激光測距系統(tǒng)之間聯(lián)合標(biāo)定及協(xié)同檢測，主要運用到Kinect傳感器與三維激光測距系統(tǒng)協(xié)同對工作物件的位置和姿態(tài)的在線檢測。

背景技術(shù)：

工業(yè)機(jī)器人是集機(jī)械、電子、控制等多學(xué)科先進(jìn)技術(shù)于一體的現(xiàn)代制造業(yè)重要的自動化裝備，技術(shù)附加值很高并且應(yīng)用范圍廣泛。要實現(xiàn)具有與人協(xié)同工作能力的新一代工業(yè)機(jī)器人系統(tǒng)，如何解決工業(yè)機(jī)器人對人機(jī)共享環(huán)境的建模、感知與場景理解是最根本的問題。

在復(fù)雜的工業(yè)機(jī)器人的應(yīng)用場景中，單一傳感器無法滿足工作環(huán)境感知和場景理解等任務(wù)的需求，因而多個傳感器之間的協(xié)同工作是提高工業(yè)機(jī)器人對環(huán)境感知與場景理解性能的必要手段。國內(nèi)外學(xué)者已經(jīng)意識到該研究領(lǐng)域的重要性，并開展了多傳感器協(xié)同工作方向探索性的研究工作。Kinect傳感器可以使用深度相機(jī)與視覺相機(jī)生成深度圖像信息。(CAO Hongjun,WU Panlong,YAO Xiang，On Depth Smoothing and Fake Laser Rangefinder Based on Kinect，Proceedings of the 33rd Chinese Control Conference,2014，8491-8494)Kinect傳感器主要由視覺、紅外發(fā)射器和紅外接收器組成。Kinect傳感器可以每秒30幀的速度生成深度圖像信息，但其獲取的點云數(shù)據(jù)相對稀疏，誤差在厘米級，可以用來獲取工作場景中工件粗略的位置信息和尺寸信息，卻不能夠準(zhǔn)確獲取工作場景中工件準(zhǔn)確的尺寸信息和位姿信息。三維激光測距系統(tǒng)掃描是通過記錄激光脈沖的方向及目標(biāo)表面反射激光的時間來獲取目標(biāo)點三維坐標(biāo)，(張啟福，孫現(xiàn)申，三維激光掃描儀測量方法與前景展望，北京測繪2011年第1期)三維激光測距系統(tǒng)可以獲取掃描物體表面的高精度點云數(shù)據(jù)集，其具有數(shù)據(jù)精度精確、測量非接觸等優(yōu)點。但是使用三維激光測距系統(tǒng)采集點云數(shù)據(jù)來構(gòu)建場景的三維模型時，為了使得數(shù)據(jù)更加準(zhǔn)確，保證對場景中的某些細(xì)節(jié)進(jìn)行精確描述，所需采集的點云數(shù)據(jù)需要盡可能的多，盡可能的密集，因此精度與掃描速率存在矛盾關(guān)系。

技術(shù)實現(xiàn)要素：

為了克服現(xiàn)有技術(shù)中存在的不足，本發(fā)明提供了一種基于Kinect傳感器與三維激光測距系統(tǒng)協(xié)同的工件位姿在線檢測方法，獲取工件尺寸、位置、姿態(tài)的精確信息。Kinect傳感器具有很好的實時性，可以快速的完成對場景數(shù)據(jù)的采集工作，三維激光測距系統(tǒng)采集的點云數(shù)據(jù)相對精細(xì)，具有很高的精度。因此，在本發(fā)明專利中將兩種傳感器協(xié)同使用，有效的提高了獲取工作物件尺寸和位姿信息的精度和效率，并實現(xiàn)在線檢測。如附圖1所示，光學(xué)實驗平臺安裝了Kinect傳感器與三維激光測距傳感器，圖中1區(qū)域為Kinect傳感器所能掃描的區(qū)域，2區(qū)域為三維激光測距系統(tǒng)所能掃描的區(qū)域，兩者傳感器的掃描區(qū)域均能覆蓋實驗平臺的工作區(qū)域。

本發(fā)明主要使用Kinect傳感器與三維激光測距系統(tǒng)，利用計算機(jī)對數(shù)據(jù)進(jìn)行實時的采集及處理。使用Kinect傳感器對空間中的工件進(jìn)行實時的點云獲取，可得到工件粗略的尺寸信息和位置信息；再將工件的初步位置檢測結(jié)果轉(zhuǎn)化成三維激光測距系統(tǒng)所要掃描的角度信息，通過CAN總線傳輸，使用驅(qū)動器控制電機(jī)的旋轉(zhuǎn)方向和速度，將三維激光測距系統(tǒng)快速旋轉(zhuǎn)到工件所在區(qū)域，并對其進(jìn)行精確的數(shù)據(jù)獲取，從而得到工件更加準(zhǔn)確的激光點云數(shù)據(jù)。通過算法處理，可計算出工件精確的尺寸和位姿信息。

本發(fā)明的技術(shù)方案采用如下步驟：

一、數(shù)據(jù)的采集及點云的預(yù)處理

Kinect傳感器和三維激光測距系統(tǒng)在工作時都會受到噪聲的影響，這種影響無法避免，因此在使用采集到的點云數(shù)據(jù)之前，需要對其進(jìn)行預(yù)處理，即去除離群點。在點云數(shù)據(jù)中指定每個點的鄰域范圍內(nèi)，如果近鄰點的數(shù)目少于一定閾值時，則稱之為離群點。如附圖2所示，如果取閾值數(shù)目為1時(即一定范圍內(nèi)的個數(shù)為1)，那么其中三角形點為離群點，將會被剔除；而取閾值數(shù)目為2時，那么圖中三角形點和正方形點將被當(dāng)為離群點被剔除。顯然，離群點跟所設(shè)定的閾值數(shù)目大小有關(guān)。通過移除點云中的離群點，可以有效的去除部分不合理的點或者錯誤的孤點，對于降低點云數(shù)目和提高點云精度是很有幫助的。離群點與研究對象毫不相關(guān)，卻以無用的信息形式出現(xiàn)，會對實驗數(shù)據(jù)帶來一定的影響，也會對后續(xù)的重建質(zhì)量產(chǎn)生嚴(yán)重的影響，因此，對于噪聲進(jìn)行處理就成為一項十分重要的工作。

在計算三維點云數(shù)據(jù)之前，首先需要消除深度圖像中的噪聲。我們主要是基于高斯分布的方法移除離群點，對每個點的鄰域進(jìn)行數(shù)學(xué)統(tǒng)計，剔除離群點的濾波方法需要經(jīng)過兩步的迭代：

(1)首先，對于某個點通過k-d樹查找算法得到它的k鄰域點，并計算出鄰域點到該點歐式距離的平均值m，計算公式如下：

其中，x、y、z均為點的坐標(biāo)值。

(2)然后，計算上面得到的所有m的平均值m₁和標(biāo)準(zhǔn)差std，根據(jù)這兩個參數(shù)擬合一個距離的正態(tài)分布曲線。根據(jù)所有點的分布情況確定離群點，置信區(qū)間R＝[S₁,S₂]，計算公式如下所示：

S₁＝m₁-mult×std (2)

S₂＝m₁+mult×std (3)

公式中,mult是一個常數(shù)因素。當(dāng)點在區(qū)間R外時，該點被定義為離群點。本文中常數(shù)因子mult取1，即認(rèn)為超過均值一倍方差范圍內(nèi)的點為離群點，則剔除該離群點即可。

二、Kinect傳感器與三維激光測距系統(tǒng)的聯(lián)合標(biāo)定

點云數(shù)據(jù)的聯(lián)合標(biāo)定是將Kinect傳感器和三維激光測距系統(tǒng)采集的點云數(shù)據(jù)，以一個初始位置作為初值，迭代求取更加精確的剛體變換。設(shè)三維激光測距系統(tǒng)和Kinect傳感器采集的點云數(shù)據(jù)分別為P和P₁，本專利中將Kinect傳感器的坐標(biāo)系標(biāo)定到三維激光測距系統(tǒng)的坐標(biāo)系中，則有

P＝RP₁+t (4)

其中R為旋轉(zhuǎn)矩陣，t為平移矩陣。Kinect傳感器和三維激光測距系統(tǒng)之間坐標(biāo)系的映射關(guān)系如附圖3所示。

標(biāo)定過程，首先對點云中的點進(jìn)行配對，由P₁中的點在P中搜索出其最近的點，組成一個點對，將兩個點云數(shù)據(jù)中所有的點對分別找出，點對的集合相當(dāng)于進(jìn)行有效計算得到新點集對，根據(jù)新得到的點集對，計算兩個點集的重心，由新點集對，計算出下一步計算的旋轉(zhuǎn)矩陣R和平移矩陣t，根據(jù)得到的旋轉(zhuǎn)矩陣R和平移矩陣t，就可以計算點集P進(jìn)行剛體變換之后的新點集P′，并計算P到P′的距離平方和，再連續(xù)計算兩次距離平方和之差的絕對值作為是否達(dá)到收斂的依據(jù)，若小于閾值就停止收斂，得到最優(yōu)的旋轉(zhuǎn)矩陣R和平移向量t。但是此算法具有比較明顯的缺陷，要求數(shù)據(jù)點云上的每一點在模型點云上都要找到對應(yīng)點，為了尋找對應(yīng)點，算法需要遍歷模型點云上的每一點，配準(zhǔn)速度很慢，所以一般通過采用匹配特征點之間的旋轉(zhuǎn)平移矩陣，降低此步驟的時間消耗，從而提高效率。

三、Kinect傳感器與三維激光測距系統(tǒng)協(xié)同的在線檢測

針對上述三維激光測距系統(tǒng)和Kinect傳感器的特性，設(shè)計出了三維激光測距系統(tǒng)與Kinect傳感器協(xié)同的在線檢測方法。使用Kinect傳感器獲取工件的點云信息，先要采集背景信息的點云數(shù)據(jù)，當(dāng)有工件在工作區(qū)域中時，Kinect傳感器獲取實時的點云數(shù)據(jù)P，其數(shù)據(jù)點在坐標(biāo)軸方向上的極值分別為x_max,y_max,z_max。按照給定邊長L將空間點云劃分為n個柵格，根據(jù)公式(5)可以得到n：

立體柵格化分是一種常用且有效的方法。對于空間散亂點云數(shù)據(jù)，首先求出所有數(shù)據(jù)點坐標(biāo)的最大值與最小值，再根據(jù)最大值和最小值的坐標(biāo)建立一個與坐標(biāo)軸平行的最小長方體柵格。根據(jù)給定的劃分邊長，將長方體空間劃分成一系列柵格，把所有點云數(shù)據(jù)歸入到其對應(yīng)的柵格中，并做差提取點云空間中工件的點云信息。

使用立體柵格化得到被測工件的點云后，要使用濾波算法去除雜點，得到準(zhǔn)確的工件點云信息。之后讀取工件在Kinect傳感器坐標(biāo)系中各個點的三維點云信息，即每個點的x、y、z值。根據(jù)點云坐標(biāo)值進(jìn)行分析比對，獲得在水平方向上x的最大值和最小值，即x_max和x_min，以及在垂直方向上z的最大值和最小值,即z_max和z_min，并將這四個點投影到XZ坐標(biāo)系中。

實驗原理示意圖如圖4所示，A點為Kinect傳感器的坐標(biāo)點，B點為三維激光測距系統(tǒng)坐標(biāo)點，C點為實驗平臺的邊界點,D點為z_max在XZ坐標(biāo)系的投影點，F(xiàn)點為z_min在XZ坐標(biāo)系的投影點，G點為x_max在XZ坐標(biāo)系的投影點，E點為x_min在XZ坐標(biāo)系的投影點。

由標(biāo)定實驗，可以得到A、B、C三個點在三維激光測距系統(tǒng)坐標(biāo)系中的坐標(biāo)值，根據(jù)Kinect傳感器與三維激光測距系統(tǒng)的聯(lián)合標(biāo)定求出的旋轉(zhuǎn)平移矩陣R和t，可以得到D，E，F(xiàn)，G點在三維激光坐標(biāo)系下的坐標(biāo)值。以D點為例，

[x_D,y_D,z_D]^T＝R[x₁,y₁,z₁]^T+t (6)

根據(jù)三角形余弦定理，分別求出∠DBE,∠DBG,∠DBF,∠EBG,∠EBF,∠GBF的度數(shù)，比較得出最大的角度，即為三維激光測距系統(tǒng)所要掃描的工件所在的區(qū)域，在此范圍內(nèi)三維激光測距系統(tǒng)精細(xì)掃描，獲取工件準(zhǔn)確的尺寸信息和位姿信息。在△BCF中，同樣根據(jù)三角形余弦定理，計算出∠CBF，則為三維激光測距系統(tǒng)快速旋轉(zhuǎn)區(qū)域。

計算機(jī)得到三維激光測距系統(tǒng)快速掃描范圍和精細(xì)掃描范圍后，轉(zhuǎn)換成控制器的控制代碼(控制代碼包括旋轉(zhuǎn)角度和旋轉(zhuǎn)速度)。三維激光測距系統(tǒng)采集過程包括快速掃描模式和精細(xì)掃描模式，具體過程如下：從三維激光測距系統(tǒng)的初始位置開始進(jìn)行快速掃描模式，使激光傳感器快速旋轉(zhuǎn)到精細(xì)掃描區(qū)域的起始角度，則開始精細(xì)掃描模式，在此模式下激光傳感器精細(xì)掃描，旋轉(zhuǎn)到終止角度時，則此工作周期完成，并進(jìn)入到下一個工作周期。

采集后的點云數(shù)據(jù)提取出精細(xì)掃描模式中的數(shù)據(jù)，采集的點云數(shù)據(jù)密集，能夠得到工件精確的的尺寸、位置和姿態(tài)信息。

附圖說明

圖1是Kinect傳感器與三維激光測距系統(tǒng)的掃描區(qū)域示意圖。

圖2是去除離散點原理圖。

圖3是Kinect傳感器和三維激光測距系統(tǒng)坐標(biāo)系聯(lián)合標(biāo)定示意圖。

圖4是Kinect傳感器和三維激光測距系統(tǒng)角度轉(zhuǎn)換示意圖。

具體實施方法

為了驗證本方法的有效性，本發(fā)明的具體實施方式包括三個方面，一是Kinect傳感器的數(shù)據(jù)采集過程，二是將Kinect傳感器的點云數(shù)據(jù)柵格化得到工件的粗略位置信息，三是根據(jù)位置信息換算到三維激光測距系統(tǒng)的掃描角度，進(jìn)行數(shù)據(jù)的精細(xì)采集。

本方法使用的傳感器是Kinect傳感器和三維激光測距系統(tǒng)。Kinect傳感器是微軟公司開發(fā)的一款體感外設(shè)，可獲取深度圖像信息和視覺圖像信息。但是Kinect傳感器感知環(huán)境的最佳距離是1.2米至3.5米，誤差在厘米級，但是隨著感知距離的增大，誤差也會增大。本發(fā)明使用的三維激光測距系統(tǒng)是由二維激光傳感器和帶有絕對編碼器的電機(jī)的旋轉(zhuǎn)云臺構(gòu)成的，旋轉(zhuǎn)云臺在水平面上旋轉(zhuǎn)，二維激光傳感器扇形掃描獲取三維點云數(shù)據(jù)。三維激光測距系統(tǒng)的最佳感知距離是0.5米至10米，誤差在毫米級，精度較高、穩(wěn)定性強(qiáng)，但是獲取信息的時間較長，因此，三維激光測距系統(tǒng)與Kinect傳感器組合成為三維場景重構(gòu)和環(huán)境感知的優(yōu)秀組合。我們使用的硬件平臺為配置Linux操作系統(tǒng)的計算機(jī)，配置環(huán)境為ROS(Robot Operating System)系統(tǒng)。

首先使用Kinect傳感器和三維激光測距系統(tǒng)對環(huán)境進(jìn)行數(shù)據(jù)采集，對采集到的點云數(shù)據(jù)進(jìn)行預(yù)處理，并剔除離散點。之后對Kinect傳感器和三維激光測距系統(tǒng)的點云信息進(jìn)行標(biāo)定，獲得Kinect傳感器和三維激光測距系統(tǒng)之間的旋轉(zhuǎn)矩陣R和平移矩陣t。

實驗過程中，使用Kinect傳感器對工件進(jìn)行信息采集，對采集到的點云數(shù)據(jù)進(jìn)行立體柵格化處理，進(jìn)而獲取工件所在的區(qū)域信息，并且通過角度轉(zhuǎn)換得到三維激光測距系統(tǒng)所需要旋轉(zhuǎn)的角度，生成指令，通過CAN總線進(jìn)行傳輸，以控制電機(jī)進(jìn)行旋轉(zhuǎn)。三維激光測距系統(tǒng)在工作時，快速掃描模式時三維激光測距系統(tǒng)電機(jī)的轉(zhuǎn)速為每分鐘30轉(zhuǎn)，精細(xì)掃描模式時三維激光測距系統(tǒng)電機(jī)的轉(zhuǎn)速為每分鐘3轉(zhuǎn)，使用此方法可以快速尋找到工件，獲取工件精確的尺寸信息和位姿信息，同時實現(xiàn)在線的檢測功能。