煤矸石分揀機器人動態目標穩定抓取軌跡規劃

馬宏偉，孫那新，張燁，王鵬，曹現剛，夏晶

（1.西安科技大學 機械工程學院，陜西 西安 710054；2.陜西省礦山機電裝備智能監測重點實驗室，陜西 西安 710054）

0 引言

煤和矸石分離是煤炭清潔能源生產和利用必不可少的工序[1]。依靠人工完成煤矸石分揀工作存在效率低、勞動強度大等問題[2-3]。機器人、人工智能、計算機等技術為煤礦智能化提供了新的契機[4-5]。利用機器替人分揀煤矸石可降低勞動成本，提高分揀效率。目前，利用機器人進行動態目標跟蹤抓取以點到點（Point to Point，PTP）[6]、比例導引[7]、金字塔尋優[8]等方法為主，這些方法能夠快速跟蹤運動目標并對其進行抓取，在機械臂末端達到抓取點時只需確保位置精度，無需速度同步。但是對于機器人分揀煤矸石來講，矸石隨帶式輸送機運動具有高速度特性，且矸石平均質量大，如果機械臂末端在抓取時只要求位置同步而對速度不做要求，則會因目標和機械臂末端之間速度差產生載荷沖擊，嚴重時會造成機械臂末端損壞，降低機器人安全性和可靠性。同時，帶式輸送機在運輸過程中存在打滑和跑偏問題，這對矸石定位提出了更高要求[9-11]。因此，研究一種適應大質量、高速度的精確跟蹤方法是目前機械臂末端精確穩定抓取矸石的關鍵所在。

目標定位和軌跡規劃對機器人的工作效率、運動穩定性和能量消耗具有重大影響，是機器人運動控制的基礎[12]。許多學者對分揀機器人在高速高頻作業時跟蹤動態目標的運動軌跡和運動規律進行了研究。王錚等[13]通過控制傳送帶速度，預測目標物體位置，實現了一種基于位置預測的攔截抓取，在保證效率的同時降低了漏抓率，但未考慮垂直膠帶運動方向的目標位置偏差及抓取時刻的沖擊問題。曹現剛等[14]采用比例導引法實現在矸石勻速狀態下的動態目標軌跡規劃，進一步采用增量比例導引法實現矸石非勻速運動下動態跟蹤過程的機械臂運動軌跡規劃，但是煤矸石運動過程屬于大質量高速運動，此方法在抓取時刻由于速度及運動方向不同，易導致機械臂末端與矸石發生沖擊。王鵬等[15]通過余弦定理快速到達目標矸石理論抓取點，再利用PID 進行跟蹤，能夠實現煤矸石分揀機器人以最優時間進行大質量、動態目標的穩準抓取，但在抓取精度上未能進行誤差分析。

針對上述問題，本文結合煤矸石分揀機器人實際工況，提出了一種基于機器視覺的煤矸石分揀機器人動態目標穩定抓取軌跡規劃方法。首先，結合機器人系統，利用基于HU 不變矩的目標匹配算法實現視覺識別系統與視覺伺服系統信息傳遞，完成視覺伺服跟蹤目標捕獲；其次，通過建立相機-機器人運動學模型，實現運動目標精確定位；最后，通過位置-速度-加速度三環PID 控制算法實現機械臂末端穩準抓取矸石。

1 煤矸石分揀機器人系統

煤矸石分揀機器人系統由煤矸石識別系統、機械臂分揀系統、上位PC 機、機器人控制器等組成，如圖1 所示。當目標矸石進入機械臂工作空間后，通過煤矸石識別系統對目標矸石進行匹配識別并獲取目標矸石位姿，將獲取的目標矸石信息發送給上位PC 機。上位PC 機通過跟蹤算法進行各軸機械臂運動增量軌跡計算，將計算得到的軌跡坐標點數據通過以太網通信方式發送至機器人控制器，控制機器人完成對目標軌跡的跟蹤和抓取。

圖1 煤矸石分揀機器人系統組成Fig.1 Composition of coal gangue sorting robot system

采用雙目立體相機獲取目標矸石的像素坐標，并結合坐標系轉換關系得到機器人坐標系下的目標矸石坐標，實現矸石實時定位；通過三環PID 控制算法結合機器人的位姿轉換關系實現機械臂對目標矸石的同步跟蹤抓取。具體流程如圖2 所示。

圖2 動態目標穩定抓取軌跡控制流程Fig.2 Dynamic target stable grasping track control process

2 基于HU 不變矩的目標匹配

對目標矸石質心進行識別是實現精準跟蹤抓取的基礎。機械臂末端在接近目標矸石過程中，通過相機對輸送帶上的目標矸石進行圖像采集，圖像采集過程中會產生攝像頭所在的視角及物體本身的移動[16]，導致圖像變化（如旋轉和尺度的變化），引起圖像像素變化。HU 不變矩具有平移、旋轉和尺度不變性，以及高穩定性和速度快等優點，因此，選擇HU 不變矩作為特征向量來實現目標矸石識別。基于HU 不變矩的目標匹配流程如圖3 所示。利用相機獲取煤矸石分揀機器人當前位姿下所對應的煤矸石圖像，對模板圖像（已知）和采集到的圖像進行圖像預處理后，獲取最小封閉輪廓。以最小封閉輪廓為輸入，分別計算HU 不變矩特征向量，以歐氏距離作為目標矸石與模板圖像的相似性測量函數，計算采樣圖像輪廓與模板圖像的相似性測量函數值。如相似性測量函數值不小于閾值，則繼續循環采樣，否則判斷為目標矸石輪廓。采用最小外接矩形對得到的目標輪廓進行處理，得到目標矸石位置，實現目標矸石位姿確定。

圖3 基于HU 不變矩的動態目標匹配流程Fig.3 Flow chart of dynamic target matching based on HU moment invariants

為獲取目標矸石在機器人坐標系下質心坐標，本文在煤矸石分揀機器人實驗平臺上進行煤矸石圖像的動態采集，以所采集圖像為例，進行目標矸石匹配識別及質心獲取。采集的圖像分辨率為1 920×1 080，圖像尺寸為960 mm×540 mm。安裝相機時，相機坐標系與機械臂末端坐標系之間相對固定，通過機器人控制器獲取機械臂末端位姿信息。

2.1 目標矸石邊緣特征提取

為更好地提取圖像邊緣特征，需先對圖像進行預處理，再進行邊緣提取[17-18]。

（1）對采集到的RGB 圖像進行灰度轉換，減少計算量，提升圖像識別速度。

（2）對灰度圖像進行中值濾波去噪操作，降低圖像干擾，提高圖像識別準確度。

（3）對經過去噪處理后的圖像進行二值化處理，將感興趣的目標和背景分離。

（4）進行形態學運算子處理，填充空洞，去除孤立雜點，平滑邊界。

（5）對濾波后圖像使用Canny 算子提取邊緣，跟蹤邊緣得到外輪廓圖像。

2.2 目標匹配

提取目標矸石輪廓后，采用HU 不變矩特征進行圖像的匹配識別。物體可通過HU 不變矩特征對圖像進行分類等操作。像素尺寸為G×B的二維離散圖像g（x，y）的p+q（p，q=0，1，…）階幾何矩Hpq和中心矩ηpq可表示為

利用中心矩的構造即可實現平移不變性。為抵消尺度變化對中心矩的影響，對中心矩進行歸一化處理，得到歸一化中心矩：

式中 η00為零階中心距。

利用二階歸一化中心距 μ20，μ02，μ11和三階歸一化中心矩 μ30，μ21，μ12，μ03推導對平移、縮放、鏡像和旋轉都不敏感的7 個二維不變矩I1-I7，則[I1I2···I7]為HU 不變矩的特征向量。

在目標匹配過程中，以歐氏距離作為搜索窗內圖像特征與目標矸石圖像匹配程度的客觀表征量，可表示為

式中：Dn為第n個搜索窗與模板特征向量的歐氏距離，n為搜索匹配窗的標號；F(e)為模板圖像HU 不變矩特征向量的第e個不變矩；Fn(e)為第n個搜索窗的HU 不變矩特征向量的第e個不變矩；Dmin為最小歐氏距離，對應當前圖像中相似度最大的匹配窗。

2.3 動態目標質心獲取

在完成目標匹配后，采用最小外接矩形對得到的目標輪廓進行紅框標志，得到目標矸石位置。目標矸石形心像素坐標（X，Y）為

式中：umax，umin分別為最小外接矩形在像素坐標系（ouv）u軸方向的最大、最小像素值；vmax，vmin分別為最小外接矩形在像素坐標系v軸方向的最大、最小像素值。

目標矸石匹配結果如圖4 所示。以左上角為像素坐標系ouv原點o，以水平向右為u軸正向，以豎直向下為v軸正向。左相機目標矸石形心像素坐標為（1 098 mm，355 mm），右相機目標矸石形心像素坐標為（869 mm，355 mm）。為獲取相機坐標系下目標矸石位置，對左右兩側相機獲取的目標矸石形心像素坐標進行像素坐標到圖像坐標的轉換，可得目標矸石形心在左右兩側圖像坐標系下的坐標分別為（224.96 mm，-389.58 mm），（-222.75 mm，-389.58 mm）。根據圖像坐標進行雙目測距原理計算，可得目標矸石到相機坐標系的垂直距離為555.3 mm。以右側像素坐標為基礎，結合目標矸石到相機坐標系的垂直距離，根據圖像坐標系與相機坐標系之間轉換關系，可得目標矸石質心在右側相機坐標系下的坐標為（-57.64 mm，-100.84 mm，555.30 mm）。機械臂與相機之間的轉換關系由相機安裝方式決定，將該點坐標信息發送至被控機械臂，完成機械臂末端對目標矸石的精確跟蹤。

3 相機-機器人運動學分析

3.1 煤矸石分揀機器人運動學分析

煤矸石分揀機器人根據獲取的目標矸石絕對位置坐標進行軌跡規劃，軌跡規劃前需要對煤矸石分揀機器人進行運動學分析。采用D-H 參數法[19]建立運動學模型，運用4×4 的齊次變換矩陣代替復雜的運動學計算，煤矸石分揀機器人運動學坐標系模型如圖5 所示，其中機器人坐標系為OWXWYWZW、滑軌坐標系為O1X1Y1Z1、機械臂坐標系為O2X2Y2Z2、機械臂末端坐標系為OEXEYEZE。

煤矸石分揀機器人運動結構參數見表1。其中，為坐標系OWXWYWZW變換至坐標系O1X1Y1Z1的轉換矩陣，α1為繞XW軸轉動量，α1=0，l1為 沿XW軸平移量，d1為沿ZW軸平移量，θ1為繞ZW軸轉動量，θ1=0；為坐標系O1X1Y1Z1變換至坐標系O2X2Y2Z2的轉換矩陣，α2為繞X1軸轉動量，α2=-90°，l2為沿X1軸平移量，l2=0，d2為沿Z1軸平移量，θ2為繞Z1軸轉動量，θ2=90°；為坐標系O2X2Y2Z2變換至坐標系OEXEYEZE的轉換矩陣，α3為繞X2軸轉動量，α3=-90，l3為沿X2軸平移量，d3為沿Z2軸平移量，θ3為繞Z2軸轉動量。

表1 煤矸石分揀機器人運動結構參數Table 1 Motion structure parameters of coal gangue sorting robot

根據D-H 參數法建立機械臂末端坐標系相對機器人坐標系的正運動學模型，并將α1=0，α2=-90°，α3=-90°，l2=0，θ1=0，θ2=90°代入正運動學模型，可得正運動學方程：

通過逆運動學方程可完成機械臂末端到達期望位置的運動結構參數轉換，從而驅動煤矸石分揀機器人運動。

3.2 相機-機器人轉換關系分析

為實現矸石精確抓取，需通過相機獲取目標矸石實時位置，并將得到的像素坐標轉換為機器人坐標[20]。機器人坐標系、相機坐標系、圖像坐標系及像素坐標系之間的轉換關系如圖6 所示。其中分別為相機坐標系相對于機械臂末端坐標系的旋轉矩陣和平移矩陣，分別為機械臂末端坐標系相對于機器人坐標系的旋轉矩陣和平移矩陣，i為相機焦距，即像素坐標系與相機坐標系的原點距。

圖6 坐標系轉換關系Fig.6 Coordinate system transformation diagram

圖6 中，點P在機器人坐標系下的坐標與在像素坐標系下坐標之間的轉換矩陣為

式中PC為點P在相機坐標系下的坐標。

4 基于視覺伺服三環PID 動態目標跟蹤

為實現機械臂末端快速跟蹤和穩定抓取，采用三環PID 控制算法[21]進行目標矸石動態跟蹤。三環PID 控制算法原理是以位置環控制器的輸入為期望位置，即機械臂上攝像頭捕捉到目標矸石的當前位置，位置環控制器的輸出作為速度環控制器的輸入，速度環控制器的輸出作為加速度環控制器的輸入，將加速度環控制器的輸出疊加到伺服電動機上，共同作用控制機械臂運動[22]，使機械臂末端與目標矸石達到位置、速度同步運動的效果，實現平穩快速抓取。對返回的位置信息進行位置式PID 控制（由比例控制、積分控制、微分控制3 部分合成）。在位置式PID 控制的基礎上分別擴展加速度、速度、位置控制表達式。

式中：A（k），V（k），S（k）分別為第k次加速度、速度、位置控制調整量；KP，KI，KD分別為比例、積分、微分控制系數；ΔAk，ΔVk，ΔSk分別為第k次加速度、速度、位置偏差；ΔAsum，ΔVsum，ΔSsum分別為累計末端的加速度、速度、位置偏差；ΔA′，ΔV′，ΔS′分別為末端加速度、速度、位置偏差。

式中：Sk為第k次機械臂末端位置；Slast為上一次機械臂末端的位置；ΔSlast為上一次機械臂末端位置偏差。

式中：Vk為第k次機械臂末端速度；Vlast為上一次機械臂末端的速度；ΔVlast為上一次機械臂末端速度偏差；Δt為時間步長；Vmax為最大速度。

式中：Ak為第k次機械臂末端加速度；Alast為上一次機械臂末端的加速度；ΔAlast為上一次機械臂末端加速度偏差；Amax為最大加速度。

5 仿真及實驗驗證

5.1 軌跡規劃仿真

根據煤矸石分揀機器人實驗平臺實際情況，設置機器人最大速度為4.2 m/s，最大加速度為10 m/s2。目標矸石運動速度為0.9 m/s。將三環PID 控制算法與比例導引算法、偏置比例導引算法[23]進行對比分析。用Matlab 軟件對動態目標跟蹤抓取在追隨式（機械臂末端起始位置在目標矸石起始位置斜后方）、同步式（機械臂末端起始位置和目標矸石起始位置在X軸重合）、攔截式（機械臂末端起始位置在目標矸石起始位置斜前方）3 種情況下的軌跡規劃進行動態仿真分析，3 種算法運動仿真參數見表2，分析結果如圖7-圖18 所示。①在追隨式軌跡下目標矸石隨輸送帶沿X軸正方向運動，目標矸石初始位置為（0.5，0.5，0），機械臂末端初始位置為（0，0，0.4），機械臂末端位于目標矸石運動方向的斜后方。② 在同步式軌跡下目標矸石隨輸送帶沿X軸正方向運動，目標矸石初始位置為（0，0.5，0），機械臂末端初始位置為（0，0，0.4），機械臂末端位于目標矸石運動方向的正后上方。③在攔截式軌跡下目標矸石隨輸送帶沿X軸負方向運動，目標矸石初始位置為（0.5，0.5，0），機械臂末端初始位置為（0，0，0.4），機械臂末端位于目標矸石運動方向的斜前方。

表2 3 種算法運動仿真參數Table 2 3 kinds of algorithm motion simulation parameters

從圖7、圖11、圖15 可看出，三環PID 控制運動軌跡和偏置比例導引運動軌跡在機械臂抓取時刻與目標矸石運動方向相同，而比例導引運動軌跡在機械臂抓取時刻與目標矸石運動方向存在一定角度。仿真結果顯示，3 種算法跟蹤位置偏差控制在1 mm左右，偏差較小，均滿足位置偏差要求。目標矸石沿X軸正方向勻速運動，因此以目標矸石在X軸向速度為0.9 m/s，在Y，Z軸向的速度及X，Y，Z軸向的加速度均為0 作為目標值。從圖8-圖10、圖12-圖14、圖16-圖18 可看出，只有三環PID 控制運動軌跡各軸向速度、加速度在抓取點完全趨近于目標矸石，即實現了同步跟蹤，且時間最短；而偏置比例導引運動軌跡速度、加速度只在Y軸和Z軸2 個方向趨近目標值，在X軸向速度和加速度均未趨近目標值，即同向不同速；比例導引運動軌跡各軸向速度、加速度均未與目標矸石達到一致。

圖7 追隨式3 種算法動態目標軌跡規劃曲線Fig.7 Dynamic target track planning curves of three algorithms under following track planning mode

圖8 追隨式3 種算法X 軸向位置、速度、加速度變化曲線Fig.8 X axial position,velocity and acceleration curves of three algorithms under following track planning mode

圖9 追隨式3 種算法Y 軸向位置、速度、加速度變化曲線Fig.9 Y axial position,velocity and acceleration curves of three algorithms under following track planning mode

圖10 追隨式3 種算法Z 軸向位置、速度、加速度變化曲線Fig.10 Z axial position,velocity and acceleration curves of three algorithms under following track planning mode

圖11 同步式3 種算法動態目標軌跡規劃曲線Fig.11 Dynamic target track planning curves of three algorithms under synchronous track planning mode

圖12 同步式3 種算法X 軸向位置、速度、加速度變化曲線Fig.12 X axial position,velocity and acceleration curves of three algorithms under synchronous track planning mode

圖13 同步式3 種算法Y 軸向位置、速度、加速度變化曲線Fig.13 Y axial position,velocity and acceleration curves of three algorithms under synchronous track planning mode

圖14 同步式3 種算法Z 軸向位置、速度、加速度變化曲線Fig.14 Z axial position,velocity and acceleration curves of three algorithms under synchronous track planning mode

圖15 攔截式3 種算法動態目標軌跡規劃曲線Fig.15 Dynamic target track planning curves of three algorithms under intercepting track planning mode

圖16 攔截式3 種算法X 軸向位置、速度、加速度變化曲線Fig.16 X axial position,velocity and acceleration curves of three algorithms under intercepting track planning mode

圖17 攔截式3 種算法Y 軸向位置、速度、加速度變化曲線Fig.17 Y axial position,velocity and acceleration curves of three algorithms under intercepting track planning mode

圖18 攔截式3 種算法Z 軸向位置、速度、加速度變化曲線Fig.18 Z axial position,velocity and acceleration curves of three algorithms under intercepting track planning mode

在整個過程中，三環PID 控制運動軌跡在各軸速度、加速度上無突變現象，且沒有出現超限現象，而偏置比例導引運動軌跡和比例導引運動軌跡在各軸加速度曲線上均出現突變現象，且加速度存在超限現象，這樣會導致機械臂在運動過程中發生振蕩。

綜上，三環PID 控制算法在追隨式、同步式和攔截式3 種情況下響應時間、跟蹤抓取時間均較比例導引算法及偏置比例導引算法短，且三環PID 控制算法在整個過程中各軸速度、加速度連續、平滑，沒有出現突變情況，狀態平穩，沒有大幅度振蕩，適應性強，可實現同步跟蹤和精準抓取。

5.2 實驗驗證

煤矸石分揀機器人實驗平臺如圖19 所示。本實驗煤矸石分揀機器人為桁架式結構，相機安裝在機械臂上，機器人可實現機械臂在空間X，Y，Z方向的移動及機械臂末端繞Z軸的轉動。

圖19 煤矸石分揀機器人實驗平臺Fig.19 Experimental platform of coal gangue sorting robot

為驗證三環PID 控制算法的適應性、高效性，結合雙目相機所獲得的目標矸石位置信息，在已有的雙機械臂煤矸石分揀系統平臺上應用三環PID 控制算法、比例導引算法和偏置比例導引算法進行適應性實驗，結果見表3。其中，ΔX，ΔY，ΔZ為目標矸石與機械臂末端三軸向起始位置差，v為輸送帶速度，tPID，tP，tB分別為三環PID 控制算法、比例導引算法和偏置比例導引算法的平均抓取時間，ESPID，ESB，ESP分別為三環PID 控制算法、比例導引算法和偏置比例導引算法的抓取時刻平均位置偏差，EVPID，EVB，EVP分別為三環PID 控制算法、比例導引算法和偏置比例導引算法的抓取時刻平均速度偏差。

表3 3 種算法實驗結果Table 3 Experimental results of three algorithms

3 種算法在機器人運行時各個關節均未超限，同時由表3 可看出，三環PID 控制算法完成抓取的平均時間比比例導引算法和偏置比例導引算法短；三環PID 控制算法的位置偏差與其他2 種算法相差不大，都控制在1 mm 左右；輸送帶速度的變化對3 種算法的跟蹤抓取時間影響不大。比例導引算法和偏置比例導引算法在抓取點的平均速度偏差在2 m/s 左右，并未實現同速，而三環PID 控制算法在抓取點的平均速度偏差在1 mm/s 左右，跟蹤速度偏差較小，可滿足對高速度運動目標的同步跟蹤、精準抓取要求。

6 結論

針對機器人穩定分揀煤矸石問題，提出了一種基于機器視覺的煤矸石分揀機器人動態目標穩定抓取軌跡規劃方法。該方法通過視覺獲取煤矸石實時位置，并通過三環PID 控制算法進行機器人軌跡規劃，實現了機器人分揀系統的全閉環控制。結論如下：

（1）通過HU 不變矩對目標矸石進行目標匹配和精確定位，有效解決了煤矸石在傳送過程中因輸送帶打滑、左右擺動而造成的目標定位不準確問題，實現快速目標定位，提高目標位置的精確性。

（2）用Matlab 軟件對實際工況中的追隨式、同步式、攔截式3 種情況軌跡規劃進行動態仿真分析，結果表明：三環PID 控制算法在追隨式、同步式和攔截式3 種情況下響應時間、跟蹤抓取時間均較比例導引算法及偏置比例導引算法的時間短，且三環PID 控制算法在整個過程中各軸速度、加速度連續、平滑，沒有出現突變情況，狀態平穩，沒有大幅度的振蕩，適應性強，可實現同步跟蹤、精準抓取。

（3）對三環PID 控制算法、比例導引法和偏置比例導引算法進行了實驗驗證，結果表明：3 種算法在規劃控制機器人運行過程中，均未出現機器人關節超限現象；三環PID 控制算法完成抓取的平均時間比比例導引算法和偏置比例導引算法短；三環PID 控制算法在抓取點的平均速度偏差在1 mm/s 左右，跟蹤速度偏差較小，可滿足對高速度目標的同步跟蹤、精準抓取要求。