馬宏偉，孫那新，張燁，王鵬，曹現(xiàn)剛，夏晶

（1.西安科技大學(xué) 機(jī)械工程學(xué)院，陜西 西安 710054；2.陜西省礦山機(jī)電裝備智能監(jiān)測(cè)重點(diǎn)實(shí)驗(yàn)室，陜西 西安 710054）

0 引言

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

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

針對(duì)上述問(wèn)題，本文結(jié)合煤矸石分揀機(jī)器人實(shí)際工況，提出了一種基于機(jī)器視覺(jué)的煤矸石分揀機(jī)器人動(dòng)態(tài)目標(biāo)穩(wěn)定抓取軌跡規(guī)劃方法。首先，結(jié)合機(jī)器人系統(tǒng)，利用基于HU 不變矩的目標(biāo)匹配算法實(shí)現(xiàn)視覺(jué)識(shí)別系統(tǒng)與視覺(jué)伺服系統(tǒng)信息傳遞，完成視覺(jué)伺服跟蹤目標(biāo)捕獲；其次，通過(guò)建立相機(jī)-機(jī)器人運(yùn)動(dòng)學(xué)模型，實(shí)現(xiàn)運(yùn)動(dòng)目標(biāo)精確定位；最后，通過(guò)位置-速度-加速度三環(huán)PID 控制算法實(shí)現(xiàn)機(jī)械臂末端穩(wěn)準(zhǔn)抓取矸石。

1 煤矸石分揀機(jī)器人系統(tǒng)

煤矸石分揀機(jī)器人系統(tǒng)由煤矸石識(shí)別系統(tǒng)、機(jī)械臂分揀系統(tǒng)、上位PC 機(jī)、機(jī)器人控制器等組成，如圖1 所示。當(dāng)目標(biāo)矸石進(jìn)入機(jī)械臂工作空間后，通過(guò)煤矸石識(shí)別系統(tǒng)對(duì)目標(biāo)矸石進(jìn)行匹配識(shí)別并獲取目標(biāo)矸石位姿，將獲取的目標(biāo)矸石信息發(fā)送給上位PC 機(jī)。上位PC 機(jī)通過(guò)跟蹤算法進(jìn)行各軸機(jī)械臂運(yùn)動(dòng)增量軌跡計(jì)算，將計(jì)算得到的軌跡坐標(biāo)點(diǎn)數(shù)據(jù)通過(guò)以太網(wǎng)通信方式發(fā)送至機(jī)器人控制器，控制機(jī)器人完成對(duì)目標(biāo)軌跡的跟蹤和抓取。

圖1 煤矸石分揀機(jī)器人系統(tǒng)組成Fig.1 Composition of coal gangue sorting robot system

采用雙目立體相機(jī)獲取目標(biāo)矸石的像素坐標(biāo)，并結(jié)合坐標(biāo)系轉(zhuǎn)換關(guān)系得到機(jī)器人坐標(biāo)系下的目標(biāo)矸石坐標(biāo)，實(shí)現(xiàn)矸石實(shí)時(shí)定位；通過(guò)三環(huán)PID 控制算法結(jié)合機(jī)器人的位姿轉(zhuǎn)換關(guān)系實(shí)現(xiàn)機(jī)械臂對(duì)目標(biāo)矸石的同步跟蹤抓取。具體流程如圖2 所示。

圖2 動(dòng)態(tài)目標(biāo)穩(wěn)定抓取軌跡控制流程Fig.2 Dynamic target stable grasping track control process

2 基于HU 不變矩的目標(biāo)匹配

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

圖3 基于HU 不變矩的動(dòng)態(tài)目標(biāo)匹配流程Fig.3 Flow chart of dynamic target matching based on HU moment invariants

為獲取目標(biāo)矸石在機(jī)器人坐標(biāo)系下質(zhì)心坐標(biāo)，本文在煤矸石分揀機(jī)器人實(shí)驗(yàn)平臺(tái)上進(jìn)行煤矸石圖像的動(dòng)態(tài)采集，以所采集圖像為例，進(jìn)行目標(biāo)矸石匹配識(shí)別及質(zhì)心獲取。采集的圖像分辨率為1 920×1 080，圖像尺寸為960 mm×540 mm。安裝相機(jī)時(shí)，相機(jī)坐標(biāo)系與機(jī)械臂末端坐標(biāo)系之間相對(duì)固定，通過(guò)機(jī)器人控制器獲取機(jī)械臂末端位姿信息。

2.1 目標(biāo)矸石邊緣特征提取

為更好地提取圖像邊緣特征，需先對(duì)圖像進(jìn)行預(yù)處理，再進(jìn)行邊緣提取[17-18]。

（1）對(duì)采集到的RGB 圖像進(jìn)行灰度轉(zhuǎn)換，減少計(jì)算量，提升圖像識(shí)別速度。

（2）對(duì)灰度圖像進(jìn)行中值濾波去噪操作，降低圖像干擾，提高圖像識(shí)別準(zhǔn)確度。

（3）對(duì)經(jīng)過(guò)去噪處理后的圖像進(jìn)行二值化處理，將感興趣的目標(biāo)和背景分離。

（4）進(jìn)行形態(tài)學(xué)運(yùn)算子處理，填充空洞，去除孤立雜點(diǎn)，平滑邊界。

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

2.2 目標(biāo)匹配

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

利用中心矩的構(gòu)造即可實(shí)現(xiàn)平移不變性。為抵消尺度變化對(duì)中心矩的影響，對(duì)中心矩進(jìn)行歸一化處理，得到歸一化中心矩：

式中 η00為零階中心距。

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

在目標(biāo)匹配過(guò)程中，以歐氏距離作為搜索窗內(nèi)圖像特征與目標(biāo)矸石圖像匹配程度的客觀表征量，可表示為

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

2.3 動(dòng)態(tài)目標(biāo)質(zhì)心獲取

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

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

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

3 相機(jī)-機(jī)器人運(yùn)動(dòng)學(xué)分析

3.1 煤矸石分揀機(jī)器人運(yùn)動(dòng)學(xué)分析

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

煤矸石分揀機(jī)器人運(yùn)動(dòng)結(jié)構(gòu)參數(shù)見(jiàn)表1。其中，為坐標(biāo)系OWXWYWZW變換至坐標(biāo)系O1X1Y1Z1的轉(zhuǎn)換矩陣，α1為繞XW軸轉(zhuǎn)動(dòng)量，α1=0，l1為 沿XW軸平移量，d1為沿ZW軸平移量，θ1為繞ZW軸轉(zhuǎn)動(dòng)量，θ1=0；為坐標(biāo)系O1X1Y1Z1變換至坐標(biāo)系O2X2Y2Z2的轉(zhuǎn)換矩陣，α2為繞X1軸轉(zhuǎn)動(dòng)量，α2=-90°，l2為沿X1軸平移量，l2=0，d2為沿Z1軸平移量，θ2為繞Z1軸轉(zhuǎn)動(dòng)量，θ2=90°；為坐標(biāo)系O2X2Y2Z2變換至坐標(biāo)系OEXEYEZE的轉(zhuǎn)換矩陣，α3為繞X2軸轉(zhuǎn)動(dòng)量，α3=-90，l3為沿X2軸平移量，d3為沿Z2軸平移量，θ3為繞Z2軸轉(zhuǎn)動(dòng)量。

表1 煤矸石分揀機(jī)器人運(yùn)動(dòng)結(jié)構(gòu)參數(shù)Table 1 Motion structure parameters of coal gangue sorting robot

根據(jù)D-H 參數(shù)法建立機(jī)械臂末端坐標(biāo)系相對(duì)機(jī)器人坐標(biāo)系的正運(yùn)動(dòng)學(xué)模型，并將α1=0，α2=-90°，α3=-90°，l2=0，θ1=0，θ2=90°代入正運(yùn)動(dòng)學(xué)模型，可得正運(yùn)動(dòng)學(xué)方程：

通過(guò)逆運(yùn)動(dòng)學(xué)方程可完成機(jī)械臂末端到達(dá)期望位置的運(yùn)動(dòng)結(jié)構(gòu)參數(shù)轉(zhuǎn)換，從而驅(qū)動(dòng)煤矸石分揀機(jī)器人運(yùn)動(dòng)。

3.2 相機(jī)-機(jī)器人轉(zhuǎn)換關(guān)系分析

為實(shí)現(xiàn)矸石精確抓取，需通過(guò)相機(jī)獲取目標(biāo)矸石實(shí)時(shí)位置，并將得到的像素坐標(biāo)轉(zhuǎn)換為機(jī)器人坐標(biāo)[20]。機(jī)器人坐標(biāo)系、相機(jī)坐標(biāo)系、圖像坐標(biāo)系及像素坐標(biāo)系之間的轉(zhuǎn)換關(guān)系如圖6 所示。其中分別為相機(jī)坐標(biāo)系相對(duì)于機(jī)械臂末端坐標(biāo)系的旋轉(zhuǎn)矩陣和平移矩陣，分別為機(jī)械臂末端坐標(biāo)系相對(duì)于機(jī)器人坐標(biāo)系的旋轉(zhuǎn)矩陣和平移矩陣，i為相機(jī)焦距，即像素坐標(biāo)系與相機(jī)坐標(biāo)系的原點(diǎn)距。

圖6 坐標(biāo)系轉(zhuǎn)換關(guān)系Fig.6 Coordinate system transformation diagram

圖6 中，點(diǎn)P在機(jī)器人坐標(biāo)系下的坐標(biāo)與在像素坐標(biāo)系下坐標(biāo)之間的轉(zhuǎn)換矩陣為

式中PC為點(diǎn)P在相機(jī)坐標(biāo)系下的坐標(biāo)。

4 基于視覺(jué)伺服三環(huán)PID 動(dòng)態(tài)目標(biāo)跟蹤

為實(shí)現(xiàn)機(jī)械臂末端快速跟蹤和穩(wěn)定抓取，采用三環(huán)PID 控制算法[21]進(jìn)行目標(biāo)矸石動(dòng)態(tài)跟蹤。三環(huán)PID 控制算法原理是以位置環(huán)控制器的輸入為期望位置，即機(jī)械臂上攝像頭捕捉到目標(biāo)矸石的當(dāng)前位置，位置環(huán)控制器的輸出作為速度環(huán)控制器的輸入，速度環(huán)控制器的輸出作為加速度環(huán)控制器的輸入，將加速度環(huán)控制器的輸出疊加到伺服電動(dòng)機(jī)上，共同作用控制機(jī)械臂運(yùn)動(dòng)[22]，使機(jī)械臂末端與目標(biāo)矸石達(dá)到位置、速度同步運(yùn)動(dòng)的效果，實(shí)現(xiàn)平穩(wěn)快速抓取。對(duì)返回的位置信息進(jìn)行位置式PID 控制（由比例控制、積分控制、微分控制3 部分合成）。在位置式PID 控制的基礎(chǔ)上分別擴(kuò)展加速度、速度、位置控制表達(dá)式。

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

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

式中：Vk為第k次機(jī)械臂末端速度；Vlast為上一次機(jī)械臂末端的速度；ΔVlast為上一次機(jī)械臂末端速度偏差；Δt為時(shí)間步長(zhǎng)；Vmax為最大速度。

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

5 仿真及實(shí)驗(yàn)驗(yàn)證

5.1 軌跡規(guī)劃仿真

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

表2 3 種算法運(yùn)動(dòng)仿真參數(shù)Table 2 3 kinds of algorithm motion simulation parameters

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

圖7 追隨式3 種算法動(dòng)態(tài)目標(biāo)軌跡規(guī)劃曲線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 種算法動(dòng)態(tài)目標(biāo)軌跡規(guī)劃曲線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 種算法動(dòng)態(tài)目標(biāo)軌跡規(guī)劃曲線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

在整個(gè)過(guò)程中，三環(huán)PID 控制運(yùn)動(dòng)軌跡在各軸速度、加速度上無(wú)突變現(xiàn)象，且沒(méi)有出現(xiàn)超限現(xiàn)象，而偏置比例導(dǎo)引運(yùn)動(dòng)軌跡和比例導(dǎo)引運(yùn)動(dòng)軌跡在各軸加速度曲線上均出現(xiàn)突變現(xiàn)象，且加速度存在超限現(xiàn)象，這樣會(huì)導(dǎo)致機(jī)械臂在運(yùn)動(dòng)過(guò)程中發(fā)生振蕩。

綜上，三環(huán)PID 控制算法在追隨式、同步式和攔截式3 種情況下響應(yīng)時(shí)間、跟蹤抓取時(shí)間均較比例導(dǎo)引算法及偏置比例導(dǎo)引算法短，且三環(huán)PID 控制算法在整個(gè)過(guò)程中各軸速度、加速度連續(xù)、平滑，沒(méi)有出現(xiàn)突變情況，狀態(tài)平穩(wěn)，沒(méi)有大幅度振蕩，適應(yīng)性強(qiáng)，可實(shí)現(xiàn)同步跟蹤和精準(zhǔn)抓取。

5.2 實(shí)驗(yàn)驗(yàn)證

煤矸石分揀機(jī)器人實(shí)驗(yàn)平臺(tái)如圖19 所示。本實(shí)驗(yàn)煤矸石分揀機(jī)器人為桁架式結(jié)構(gòu)，相機(jī)安裝在機(jī)械臂上，機(jī)器人可實(shí)現(xiàn)機(jī)械臂在空間X，Y，Z方向的移動(dòng)及機(jī)械臂末端繞Z軸的轉(zhuǎn)動(dòng)。

圖19 煤矸石分揀機(jī)器人實(shí)驗(yàn)平臺(tái)Fig.19 Experimental platform of coal gangue sorting robot

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

表3 3 種算法實(shí)驗(yàn)結(jié)果Table 3 Experimental results of three algorithms

3 種算法在機(jī)器人運(yùn)行時(shí)各個(gè)關(guān)節(jié)均未超限，同時(shí)由表3 可看出，三環(huán)PID 控制算法完成抓取的平均時(shí)間比比例導(dǎo)引算法和偏置比例導(dǎo)引算法短；三環(huán)PID 控制算法的位置偏差與其他2 種算法相差不大，都控制在1 mm 左右；輸送帶速度的變化對(duì)3 種算法的跟蹤抓取時(shí)間影響不大。比例導(dǎo)引算法和偏置比例導(dǎo)引算法在抓取點(diǎn)的平均速度偏差在2 m/s 左右，并未實(shí)現(xiàn)同速，而三環(huán)PID 控制算法在抓取點(diǎn)的平均速度偏差在1 mm/s 左右，跟蹤速度偏差較小，可滿足對(duì)高速度運(yùn)動(dòng)目標(biāo)的同步跟蹤、精準(zhǔn)抓取要求。

6 結(jié)論

針對(duì)機(jī)器人穩(wěn)定分揀煤矸石問(wèn)題，提出了一種基于機(jī)器視覺(jué)的煤矸石分揀機(jī)器人動(dòng)態(tài)目標(biāo)穩(wěn)定抓取軌跡規(guī)劃方法。該方法通過(guò)視覺(jué)獲取煤矸石實(shí)時(shí)位置，并通過(guò)三環(huán)PID 控制算法進(jìn)行機(jī)器人軌跡規(guī)劃，實(shí)現(xiàn)了機(jī)器人分揀系統(tǒng)的全閉環(huán)控制。結(jié)論如下：

（1）通過(guò)HU 不變矩對(duì)目標(biāo)矸石進(jìn)行目標(biāo)匹配和精確定位，有效解決了煤矸石在傳送過(guò)程中因輸送帶打滑、左右擺動(dòng)而造成的目標(biāo)定位不準(zhǔn)確問(wèn)題，實(shí)現(xiàn)快速目標(biāo)定位，提高目標(biāo)位置的精確性。

（2）用Matlab 軟件對(duì)實(shí)際工況中的追隨式、同步式、攔截式3 種情況軌跡規(guī)劃進(jìn)行動(dòng)態(tài)仿真分析，結(jié)果表明：三環(huán)PID 控制算法在追隨式、同步式和攔截式3 種情況下響應(yīng)時(shí)間、跟蹤抓取時(shí)間均較比例導(dǎo)引算法及偏置比例導(dǎo)引算法的時(shí)間短，且三環(huán)PID 控制算法在整個(gè)過(guò)程中各軸速度、加速度連續(xù)、平滑，沒(méi)有出現(xiàn)突變情況，狀態(tài)平穩(wěn)，沒(méi)有大幅度的振蕩，適應(yīng)性強(qiáng)，可實(shí)現(xiàn)同步跟蹤、精準(zhǔn)抓取。

（3）對(duì)三環(huán)PID 控制算法、比例導(dǎo)引法和偏置比例導(dǎo)引算法進(jìn)行了實(shí)驗(yàn)驗(yàn)證，結(jié)果表明：3 種算法在規(guī)劃控制機(jī)器人運(yùn)行過(guò)程中，均未出現(xiàn)機(jī)器人關(guān)節(jié)超限現(xiàn)象；三環(huán)PID 控制算法完成抓取的平均時(shí)間比比例導(dǎo)引算法和偏置比例導(dǎo)引算法短；三環(huán)PID 控制算法在抓取點(diǎn)的平均速度偏差在1 mm/s 左右，跟蹤速度偏差較小，可滿足對(duì)高速度目標(biāo)的同步跟蹤、精準(zhǔn)抓取要求。