基于視覺(jué)的平面關(guān)節(jié)機(jī)器人避障路徑規(guī)劃

，

(哈爾濱工業(yè)大學(xué)(深圳) 機(jī)電工程及自動(dòng)化學(xué)院，廣東 深圳 518000)

0 引言

工業(yè)機(jī)器人在工業(yè)生產(chǎn)中的廣泛應(yīng)用極大地提高了生產(chǎn)效率。在2015年，我國(guó)提出“中國(guó)制造2025”更是將機(jī)器人作為十大重點(diǎn)發(fā)展領(lǐng)域之一。機(jī)械臂作為目前工業(yè)上應(yīng)用最為廣泛的工業(yè)機(jī)器人，在汽車(chē)制造、焊接和碼垛等領(lǐng)域大范圍普及。在焊接、碼垛等應(yīng)用場(chǎng)景中，機(jī)械臂的工作空間中一般沒(méi)有障礙物存在，通過(guò)示教方式就可以實(shí)現(xiàn)點(diǎn)到點(diǎn)的運(yùn)動(dòng)。隨著智能制造概念的普及，工業(yè)機(jī)器人被用在越來(lái)越多的場(chǎng)景中，機(jī)器人的工作環(huán)境越來(lái)越復(fù)雜。在鋰電池制造行業(yè)，有一道工藝是將薄片電池疊在一起，通常這一工藝是由疊片機(jī)完成，平面關(guān)節(jié)機(jī)器人(SCARA)可以在疊片機(jī)內(nèi)完成疊放薄片電池的步驟。然而疊片機(jī)內(nèi)部還有其他機(jī)構(gòu)，且空間緊湊，因此要想順利將平面關(guān)節(jié)機(jī)器人應(yīng)用到疊片機(jī)內(nèi)，還需要具備避障路徑規(guī)劃的功能。

避障路徑規(guī)劃的目的是在給定起始點(diǎn)和目標(biāo)點(diǎn)以及障礙物信息的情況下，規(guī)劃出一條無(wú)碰撞到達(dá)目標(biāo)點(diǎn)位的路徑。關(guān)于避障路徑規(guī)劃，目前已經(jīng)提出了許多方法，主要分為傳統(tǒng)算法和智能算法2類[1]。傳統(tǒng)算法包括C空間法、人工勢(shì)場(chǎng)法和A*算法等，智能算法包括遺傳算法和粒子群算法等[2-6]。

平面關(guān)節(jié)機(jī)器人由于其在運(yùn)動(dòng)過(guò)程中執(zhí)行機(jī)構(gòu)一般與XOY平面保持垂直，因此空間路徑規(guī)劃問(wèn)題可以簡(jiǎn)化為平面路徑規(guī)劃問(wèn)題。除此之外，平面關(guān)節(jié)機(jī)器人的工作空間范圍有限，地圖信息構(gòu)建較為容易。針對(duì)平面關(guān)節(jié)機(jī)器人的應(yīng)用場(chǎng)景，結(jié)合機(jī)器視覺(jué)技術(shù)，從相機(jī)中獲取機(jī)器人工作平面的信息并構(gòu)建地圖；利用圖像分割技術(shù)實(shí)現(xiàn)工作平面上的障礙物輪廓提取，做為路徑規(guī)劃問(wèn)題的約束條件；同時(shí)利用改進(jìn)的A*避障路徑算法，實(shí)現(xiàn)可控安全距離的避障路徑搜索；最后在樣機(jī)上完成了算法的驗(yàn)證。

1 樣機(jī)模型

實(shí)驗(yàn)采用的平面關(guān)節(jié)機(jī)器人為吊裝型SCARA，如圖1所示。

圖1 實(shí)驗(yàn)用吊裝SCARA

樣機(jī)的D-H參數(shù)如表1所示。

表1 樣機(jī)的D-H參數(shù)

2 機(jī)器人工作空間地圖信息的建立

實(shí)現(xiàn)機(jī)器人避障的前提是獲取工作空間的地圖信息，構(gòu)建地圖信息的方式是將地圖柵格化，柵格地圖中的柵格分為自由柵格和障礙物柵格，避障路徑規(guī)劃的目的就是尋找到一條由自由柵格連接而成的從起始點(diǎn)位到目標(biāo)點(diǎn)位的路徑。一般構(gòu)建柵格地圖的方法是對(duì)環(huán)境進(jìn)行數(shù)學(xué)建模，然而障礙物的輪廓往往不是規(guī)則的矩形，為了簡(jiǎn)化模型，需要將障礙物的輪廓用矩形組合表示。障礙物輪廓的矩形組合表示存在將自由柵格標(biāo)記為障礙物柵格的情況，為了更精確地構(gòu)建地圖信息，利用相機(jī)獲取機(jī)器人工作空間的環(huán)境信息，再通過(guò)圖像處理技術(shù)，提取障礙物的位置信息，進(jìn)而實(shí)現(xiàn)地圖信息的建立。

2.1 視覺(jué)系統(tǒng)構(gòu)建

根據(jù)相機(jī)的安裝位置，可以將視覺(jué)系統(tǒng)分為ETH(eye to hand)形式和EIH(eye in hand)形式。ETH形式的視覺(jué)系統(tǒng)是將相機(jī)安裝在固定的支架上，在SCARA運(yùn)動(dòng)的過(guò)程中，相機(jī)的位置不發(fā)生變化，這種安裝方式的相機(jī)坐標(biāo)系到世界坐標(biāo)系的映射矩陣是固定的；EIH形式的視覺(jué)系統(tǒng)是將相機(jī)安裝在SCARA上，因此在SCARA運(yùn)動(dòng)過(guò)程中，相機(jī)的位姿也相對(duì)SCARA的基座不斷變化，這種安裝方式需要?jiǎng)討B(tài)更新相機(jī)坐標(biāo)系到世界坐標(biāo)系的映射矩陣。

為方便相機(jī)安裝及提高路徑規(guī)劃的速度，采用ETH形式安裝相機(jī)，將相機(jī)安裝在機(jī)架上，圖2所示為硬件系統(tǒng)的組成。

圖2 硬件系統(tǒng)示意

2.2 手眼標(biāo)定[7

手眼標(biāo)定的目的是確定世界坐標(biāo)系和像素坐標(biāo)系的轉(zhuǎn)換矩陣，是將障礙物位置信息從像素坐標(biāo)系轉(zhuǎn)換到世界坐標(biāo)系的前提。手眼標(biāo)定涉及到的坐標(biāo)系主要有3個(gè)，分別是世界坐標(biāo)系、相機(jī)坐標(biāo)系和像素坐標(biāo)系。其中世界坐標(biāo)系通常是以基座為原點(diǎn)，SCARA工作平面為XOY平面構(gòu)建的坐標(biāo)系；相機(jī)坐標(biāo)系是以相機(jī)光軸為Z方向，以光心為原點(diǎn)構(gòu)建的坐標(biāo)系；像素坐標(biāo)系是光軸與成像平面交點(diǎn)為圓心，成像平面為XOY平面，以像素為單位構(gòu)成的二維坐標(biāo)系。相機(jī)坐標(biāo)系和像素坐標(biāo)系的關(guān)系如圖3所示。

圖3 相機(jī)坐標(biāo)系與像素坐標(biāo)系

世界坐標(biāo)系到相機(jī)坐標(biāo)系的變換為剛體變換，可通過(guò)旋轉(zhuǎn)和平移表示[8-9]。

(1)

其中，R為旋轉(zhuǎn)矩陣;T為平移向量。旋轉(zhuǎn)和平移共同構(gòu)成外參矩陣。

相機(jī)坐標(biāo)系到像素坐標(biāo)系的轉(zhuǎn)換主要是縮放變換，根據(jù)相機(jī)的針孔模型可得到相機(jī)坐標(biāo)系到像素坐標(biāo)系的轉(zhuǎn)換關(guān)系。

(2)

Mc為相機(jī)的內(nèi)參矩陣。

結(jié)合式(1)和式(2)，可以得到世界坐標(biāo)到像素坐標(biāo)的轉(zhuǎn)換關(guān)系。

(3)

將內(nèi)參矩陣和外參矩陣融合為一個(gè)3×4的矩陣后，將式(3)展開(kāi)得到

(4)

矩陣包含12個(gè)未知參數(shù)，求解需要至少12個(gè)方程，但是注意到由于SCARA的工作平面一般是固定的，在世界坐標(biāo)系建立的過(guò)程中，可以將工作平面的Z坐標(biāo)設(shè)為零；除此之外，在相機(jī)安裝過(guò)程中，保證相機(jī)光軸與工作平面盡量垂直，則工作平面上的點(diǎn)在相機(jī)坐標(biāo)系下的Z坐標(biāo)也為一常數(shù)。根據(jù)這2個(gè)假設(shè)條件，可以將式(4)簡(jiǎn)化，得到世界坐標(biāo)系下XOY平面的點(diǎn)到像素坐標(biāo)系的映射關(guān)系。

(5)

矩陣的未知參數(shù)有9個(gè)，需要至少9個(gè)方程來(lái)求解。M為世界坐標(biāo)系中XOY平面上的點(diǎn)到像素坐標(biāo)系的映射矩陣，M-1則為像素坐標(biāo)系到世界坐標(biāo)系XOY平面上點(diǎn)的映射矩陣。標(biāo)定工具選擇棋盤(pán)格，通過(guò)OpenCV獲取相機(jī)圖片中的棋盤(pán)格角點(diǎn)坐標(biāo)和棋盤(pán)格角點(diǎn)在世界坐標(biāo)系下的坐標(biāo)值，可以得到一組矩陣方程，最終求解映射矩陣。

2.3 障礙物輪廓提取

手眼標(biāo)定實(shí)現(xiàn)了像素坐標(biāo)系和世界坐標(biāo)系之間的相互轉(zhuǎn)換，相機(jī)能夠獲取SCARA工作平面的環(huán)境信息，圖片中的障礙物是由一系列像素點(diǎn)組成的，可以通過(guò)手眼標(biāo)定矩陣將圖像中的障礙物像素點(diǎn)位轉(zhuǎn)換為世界坐標(biāo)系下的坐標(biāo)。

障礙物的像素點(diǎn)位在不考慮障礙物重疊的情況下，是由互相獨(dú)立的區(qū)域組成的，對(duì)于避障路徑規(guī)劃任務(wù)而言，障礙物的輪廓本身已經(jīng)屬于障礙物柵格的點(diǎn)位，輪廓為封閉曲線，因此輪廓內(nèi)部的點(diǎn)位也不可能到達(dá)。根據(jù)這一特點(diǎn)，只需要將障礙物的輪廓從背景中提取出來(lái)，就能得到完整的工作平面環(huán)境信息。

障礙物輪廓提取最常用的方式就是對(duì)圖像進(jìn)行邊緣檢測(cè)[10-11]，但直接進(jìn)行邊緣檢測(cè)的方式要求圖片的質(zhì)量較高，在常規(guī)的應(yīng)用中，圖片的質(zhì)量往往達(dá)不到能直接提取邊緣的要求。為了能夠適應(yīng)更多的應(yīng)用場(chǎng)景，決定先對(duì)圖像進(jìn)行分割，預(yù)先將物體和輪廓區(qū)分開(kāi)，增強(qiáng)物體邊緣，再提取物體輪廓。根據(jù)這一處理步驟，可以從圖4a所示工作平面原圖得到圖4b所示輪廓。

圖4 圖片提取障礙物輪廓

物體的輪廓被較好地從背景中提取出來(lái)，輪廓的像素點(diǎn)位即障礙物點(diǎn)位，在路徑規(guī)劃過(guò)程中，這些點(diǎn)位不可到達(dá)。避障路徑規(guī)劃可以將像素點(diǎn)位的環(huán)境信息通過(guò)手眼映射矩陣轉(zhuǎn)化為工作平面上的世界坐標(biāo)信息，進(jìn)而在世界坐標(biāo)系下進(jìn)行規(guī)劃；或是直接在像素坐標(biāo)系下進(jìn)行規(guī)劃，再映射到世界坐標(biāo)系中。本文采用在像素坐標(biāo)系下進(jìn)行規(guī)劃的方案。

3 避障路徑規(guī)劃

3.1 改進(jìn)A*算法的避障路徑規(guī)劃

A*算法屬于啟發(fā)式搜索算法，實(shí)現(xiàn)啟發(fā)式搜索路徑的關(guān)鍵是構(gòu)造每個(gè)節(jié)點(diǎn)的估價(jià)函數(shù)。

f(n)=g(n)+h(n)

(6)

f(n)為節(jié)點(diǎn)n的估價(jià)函數(shù)；g(n)為初始節(jié)點(diǎn)到節(jié)點(diǎn)n的實(shí)際代價(jià)；h(n)是節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的最佳路徑代價(jià)。A*算法搜索路徑的過(guò)程中采用貪心策略，即每次都選擇估價(jià)函數(shù)最小的節(jié)點(diǎn)作為下一節(jié)點(diǎn)。在路徑搜索前，障礙物節(jié)點(diǎn)的估價(jià)函數(shù)被初始化為無(wú)窮大，因此障礙物節(jié)點(diǎn)不會(huì)被選擇。估價(jià)函數(shù)的構(gòu)造形式直接影響A*算法的搜索效率，實(shí)際代價(jià)的構(gòu)造與路徑距離的計(jì)算方式有關(guān)。在柵格化地圖中，路徑距離的計(jì)算方式可以分為Manhattan距離和Euler距離，Manhattan距離是移動(dòng)起點(diǎn)和移動(dòng)終點(diǎn)的X和Y方向坐標(biāo)差值的絕對(duì)值之和，即

gM=|x2-x1|+|y2-y1|

(7)

Euler距離是移動(dòng)起點(diǎn)和移動(dòng)終點(diǎn)的連線距離，即

(8)

(x1,y1)為移動(dòng)的起點(diǎn)；(x2,y2)為移動(dòng)的終點(diǎn)。

最佳路徑代價(jià)可表示為目標(biāo)節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)的Manhattan距離，即

h(n)=|xd-xn|+|yd-yn|

(9)

根據(jù)A*算法搜索到的路徑是由一系列的節(jié)點(diǎn)連接而成的折線，在圖片中表現(xiàn)為一系列的像素點(diǎn)連線，圖5是以圖4a為基本環(huán)境信息得到的避障路徑。

一系列的路徑節(jié)點(diǎn)能準(zhǔn)確避開(kāi)障礙物前進(jìn)，但是對(duì)于SCARA的軌跡規(guī)劃而言，過(guò)多的路徑節(jié)點(diǎn)會(huì)極大提升速度規(guī)劃的難度，因此需要對(duì)A*算法進(jìn)行改進(jìn)。注意到規(guī)劃出的路徑節(jié)點(diǎn)中有相當(dāng)一部分節(jié)點(diǎn)屬于冗余節(jié)點(diǎn)，冗余節(jié)點(diǎn)是指剔除后對(duì)避障功能無(wú)實(shí)際影響的節(jié)點(diǎn)。判斷冗余節(jié)點(diǎn)的方法是通過(guò)判斷某一節(jié)點(diǎn)前后節(jié)點(diǎn)的連線是否會(huì)與障礙物干涉來(lái)實(shí)現(xiàn)的，若節(jié)點(diǎn)前后節(jié)點(diǎn)的連線不與障礙物干涉，則說(shuō)明節(jié)點(diǎn)剔除后不影響路徑的避障功能，為冗余節(jié)點(diǎn)；反之，說(shuō)明為非冗余節(jié)點(diǎn)[12]。改進(jìn)后的A*避障路徑如圖6所示。從圖6中可以看到，原先一系列的節(jié)點(diǎn)最終被優(yōu)化為4個(gè)關(guān)鍵節(jié)點(diǎn)，4個(gè)關(guān)鍵節(jié)點(diǎn)連接而成的折線依然能夠滿足避障的要求。

圖5 A*算法的避障路徑

圖6 改進(jìn)A*算法的避障路徑

3.2 可控安全距離的避障路徑規(guī)劃

根據(jù)以上改進(jìn)A*算法路徑規(guī)劃得到的是一系列節(jié)點(diǎn)，雖然能夠?qū)崿F(xiàn)避障功能，但是節(jié)點(diǎn)到障礙物的距離是不可控的。由于算法中估價(jià)函數(shù)只包含實(shí)際代價(jià)函數(shù)和最佳路徑函數(shù)，采用貪心策略選取得到的節(jié)點(diǎn)一般是貼著障礙物輪廓的節(jié)點(diǎn)，在實(shí)際的應(yīng)用中，SCARA的末端執(zhí)行器的尺寸還需要考慮進(jìn)去，因此需要控制路徑上節(jié)點(diǎn)到障礙物的安全距離。

在構(gòu)建工作平面地圖信息時(shí)，直接將障礙物輪廓作為障礙物節(jié)點(diǎn)的情況下，若避障路徑需要繞開(kāi)障礙物，則節(jié)點(diǎn)將沿著障礙物輪廓分布。根據(jù)A*算法的這一特點(diǎn)，要實(shí)現(xiàn)安全距離的可控性，可以將障礙物輪廓沿曲線法向向外部擴(kuò)展，則擴(kuò)展的像素距離就能映射成為工作平面上的安全距離。圖7所示為原始輪廓與擴(kuò)展輪廓，從圖7可以看出，擴(kuò)展輪廓的形狀與原始輪廓一致，能保證節(jié)點(diǎn)到障礙物的最近距離為擴(kuò)展的安全距離。

圖7 原始輪廓與擴(kuò)展輪廓

將擴(kuò)展后的輪廓點(diǎn)位作為障礙物點(diǎn)位進(jìn)行避障路徑規(guī)劃，最終得到的路徑如圖8所示。

圖8 安全距離為20個(gè)像素距離的避障路徑

避障路徑規(guī)劃得到的一系列點(diǎn)位為路徑節(jié)點(diǎn)，如果直接將節(jié)點(diǎn)相連得到的將是一條折線，對(duì)于速度規(guī)劃而言難度較大，因此對(duì)折線進(jìn)行平滑處理，轉(zhuǎn)折處采用圓角過(guò)渡，最終得到的避障路徑如圖9所示。

圖9 采用圓角過(guò)渡的避障路徑

4 避障算法對(duì)比

為對(duì)比不同算法在避障路徑規(guī)劃上的性能，隨機(jī)選取10對(duì)起點(diǎn)和終點(diǎn)進(jìn)行避障路徑規(guī)劃。在都能實(shí)現(xiàn)避障功能的前提下，將避障路徑的長(zhǎng)度作為避障算法性能比較的主要依據(jù)，得到的結(jié)果如圖10所示。

圖10 不同避障算法對(duì)比

通過(guò)路徑長(zhǎng)度對(duì)比可以發(fā)現(xiàn)，A*算法規(guī)劃的路徑長(zhǎng)度最長(zhǎng)，改進(jìn)A*算法可以減少路徑中的冗余點(diǎn)，進(jìn)而縮短路徑長(zhǎng)度。圓角過(guò)渡對(duì)路徑長(zhǎng)度的影響幾乎可以忽略。10次隨機(jī)路徑規(guī)劃中，改進(jìn)A*算法平均縮短3.47%的長(zhǎng)度，圓角過(guò)渡后平均縮短3.55%的長(zhǎng)度。

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

針對(duì)平面關(guān)節(jié)機(jī)器人在工作平面作業(yè)時(shí)需要進(jìn)行避障路徑規(guī)劃的情況，利用視覺(jué)信息構(gòu)建工作平面的環(huán)境信息，并采用圖像處理技術(shù)，將安全距離映射為安全像素距離，擴(kuò)展障礙物輪廓，最后利用改進(jìn)的A*算法實(shí)現(xiàn)了避障路徑規(guī)劃。

利用視覺(jué)信息，將工作平面上的障礙物表示為圖片中的輪廓像素點(diǎn)，完成了柵格地圖的創(chuàng)建；利用圖像處理技術(shù)，首先將安全距離映射為安全像素距離，再將障礙物輪廓向外擴(kuò)展安全像素距離個(gè)單位，保證在路徑規(guī)劃過(guò)程中節(jié)點(diǎn)到障礙物的最小距離不大于安全像素距離；改進(jìn)的A*算法剔除了冗余節(jié)點(diǎn)，降低了機(jī)器人速度規(guī)劃的復(fù)雜度。