基于視覺的平面關節機器人避障路徑規劃

，

(哈爾濱工業大學(深圳) 機電工程及自動化學院，廣東 深圳 518000)

0 引言

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

避障路徑規劃的目的是在給定起始點和目標點以及障礙物信息的情況下，規劃出一條無碰撞到達目標點位的路徑。關于避障路徑規劃，目前已經提出了許多方法，主要分為傳統算法和智能算法2類[1]。傳統算法包括C空間法、人工勢場法和A*算法等，智能算法包括遺傳算法和粒子群算法等[2-6]。

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

1 樣機模型

實驗采用的平面關節機器人為吊裝型SCARA，如圖1所示。

圖1 實驗用吊裝SCARA

樣機的D-H參數如表1所示。

表1 樣機的D-H參數

2 機器人工作空間地圖信息的建立

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

2.1 視覺系統構建

根據相機的安裝位置，可以將視覺系統分為ETH(eye to hand)形式和EIH(eye in hand)形式。ETH形式的視覺系統是將相機安裝在固定的支架上，在SCARA運動的過程中，相機的位置不發生變化，這種安裝方式的相機坐標系到世界坐標系的映射矩陣是固定的；EIH形式的視覺系統是將相機安裝在SCARA上，因此在SCARA運動過程中，相機的位姿也相對SCARA的基座不斷變化，這種安裝方式需要動態更新相機坐標系到世界坐標系的映射矩陣。

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

圖2 硬件系統示意

2.2 手眼標定[7

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

圖3 相機坐標系與像素坐標系

世界坐標系到相機坐標系的變換為剛體變換，可通過旋轉和平移表示[8-9]。

(1)

其中，R為旋轉矩陣;T為平移向量。旋轉和平移共同構成外參矩陣。

相機坐標系到像素坐標系的轉換主要是縮放變換，根據相機的針孔模型可得到相機坐標系到像素坐標系的轉換關系。

(2)

Mc為相機的內參矩陣。

結合式(1)和式(2)，可以得到世界坐標到像素坐標的轉換關系。

(3)

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

(4)

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

(5)

矩陣的未知參數有9個，需要至少9個方程來求解。M為世界坐標系中XOY平面上的點到像素坐標系的映射矩陣，M-1則為像素坐標系到世界坐標系XOY平面上點的映射矩陣。標定工具選擇棋盤格，通過OpenCV獲取相機圖片中的棋盤格角點坐標和棋盤格角點在世界坐標系下的坐標值，可以得到一組矩陣方程，最終求解映射矩陣。

2.3 障礙物輪廓提取

手眼標定實現了像素坐標系和世界坐標系之間的相互轉換，相機能夠獲取SCARA工作平面的環境信息，圖片中的障礙物是由一系列像素點組成的，可以通過手眼標定矩陣將圖像中的障礙物像素點位轉換為世界坐標系下的坐標。

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

障礙物輪廓提取最常用的方式就是對圖像進行邊緣檢測[10-11]，但直接進行邊緣檢測的方式要求圖片的質量較高，在常規的應用中，圖片的質量往往達不到能直接提取邊緣的要求。為了能夠適應更多的應用場景，決定先對圖像進行分割，預先將物體和輪廓區分開，增強物體邊緣，再提取物體輪廓。根據這一處理步驟，可以從圖4a所示工作平面原圖得到圖4b所示輪廓。

圖4 圖片提取障礙物輪廓

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

3 避障路徑規劃

3.1 改進A*算法的避障路徑規劃

A*算法屬于啟發式搜索算法，實現啟發式搜索路徑的關鍵是構造每個節點的估價函數。

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

(6)

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

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

(7)

Euler距離是移動起點和移動終點的連線距離，即

(8)

(x1,y1)為移動的起點；(x2,y2)為移動的終點。

最佳路徑代價可表示為目標節點到當前節點的Manhattan距離，即

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

(9)

根據A*算法搜索到的路徑是由一系列的節點連接而成的折線，在圖片中表現為一系列的像素點連線，圖5是以圖4a為基本環境信息得到的避障路徑。

一系列的路徑節點能準確避開障礙物前進，但是對于SCARA的軌跡規劃而言，過多的路徑節點會極大提升速度規劃的難度，因此需要對A*算法進行改進。注意到規劃出的路徑節點中有相當一部分節點屬于冗余節點，冗余節點是指剔除后對避障功能無實際影響的節點。判斷冗余節點的方法是通過判斷某一節點前后節點的連線是否會與障礙物干涉來實現的，若節點前后節點的連線不與障礙物干涉，則說明節點剔除后不影響路徑的避障功能，為冗余節點；反之，說明為非冗余節點[12]。改進后的A*避障路徑如圖6所示。從圖6中可以看到，原先一系列的節點最終被優化為4個關鍵節點，4個關鍵節點連接而成的折線依然能夠滿足避障的要求。

圖5 A*算法的避障路徑

圖6 改進A*算法的避障路徑

3.2 可控安全距離的避障路徑規劃

根據以上改進A*算法路徑規劃得到的是一系列節點，雖然能夠實現避障功能，但是節點到障礙物的距離是不可控的。由于算法中估價函數只包含實際代價函數和最佳路徑函數，采用貪心策略選取得到的節點一般是貼著障礙物輪廓的節點，在實際的應用中，SCARA的末端執行器的尺寸還需要考慮進去，因此需要控制路徑上節點到障礙物的安全距離。

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

圖7 原始輪廓與擴展輪廓

將擴展后的輪廓點位作為障礙物點位進行避障路徑規劃，最終得到的路徑如圖8所示。

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

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

圖9 采用圓角過渡的避障路徑

4 避障算法對比

為對比不同算法在避障路徑規劃上的性能，隨機選取10對起點和終點進行避障路徑規劃。在都能實現避障功能的前提下，將避障路徑的長度作為避障算法性能比較的主要依據，得到的結果如圖10所示。

圖10 不同避障算法對比

通過路徑長度對比可以發現，A*算法規劃的路徑長度最長，改進A*算法可以減少路徑中的冗余點，進而縮短路徑長度。圓角過渡對路徑長度的影響幾乎可以忽略。10次隨機路徑規劃中，改進A*算法平均縮短3.47%的長度，圓角過渡后平均縮短3.55%的長度。

5 結束語

針對平面關節機器人在工作平面作業時需要進行避障路徑規劃的情況，利用視覺信息構建工作平面的環境信息，并采用圖像處理技術，將安全距離映射為安全像素距離，擴展障礙物輪廓，最后利用改進的A*算法實現了避障路徑規劃。

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