周恒旭，何志敏，周 燕

(1.佛山科學(xué)技術(shù)學(xué)院電子信息工程學(xué)院，廣東 佛山 528225；2.佛山科學(xué)技術(shù)學(xué)院自動化學(xué)院，廣東 佛山 528225)

0 引言

目前，對于解決機(jī)械臂避障問題，一般是利用先驗(yàn)知識進(jìn)行示教避障。機(jī)器人雖然擁有某種程度的智能化，但遠(yuǎn)不符合人們的預(yù)期，其中一個重要因素是機(jī)器人感知系統(tǒng)中仍存在一些問題亟待解決，其中最重要的是如何讓機(jī)器人對現(xiàn)實(shí)三維場景感知和理解[1]。這些現(xiàn)實(shí)問題極大地制約著機(jī)器人的應(yīng)用。非結(jié)構(gòu)場景的感知是機(jī)械臂實(shí)現(xiàn)自主避障的基礎(chǔ)，使用三維傳感器是非結(jié)構(gòu)場景感知的前提。

激光雷達(dá)和深度攝像頭都是獲取點(diǎn)云的主流設(shè)備之一，但深度攝像頭存在的問題有：深度攝像頭的測量距離短，隨著距離增大誤差變大；會因反射率不同而導(dǎo)致深度圖里的物體邊緣出現(xiàn)空洞現(xiàn)象[2]；其成像程序計(jì)算量大，在有遮擋物情況下不適用。

激光雷達(dá)應(yīng)用場景多，文獻(xiàn)[3]使用2D激光雷達(dá)通過掃描站獲取個體植物的位置信息；文獻(xiàn)[4]使用機(jī)載激光雷達(dá)進(jìn)行輸電線的三維重建；文獻(xiàn)[5]描述了歐洲航天局的e.Deorbit軌道碎片清除計(jì)劃中的B1階段里，激光雷達(dá)是清理衛(wèi)星的GNC技術(shù)的一個組成部分，能在近距離上切換成3D模式掃描生成點(diǎn)云。綜上所述，本文將使用三維激光雷達(dá)作為場景感知的傳感器以此獲取點(diǎn)云數(shù)據(jù)。

在點(diǎn)云處理算法方面，近年來流行使用深度學(xué)習(xí)的算法對點(diǎn)云進(jìn)行處理，其主要分為2類：基于鳥瞰圖的算法和基于點(diǎn)云圖的算法。基于鳥瞰圖的思想是將點(diǎn)云數(shù)據(jù)轉(zhuǎn)換為鳥瞰圖，再使用近些年出現(xiàn)的RCNN、Fast RCNN、Faster RCNN、YOLO和SSD等算法[6-12]對鳥瞰圖進(jìn)行目標(biāo)檢測?；邳c(diǎn)云圖的算法起源于2017年出現(xiàn)的PointNet[13]，該深度學(xué)習(xí)網(wǎng)絡(luò)直接處理點(diǎn)云，在目標(biāo)分割和識別上取得良好效果，但其僅僅是用于物件的抓取任務(wù)上?；谏疃葘W(xué)習(xí)的算法雖然表現(xiàn)最好，但實(shí)際上，數(shù)據(jù)的處理決定著算法的表現(xiàn)效果，而這些算法往往是針對數(shù)據(jù)和特定場景進(jìn)行設(shè)計(jì)的。深度學(xué)習(xí)是數(shù)據(jù)驅(qū)動型，需要大量數(shù)據(jù)訓(xùn)練才能取得良好表現(xiàn)。在現(xiàn)實(shí)世界中大多數(shù)數(shù)據(jù)都是未標(biāo)記的，不存在標(biāo)簽信息，因此有監(jiān)督的深度學(xué)習(xí)適合解決的AI問題是非常狹窄的，更多的是需要無監(jiān)督學(xué)習(xí)解決問題[14]。面向機(jī)械臂避障的非結(jié)構(gòu)場景中，場景形態(tài)結(jié)構(gòu)千變?nèi)f化，并無具體的特征結(jié)構(gòu)，無法進(jìn)行針對性的數(shù)據(jù)處理，非結(jié)構(gòu)場景的每一個構(gòu)成部分也不存在標(biāo)簽，無法制作大規(guī)模的數(shù)據(jù)集進(jìn)行深度學(xué)習(xí)，故雖然深度學(xué)習(xí)在某方面表現(xiàn)效果良好，但是在非結(jié)構(gòu)場景里的應(yīng)用中始終無法代替非深度學(xué)習(xí)方法進(jìn)行點(diǎn)云的處理操作。故本文將使用非深度學(xué)習(xí)的方法對點(diǎn)云進(jìn)行處理。

本文目的在于，針對目前實(shí)現(xiàn)機(jī)械臂避障過程中需要感知場景信息，而使用激光雷達(dá)對非結(jié)構(gòu)場景感知進(jìn)行研究的情況，通過對三維激光雷達(dá)點(diǎn)云進(jìn)行地面分割、濾波和聚類操作，實(shí)現(xiàn)對場景信息的獲取。在地面分割過程中，針對傳統(tǒng)的RANSAC算法表現(xiàn)不佳情況，提出了基于橫截面積的地面分割方法。在聚類過程中，針對經(jīng)典的DBSCAN算法存在參數(shù)選擇難且影響聚類效果的問題，根據(jù)激光點(diǎn)云的特點(diǎn)，提出一種基于有序點(diǎn)云間的切比雪夫距離確定DBSCAN 算法的鄰域半徑參數(shù)方法。

1 非結(jié)構(gòu)場景的點(diǎn)云數(shù)據(jù)獲取

本文通過CoppeliaSim機(jī)器人仿真軟件進(jìn)行非結(jié)構(gòu)場景的搭建。使用仿真軟件進(jìn)行機(jī)器人感知算法研究的優(yōu)點(diǎn)在于快速且準(zhǔn)確，可搭建任意現(xiàn)實(shí)世界場景。為了獲取場景的激光點(diǎn)云數(shù)據(jù)，在CoppeliaSim里使用Velodyne公司的HDL-64E三維激光雷達(dá)，該傳感器專為障礙物探測和車船導(dǎo)航而設(shè)計(jì)，探測精度為1 cm，垂直視場角為26.8°，水平視場角為360°。設(shè)計(jì)的場景為常見的室內(nèi)非結(jié)構(gòu)場景，2張桌子，桌子上面有2個立方體和1個盆栽，桌子對面為激光雷達(dá)，如圖1所示。

圖1 非結(jié)構(gòu)場景示意

利用CoppeliaSim的遠(yuǎn)程API與Python進(jìn)行通信，獲取激光雷達(dá)的點(diǎn)云數(shù)據(jù)，取其1/4視場并保存成.ply格式文件，數(shù)量為10 952個。讀取.ply文件，并可視化如圖2所示。

圖2 非結(jié)構(gòu)場景的點(diǎn)云示意

2 地面分割

在獲取到點(diǎn)云數(shù)據(jù)后，一般需要進(jìn)行地面分割操作，減少點(diǎn)云數(shù)據(jù)量。地面分割一般采用基于隨機(jī)采樣一致性(RANSAC)算法進(jìn)行操作，該算法是Fischler和Bolles在1981年提出[15]，被廣泛用于平面提取[16]、邊界擬合與提取[17-18]、剔除誤匹配點(diǎn)[19]和建筑物立面分割[20]等領(lǐng)域。雖然RANSAC算法具有較強(qiáng)的魯棒性，但是也存在缺點(diǎn)：局外點(diǎn)的數(shù)目不能過多；計(jì)算參數(shù)過程中迭代次數(shù)沒有上限，耗費(fèi)計(jì)算資源較大；需確定模型閾值和適用數(shù)據(jù)數(shù)目。這些不足限制了RANSAC算法在點(diǎn)云分割地面上的應(yīng)用。針對圖1的場景，使用RANSAC算法對點(diǎn)云數(shù)據(jù)進(jìn)行處理，發(fā)現(xiàn)效果不佳，如圖3所示。在圖3中，之所以擬合的平面與地面形成大幅度的角度，這是因?yàn)閳鼍暗狞c(diǎn)云相對于地面來說是局外點(diǎn)，而局外點(diǎn)的數(shù)目過多，導(dǎo)致z方向的權(quán)重過大，故出現(xiàn)擬合地面效果不佳情況。

圖3 RANSAC算法分割地面效果

2.1 基于橫截面積的地面劃分

結(jié)合非結(jié)構(gòu)場景，提出了一種基于橫截面積的地面分割原理。通過研究發(fā)現(xiàn)，從z軸方向看，在非結(jié)構(gòu)場景中，地面點(diǎn)云分布范圍廣泛，而其他物體點(diǎn)云分布范圍集中，為了實(shí)現(xiàn)分離地面，提出一種以橫截面為依據(jù)的地面分割的方法。其具體原理是：以z軸為分割標(biāo)準(zhǔn)，設(shè)置分層數(shù)n對z軸進(jìn)行分割，即分為n個截面。對于第i個截面(i=1,…,n)，遍歷所有點(diǎn)云的z坐標(biāo)是否屬于第i個截面上，如果是，將其x坐標(biāo)值和y坐標(biāo)值保存在臨時存儲單元里tempi，查找tempi里x最大值xmax和最小值xmin，求xmax與xmin的差，即xei。同樣方法計(jì)算出yei。用xei與yei相乘計(jì)算出最大橫截面積si。最后，si最大的橫截面層即為包含的地面層。根據(jù)地面層所在的位置，可以使用直通濾波器進(jìn)行地面分割操作。另外，關(guān)于分層數(shù)n的選擇，n越大則z軸方向分的層越細(xì)。但層數(shù)n并非越大越好，因?yàn)閚決定著每層的高度，若每層有一定的高度，則可以囊括整個地面的小幅度的坡度，有小幅度坡度的地面是會在z方向上產(chǎn)生高度，故每層的高度需要高于可能存在的坡度地面產(chǎn)生的高度。假設(shè)點(diǎn)云在x-y平面內(nèi)的范圍為t×t，單位為m，地面坡度為α°，則通過正切函數(shù)得到其高度為t×tanα。

根據(jù)場景的實(shí)際長寬，將分層數(shù)n設(shè)置為15，每層的高度h為0.09 m，再對其進(jìn)行每層橫截面積計(jì)算，把每層的橫截面積數(shù)值繪制成直方圖，如圖4所示。由圖4可知，第1層的面積遠(yuǎn)遠(yuǎn)大于其他層，明顯是由地面的點(diǎn)云分布過廣所致，從第2層開始，面積逐步下降，但第11層和第12層的面積略微上升，這是盆栽的植物下窄中寬上窄所致。

圖4 每層的橫截面積直方圖

2.2 利用直通濾波器進(jìn)行地面分割

點(diǎn)云的直通濾波器，是在點(diǎn)云的某個維度上設(shè)置閾值，將某個維度上的點(diǎn)云數(shù)據(jù)分為在閾值內(nèi)和閾值外，從而決定是否濾除。此外，在面向機(jī)械臂避障的三維場景中，機(jī)械臂有一定的工作空間，即機(jī)械臂的末端執(zhí)行范圍有限，一般為x-y-z軸的±2 m之間。采用直通濾波器對點(diǎn)云數(shù)據(jù)處理，既符合機(jī)械臂的工作空間需要，同時也有利于減少點(diǎn)云數(shù)據(jù)量。

依據(jù)每層高度h，地面在第1層，以及點(diǎn)云z軸方向上最小值為-0.023 0 m、最大值為1.902 1 m，并結(jié)合一般機(jī)械臂的最大工作空間，設(shè)置直通濾波器的閾值，對點(diǎn)云進(jìn)行直通濾波操作。處理后的點(diǎn)云可視化如圖5所示，其數(shù)量為4 314個。

圖5 地面分割后的場景點(diǎn)云

經(jīng)過直通濾波處理后，僅僅是去除地面點(diǎn)云，但目前場景中的點(diǎn)云數(shù)量依然較大，這是由于使用了高分辨率的三維激光雷達(dá)獲取的點(diǎn)云，有些地方往往非常密集，存在極大的冗余信息。同時，大量冗余的點(diǎn)云對于后續(xù)的分割工作提出了挑戰(zhàn)，故對場景的點(diǎn)云數(shù)據(jù)進(jìn)行采樣是必要操作。本文采用體素化網(wǎng)格濾波器對點(diǎn)云數(shù)據(jù)進(jìn)行采樣操作，從而實(shí)現(xiàn)點(diǎn)云的濾波。效果如圖6所示，其點(diǎn)云數(shù)量為2 157個。

對比圖5與圖6，明顯發(fā)現(xiàn)點(diǎn)云數(shù)量減少，但依然保留場景中許多細(xì)節(jié)特征，即有利于分割處理的效率，場景也具備較高的保真度。

圖6 體素網(wǎng)格濾波后的場景點(diǎn)云

3 利用聚類進(jìn)行點(diǎn)云語義分割

在點(diǎn)云濾波之后，需要進(jìn)行分割操作。點(diǎn)云的分割是通過點(diǎn)云的坐標(biāo)、幾何結(jié)構(gòu)等信息進(jìn)行歸類區(qū)分，使得同一區(qū)分內(nèi)的點(diǎn)云具有相似的特征。點(diǎn)云的高效分割是其他應(yīng)用的基礎(chǔ)。目前，魯棒性比較好且實(shí)用的點(diǎn)云分割算法是聚類分割。在聚類后，每一個簇進(jìn)行包圍盒操作提取物體，可實(shí)現(xiàn)語義分割的功能。

3.1 基于密度的對噪聲魯棒空間聚類

點(diǎn)云在空間上的分布因物體的存在密集，而無物體處稀疏，故采用基于密度的聚類。在基于密度的聚類算法中，DBSCAN(density-based spatial clustering of applications with noise)是最具有代表性的基于密度的聚類算法[21]。

DBSCAN算法能找到點(diǎn)云的密集區(qū)域，并將點(diǎn)云空間中密集區(qū)域歸為聚類簇。相較于其他聚類方法，DBSCAN算法具有一些優(yōu)點(diǎn)：基于密度進(jìn)行聚類，對遠(yuǎn)離密度核心的噪聲點(diǎn)具有較強(qiáng)的魯棒性；不需提前指定聚類簇的數(shù)量，而k-means聚類需要提前指定聚類簇的數(shù)量，且不同數(shù)量會對聚類結(jié)果產(chǎn)生很大影響，需要進(jìn)行多次實(shí)驗(yàn)；能聚類出任意空間形態(tài)的聚類。

DBSCAN算法有2個重要的算法參數(shù)：鄰域半徑R和最少點(diǎn)數(shù)目minpoints。R與minpoints組合能描述數(shù)據(jù)樣本的密集程度，當(dāng)R內(nèi)的數(shù)據(jù)點(diǎn)的個數(shù)大于或等于minpoints即為密集。在R內(nèi)點(diǎn)的數(shù)量大于minpoints的點(diǎn)叫做核心點(diǎn)，不屬于核心點(diǎn)但位于其他某個核心點(diǎn)的R內(nèi)的點(diǎn)叫做邊界點(diǎn)，噪聲點(diǎn)既不是核心點(diǎn)也不是邊界點(diǎn)。DBSCAN算法中描述點(diǎn)與點(diǎn)之間的關(guān)系有4種：密度直達(dá)、密度可達(dá)、密度相連和非密度相連。DBSCAN的算法流程如圖7所示。

圖7 DBSCAN的算法流程

3.2 改進(jìn)DBSCAN算法

經(jīng)典的DBSCAN算法雖能分類出任意形態(tài)的簇并且能較為準(zhǔn)確地識別噪聲點(diǎn)，但仍存在算法對參數(shù)鄰域半徑R極其敏感的特性。若參數(shù)選取不當(dāng)，會對聚類結(jié)果會產(chǎn)生不良影響[22]，故需要從參數(shù)選擇方面改進(jìn)DBSCAN算法。

通過研究三維激光雷的點(diǎn)云，發(fā)現(xiàn)激光雷達(dá)輸出點(diǎn)云數(shù)據(jù)是分布不均衡的，距離三維激光雷達(dá)較近位置處點(diǎn)的分布較為密集，距離激光雷達(dá)較遠(yuǎn)位置處點(diǎn)的分布較為稀疏。文獻(xiàn)[23]使用切比雪夫距離作為鄰域距離，但其點(diǎn)云數(shù)據(jù)是由深度攝像頭獲取，點(diǎn)云分布較均勻。文獻(xiàn)[24]在進(jìn)行體素化過程中使用切比雪夫距離作為有效計(jì)算距離。在此，為了尋找到合適的參數(shù)R，結(jié)合三維激光生成點(diǎn)云數(shù)據(jù)分布的特點(diǎn)，提出一種基于有序點(diǎn)云間的切比雪夫距離確定鄰域半徑的方法。

雖然點(diǎn)云里的點(diǎn)的排列順序并不影響其表達(dá)的內(nèi)容，但會影響點(diǎn)與點(diǎn)之間的距離計(jì)算。為了統(tǒng)一計(jì)算點(diǎn)間的距離，需要將點(diǎn)云按照一定規(guī)則排序。已知點(diǎn)云信息[xi,yi,zi],i=1,…,n,n為點(diǎn)云總數(shù)。先以x維度的數(shù)值為基準(zhǔn)進(jìn)行排序，數(shù)值從低到高，若x維度上的數(shù)值相同，則以y維度上的數(shù)值為基準(zhǔn)進(jìn)行排序，若y維度上的數(shù)值相同，則以z軸上數(shù)值為基準(zhǔn)進(jìn)行排序，實(shí)際的點(diǎn)云數(shù)據(jù)不存在2個位置完全一樣的點(diǎn)。將排好序的點(diǎn)云列表按照式(1)點(diǎn)i與點(diǎn)i+1之間計(jì)算出切比雪夫距離di。對di求其平均值d，并按照式(2)求其標(biāo)準(zhǔn)差σ。

di=max(|xi+1-xi|,|yi+1-yi|,|zi+1-zi|)

(1)

(2)

最終的鄰域半徑為R=d+σ。相較于常用的歐式距離，切比雪夫距離更長些，所囊括的范圍更廣，適用于高維空間環(huán)境。

3.3 實(shí)驗(yàn)對比及分析

為了驗(yàn)證本文改進(jìn)DBSCAN算法的效果，將用本文改進(jìn)的DBSCAN算法與經(jīng)典DBSCA算法進(jìn)行對比測試，并且還與k-means算法進(jìn)行對比測試。通過第3節(jié)處理后的點(diǎn)云數(shù)據(jù)，根據(jù)3.2節(jié)的方法，計(jì)算得到鄰域半徑R=0.083。對于經(jīng)典的DBSCAN算法的鄰域半徑R的選擇，參考文獻(xiàn)[21]中給出的建議，這里選取R=0.200。對于參數(shù)最少點(diǎn)數(shù)目minpoints的選取，其對效果影響不大，一般選取范圍5～8，這里都取minpoints=7。k-means算法、經(jīng)典DBSCA算法和改進(jìn)DBSCAN算法的效果分別如圖8、圖9和圖10所示，圖中的每一簇已框出標(biāo)識。

圖8 k-means算法聚類效果

圖9 經(jīng)典DBSCAN算法聚類效果

對比圖8與圖10，k-means算法把盆栽、立方體和桌子一部分歸為1簇。對比圖9與圖10，改進(jìn)DBSCAN算法聚類簇比經(jīng)典DBSCAN算法的多出了8個簇，意味著改進(jìn)DBSCAN算法聚類更精細(xì)，同時，經(jīng)典DBSCAN算法把盆栽和桌子歸為1簇，2個立方體通過桌子點(diǎn)云也被歸為1簇，說明經(jīng)典DBSCAN算法在非結(jié)構(gòu)場景聚類中表現(xiàn)差，而改進(jìn)DBSCAN算法能把盆栽、桌子和2個立方體各歸1簇，已達(dá)到精準(zhǔn)分割物體的作用。通過以上實(shí)驗(yàn)證明，在處理激光點(diǎn)云數(shù)據(jù)時，改進(jìn)DBSCAN算法相較于經(jīng)典的DBSCAN算法和k-means算法聚類分割更精細(xì)合理。

圖10 改進(jìn)DBSCAN算法聚類效果

3.4 障礙物的提取

為了使得聚類分割后的障礙物更加直觀，需要使用擬合包圍盒來產(chǎn)生具體的障礙物信息[25-27]，即經(jīng)過分割后得到場景中的一些障礙物，需要找出各個障礙物的邊界，擬合出邊界將各個障礙物框住，進(jìn)而達(dá)到障礙物提取的作用。在三維點(diǎn)云數(shù)據(jù)障礙物感知提取領(lǐng)域，實(shí)用且簡單的擬合包圍盒為MAR(minimum area rectangle)方法[28-29]。

經(jīng)過3.3節(jié)聚類處理后獲得了13個簇，由于篇幅有限，只對具有代表性的盆栽簇和立方體簇使用MAR方法進(jìn)行擬合包圍盒操作，實(shí)現(xiàn)障礙物提取，其效果如圖11和圖12所示。盆栽包圍盒的中心坐標(biāo)為[ 0.247 6,0.189 7,0.419 5]，長為0.495 1 m，寬為0.379 3 m，高為0.839 0 m，體積為0.157 6 m3。立方體包圍盒的中心坐標(biāo)為[0.294 6,0.871 5,0.149 4]，長為0.589 1 m，寬為0.275 4 m，高為0.298 7 m，體積為0.048 5 m3。

圖11 包圍盒提取的盆栽

圖12 包圍盒提取的立方體

4 聯(lián)合仿真實(shí)驗(yàn)

為了進(jìn)一步測試本文的所提出的方法在機(jī)械臂避障方面的應(yīng)用效果，本節(jié)使用CoppeliaSim機(jī)器人仿真軟件及MATLAB進(jìn)行聯(lián)合仿真實(shí)驗(yàn)。搭建常見的非結(jié)構(gòu)場景，使用UR5機(jī)械臂進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)?zāi)康氖歉鶕?jù)給定的起始點(diǎn)和目標(biāo)點(diǎn)信息，利用感知系統(tǒng)和處理算法獲取場景的信息，作為復(fù)合機(jī)器人的路徑規(guī)劃依據(jù)。

將獲取到的點(diǎn)云數(shù)據(jù)使用本文的地面分割方法進(jìn)行分割；經(jīng)過濾波操作后，再使用本文的改進(jìn)DBSCAN算法進(jìn)行聚類，如圖13所示；最后使用包圍盒進(jìn)行物體的提取，如圖14所示。

圖13 改進(jìn)DBSCAN算法聚類效果

圖14 提取場景物體效果

利用提取到的場景信息，使用Informed RRT*算法進(jìn)行路徑規(guī)劃，對規(guī)劃出的路徑信息進(jìn)行逆運(yùn)動學(xué)求解以驅(qū)動仿真環(huán)境中的機(jī)器人伺服。機(jī)械臂的避障運(yùn)動效果如圖15所示，圖中曲線為機(jī)械臂末端運(yùn)動軌跡。記錄運(yùn)動過程中機(jī)械臂的6個關(guān)節(jié)的角度數(shù)據(jù)，并利用MATLAB將數(shù)據(jù)可視化出來，如圖16所示。 由圖16可知，機(jī)械臂關(guān)節(jié)角度變化平緩無突變，說明機(jī)械臂運(yùn)動姿態(tài)穩(wěn)定。總體而言，在按照規(guī)劃的路徑運(yùn)動時，機(jī)械臂姿態(tài)平穩(wěn)，效果達(dá)到預(yù)期。

圖15 復(fù)合機(jī)器人避障效果

圖16 機(jī)械臂關(guān)節(jié)角度變化

5 結(jié)束語

精準(zhǔn)提取非結(jié)構(gòu)場景中的障礙物是機(jī)械臂進(jìn)行可靠避障的基礎(chǔ)。為了達(dá)到精確提取障礙物，本文進(jìn)行了激光點(diǎn)云的研究和相關(guān)的處理算法的操作，驗(yàn)證了基于有序點(diǎn)云間的切比雪夫距離確定DBSCAN算法的鄰域半徑參數(shù)方法的可行性，以及其相較于經(jīng)典的DBSCAN算法具有的優(yōu)良性。同時，在進(jìn)行地面分割操作過程中，發(fā)現(xiàn)RANSAC算法分割地面效果不佳的情況，設(shè)計(jì)了一種基于橫截面的地面分割方法，其原理簡單易懂，操作使用方便，且仿真表現(xiàn)效果良好，也為后續(xù)的聚類分割處理達(dá)到預(yù)期效果提供了保障。