雙懸臂截割機器人相對動力學建模與力位混合控制研究

關鍵詞：巷道掘進；雙懸臂截割機器人；力位混合控制；相對動力學；相對雅可比矩陣

中圖分類號：TD632 文獻標志碼：A

0引言

隨著我國煤礦開采深度不斷增加及煤炭賦存條件日益復雜化，現有的單縱軸斷面成形工藝因可達范圍有限，對于大斷面無法一次截割成形，在應對挑戰時顯露出種種局限性[1]。對此，馬宏偉等[2]提出了雙懸臂快速掘進機器人系統方案，可實現大斷面巷道的掘支錨運同步作業。該系統中，雙懸臂截割機器人能夠在不左右橫移的情況下完成大斷面巷道截割成形。雙懸臂截割機器人的研究與應用主要面臨2 個關鍵難題：① 雙輸出共運動導致統一建模難。在進行大斷面截割時，移動平臺需要同時配合左右截割臂產生的截割軌跡，移動平臺對雙臂提供的公共運動勢必對兩臂末端截割頭的動態產生重要影響，兩截割頭輸出導致難以建立機器人統一描述模型。② 雙輸出強交互導致截割控制難。雙懸臂截割機器人通過兩截割頭同時對煤巖施加作用力實現大斷面快速成形，雙臂需同時滿足規劃的運動及對煤巖施加的力2 個方面要求。因此，雙懸臂截割機器人與煤巖的動態交互給雙懸臂截割機器人控制帶來了嚴峻挑戰。

為解決上述兩大關鍵難題，需建立雙懸臂截割機器人的動力學模型，并基于動力學模型設計控制器。目前關于雙懸臂截割機器人動力學建模與截割控制的研究鮮見報道。雙懸臂截割機器人通過兩截割臂相互配合實現大斷面成形，是一種典型的雙臂機器人。在雙臂機器人建模方面，A. Caballero 等[3]在規劃器中引入雙臂空中機器人系統的動力學模型來保證避障的可靠性。Shen Haoyu 等[4]基于解耦自然正交補的遞歸實現了雙臂機器人的逆動力學建模。Wang Jian 等[5]分析了雙臂協同機器人的動力學模型，求解了雙臂機器人在搬運、裝配和加工任務時的動力學特性。程靖等[6]使用拉格朗日方程和牛頓?歐拉法建立了雙臂空間機器人和目標衛星的系統動力學模型，推導了閉鏈混合體系統的動力學模型。劉佳等[7]基于Udwadia?Kalaba 方程建模，克服了傳統拉格朗日方程求解動力學方程需借助拉格朗日乘子的缺點。R. Jamisola 等[8]將單臂動力學模型與相對雅可比矩陣結合，建立了模塊化雙臂動力學模型。董楸煌等[9]研究了雙臂空間機器人抓取目標的動力學控制，使用第二拉格朗日方程推導了機器人動力學模型。張建華等[10]基于矢量解析法和相對動力學模型推導了雙臂機器人的協作動力學模型，將雙臂獨立的動力學整合為1 個整體。王登峰等[11]建立了關節一體化雙臂機器人的動力學模型，并采用最小二乘法實現動力學模型參數辨識。對于雙懸臂截割機器人而言，移動平臺為機器人雙臂提供公共運動，使得傳統的動力學建模方法無法準確地同時描述兩臂運動狀態。

在雙臂機器人控制方面，Jing Xin 等[12]提出了內/外循環混合控制策略，用于控制雙臂機器人在抓取物體時冗余自由度的內部力。艾海平等[13]提出了減輕雙臂空間機器人捕捉目標產生的沖擊效應及保持系統穩定性的控制算法。Zhang Fuhai 等[14]設計了協調控制策略，用于根據空間平行彈簧和阻尼模型控制雙臂機器人的相對臂姿態。Jiang Yiming 等[15]引入徑向基函數神經網絡來處理雙臂機器人動力學參數的不確定性問題。Jiang Wei 等[16]設計了力/位置控制器，以減少雙臂機器人形成閉環鏈路時產生的內部力對機器人可靠性的影響。趙明輝[17]設計了雙臂并聯式矸石分揀機器人，通過關節空間軌跡規劃提高機器人分揀速度。李賀立等[18]設計了雙臂機器人阻抗控制方法，克服了雙臂機器人搬運柔性物體時協調性不足的問題。劉江文等[19]基于雙臂機器人的動力學建模與伺服系統控制算法，設計了控制機器人關節轉矩的控制器。鄭曉薇等[20]提出了基于力同步的自適應阻抗控制方法，實現了雙臂手術機器人對期望接觸力的跟蹤。上述研究主要以雙臂接觸同一對象形成運動閉鏈為前提。而雙懸臂截割機器人的控制需要同時滿足雙臂運動及末端截割頭輸出力的要求，控制算法更為復雜。因此，如何將雙臂整合為1 個系統進行控制尚待研究。

本文基于相對雅可比矩陣建立了統一描述機器人兩臂動態的相對動力學模型，基于動力學模型設計了機器人力位混合控制系統，以實現對雙臂運動和截割頭輸出力的同步控制。

1雙懸臂截割機器人建模

雙懸臂截割機器人采用全斷面雙縱軸結構，其模型如圖1 所示。其單臂結構與EBZ200A 懸臂式掘進機（伸縮型）相同。2 個偏航關節的軸線間距為2 m，俯仰關節與偏航關節軸線間的距離為1m，伸長關節的伸縮行程為4.18～4.73 m，移動平臺質量為120000kg，連接偏航關節與俯仰關節的連桿質量為10000 kg，伸長關節質量為5000 kg。機器人處于巷道最前端，且可在沿巷道掘進方向的滑軌上前進。在切割大斷面時，兩截割臂的工作相互協調，處于滑軌上的移動平臺給左右截割臂提供公共運動，為雙懸臂截割機器人提供冗余的自由度，使兩截割臂同時實現在滑軌上的前后推移。在雙臂截割大斷面時，左右截割臂沿著規劃的路徑互相配合開挖巷道，移動平臺根據實際情況推移機身。兩截割臂下方的鏟板用于收集碎巖。

4雙懸臂截割機器人控制仿真

在實現大斷面截割時，基于蒙特卡洛方法生成的單懸臂與雙懸臂截割機器人產生的工作空間如圖6所示。

從圖6可看出，對于單懸臂截割機器人而言，因工作空間有限，大斷面的截割需開挖2 條巷道才能完成，在完成一側巷道截割后移機進行另一條巷道的截割[21]，工序復雜且效率低下。而雙懸臂截割機器人的移動平臺為2 個截割臂提供了公共運動，機器人擁有更大的工作空間，可在不移動機身的前提下一次性完成大斷面截割。

為了使控制系統產生的解與實際工況下掘進機運動相符，假設移動平臺的實際運動與期望運動相符，設置從靜止狀態開始加速運動，到達工作速度后保持勻速，兩截割臂沿S 形軌跡進行異步截割。為了減小機器人截割過程中產生的動力學耦合效應，對機器人移動平臺運動及雙臂截割運動均進行五次多項式插值規劃， 端點之間的移動時間為10 s，每秒被劃分為10 個步長。五次多項式規劃的邊界條件：① 起點處移動平臺速度和加速度均為0，到達S 形軌跡的第1 個拐點處時移動平臺的速度為0.02 m/s，加速度為0。② 雙臂各關節在S 形軌跡拐點處的速度和加速度均為0。

五次多項式規劃后的機器人截割頭運動軌跡如圖7 所示，其中序號1?8 表示截割頭運動軌跡拐點。當截割頭處于S 形軌跡的拐點時，移動平臺和各關節參數見表2。

由于雙懸臂截割機器人在工作時不形成運動學閉鏈，仿真不考慮機器人在工作過程中的關節內力分配問題，即式（13）的內力項中為零矩陣。當雙懸臂截割機器人處在靜止狀態時，移動平臺的質心相對于基坐標系的位置為[1 m 0 0]T，此時兩截割臂的偏航關節處于0 弧度。由于截割頭自上而下截割，截割臂俯仰關節的初始角度和伸長關節的伸長量均為最大值，根據雙懸臂截割機器人的相對運動學模型，兩截割頭的期望相對位置可由五次多項式規劃的關節運動代入式（1）得出。期望的相對力設置為定值[255 N 240 N ?25 N]^T。兩末端截割頭的初始相對位置為[?2.4271m 4.2039 m 2.025 8 m]^T。兩截割頭期望的相對位置變化如圖8所示。

在力位混合控制系統的作用下，兩臂對規劃的軌跡進行跟蹤，系統輸出的機器人關節力如圖9 所示。由于移動平臺的運動被鎖定為勻速前推，力位混合控制系統只產生了1組符合條件的解。由于兩截割頭需同時滿足圖7 中截割軌跡并保持期望的相對力，關節輸出力將同時受到相對力控制環和相對控制位置環產生的力的影響而發生改變。當兩截割頭經過圖7 中軌跡拐點時，各關節的輸出力發生較大振蕩。這是因為截割頭運動經五次多項式規劃后，處于截割軌跡拐點時兩截割頭相對移動平臺是靜止的，此時截割頭僅輸出沿x₀ 方向的力，在其他方向的力為0，截割頭實際相對力與期望相對力存在較大誤差，即相對力控制環中e_f 較大，導致關節輸出力發生振蕩。

根據控制系統輸出的機器人各關節力矩，相對位置的控制結果如圖10所示。可看出雙懸臂截割機器人兩截割頭的實際相對位置對期望值保持了良好的跟蹤，但實際相對位置在S 形軌跡的拐點處發生了不同程度的振蕩，原因是期望的相對力為常數向量，而在軌跡拐點處，兩截割頭期望的絕對速度和加速度均為0，導致下一步長的兩截割頭輸出力產生較大變化，以實現兩截割頭對期望相對力的跟蹤。在S 形軌跡的拐點處，兩截割頭輸出力發生的振蕩通過圖4 的相對力控制環轉換為關節輸出力，使力位混合控制系統輸出的總關節力產生較大的變化，導致實際相對位置發生波動，即軌跡拐點處發生相對位置振蕩。

系統對截割頭相對力的控制結果如圖11所示。在S形軌跡拐點處，兩截割頭的相對位置和相對力均發生不同程度的振蕩，在控制系統作用下，兩截割頭的相對力依然能夠收斂到期望值。

考慮雙懸臂截割機器人兩截割頭的初始位置并不總是位于規劃的截割軌跡上，且不同硬度的煤巖巷道所需截割輸出力不同，為更好地模擬不同截割負載下的實際工況，令雙懸臂截割機器人初始相對位置為隨機向量[?3.4375 m 3.149 0 m 0.8631 m]T，期望相對力為隨機向量[65 N 60 N 36 N]T，此時力位混合控制結果如圖12 所示。可看出存在初始誤差且期望相對力發生改變時，在30個步長內實際相對位置仍能振蕩收斂至期望值附近，且實現了對期望相對力的同步跟蹤。

對于雙懸臂截割機器人的力位混合控制系統，定義絕對誤差e 為在雙臂對期望軌跡的跟蹤過程中，每個步長下兩截割頭實際相對位置與期望相對位置之間誤差向量最大值的二范數，即

經計算，力位混合控制系統對相對位置跟蹤的絕對誤差為0.3132m，均方根誤差為0.1447 m。

綜合仿真和計算結果可看出，力位混合控制系統能夠有效跟蹤截割頭的期望相對位置，且對每個關節變量的期望值具有良好的跟蹤效果。

5結論

1） 基于機器人相對雅可比矩陣建立雙懸臂截割機器人模型，能將機器人雙臂動力學整合為1 個整體，通過單一變量同時描述機器人雙臂運動狀態。

2） 基于雙懸臂截割機器人相對動力學模型，提出機器人雙臂力位混合控制方案，通過李雅普諾夫函數對系統進行穩定性分析，驗證了系統的可行性。

3） 對雙懸臂截割機器人結構及力位混合控制系統進行仿真分析，結果表明：雙懸臂截割工藝較單懸臂截割擁有更大的工作空間，具有一次性實現大斷面截割的能力，簡化了大斷面成形工序，具有更高的效率；力位混合控制系統能夠完成對期望相對位置和期望相對力的同步控制，從而實現雙臂對煤巖的截割，相對位置跟蹤絕對誤差在0.3132 m 以內，均方根誤差為0.1447 m。