柔索驅(qū)動(dòng)揀矸機(jī)器人分揀軌跡跟蹤控制研究

劉 鵬，龔 莉，馬宏偉，曹現(xiàn)剛，張旭輝，喬心州，夏 晶

（1.西安科技大學(xué) 陜西省礦山機(jī)電裝備智能監(jiān)測(cè)重點(diǎn)實(shí)驗(yàn)室,陜西 西安 710054；2.西安電子科技大學(xué) “電子裝備結(jié)構(gòu)設(shè)計(jì)”教育部重點(diǎn)實(shí)驗(yàn)室,陜西 西安 710000）

0 引言

煤炭是我國(guó)最主要的一次能源，在我國(guó)經(jīng)濟(jì)發(fā)展中占有舉足輕重的地位。同時(shí)，隨著全球氣候問(wèn)題日益突出，保護(hù)環(huán)境迫在眉睫。在煤炭生產(chǎn)過(guò)程中，原煤中不可避免地混有矸石。矸石不僅發(fā)熱值低，還是主要的環(huán)境污染源之一，因此將矸石從原煤中分選出來(lái)是提高原煤質(zhì)量，實(shí)現(xiàn)煤炭清潔利用的關(guān)鍵環(huán)節(jié)。智能化煤礦需要重點(diǎn)建設(shè)十大智能系統(tǒng)，其中煤炭分選智能化建設(shè)是煤礦智能化建設(shè)的一項(xiàng)主要內(nèi)容[1]。針對(duì)煤矸石分揀難題，曹現(xiàn)剛等[2]設(shè)計(jì)了基于卷積神經(jīng)網(wǎng)絡(luò)的煤矸識(shí)別定位系統(tǒng)，識(shí)別出的目標(biāo)矸石需要通過(guò)機(jī)器人進(jìn)行自動(dòng)化分揀。但目前依舊采用人工分揀的方式完成，機(jī)器人完成矸石分揀工作也處于起步和探索階段。柔索驅(qū)動(dòng)并聯(lián)機(jī)器人具有諸多優(yōu)點(diǎn)而被廣泛應(yīng)用于風(fēng)洞實(shí)驗(yàn)[3]、天文觀測(cè)[4]、3D 打印[5]以及高空攝像[6]等諸多領(lǐng)域。結(jié)合柔索驅(qū)動(dòng)并聯(lián)機(jī)器人的構(gòu)型特點(diǎn)與優(yōu)勢(shì)，筆者設(shè)計(jì)了一種4 柔索驅(qū)動(dòng)的3 自由度柔索驅(qū)動(dòng)揀矸機(jī)器人[7-9]，該機(jī)器人橫跨于帶式輸送機(jī)上方，通過(guò)4 根柔索的協(xié)調(diào)收放驅(qū)動(dòng)末端抓斗在三維空間運(yùn)動(dòng)，實(shí)現(xiàn)煤矸石的精準(zhǔn)抓取和自動(dòng)化分揀。然而，采用柔索驅(qū)動(dòng)并聯(lián)機(jī)器人完成動(dòng)態(tài)目標(biāo)矸石的分揀工作勢(shì)必面臨著許多新的挑戰(zhàn)。一方面，由于柔索的柔性、彈性、非線性和單向約束特性，以及抓矸置矸過(guò)程的動(dòng)態(tài)沖擊等擾動(dòng)影響，很難精確獲取揀矸機(jī)器人的動(dòng)力學(xué)模型；另一方面，柔索驅(qū)動(dòng)力的單向約束特性決定了揀矸機(jī)器人末端抓斗在分揀運(yùn)動(dòng)過(guò)程中必須時(shí)刻保持張緊，否則就會(huì)發(fā)生柔索的松弛，破壞機(jī)器人構(gòu)型，導(dǎo)致目標(biāo)矸石的分揀任務(wù)無(wú)法順利完成。揀矸機(jī)器人分揀軌跡跟蹤控制解決的問(wèn)題是如何補(bǔ)償和抵消未能精確建模的動(dòng)力學(xué)、抓矸置矸過(guò)程的動(dòng)態(tài)沖擊、復(fù)雜環(huán)境所帶來(lái)的外部擾動(dòng)等因素影響，使機(jī)器人末端抓斗能夠按照既定的軌跡平穩(wěn)運(yùn)行，完成動(dòng)態(tài)目標(biāo)矸石的分揀工作。因此保證揀矸機(jī)器人末端抓斗的精準(zhǔn)軌跡跟蹤運(yùn)動(dòng)控制及其穩(wěn)定性成為需要解決的關(guān)鍵難題。

采用柔性和僅能單向受力的柔索驅(qū)動(dòng)末端執(zhí)行器運(yùn)動(dòng)，使柔索驅(qū)動(dòng)并聯(lián)機(jī)器人的控制面臨著諸多嚴(yán)峻挑戰(zhàn)，國(guó)內(nèi)外諸多學(xué)者致力于柔索驅(qū)動(dòng)并聯(lián)機(jī)器人運(yùn)動(dòng)控制方面的研究，所采用的控制算法包括魯棒控制[10]、計(jì)算力矩控制[11]、反饋線性化控制[12]、滑?？刂芠13]、自適應(yīng)控制[14]、神經(jīng)網(wǎng)絡(luò)控制[15]以及模糊控制等[16]。OYMAN 等[17]基于阻抗控制方法對(duì)柔索驅(qū)動(dòng)康復(fù)機(jī)器人進(jìn)行控制，并通過(guò)試驗(yàn)和仿真驗(yàn)證了控制器的有效性。KHALILPOUR 等[18]采用集中質(zhì)量法建立了柔索動(dòng)力學(xué)特性的柔索驅(qū)動(dòng)并聯(lián)機(jī)器人動(dòng)力學(xué)模型，依據(jù)機(jī)器人動(dòng)力學(xué)模型設(shè)計(jì)了魯棒無(wú)源控制器，證明了所設(shè)計(jì)控制系統(tǒng)的穩(wěn)定性。YANG 等[19]針對(duì)柔索驅(qū)動(dòng)康復(fù)機(jī)器人的運(yùn)動(dòng)控制問(wèn)題，設(shè)計(jì)了能夠識(shí)別康復(fù)者運(yùn)動(dòng)意圖的力位混合控制策略，并通過(guò)試驗(yàn)驗(yàn)證了所設(shè)計(jì)的控制器能夠連續(xù)的識(shí)別康復(fù)者的運(yùn)動(dòng)意圖，保證了機(jī)器人有更好的運(yùn)動(dòng)性能。ZI[20]基于拉格朗日方程建立了四索驅(qū)動(dòng)機(jī)器人動(dòng)力學(xué)模型，設(shè)計(jì)了自適應(yīng)模糊控制系統(tǒng)，采用Lyapunov 方法證明了控制器的穩(wěn)定性，并通過(guò)仿真驗(yàn)證了所設(shè)計(jì)控制系統(tǒng)的有效性。KUMAR 等[21]提出采用輸入-輸出反饋線性化方法對(duì)欠約束柔索驅(qū)動(dòng)并聯(lián)機(jī)器人進(jìn)行控制，通過(guò)仿真驗(yàn)證了該控制方法的有效性。張立勛等[22]提出了基于柔索驅(qū)動(dòng)的宇航員深蹲訓(xùn)練機(jī)器人，提出了包含多余力補(bǔ)償力伺服控制策略，并通過(guò)試驗(yàn)驗(yàn)證了所提出控制策略具有較好的控制效果。SEYFI 等[23-24]將魯棒控制方法應(yīng)用到柔索驅(qū)動(dòng)并聯(lián)機(jī)器人的運(yùn)動(dòng)控制中，設(shè)計(jì)了一種帶補(bǔ)償效果的類計(jì)算力矩控制器，并通過(guò)仿真說(shuō)明了所設(shè)計(jì)的魯棒控制器對(duì)模型不確定性和外界擾動(dòng)具有很好的控制效果。SHANG等[25-26]考慮柔索驅(qū)動(dòng)并聯(lián)機(jī)器人各支鏈運(yùn)動(dòng)誤差對(duì)末端執(zhí)行器運(yùn)動(dòng)控制的影響，設(shè)計(jì)了自適應(yīng)同步控制器對(duì)機(jī)器人末端執(zhí)行器的運(yùn)動(dòng)軌跡進(jìn)行跟蹤控制，并通過(guò)仿真和試驗(yàn)驗(yàn)證了自適應(yīng)同步控制器具有較好的軌跡跟蹤效果。KORAYEM[27]提出了一種移動(dòng)的柔索驅(qū)動(dòng)并聯(lián)機(jī)器人，建立了該機(jī)器人的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型，設(shè)計(jì)了輸入-輸出反饋線性化控制系統(tǒng)，并通過(guò)仿真和實(shí)驗(yàn)驗(yàn)證了所設(shè)計(jì)控制系統(tǒng)的有效性。

上述文獻(xiàn)針對(duì)柔索驅(qū)動(dòng)并聯(lián)機(jī)器人控制困難的問(wèn)題，采用不同方法實(shí)現(xiàn)了機(jī)器人末端執(zhí)行器的高性能軌跡跟蹤控制。柔索驅(qū)動(dòng)揀矸機(jī)器人屬于多輸入多輸出的復(fù)雜非線性系統(tǒng)，其末端抓斗必須跟隨皮帶運(yùn)輸機(jī)運(yùn)動(dòng)，并在運(yùn)動(dòng)過(guò)程中完成煤矸石的分揀工作，傳統(tǒng)控制方法如PID 控制以及計(jì)算力矩控制等方法，很難取得良好的控制效果。柔索驅(qū)動(dòng)揀矸機(jī)器人需要根據(jù)預(yù)定運(yùn)動(dòng)軌跡平穩(wěn)地完成揀矸任務(wù)，控制系統(tǒng)的設(shè)計(jì)是至關(guān)重要的。由于柔索的單向受力特性，其易受外部沖擊載荷等外部載荷的干擾。因此，考慮補(bǔ)償外部干擾的控制策略，是保證柔索驅(qū)動(dòng)揀矸機(jī)器人順利完成揀矸任務(wù)的關(guān)鍵。隨著諸如模糊控制、神經(jīng)網(wǎng)絡(luò)控制等諸多智能控制方法的發(fā)展與應(yīng)用，復(fù)雜非線性系統(tǒng)的控制得到了較好的解決[28-30]。針對(duì)矸石分揀過(guò)程中存在的動(dòng)力學(xué)模型不確定以及外部擾動(dòng)問(wèn)題，采用模糊控制方法對(duì)擾動(dòng)項(xiàng)進(jìn)行逼近，并采用變結(jié)構(gòu)魯棒性對(duì)模糊控制系統(tǒng)的逼近誤差進(jìn)行補(bǔ)償，提出柔索驅(qū)動(dòng)揀矸機(jī)器人的自適應(yīng)模糊魯棒抓取軌跡跟蹤控制策略，實(shí)現(xiàn)揀矸機(jī)器人末端抓斗的精準(zhǔn)跟蹤控制，為順利完成矸石分揀任務(wù)奠定堅(jiān)實(shí)基礎(chǔ)。

1 揀矸機(jī)器人系統(tǒng)方案與模型

1.1 系統(tǒng)方案

如圖1 所示，矸石分揀機(jī)器人系統(tǒng)由柔索驅(qū)動(dòng)揀矸機(jī)器人、帶式輸送機(jī)以及煤和矸石、機(jī)器視覺(jué)系統(tǒng)、位置傳感與檢測(cè)系統(tǒng)、矸石回收倉(cāng)等組成。柔索驅(qū)動(dòng)揀矸機(jī)器人為矸石分揀機(jī)器人系統(tǒng)的執(zhí)行機(jī)構(gòu)，包括機(jī)械模塊和控制模塊2 部分。其中，機(jī)械模塊包括定機(jī)架、滑輪、電機(jī)-柔索驅(qū)動(dòng)系統(tǒng)、末端抓斗、連接柔索與末端抓斗的點(diǎn)狀鉸鏈等組成；控制模塊由工業(yè)計(jì)算機(jī)、運(yùn)動(dòng)控制器、激光跟蹤儀、編碼器等組成。采用機(jī)器視覺(jué)系統(tǒng)獲取待抓取目標(biāo)矸石位置和維度信息，目標(biāo)矸石的質(zhì)量可以通過(guò)其形狀和尺寸進(jìn)行估算。同時(shí)，采用3D 激光跟蹤器與伺服電機(jī)編碼器獲得末端抓斗的空間三維位置?；谏鲜鰷y(cè)量系統(tǒng)和設(shè)備，對(duì)機(jī)器人末端抓斗采用閉環(huán)控制，從而精確地執(zhí)行目標(biāo)矸石的精準(zhǔn)軌跡跟蹤控制與可靠分揀操作。

圖1 揀矸機(jī)器人系統(tǒng)方案Fig.1 Scheme of the cable-driven gangue sorting robot

揀矸機(jī)器人橫跨于帶式輸送機(jī)上方，通過(guò)電機(jī)協(xié)調(diào)驅(qū)動(dòng)4 根柔索的收放，驅(qū)動(dòng)末端抓斗運(yùn)動(dòng)到目標(biāo)矸石所在位置的鄰域內(nèi)，并抓取矸石，將矸石送至矸石回收倉(cāng)，重復(fù)上述分揀過(guò)程，完成目標(biāo)矸石的分揀工作。根據(jù)揀矸置矸作業(yè)過(guò)程和特點(diǎn)，將機(jī)器人末端抓斗的分揀軌跡分為4 個(gè)階段，即啟動(dòng)段、準(zhǔn)備段、抓矸段以及置矸段[31]。文獻(xiàn)[31]對(duì)柔索驅(qū)動(dòng)揀矸機(jī)器人分揀軌跡規(guī)劃進(jìn)行了深入研究，得到了能夠保證末端抓斗光滑連續(xù)運(yùn)行的分揀軌跡，為機(jī)器人的運(yùn)動(dòng)控制奠定了基礎(chǔ)。目標(biāo)矸石的分選過(guò)程如下：首先，與帶式輸送機(jī)同步運(yùn)動(dòng)的目標(biāo)矸石將進(jìn)入視覺(jué)識(shí)別區(qū)，機(jī)器視覺(jué)系統(tǒng)識(shí)別并采集目標(biāo)矸石的形狀和位置信息，并將其傳送給機(jī)器人的主控制器；其次，目標(biāo)矸石移動(dòng)一段時(shí)間到達(dá)揀選區(qū)域，機(jī)器人末端抓斗在此執(zhí)行目標(biāo)矸石的抓取操作；最后，目標(biāo)矸石被放入矸石回收倉(cāng)，完成目標(biāo)矸石的抓取和分揀工作。機(jī)器人末端抓斗回到零位，繼續(xù)完成后續(xù)矸石的分揀任務(wù)。

1.2 運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型

機(jī)器人運(yùn)動(dòng)學(xué)模型是機(jī)器人軌跡規(guī)劃、動(dòng)力學(xué)模型以及運(yùn)動(dòng)控制的基礎(chǔ)。采用矢量封閉原理建立揀矸機(jī)器人索長(zhǎng)空間與末抓斗空間的運(yùn)動(dòng)學(xué)模型。圖2 為柔索驅(qū)動(dòng)揀矸機(jī)器人運(yùn)動(dòng)學(xué)簡(jiǎn)圖，a，b和h分別表示柔索驅(qū)動(dòng)揀矸機(jī)器人空間結(jié)構(gòu)尺寸。根據(jù)柔索驅(qū)動(dòng)揀矸機(jī)器人幾何構(gòu)型，用Ai(xi,yi,zi)，(i=1,2,3,4)表示4 根立柱頂端滑輪的圓心，以A1立柱底部為坐標(biāo)原點(diǎn)，建立全局坐標(biāo)系o-xyz。

圖2 柔索驅(qū)動(dòng)揀矸機(jī)器人運(yùn)動(dòng)學(xué)簡(jiǎn)圖Fig.2 Kinematics schematic of the cable-driven gangue sorting robot

則各根柔索索長(zhǎng)可表示為

式(1)描述了柔索驅(qū)動(dòng)揀矸機(jī)器人末端抓斗空間與索長(zhǎng)空間的映射關(guān)系。當(dāng)給定揀矸機(jī)器人末端抓斗的空間運(yùn)動(dòng)軌跡x,y,z，基于式(1)可求得其對(duì)應(yīng)的索長(zhǎng)變化。

柔索驅(qū)動(dòng)力方向的單位向量可以表示為

柔索驅(qū)動(dòng)揀矸機(jī)器人動(dòng)力學(xué)模型是實(shí)現(xiàn)末端抓斗精準(zhǔn)跟蹤控制的基礎(chǔ)和前提。建立機(jī)器人動(dòng)力學(xué)模型常用的包括牛頓-歐拉法、拉格朗日方程、虛功原理等。本文采用牛頓-歐拉法建立柔索驅(qū)動(dòng)揀矸機(jī)器人的動(dòng)力學(xué)模型，以揀矸機(jī)器人末端抓斗為研究對(duì)象，設(shè)施加于末端抓斗上的各柔索驅(qū)動(dòng)力為Ti，則末端抓斗受力如圖3 所示。因此，根據(jù)牛頓-歐拉法可得機(jī)器人末端抓斗的動(dòng)力學(xué)方程：

圖3 柔索驅(qū)動(dòng)揀矸機(jī)器人動(dòng)力學(xué)簡(jiǎn)圖Fig.3 Dynamics schematic of the cable-driven gangue sorting robot

式中：m為末端抓斗的質(zhì)量分別表示機(jī)器人末端抓斗沿著3 個(gè)方向的加速度；g為重力加速度。

可以將方程式(3)的動(dòng)力學(xué)模型寫成如下簡(jiǎn)化形式：

由于柔索的柔性和單向受力特性，以及抓矸置矸過(guò)程中動(dòng)態(tài)沖擊等影響，機(jī)器人在執(zhí)行目標(biāo)矸石分揀任務(wù)的過(guò)程中，很難精確獲取機(jī)器人的動(dòng)力學(xué)模型。且上述參數(shù)的改變勢(shì)必影響機(jī)器人的控制性能。因此，綜合考慮機(jī)器人動(dòng)力學(xué)模型參數(shù)的變化和外部擾動(dòng)，揀矸機(jī)器人實(shí)際的動(dòng)力學(xué)模型可以表示為

式中：M(X)=M0+ΔM和H(X)=H0+ΔH為揀矸機(jī)器人實(shí)際的動(dòng)力學(xué)參數(shù)，M0和H0分別為質(zhì)量矩陣和哥氏力、向心力以及重力的估計(jì)值，ΔM和ΔH為建模誤差，τd為外部擾動(dòng)，f為摩擦力項(xiàng)。

因此，柔索驅(qū)動(dòng)揀矸機(jī)器人實(shí)際的動(dòng)力學(xué)模型可以表示為

式中：D=+ΔH+f+τd為復(fù)合擾動(dòng)項(xiàng)，包括了建模誤差、摩擦力和外部擾動(dòng)等。

由于τ項(xiàng)中J為非方陣，使得機(jī)器人動(dòng)力學(xué)方程式解不唯一，而機(jī)器人控制需要實(shí)時(shí)獲取柔索驅(qū)動(dòng)力。因此，需選擇合適的優(yōu)化指標(biāo)和方法對(duì)柔索拉力進(jìn)行優(yōu)化求解。采用廣義逆矩陣、零空間基底對(duì)柔索驅(qū)動(dòng)力進(jìn)行優(yōu)化，獲取柔索驅(qū)動(dòng)力的優(yōu)化解[6,32-33]。

2 揀矸機(jī)器人分揀軌跡跟蹤控制策略

由于柔索單向受力特性，機(jī)器人運(yùn)行過(guò)程中易受外部沖擊載荷等外部干擾的影響，本節(jié)采用模糊控制和魯棒控制思想，根據(jù)動(dòng)力學(xué)模型式(6)，為末端抓斗的軌跡跟蹤運(yùn)動(dòng)設(shè)計(jì)魯棒自適應(yīng)模糊追蹤控制器，保證機(jī)器人末端抓斗的精準(zhǔn)軌跡跟蹤控制。本文所設(shè)計(jì)的控制器采用模糊控制系統(tǒng)逼近動(dòng)力學(xué)模型式(6)中的復(fù)合擾動(dòng)項(xiàng)，并設(shè)計(jì)魯棒項(xiàng)消除逼近誤差，以達(dá)到精準(zhǔn)的軌跡跟蹤控制。

模糊邏輯控制系統(tǒng)是基于L.A.Zadeh 提出的模糊集合理論，在模糊數(shù)學(xué)的基礎(chǔ)上，建立起來(lái)的控制系統(tǒng)，其主要由模糊化接口、模糊知識(shí)庫(kù)、模糊推理機(jī)和模糊判決4 個(gè)基本單元組成，相較于傳統(tǒng)的控制方法，其具有明顯的優(yōu)越性，特別是智能控制方面其控制效果顯著。

模糊邏輯控制系統(tǒng)可以用來(lái)逼近假設(shè)模糊邏輯系統(tǒng)是模糊集U?Rn到模糊集V?Rm的映射，則整個(gè)模糊規(guī)則庫(kù)包含的模糊關(guān)系可表示[34-35]為

因此，模糊系統(tǒng)的非線性映射可表示為

根據(jù)公式(12)，則模糊控制系統(tǒng)的輸出可以表示為

柔索驅(qū)動(dòng)揀矸機(jī)器人的控制系統(tǒng)的任務(wù)是：計(jì)算作用于機(jī)器人末端抓斗的柔索驅(qū)動(dòng)力T，保證末端抓斗在運(yùn)行過(guò)程中的實(shí)際位置X趨近于期望值Xd。因此，定義揀矸機(jī)器人，末端抓斗軌跡跟蹤誤差為

式中：X為末端抓斗的實(shí)際位置；Xd為末端抓斗的期望位置。

定義滑模函數(shù)如下：

式中，Λ為正定矩陣。

定義參考跟蹤速度：

為了保證揀矸機(jī)器人末端抓斗的精準(zhǔn)運(yùn)動(dòng)控制，根據(jù)機(jī)器人動(dòng)力學(xué)模型，設(shè)計(jì)魯棒自適應(yīng)模糊控制律為：

采用Lyapunov 方法證明所設(shè)計(jì)控制策略式(17)對(duì)于揀矸機(jī)器人動(dòng)力學(xué)模型式(6)是穩(wěn)定的。

依據(jù)魯棒自適應(yīng)模糊控制律(17)，定義Lyapunov 函數(shù)：

由式(18)和式(21)可知：Lyapunov 函數(shù)V(t)為正定，且Lyapunov 函數(shù)的導(dǎo)數(shù)(t)為負(fù)定。由Lyapunov 穩(wěn)定性理論可得所設(shè)計(jì)控制策略是穩(wěn)定的，能夠?qū)崿F(xiàn)對(duì)揀矸機(jī)器人末端抓斗的精準(zhǔn)控制。

3 仿真分析

本章所有算例的仿真均在MATLAB?2020a Simulink 下完成。揀矸機(jī)器人分揀軌跡跟蹤控制器Simulink 模型如圖4 所示。取模糊自適應(yīng)控制策略的隸屬度函數(shù)為其中分別為0、1、2、3、4，i=1，2，··，5。Ai分別為NB、NS、ZO、PS 和PB。取復(fù)合擾動(dòng)項(xiàng)D=[4sin(10t) 2sin(10t)4sin(10t)]T，W=[0.2 0 0;0 0.2 0;0 0 0.2]，Γ=diag(10,10,10)×10-4，KD=250I。柔索驅(qū)動(dòng)揀矸機(jī)器人結(jié)構(gòu)參數(shù)a=4 m，b=4 m，h=3 m，m=5 kg，g=9.8 N/kg。

圖4 魯棒自適應(yīng)模糊控制器Simulink 模型Fig.4 Simulink model of the proposed robust adaptive fuzzy controller

采用空間螺旋線和文獻(xiàn)[31]所提出的分揀軌跡對(duì)所設(shè)計(jì)的魯棒自適應(yīng)模糊追蹤控制器性能進(jìn)行仿真驗(yàn)證。為了說(shuō)明文中所設(shè)計(jì)控制策略的有效性和優(yōu)越性，將文中所設(shè)計(jì)的控制策略與文獻(xiàn)[20]的模糊控制器進(jìn)行對(duì)比。因此，將文獻(xiàn)[20]中設(shè)計(jì)的控制律應(yīng)用于柔索驅(qū)動(dòng)揀矸機(jī)器人，可以表示為

此外，提出最大跟蹤誤差和均方根誤差兩個(gè)指標(biāo)定量評(píng)價(jià)上述兩個(gè)控制器的控制性能，分別表示為

式中：e(k)為揀矸機(jī)器人末端抓斗的位置跟蹤誤差，N和k分別為軌跡離散個(gè)數(shù)和當(dāng)前采樣點(diǎn)。

3.1 算例1

當(dāng)揀矸機(jī)器人末端抓斗的期望軌跡為上述螺旋線時(shí)，采用魯棒自適應(yīng)模糊控制律對(duì)末端抓斗的運(yùn)動(dòng)軌跡進(jìn)行跟蹤控制，圖5 為仿真得到期望螺旋線軌跡與實(shí)際軌跡三維圖。從圖中可以看出：采用本文所提出的魯棒自適應(yīng)模糊控制策略，保證了柔索驅(qū)動(dòng)揀矸機(jī)器人末端抓斗的運(yùn)動(dòng)具有良好的軌跡跟蹤效果。

圖5 末端抓斗空間螺旋線軌跡與跟蹤軌跡Fig.5 Spatial spiral and trajectory tracking of the end-grab

揀矸機(jī)器人末端抓斗沿著x、y和z三個(gè)方向期望位移和實(shí)際位移、位移跟蹤誤差如圖6 所示。從上述軌跡跟蹤曲線可以看出：采用魯棒自適應(yīng)模糊控制策略，沿著末端抓斗運(yùn)行的3 個(gè)方向均能夠獲得較好的位置跟蹤效果。相較于中間位置點(diǎn)，軌跡曲線起始點(diǎn)的跟蹤效果是最差的。通過(guò)圖6d 可以發(fā)現(xiàn)：x、y和z三個(gè)方向在最初的0～0.5 s 左右的波動(dòng)呈現(xiàn)出不穩(wěn)定的現(xiàn)象，但隨后均保持穩(wěn)定變化，表明所提出控制策略的有效性和穩(wěn)定性；相較于x和y方向，z方向的跟蹤效果最佳，其誤差絕對(duì)值不超過(guò)3‰，且z方向的誤差明顯按固定周期先變小后變大的趨勢(shì)穩(wěn)定變化；x和y方向誤差絕對(duì)值均在6‰以內(nèi)，波動(dòng)絕對(duì)值不超過(guò)3‰，且誤差明顯按固定周期在上述范圍內(nèi)穩(wěn)定變化。

圖6 x、y 和z 方向位置跟蹤與跟蹤誤差Fig.6 x,y and z-direction position tracking and tracking errors for the spatial spiral

揀矸機(jī)器人末端抓斗沿著x、y和z三個(gè)方向的速度跟蹤誤差的變化曲線如圖7 所示。從圖7 可以看出：x、y和z三個(gè)方向速度跟蹤誤差在最初的0～0.5s 左右的波動(dòng)呈現(xiàn)出不穩(wěn)定的現(xiàn)象，但隨后均保持穩(wěn)定變化。其中x方向速度誤差絕對(duì)值在1.6%以內(nèi)，y方向速度誤差在1.6%以內(nèi)，z方向速度誤差在1.5%以內(nèi)。對(duì)于圖7b，在初始時(shí)刻，y方向期望速度存在直線上升的情況，這是Simulink 模塊中sin 函數(shù)求導(dǎo)本身導(dǎo)致的問(wèn)題，這也導(dǎo)致了圖7e最初的速度誤差達(dá)到了32%，之后其保持穩(wěn)定波動(dòng)。

圖7 x、y 和z 方向速度跟蹤誤差Fig.7 x,y and z-direction velocity tracking errors for the spatial spiral

被控量柔索拉力Ti(i=1,2,··,4)隨時(shí)間變化如圖8 所示。其結(jié)果表明：4 根柔索的驅(qū)動(dòng)力變化連續(xù)，均介于10～40 N 之間，滿足柔索單向受力特性。且被控量柔索拉力的波動(dòng)，其相位、周期和波動(dòng)范圍均穩(wěn)定變化。

圖8 空間螺旋軌跡下四根柔索的驅(qū)動(dòng)力Fig.8 Four cable tensions for the spatial spiral

3.2 算例2

本章節(jié)選用文獻(xiàn)[31]所提出的4 段式分揀軌跡作為期望軌跡，采用魯棒自適應(yīng)模糊控制策略控制末端抓斗運(yùn)動(dòng)，說(shuō)明所提出控制策略的有效性和穩(wěn)定性。揀矸機(jī)器人末端抓斗初始運(yùn)動(dòng)狀態(tài)設(shè)置為：仿真時(shí)間為4 s。

圖9 所示為采用文中所提控制策略跟蹤4 段式分揀軌跡，仿真得到的期望軌跡與實(shí)際軌跡三維圖。從圖中可以看出：采用魯棒自適應(yīng)模糊控制策略控制柔索驅(qū)動(dòng)揀矸機(jī)器人末端抓斗的運(yùn)動(dòng)，其軌跡跟蹤效果良好。

圖9 期望軌跡與實(shí)際軌跡三維圖Fig.9 Three-dimensional diagram of the expected pick-andplace trajectory and trajectory tracking

柔索驅(qū)動(dòng)揀矸機(jī)器人末端抓斗沿著x、y和z三個(gè)方向的位置誤差如圖10 所示。從圖中可以看出：x，y和z三個(gè)方向誤差絕對(duì)值均在2‰以內(nèi)，其中y方向的誤差最大，其誤差最大絕對(duì)值在3.8 s 左右達(dá)到了1.75‰。根據(jù)文獻(xiàn)[31]所提出軌跡規(guī)劃方案，此時(shí)已處于分揀軌跡置矸段，末端抓斗已經(jīng)空載，其不影響柔索驅(qū)動(dòng)揀矸機(jī)器人的平穩(wěn)運(yùn)行。

圖10 x、y 和z 方向位置誤差Fig.10 x,y and z-direction position tracking errors for the pickand-place trajectory

將文中所提出的魯棒模糊控制器與文獻(xiàn)[20]所提出的控制器性能進(jìn)行對(duì)比，分別采用公式(23)和(24)計(jì)算最大跟蹤誤差和均方根誤差2 個(gè)指標(biāo)，其結(jié)果見(jiàn)表1。從表中數(shù)據(jù)可以看出：文中提出的魯棒模糊控制器比文獻(xiàn)[20]所提出控制器具有更好的軌跡跟蹤精度和跟蹤效果。采用2 個(gè)控制器所得到的最大跟蹤誤差相同，是因?yàn)榉抡孢^(guò)程中末端抓斗的初始狀態(tài)設(shè)置為相同，且最大跟蹤誤差發(fā)生在軌跡初始點(diǎn)，因此，兩者具有相同的MAE 值。

表1 兩個(gè)控制器性能指標(biāo)RMSE 和MAE 的對(duì)比Table 1 Comparison of RMSE and MAE for the both controllers

圖11 給出了揀矸機(jī)器人跟蹤4 段式分揀軌跡過(guò)程中的速度跟蹤及速度跟蹤誤差。從圖中可以看出：x，y和z三個(gè)方向的速度跟蹤效果良好，跟蹤效果最差的區(qū)域位于軌跡起始處與極值處，但總體效果良好。x，y和z三個(gè)方向的速度誤差絕對(duì)值在1.8%以內(nèi)。1.8～3.2 s，速度波動(dòng)最為復(fù)雜，但此時(shí)末端抓斗處于抓矸段，相較于其他階段，該階段末端抓斗的運(yùn)動(dòng)較復(fù)雜，但從圖中可知速度誤差最大波動(dòng)絕對(duì)值約為0.75%。

圖11 x、y 和z 方向速度跟蹤及速度跟蹤誤差Fig.11 x,y and z-direction velocity tracking and velocity tracking errors

圖12 為采用魯棒自適應(yīng)模糊控制策略情況下，被控量柔索驅(qū)動(dòng)力Ti(i=1,2,··,4)隨時(shí)間的變化圖。從圖中可以看出：柔索驅(qū)動(dòng)力均介于5～100 N，滿足柔索單向受力特性。雖然2.4～2.8 s 內(nèi)，1 號(hào)與4 號(hào)柔索驅(qū)動(dòng)力存在尖點(diǎn)情況，但其仍然是連續(xù)變化，不會(huì)造成沖擊。此外還發(fā)現(xiàn)，1 號(hào)與4 號(hào)柔索，2 號(hào)與3 號(hào)柔索驅(qū)動(dòng)力從2.3 s 開始兩兩按相同變化規(guī)律變化，這與柔索驅(qū)動(dòng)揀矸機(jī)器人的對(duì)稱關(guān)系式相符合。

圖12 分揀軌跡下4 根柔索的驅(qū)動(dòng)力Fig.12 Four cable tensions for the pick-and-place trajectory

通過(guò)上述軌跡跟蹤仿真結(jié)果可以看出：揀矸機(jī)器人末端抓斗軌跡跟蹤控制效果良好，在保證末端抓斗位于最小索拉力工作空間中時(shí)，被控量柔索拉力均大于最小索拉力5 N，且均在合適的范圍內(nèi)波動(dòng)，速度跟蹤效果也較好。

4 結(jié)論

1) 考慮揀矸置矸過(guò)程中的動(dòng)態(tài)沖擊以及外部干擾，基于牛頓-歐拉法建立了柔索驅(qū)動(dòng)揀矸機(jī)器人的動(dòng)力學(xué)模型；設(shè)計(jì)了能夠保證揀矸機(jī)器人末端抓斗高性能軌跡跟蹤控制的魯棒自適應(yīng)模糊控制器，并基于Lyapunov 穩(wěn)定性理論證明了所設(shè)計(jì)控制系統(tǒng)的穩(wěn)定性。

2) 以空間螺旋線軌跡和作者所提出的4 段式實(shí)用分揀軌跡為末端抓斗的期望運(yùn)動(dòng)軌跡，對(duì)柔索驅(qū)動(dòng)揀矸機(jī)器人控制系統(tǒng)進(jìn)行了仿真分析。仿真結(jié)果表明：揀矸機(jī)器人末端抓斗的位置跟蹤和速度跟蹤效果良好，誤差均在可接受范圍內(nèi)；且被控量柔索驅(qū)動(dòng)力均滿足柔索的單向受力要求。說(shuō)明了文中所提出的模糊魯棒自適應(yīng)控制策略對(duì)柔索驅(qū)動(dòng)揀矸機(jī)器人的軌跡跟蹤控制有效且可靠。

對(duì)比文獻(xiàn)[20]提出的控制器，所提出的魯棒自適應(yīng)模糊控制器具有更好的位置跟蹤和速度跟蹤效果。下一步，將在試驗(yàn)樣機(jī)上對(duì)所提控制方法和控制策略進(jìn)行試驗(yàn)驗(yàn)證，并進(jìn)行選煤現(xiàn)場(chǎng)的工業(yè)試驗(yàn)。