基于軌跡規劃的挖掘機遙控系統研究與開發

韓 斌 高公如 丁榮誠 林海波

（1.山東常林機械集團股份有限公司，臨沂 276700；2.青島理工大學 機械與汽車工程學院，青島 266520）

挖掘機是經濟建設中主要的工程機械之一。據中國工程機械工業協會統計，截至2017年底，工程機械主要設備保有量在690萬～747萬臺，2018年挖掘機、裝載機等工程機械產品銷量均保持兩位數增長，2020年各類挖掘機械產品同比增長39%。隨著礦山開采、交通運輸、港口建設和軍事工程等對施工機械要求的不斷提高，挖掘機行業得到了快速發展。目前，國內外的挖掘機大部分需要人工操作來完成作業，但是有些惡劣工作場景，如搶險救災、礦藏開采等，工作環境復雜，常伴有強烈的振動、噪聲和大量的塵埃，有時甚至面臨輻射、爆炸、塌方以及有毒化學物等威脅，不適合人工操作。因此，遙控挖掘機的研究開發具有重要的應用價值。

國內外在遙控挖掘機方面進行了諸多研究，并取得了一些成果。20世紀70年代，德國卡爾斯魯厄核研究中心研制并投入使用的12噸級牽引式挖掘機采用人工無線遙控方式；20世紀80年代，美國的Kraft TeleRobtics公司和John Deer公司等相繼研發出遙控挖掘機，并進行了批量生產；日本的小松制作所對PC200R-2型液壓挖掘機進行了改造，使其可以被遙控。同濟大學于1986年開發了微機操縱的挖掘機試驗臺，開始了國內挖掘機控制方面的研究。近些年，遙控挖掘機發展迅速。2017年，QUANG H L等開發了一種基于頭戴式顯示器的1.5噸遙控挖掘機[1]；2018年，佘江雪等基于EPEC2024控制器直接驅動直線油門電機和電液比例減壓閥組，實現了挖掘機發動機工作和行走裝置的有效控制[2]；2019年，由三一、華為及躍薪智能等聯合打造的全球第一臺5G遙控挖掘機在上海世界移動通信大會正式亮相[3]；JOOSUNG L等將來自3D掃描儀、攝像機和傳感器的數據結合在一起，以指導遠程挖掘系統[4]；孫偉等提出了一種面向智能化礦用電鏟的挖掘軌跡優化設計方法[5]；2020年，溫時豪設計開發了一種基于無線通信的電動挖掘機遠程遙控及監測系統[6]。

可見，作為使用量較多的傳統工程機械，如何提高挖掘機的工作效率和保障工作安全，一直是研究的重點。本文針對遙控挖掘機操作的平順性、可靠性以及安全性問題，從理論分析入手，對挖掘機動作進行分析建模，建立基于軌跡規劃的動作控制模型，并在控制器中編程實現其控制算法。從通信協議、控制邏輯及界面顯示等方面進行開發，以實現遙控挖掘機的安全、可靠工作。最后，對挖掘機遙控系統進行實驗測試，以驗證遙控系統的功能和性能。

1 建模仿真

在對挖掘機工作裝置進行運動學和動力學分析的基礎上，對挖掘機機械臂進行軌跡規劃和控制，對處于作業狀態的挖掘機各關節的平順性進行建模 仿真。

1.1 建立運動坐標系

挖掘機的工作裝置包含回轉系統、動臂、斗桿和鏟斗，是一個4自由度的結構。為表示挖掘機機械臂各連桿之間的關系，采用Denavit-Hartenberg（D-H）模型建立運動坐標系[7-9]，如圖1所示。在此坐標系中，相鄰的任意連桿在坐標系中的關系可用ai、αi、θi、di這4個參數表示。圖1中各參數的定義如下：ai為zi軸與zi-1軸的交點在zi方向上到第i個坐標系原點的距離；αi為按右手規則zi從zi-1轉到zi軸所轉動的角度；θi為按右手規則zi-1從xi-1轉到xi軸所轉動的角度；di為zi軸到zi-1軸的距離；①和②表示的是兩對平行直線；Linki-1和Linki表示的是連接桿；Axisi-1、Axisi和Axisi+1表示的是關節在坐標系中的位置。

1.2 正逆運動學分析

1.2.1 正運動學

從關節空間到位姿空間即為正運動學。通過控制4個關節角度，能夠達到控制鏟斗位姿的目的[10-12]。圖2是挖掘機機械臂坐標系的簡圖。其中：θw表示挖掘機鏟斗方向角；O0表示基坐標系；O1表示動臂關節坐標系；O2表示斗桿關節坐標系；O3表示鏟斗關節坐標系；O4表示鏟斗末端在基坐標系的位置。

通過上述連接關系可知，i=1Ti可表示為：

i=1Ti為從坐標系Oi-1的坐標系統變換到坐標系Oi的對應關系。

基坐標系相對于鏟斗坐標系的坐標變換0T4為：

設挖掘機鏟斗末端在此處坐標系的位置是(x,y,z,θw)，則通過坐標變換可得其位置點坐標和姿態角。挖掘機作業時，θ1通常不會發生改變，而θw是影響鏟斗方向角的唯一因素，則從關節空間到位姿空間的映射可表示為：

1.2.2 逆運動學

從位姿空間到關節空間即為逆運動學，通常用幾何法、代數法及迭代法求解逆運動學。因挖掘機結構相對簡單，故采用幾何法對其進行求解。挖掘機機械臂逆運動學計算示意圖如圖3所示。

求解步驟如下。

（1）求出θ1。

（2）求出將基坐標系原點平移后的機械臂鏟斗末端坐標(x′,y′,z′)。

（3）求出基坐標系平移后O3的位置坐標(x3,y3,z3)。

（4）計算α、β、γ。

（5）計算θ2、θ3、θ4。

1.3 B樣條曲線軌跡規劃

通過傳統B樣條曲線算法得到的運動軌跡，曲線不經過所有的關節軌跡點，導致運行精度不高，因此需要對其進行改進[13-14]。

傳統的B樣條曲線表達式為：

式中：u為局部變量；Pi是坐標向量；Bi(u)是B樣條基函數。

三次B樣條基函數表達式為：

改進B樣條，設已知P0、P1、P2、P3這4個型值點，如圖4所示。過P1做P0P2的平行線，在這條線上取兩點P1,0和P1,1，分別位于P1的兩側，并使其長度相等，等于m倍的P0P2。用同樣的方法取P2,0和P2,1兩點，使其平行于直線P1P3，長度為m倍的P1P3。將新得到的點當做新的型值點，于是曲線便會通過型值點P1和P2。當P1,0、P1,1和P1三點重合，P2,0、P2,1和P2三點重合，曲線通過P0和P3。

1.4 仿真分析

本文采用MATLAB的Robotic Toolbox工具箱進行軌跡規劃的仿真分析，用運動學正解函數fkine求出由關節空間轉換到位姿空間的變換矩陣，用ikine函數求運動學逆解算變換矩陣，用ctraj函數和jtraj函數實現笛卡爾空間軌跡規劃和關節空間軌跡規劃，用plot函數繪制挖掘機機械臂模型從初始位置到終止位置的軌跡規劃運動。

ctraj的調用格式為：

其中：Tc是機械臂從起始點T0到終止點T1的笛卡爾軌跡規劃；R是規劃路徑的距離向量，其取值范圍為0～1；N是點的數量。

Jtraj的調用格式為：

其中：q為從狀態q0到q1的關節空間軌跡規劃；t是時間向量的長度；qd是指返回的軌跡規劃的速度；qdd則是加速度。

得到的仿真曲線如圖5～圖8所示。通過位置、速度和加速度曲線可以看出，機械臂在運動過程中有較好的平順性。

2 遙控系統開發

2.1 系統總體方案

本文設計的遙控挖掘機整體系統方案如圖9所示。為提高系統安全性，該方案設計兩個控制器局域網絡（Controller Area Network，CAN），分別為由遙控接收器、車載顯示器和車載控制器組成的CAN1，由車載控制器和發動機組成的CAN2。這樣可將發動機與遙控接收器、車載顯示器進行網絡隔離，使外部命令必須通過控制策略過濾后才能控制發動機。系統通過對車載控制器的開發實現挖掘機的遙控化改造，同時在挖掘機中增加遙控、手動切換控制閥，實現在原有挖掘機操作基礎上的遠程控制。在遙控器上可以實現挖掘機的所有動作，并對挖掘機的發動機轉速（油門）、工作模式（H、L、S）、行走模式、增壓、大燈及喇叭等進行控制。遙控接收器通過CAN1與車載控制器連接，將遙控器命令輸出給挖掘機，由車載控制器計算后發出控制命令，從而控制整車各種動作。車載控制器通過CAN2獲取發動機運行參數進行發動機控制，并將參數通過CAN1發往車載顯示器和遙控器。

遙控器采用先進無線電接收發射系統、大規模集成電路、先進的可編程控制器以及高精度信號處理技術，能夠保證信號傳輸過程的抗干擾能力和設備的抗惡劣環境能力，以提高自身控制精度，滿足設備的精細化施工。同時，采用汽車級控制器標準，在野外及復雜電磁環境下具有很強的安全性和可靠性，可滿足各種控制需求。控制系統中集成雙安全系統，芯片在發射機接收加密數據時，可保證數據的安全性。指令數據對應單一的接收發射設備，不同發射機及接收機互不干擾，同時其他無線電信號不會對該設備造成干擾。系統具有糾錯檢測功能，可自動識別錯誤代碼。電器發射端可收集障礙信號，一旦出現緊急情況，可通過發射機直接傳輸到接收機端。遙控器端設有緊急停止按鈕，可及時停止工作，保證安全。

2.2 軟件開發

挖掘機控制系統軟件主要包括遙控器軟件、車載控制器軟件和車載顯示器軟件等。駕駛員操作遙控器控制挖掘機動作的精度是否與實際手動操縱時一致，人機協調，障礙預警、故障報警及緊急停止等工況的控制反饋，以及軟件系統運行邏輯的穩定性、抗干擾能力等，均為軟件系統的關鍵。本文采用目前工業界成熟的CodeSys系統進行系統軟件開發。圖10是核心控制器的開發界面，主要負責完成命令收發、通信協議、IO控制、邏輯判斷以及控制算法等功能。

挖掘機液壓系統具有非線性、機電液強耦合性及時變等特點。執行復合動作時，液壓系統參數變化范圍大，挖掘阻力復雜多變，是造成挖掘機動作卡頓的主要原因。在前面建模仿真的基礎上，車載控制器對軌跡規劃控制算法進行了編程實現，以保證挖掘動作的平順性。它的控制流程如圖11所示。

通過編程實現車載顯示器與遙控器顯示屏及時獲取挖掘機運行信息，如圖12和圖13所示，以監測挖掘機是否正常工作，確保挖掘機運行的安全性。在車載顯示器端，除了能夠顯示挖掘機運行狀態，還能夠調節挖掘機運行參數，使挖掘機能夠適應復雜的作業環境。車載顯示器、遙控器接收器與車載控制器之間的傳輸方式為CAN總線傳輸。報文不包含源地址或目標地址，僅用標志符指示功能信息和優先級信息，以確保信息傳輸的實時性。同時，為了提高它的安全性，對通信協議進行了用戶定義，并在程序中加入了權限和校驗判斷，以防止外部連接惡意接入。

3 試驗結論

軟件系統開發完成后，將整個系統安裝在力士德SC210型挖掘機上進行調試測試。系統模塊安裝如 圖14所示。其中，車載顯示器安裝在駕駛艙內，車載控制器安裝在駕駛艙后部，位于座位后面的空艙內，遙控接收器安裝在挖掘機駕駛艙頂部，以利于信號的收發。車載顯示器、車載控制器、遙控接收器以及發動機通過CAN總線線束連接，車載遙控器和車載傳感器通過硬線線束直接連接。

試驗時遙控器與挖掘機的距離為80 m左右。表1為遠程端發送數據，表2為車載端發送數據。

表1 遠程端數據發送結果

表2 車載端數據發送結果

在對挖掘機工作裝置進行動作試驗時，使用的電液比例閥組為HZ-TJ01，線圈電流最大為800 mA，系統電壓為24 V。遙控器手柄位置發生改變時，電液比例閥反饋電流發生線性變化，電流最大誤差±3 mA。

4 結語

本文研究開發的挖掘機遙控系統，經過試驗測試達到了設計要求。運行情況表明，開發的挖掘機遙控系統能通過遠程遙控實現挖掘機的各種動作和多種作業方式控制，整機運行平順、安全、可靠。智能化是未來挖掘機的發展方向，遙控挖掘機的研發為未來智能挖掘機的發展奠定了堅實基礎，對挖掘機等工程機械信息化、數字化及智能化的研究具有重要意義。