基于高斯噪聲發(fā)散的協(xié)作機器人路徑優(yōu)化及避障

胡玉蝶 ，周 勇，王宇琦 ，李衛(wèi)東

(1.武漢理工大學 交通與物流工程學院，湖北 武漢 430063；2.上海理工大學 機械工程學院，上海 200093)

0 引言

在人機協(xié)作領域，示教學習是廣泛使用的一種方法，它能使機器人在不同環(huán)境下從人類演示中學習、概括并為新環(huán)境再現(xiàn)解決方案[1-3]。高斯混合模型、高斯混合回歸、隱馬爾可夫模型、隱半馬爾可夫模型以及動態(tài)運動基元等都是實現(xiàn)示教學習的主要智能技術[4-6]。

高斯混合模型對于描述和擬合生成數(shù)據(jù)具有先天優(yōu)勢，在一定程度上被稱為概率密度的萬能近似器[7]。它具有很強的編碼能力和噪聲處理能力，魯棒性好，能夠處理高維問題，對于連續(xù)復雜軌跡具有很強的編碼再現(xiàn)能力[8]，且相對同類模型運算速度更快，這些特點使它成為眾多研究人員開展示教學習研究的優(yōu)選方案。近年來，高斯混合模型和高斯混合回歸已經(jīng)被一些學者研究，并成功用于人機協(xié)作的機器人學習[9-11]。然而，對于高斯混合模型和高斯混合回歸應用于人機協(xié)作的路徑規(guī)劃目前主要存在兩個問題：

(1)回歸路徑的平滑性問題 回歸路徑的質量會影響到機器人的運動，而其很大程度上取決于示教數(shù)據(jù)本身的質量。當數(shù)據(jù)中包含因人為操作而產(chǎn)生的不利特征時，模型會擬合這些不利特征，導致回歸路徑曲折，從而影響機器人的任務質量。如劉成菊等[12]利用高斯混合模型以及高斯混合回歸對書寫軌跡進行學習，該回歸路徑相較于原始的示教軌跡的平滑度有所提升，但不可避免地保留了一些細微的曲折。類似地，DAVID等[13]基于高斯混合模型以及高斯混合回歸的示教學習來學習裝配任務，其再現(xiàn)的裝配軌跡存在多個多余的拐角，無法平穩(wěn)地驅動機器人。研究表明，高斯混合回歸具有一定的抗干擾性，但仍然難以避免學習示教數(shù)據(jù)的一些不利特征，如人為產(chǎn)生的抖動或者數(shù)據(jù)過于分散，從而導致回歸路徑出現(xiàn)冗余或尖銳曲折，這種不平滑的路徑會對機器人運動產(chǎn)生沖擊，同時也會增加機器人的運行時間。

(2)回歸路徑的避障問題 協(xié)作機器人避障路徑規(guī)劃一直是一大關鍵研究點[14-15]，其中很重要的一方面是研究高斯混合模型/回歸的避障問題。如KYRARINI等[16]通過改變特定高斯簇的均值矩陣來調整高斯簇的位置以實現(xiàn)高斯混合模型的重構，從而引導高斯混合回歸進行避障處理。LIN等[17]還對被障礙物影響的高斯簇尺寸和形狀進行變換，基于高斯簇的新形態(tài)獲取更靈活的避障回歸路徑。上述局部調整高斯簇的方法需要人為多次嘗試，且新的回歸路徑的避障區(qū)域存在曲折。此外，對高斯簇操作不當會導致回歸路徑失敗或發(fā)生較大變化，可能帶來新的威脅。另一些研究通過整體調整高斯混合模型來實現(xiàn)避障，如MOK等[18]利用高斯混合模型對障礙物進行建模，并根據(jù)環(huán)境變化實時修改模型，利用高斯分布函數(shù)推導潛在向量來設計避障軌跡。然而，這種方法對障礙物的建模是二維的，路徑同樣會出現(xiàn)曲折，平滑度需要進一步提高。也有研究者通過重構高斯混合模型的數(shù)據(jù)分布來達到避障的目的，如LI等[19]通過對數(shù)據(jù)的重采樣來擴充示教數(shù)據(jù)量，基于高斯混合模型/回歸方法對障礙物所在的高斯簇進行特征值分解、劃分平面計算以及數(shù)據(jù)點分組刪除等操作來得到避障路徑。但是該方法增加了數(shù)據(jù)量，從而增大了計算機的計算負荷，此外，該方法的劃分平面不唯一，平面數(shù)量會隨著被影響的高斯簇個數(shù)的增加而呈指數(shù)增長，且其中存在部分不合理的平面會導致避障失敗。因此，應當在確保避障質量的前提下，盡可能減少人為干涉，降低計算復雜度。

針對回歸路徑的平滑性問題，本文提出一種基于高斯噪聲發(fā)散的路徑優(yōu)化策略，利用高斯分布產(chǎn)生的隨機值對示教數(shù)據(jù)進行發(fā)散，模糊示教數(shù)據(jù)中的抖動特征，從而提高路徑質量。針對回歸路徑的避障問題，本文基于優(yōu)化路徑的數(shù)據(jù)點云，提出一種半貫穿式船型消隱方法，通過對被障礙物影響的發(fā)散數(shù)據(jù)進行船型消隱，形成類拱橋形結構的數(shù)據(jù)點云，引導高斯簇在障礙物的上方形成，從而使回歸路徑自適應避開障礙物區(qū)域。相比于現(xiàn)有的避障方法，該方法計算簡單，結果唯一，且避障路徑平滑。

1 算法原理

1.1 高斯混合模型

高斯混合模型是多個高斯分布乘以對應權重線性組合而成的模型，高斯混合模型概率密度函數(shù)以及單個高斯分布密度函數(shù)分別如式(1)和式(2)所示：

(1)

(2)

高斯混合模型的求解過程如下：

(1)根據(jù)數(shù)據(jù)初始化高斯混合模型參數(shù)(ωi,μi,Σi)；

(2)E—步。計算第j個樣本pj由第i個高斯混合模型生成的后驗概率γji：

(3)

3)M—步。根據(jù)E—步計算(ωi,μi,Σi)：

(4)

(5)

。

(6)

(4)重復步驟(2)、(3),直至算法收斂，得到最終參數(shù)。

1.2 高斯混合回歸

高斯混合回歸相比一般回歸方法在高維數(shù)據(jù)上具有更好的穩(wěn)健性，它是通過構建高斯混合模型，從該模型中推出每個高斯條件概率密度以及回歸函數(shù)，從而構建回歸模型。對于協(xié)作機器人示教數(shù)據(jù)，本文構建了一組時間序列作為輸入變量，即t=[t1,t2,…,tm]T，則示教數(shù)據(jù)點可以改寫為[(t1,p1),(t2,p2),…,(tm,pm)]，其中pj=[xj,yj,zj]T為輸出變量。則二者的聯(lián)合概率密度函數(shù)為：

(7)

其中:

(8)

mi(t)=μip+Σipt·Σitt-1·(t-μit);

(9)

covi=Σipp-Σipt·Σitt-1·Σitp。

(10)

二者的邊緣概率密度以及條件概率密度分別如式(11)和式(12)所示：

(11)

(12)

其中

(13)

則高斯混合回歸的表達式為：

(14)

1.3 高斯噪聲發(fā)散

人工示教由于人行為的不確定性很難保證獲取高質量的示教數(shù)據(jù)。一方面，很難控制示教數(shù)據(jù)之間的距離，即可能出現(xiàn)示教數(shù)據(jù)過于分散的情況；另一方面，在示教過程中不可避免地會出現(xiàn)抖動，導致示教數(shù)據(jù)存在不利的曲折特征。以上因素都有可能造成回歸路徑過擬合即過于曲折。這里提出一種高斯發(fā)散的方法，即在原有示教數(shù)據(jù)的基礎上，使用高斯噪聲使其變?yōu)楸舜讼嗳莸狞c云。高斯噪聲發(fā)散的具體公式如下：

(15)

其中:xj、yj和zj是原始示教數(shù)據(jù)中第j個點的三維坐標值；x′j、y′j和z′j是經(jīng)過高斯發(fā)散處理后的點云三維坐標值；而rx，ry，rz分別是X，Y，Z維度服從高斯分布的隨機噪聲值,

(16)

其中：rx～N(μx-noise,σx-noise)表示X維度噪聲值rx服從于均值為μx-noise、方差為σx-noise的高斯分布，ry以及rz定義與其類似，高斯分布的具體表達式如下所示：

(17)

由于高斯分布數(shù)據(jù)生成的隨機性，原示教數(shù)據(jù)會隨機發(fā)散，使不同示教路徑數(shù)據(jù)產(chǎn)生交集，從而形成數(shù)據(jù)點云?；诟咚乖肼暟l(fā)散的方法具有以下兩個特點：

(1)通過調整X，Y，Z維度對應的噪聲值rx,ry,rz可以控制數(shù)據(jù)的發(fā)散區(qū)域，如針對三維空間中某一維度進行發(fā)散或者針對示教數(shù)據(jù)的某個區(qū)域進行發(fā)散，而將無需發(fā)散區(qū)域所包含數(shù)據(jù)點的3個維度的噪聲值設置為0，從而保證這些區(qū)域特征完全保留。

(2)針對示教數(shù)據(jù)的分散/曲折情況，可以通過調整高斯噪聲參數(shù)的值來調整發(fā)散的程度。其中[μx-noise,μy-noise,μz-noise]為偏移參數(shù)，用于控制數(shù)據(jù)發(fā)散時在3個維度的偏移，當回歸路徑偏移到理想?yún)^(qū)域時可以控制該參數(shù)來調整位置；[σx-noise,σy-noise,σz-noise]為發(fā)散參數(shù)，用于控制數(shù)據(jù)在3個維度的發(fā)散程度，該取值越大，數(shù)據(jù)發(fā)散程度越大，當數(shù)據(jù)較為發(fā)散或者抖動較為劇烈時，可以選擇較大的發(fā)散參數(shù)。

1.4 自適應避障

避障在路徑規(guī)劃問題中的地位舉足輕重，在人機協(xié)作中，可以根據(jù)障礙物所處的位置執(zhí)行避障示教，從而獲取避障回歸路徑。但該方法并不具有自適應性，即對于固定障礙物是可行的，但是當障礙物的位置隨機或不確定時，就會大大增加示教工作量，而且障礙物一旦移動，示教就需要重新進行，從而影響該方法的實時性和實用性。本文在高斯噪聲發(fā)散的基礎上，提出一種基于半貫穿船型點云消隱的避障策略。避障原理示意圖如圖1所示。

圖1所示的消隱區(qū)域主要包括兩個輔助圓消隱區(qū)及一個外接矩形消隱區(qū)，具體消隱算法的步驟如下：

(1)使用3D相機獲取障礙物的外接矩形上邊界頂點OL、OR點的位置坐標。

(2)在外接矩形左邊界和右邊界向下延長線上分別遍歷搜索，確定位于左右邊界線延長線方向上距離OL、OR的最遠點，即圖1中O1和O2點的位置坐標。

(3)以O1和O2為圓心，O1OL、O2OR為半徑分別作左輔助圓和右輔助圓，將位于左右輔助圓以及外接矩形內(nèi)所有的數(shù)據(jù)點進行消隱處理。

(4)對消隱后的數(shù)據(jù)點進行高斯混合模型重構，引導高斯混合回歸生成避障回歸路徑。

本文提出的半貫穿船型的點云消隱方法，一方面可以避免障礙物上下方都有點云留存，導致避障失敗，如圖2所示。另一方面，船型的消隱方式可以在障礙物上方形成一個類拱橋形的數(shù)據(jù)分布，易于引導高斯混合模型在障礙物上方形成高斯簇，從而使回歸路徑避開障礙物。對于尺寸較大的障礙物，可以在執(zhí)行消隱操作前，對原發(fā)散數(shù)據(jù)進行二次發(fā)散，進一步擴大點云覆蓋面積，避免消隱剩余點云不足而形成高斯簇，進而導致避障失敗。

2 實驗分析

2.1 高斯發(fā)散

本節(jié)基于UR5機器人、邁德威視雙目結構光相機、大寰AG-95電動夾爪來進行實驗研究，以驗證本文方法的有效性。通過人工多次拖拽UR5機器人的執(zhí)行端完成從不同起點搬運物體到同一終點的示教，實驗平臺以及示教操作如圖3所示，獲取的末端示教數(shù)據(jù)如圖4a所示，利用上文中高斯發(fā)散的方法，采取[0，6，0，6，0，4]的參數(shù)對示教數(shù)據(jù)進行發(fā)散處理，處理后得到的數(shù)據(jù)點云如圖4b所示。

高斯簇的個數(shù)會影響到最終回歸路徑的質量，高斯簇過少，會導致路徑丟失基本特征即欠擬合；而高斯簇個數(shù)過多，則回歸路徑會保留路徑中多余的不利特征，導致路徑曲折。為了獲得合理的高斯簇個數(shù)，本文采取廣泛使用的貝葉斯信息準則(Bayesian Information Criterion, BIC)[20]作為高斯簇個數(shù)的選取原則，對于原始示教數(shù)據(jù)以及發(fā)散數(shù)據(jù)的不同高斯混合模型的BIC計算結果如圖5所示。

其中未發(fā)散數(shù)據(jù)的BIC最低分數(shù)對應的最佳高斯簇個數(shù)為18，經(jīng)過高斯發(fā)散處理后的數(shù)據(jù)的最佳高斯簇個數(shù)為5，根據(jù)計算結果生成的回歸曲線分別如圖6a和圖6b所示。從圖中可以看出，未經(jīng)發(fā)散處理計算出來的回歸路徑相比發(fā)散處理之后的回歸路徑保留了很多不利的曲折，這些不利特征在驅動機器人時，會使機器人在速度或者加速度上產(chǎn)生突變，從而對機器人結構造成沖擊。

為了進一步量化說明，采用曲率來對兩條路徑進行比較，曲率越大表明彎曲越劇烈，即路徑比較曲折。以X-Y平面為例，兩條回歸路徑的曲率計算結果如圖7b所示。

從圖7a可以看出，經(jīng)過高斯發(fā)散處理的數(shù)據(jù)基本沒有存在冗余偏折的地方，整體都非常平滑，而原始示教數(shù)據(jù)的回歸路徑在路徑起始段與結束段都存在著尖銳曲折，故本文提出的方法使示教數(shù)據(jù)在BIC計算中更容易收斂，且回歸路徑也更為光滑，進一步驗證了該方法在改善回歸質量方面的有效性。

2.2 自適應避障

對于避障而言，這里以X-Y平面避障為例進行說明。圖8展示了數(shù)據(jù)點消隱策略對應的數(shù)據(jù)處理、高斯混合建模以及回歸曲線生成的仿真圖。由圖可以看出，重構的高斯簇避開了被障礙物影響的區(qū)域，生成的避障路徑整體非常平滑，不存在尖銳的曲折等缺陷。

根據(jù)1.4節(jié)仿真流程，分別以位于同一路徑上半徑(R)為5 mm、10 mm和15 mm的圓形障礙物進行避障仿真，避障路徑與原始路徑對比結果如圖9所示，為了清晰顯示仿真結果，這里對縱坐標進行了比例放大處理，其中虛線為避障路徑，實線為原始路徑，圓形為障礙物。針對不同尺寸障礙物的避障最小距離(避障軌跡距離障礙物最近距離)如表1所示。

表1 不同障礙物的避障距離 mm

當障礙物的尺寸半徑達到20 mm時，障礙物邊界與點云上下邊界持平，如圖10a所示，實施消隱后，避障區(qū)域沒有足夠數(shù)據(jù)支持高斯簇形成，會導致避障失敗。此時，可以對障礙物所在區(qū)域的點云進行二次發(fā)散，擴大該區(qū)域點云覆蓋范圍，同時不會影響其他區(qū)域的數(shù)據(jù)分布，如圖10b所示，再進行消隱，最后的避障軌跡如圖10c所示。

為了實際測試避障效果，使用3D相機及UR5機器人進行避障實驗，根據(jù)本文方法計算的三維避障路徑如圖11所示，利用回歸路徑驅動機器人的實驗過程如圖12所示，其中固體膠棒為障礙物，圖12a為正面拍攝避障照片，圖12b為側面拍攝避障照片。實驗表明，本文提出的方法可以使機器人平順地避開障礙物。

3 結束語

本文提出一種基于高斯噪聲數(shù)據(jù)預處理以及點云消隱的自適應避障方法，在不增加數(shù)據(jù)樣本數(shù)量的情況下，能有效改善由高斯混合模型/回歸生成的回歸路徑的平滑度。在此基礎上，基于障礙物位置尺寸等信息對發(fā)散的數(shù)據(jù)點云進行消隱操作，并重構高斯混合模型，進而引導回歸路徑的自適應生成。本文方法對于示教區(qū)域大，但示教次數(shù)少，即數(shù)據(jù)較為分散的情況效果明顯，在擴大示教區(qū)域的同時，能有效減少人為示教的不確定性導致的路徑冗余偏折，整體提高了示教學習的質量，降低了對示教質量的要求。此外，在無需二次示教的情況下能自適應生成平滑避障路徑，整體操作簡單且計算量小。所提出的高斯發(fā)散方法其參數(shù)是基于手動設置，效果易受人員主觀影響。后續(xù)工作將研究不同發(fā)散參數(shù)對結果的影響規(guī)律，利用深度學習或強化學習等手段自動選擇最優(yōu)參數(shù)組合，在減少人工干預的同時達到優(yōu)化效果。