并聯式選矸機器人路徑規劃研究

黃金鳳， 張建喜， 于江濤， 苗術佶

（1. 華北理工大學 機械工程學院，河北 唐山 063210；2. 河北省工業機器人產業技術研究院，河北 唐山 063000；3. 唐山因泰智能科技發展有限公司，河北 唐山 063000）

0 引言

傳統的選矸方法主要有人工手選和機械分選[1]。人工手選存在勞動強度大、效率低等問題，且長時間選矸作業會對工人健康造成威脅。機械分選包括跳汰選煤、重介選煤、風選等[2]，這些方法有效降低了人工成本，但均存在環保問題，不符合煤礦綠色發展理念[3-4]。煤炭分選智能化系統是煤礦十大智能化系統之一[5-6]，實現可靠、高效、綠色的煤矸石自動分選具有現實意義。為了實現煤矸石自動化分選，應用機械手完成選矸作業已成為發展趨勢[7]。

當機械手末端執行器按不同路徑進行作業時，由于路徑空間維度不同，導致機械手完成1次完整作業所用時間不同。在不超過驅動電動機額定轉矩的情況下，選矸機械手完成1次完整作業的時間長短直接決定了其選矸效率。因此，探究機械手最優作業路徑十分必要。

目前，在煤矸石自動分選領域已有不少研究成果。文獻[8]采用AUBO-i5機器人進行煤矸石分選工作，并基于G-RRT*算法對機器人進行路徑規劃。文獻[9-12]利用龍門式機械手實現煤矸石分選，并設計了多個動態目標下的多臂協同控制策略。文獻[13-15]提出了龍門式矸石分選機器人動態揀取及避障路徑規劃算法，實現了機器人在線路徑規劃。上述文獻中采用的煤矸石分選機械手多為串聯式。與串聯機械手相比，并聯機械手中的電動機和減速機在高速作業過程中無需隨機械手做高速運動，大大降低了高速運動部件的質量，使其能夠在短時間內達到較高的運動速度，即增強了其高速特性。此外，并聯機械手的作業負荷由不止1條運動支鏈承擔，提高了其負載能力，即增強了其高速重載特性。因此，并聯機械手更適用于大質量煤矸石分選，本文對并聯式選矸機器人作業路徑進行研究，對“梯”型路徑規劃與“V”型路徑規劃2種動態分選路徑規劃方法進行對比分析。

1 并聯式選矸機器人系統

并聯式選矸機器人系統主要由排隊機構、視覺系統、控制系統和執行機構（并聯機械手）組成，如圖1所示。

圖 1 并聯式選矸機器人系統Fig. 1 Parallel gangue selection robot system

大量煤和矸石的混合物被傳輸到矸石分選輸送帶上，隨輸送帶一起運動。排隊機構對混合物進行隊列修正，使混合物排成2列并處于標準線兩側，煤和矸石之間的最小距離為100 mm。視覺系統將識別到的煤矸石信息傳輸至控制系統。控制系統輸出指令至并聯機械手，并聯機械手完成矸石撥出任務，從而達到煤矸分選目的。煤矸分選過程如圖2所示。

圖 2 煤矸分選過程Fig. 2 Coal and gangue selection process

并聯機械手結構如圖3所示。橫向采用基于五桿機構的二自由度平面并聯機構，縱向由電動機驅動絲杠并帶動并聯機構運動[16-17]。

圖 3 并聯機械手結構Fig. 3 The structure of parallel manipulator

2 并聯機械手路徑規劃

機械手路徑規劃是指在有障礙物的環境中，根據任務要求，按時間最短、路徑最短、耗能最少的評判條件，尋找出一條從起始位姿到目標狀態的無碰撞最優路徑[18-19]。機械手推煤矸石任務中包括3種運動：① 機械手末端執行器將煤矸石推出輸送帶的推程運動；② 機械手末端執行器避障回歸起點的回程運動；③ 機械手追蹤輸送帶的運動，以保證與煤矸石在輸送帶運動方向相對靜止。由于每塊煤矸石的質量與位置不同，為了減少控制系統計算量，加快處理速度，設定每次推程運動前進的距離為400 mm。

2.1 “梯”型路徑規劃

當矸石運動到機械手工作空間后，機械手末端執行器將矸石推出輸送帶。推出矸石之后，機械手末端執行器位于輸送帶邊緣，為了進行后續推矸石的運動，機械手末端執行器需要回到初始位置。如果機械手末端執行器回程路徑與推出路徑相同，其碰到輸送帶上煤塊的概率很大，從而可能破壞隊列，使視覺系統給出的目標物體信息發生變化。因此，機械手末端執行器回程運動時要杜絕與煤塊發生接觸。在這種情況下，通常的處理方式是將機械手末端執行器向上抬起一定距離，再完成回程運動，即按“梯”型路徑運動。“梯”型路徑如圖4所示，（x,y）為以基座初始位置為原點的坐標。

圖 4 “梯”型路徑Fig. 4 "Ladder" path

機械手末端執行器從A點出發，經過B，C，D點后回到A點。其中AB段沿x軸的位移為400 mm，沿z軸（輸送帶運動方向）的位移與輸送帶位移相同；BC段和DA段沿x軸的位移為50 mm，沿y軸的位移為300 mm，沿z軸的位移為0；CD段沿x軸的位移為300 mm，沿z軸的位移與AB段沿z軸的位移相等。AB段為將煤或矸石推出輸送帶的工作路徑，在AB段，通過機械手的驅動電動機驅動絲桿，保證機械手末端執行器與煤矸石在輸送帶運動方向相對靜止。

為了避免機械手內部產生慣性力沖擊，當機械手進行高速運動時，其在起點與終點時的加速度應為0，整體加速度曲線應呈連續變化。

在AB，BC，CD，DA路徑上，機械手均按照統一的規律運動，位移函數為

式中：s為位移；b0-b5為多項式系數；t為時間。

對式（1）進行求導，得到速度v與加速度a：

由式（2）可知，加速度計算公式為三次多項式，加速度不存在突變。因此，機械手的慣性力也是連續變化的，不存在機械手內部慣性力沖擊。

為簡便計算，對變量作如下定義：以AB段為例，設位移為sab，沿x軸的位移為sxab，沿y軸的位移為syab，運動時間為tab。其他路段類似變量定義依此類推。

機械手在初始位置（A點）時，有

機械手在末端位置（B點）時，有

將式（3）、式（4）代入式（1）、式（2），可得機械手末端執行器在AB段沿x軸的位移：

式中amax為機械手末端執行器的最大加速度。

同理可得機械手末端執行器在BC段(tab≤t＜tbc)的位移：

式中axmax，aymax分別為機械手末端執行器在x,y軸方向的最大加速度。

機械手末端執行器在CD段沿x軸的位移為

機械手末端執行器在BC段的位移為

2.2 “V”型路徑規劃

選矸機器人利用輸送帶速度推斷煤矸石到達機械手工作空間的時刻，控制機械手末端執行器按編號依次完成推矸石任務。“V”型路徑如圖5所示，機械手末端執行器的位置變化為。其中EF段沿x軸的位移為400 mm，沿z軸的位移與輸送帶位移相同；FG段沿x軸的位移為350 mm，沿z軸的位移與輸送帶位移相同；GH段沿x軸的位移為50 mm，沿z軸的位移由下一塊待處理矸石的位置決定。

圖 5 “V”型路徑Fig. 5 "V" path

設1號矸石進入機械手工作空間的時刻為t0，則末端執行器自t0時刻開始運動，其在z軸方向的運動速度與輸送帶速度一致，在x軸方向進行運動，直至將矸石推出輸送帶，在圖5中表示為E→F路線。在回程運動中，考慮到直接退回可能會破壞隊列順序，設定機械手在z軸方向的運動速度仍與輸送帶速度一致，直至末端執行器回到標準線內，在圖5中表示為F→G路線。標準線內無障礙物，因此無需考慮避障問題，為了能夠快速到達推2號矸石的位置，當末端執行器進入標準線內后，快速運動到與2號矸石平行的位置，在圖5中表示為G→H路線。以末端執行器開始運動到與下一目標矸石平行為1個運動周期，因其路徑似字母“V”，稱為“V”型路徑。

機械手末端執行器在EF，FG，GH段沿x軸的位移分別為

3 并聯機械手動力學模型

考慮到機械手的使用壽命，要求將驅動電動機輸出轉矩控制在電動機額定轉矩以下。由于機械手末端執行器路徑有“梯”型路徑與“V”型路徑2種，且2種路徑規劃的空間維度存在差異，導致驅動電動機轉矩變化情況不同。因此，通過建立并聯機械手動力學模型，探究機械手在不同路徑運動時驅動電動機轉矩的變化情況，計算在轉矩滿足要求的前提下，沿2種路徑完成1個運動周期所用的時間，進而對比機械手沿2種路徑的作業效率。驅動電動機選用HG-RR353伺服電動機，額定轉矩為11.1 N·m。

3.1 機械手運動學逆解

在進行機構動力學分析時，機構簡圖是重要的理論基礎。機械手機構簡圖如圖6所示。

圖 6 機械手機構簡圖Fig. 6 Schematic diagram of manipulator mechanism

由圖6可得機械手逆向運動學方程：

式中：r為機械手末端執行器運動點O'的位置矢量，為起點O到兩邊轉動副旋轉中心的距離；l1，l2分別為主從臂的臂長；ui，wi分別為主從臂方向的單位矢量；θ1i和 θ2i分別為主從臂的旋轉角度。

對式（12）等號兩邊分別取模的平方，得到三角方程：

根據裝配模式求解可得

用式（12）對時間求導可得

式中：V為動平臺相對靜平臺的速度；與分別為第i個驅動支鏈中主從臂的角速度；Q為定常矩陣，

將式（21）兩端左乘，結合得

將式（22）寫為矩陣形式，得

同理，將式（21）兩端左乘，結合得

將式（27）寫為矩陣形式，得

用式（21）對時間求導，并將兩端左乘，得

3.2 機械手動力學分析

根據機械手作業時的實際情況，分析機械手運動時的受力狀態，將系統做功分為動平臺與矸石對系統做功（W1）、主動臂對系統做功（W2）、從動臂對系統做功（W3）3個部分，建立虛功方程。式中：md為動平臺質量；g為重力加速度；f0為目標物體作用在機械手上的力；δR為動平臺的虛位移。

式中：T為主動臂的轉動力矩；Ia為主動臂關于其主動鉸接點的轉動慣量，包含減速器和伺服電動機折算到主動臂的轉動慣量；為主動臂的角加速度，為主動臂關于其主動鉸接點的重力矩為2個主動臂的角度虛位移；m1為主動臂質量；c1為主動臂總長度的一半。

式中：m2為從動臂質量；a21為1號主從臂鉸接點的加速度；mp為主從臂鉸接點質量；δR21為1號主從臂鉸接點的虛位移；a22為2號主從臂鉸接點的加速度；δR22為2號主從臂鉸接點的虛位移；Ib，Ip為從動臂關于主從臂鉸接點的轉動慣量；為從動臂角加速度為從動臂的角度虛位移。

根據虛功原理，有

計算可得

4 仿真分析及現場驗證

4.1 仿真分析

通過Matlab進行仿真模擬，獲得機械手驅動電動機轉矩變化曲線。采用“梯”型路徑、“V”型路徑2種情況下的電動機轉矩變化如圖7所示。

圖 7 機械手驅動電動機轉矩變化曲線Fig. 7 Torque curves of drive motor of manipulator

由圖7（a）可知，采用“梯”型路徑時，在滿足電動機轉矩要求的條件下，完成1個運動周期所用時間為1.2 s。針對每一塊煤矸石，機械手末端執行器都要執行推出→抬升→回程→下降運動，在一定程度上造成了時間浪費，從而影響了煤矸分選效率。由圖7（b）可知，采用“V”型路徑時，在滿足電動機轉矩要求的條件下，完成1個運動周期所用時間為0.65 s，比采用“梯”型路徑時縮短了將近一半時間。

在機械手運行過程中，驅動電動機轉矩和電流成正比，在轉矩需求相同的前提下，采用“V”型路徑所用時間較短，故在功耗相同的情況下，選矸效率較高。

4.2 現場驗證

將并聯式選矸機器人應用到工業現場（圖8），對“梯”型和“V”型路徑規劃方法進行驗證。

圖 8 并聯式選矸機器人應用現場Fig. 8 Application site of parallel gangue selection robot

分別采用2種路徑規劃方法進行多組測試，每次測試采用的煤矸石混合物中矸石含量相等，混合物總數量為500塊，其中矸石200塊。混合物顆粒尺寸為50 mm×300 mm，輸送帶速度為400 mm/s。測試結果見表1。

表 1 2種路徑規劃方法測試結果Table 1 Test results of 2 path planning methods

由表1可知，采用“V”型路徑、“梯”型路徑時，平均矸石分選率分別為94.23%和88.28%，且采用“V”型路徑時的總用時比采用“梯”型路徑時少近19%，選矸效率更高。

5 結論

（1） 分析了并聯式選矸機器人工作原理，對“梯”型路徑規劃與“V”型路徑規劃2種方法進行對比分析。“V”型路徑在二維平面上，從空間維度看，比“梯”型路徑簡單，使得機械手選矸效率較高。

（2） 基于機構的逆向運動學與虛功原理，構建并聯機械手動力學模型，并利用Matlab對2種路徑進行仿真分析。仿真結果表明，采用“梯”型路徑和“V”型路徑完成1個運動周期所用時間分別為1.2，0.65 s，采用“V”型路徑所用時間較短，選矸效率較高。

（3） 現場應用結果表明，采用“V”型路徑、“梯”型路徑時，平均矸石分選率分別為94.23%和88.28%，且采用“V”型路徑時的總用時比采用“梯”型路徑時少近19%，選矸效率更高。