常州先進制造技術研究所機器人系統(tǒng)實驗室，江蘇常州213164;2.中國科學院合肥物質科學研究院先進制造技術研究所機器人系統(tǒng)實驗室，江蘇常州213164）

摘要：針對現(xiàn)有破拆機器人手工定位時間長、定位不精確的問題，研發(fā)了一個基于激光定位的破拆機器人機械臂自主運動的控制系統(tǒng)軟件。設計了運動學模塊、閉環(huán)反饋調(diào)節(jié)模塊、液壓裝置控制模塊、手眼標定模塊和運動控制模塊。實際使用效果表明，該軟件系統(tǒng)能完成控制系統(tǒng)設計指標，提高工作效率。

關鍵詞：破拆機器人；運動學；自主運動

中圖分類號：TP273文獻標識碼：ADOI：10.19358/j.issn.1674-7720.2017.09.030

引用格式：何鋒，章小建，趙江海.破拆機器人機械臂自主運動控制系統(tǒng)軟件設計［J］.微型機與應用，2017,36（9）：103-105.

引言

目前，施工現(xiàn)場作業(yè)的破拆機器人采用手動方式進行作業(yè)［1］。由于破拆機器人機械臂運動過程產(chǎn)生的慣性力大，操作人員手工操控需要反復調(diào)整才能運動到大致位置，因此，研發(fā)一種破拆機器人機械臂自主運動控制系統(tǒng)，設計該控制系統(tǒng)的上位軟件，來實現(xiàn)機械臂目標導引的自主運動控制［2］功能是十分有必要的。

本文設計了控制系統(tǒng)軟件的各個模塊，給出了各模塊的設計方法，最后，通過實驗證明了控制系統(tǒng)軟件能完成破拆機器人機械臂目標導引［3］的自主控制功能。

1  破拆機器人總體硬件架構

本文實驗平臺在驚天液壓股份公司的GTRC15型破拆機器人基礎上開展功能部件研究，機器人整機實物圖見圖1。由圖1可看出，機器人由機械臂、移動回轉平臺、行走履帶和作業(yè)支架組成。機械臂主要由大臂、二臂、三臂和末端執(zhí)行器構成?；剞D平臺由回轉馬達驅動且安裝有角度編碼器以獲得當前回轉平臺偏置角度，機械臂的各個關節(jié)由液壓缸驅動，安裝了角度編碼器傳感器，可檢測當前關節(jié)角的角度值。行走履帶使用液壓馬達驅動，可以驅使機器人進行前進、后退、旋轉等動作。機器人的通信使用CAN總線模式［4］?？刂葡到y(tǒng)主要包括機械臂各關節(jié)的電液比例控制系統(tǒng)、無線視頻采集與傳輸系統(tǒng)、激光定位平臺控制系統(tǒng)及機器人主控PC。電液比例控制系統(tǒng)主要是通過控制比例閥閥口開度大小來控制液壓缸體內(nèi)液體的流速，而流速與液壓缸體的運動速度及關節(jié)角的即時速度成近似線性關系。無線視頻采集與傳輸系統(tǒng)是用于操作員遠離作業(yè)現(xiàn)場環(huán)境下，通過遠程無線視頻來觀察定位作業(yè)點，進而自動驅動激光定位平臺找到作業(yè)目標點，使液壓機械臂自動運動到指定目標位置。

2  破拆機器人軟件設計

破拆機器人的控制系統(tǒng)軟件選用的平臺是WindowsXP系統(tǒng)，程序運行框架是用VC6的MFC類庫開發(fā)。在控制系統(tǒng)中設計了運動學計算模塊、閉環(huán)反饋調(diào)節(jié)模塊、液壓裝置控制模塊、運動控制模塊、手眼標定模塊、無線視頻采集與處理模塊等?？刂葡到y(tǒng)軟件界面如圖2。

破拆機器人激光定位下機械臂自主運動整體控制系統(tǒng)工作原理為：操作員通過遠程視頻監(jiān)控系統(tǒng)觀察作業(yè)現(xiàn)場，并根據(jù)現(xiàn)場環(huán)境確定所要破碎的目標點，然后操作激光定位云臺進行俯仰和旋轉運動，直至激光點落在定位的目標點位置上。此時控制系統(tǒng)根據(jù)激光定位平臺在兩個方向上旋轉的角度和激光測距儀當前的讀數(shù)，運用前面給出的激光云臺坐標系求解公式很容易計算出作業(yè)目標點在激光定位平臺坐標系下的空間坐標，該坐標通過剛性轉置可轉換為機械臂坐標系下的作業(yè)目標的空間坐標值。通過運動學逆解［5］或在線軌跡規(guī)劃方法［6］將空間運動量轉為關節(jié)運動輸出量，進而驅動液壓執(zhí)行元件使機械臂實現(xiàn)多軸聯(lián)動動作，快速準確到達指定破拆目標。

自主破拆的算法步驟為：（1）根據(jù)標定裝置測出的作業(yè)目標距離，經(jīng)過旋轉及偏置后得到視覺云臺坐標系下三維坐標。激光測距傳感器安裝在云臺坐標系下的Y軸原點，且激光器發(fā)射方向與Y軸正向一致，若當前激光測距傳感器讀數(shù)是dm，則作業(yè)目標在第五坐標系下的坐標表示為［0,d,0］。(2)通過基坐標系與末端執(zhí)行器坐標系之間的齊次轉換矩陣計算獲取激光測距系統(tǒng)下的目標物體坐標。(3)通過運動學逆解將第一坐標系下目標點坐標及設定的末端執(zhí)行器位姿轉換為液壓機械臂五個關節(jié)角的角度值。(4)通過關節(jié)角與液壓缸長度的映射關系得到液壓缸運動的目標長度。(5)使用PID算法控制液壓元件的速度信號，使其快速準確地到達目標點，完成破拆定位。

3  運動學模塊設計

運動學模塊主要包括關節(jié)角求解部分、正運動學求解部分和逆運動學求解部分［7］。其中關節(jié)角求解部分主要完成液壓缸的變化量與關節(jié)角變化量的線性映射關系的建立。正運動學求解部分完成各關節(jié)角的當前值與末端破碎錘的空間坐標和姿態(tài)的對應關系建立。逆運動學求解部分完成在指定目標位置和指定末端破碎錘姿態(tài)的情況下求出滿足能量最優(yōu)條件下［8］的各關節(jié)角的目標值。通過運動學模塊設計，可以在線獲得當前機械臂的姿態(tài)和末端執(zhí)行器的位置，也能根據(jù)指定目標點和末端破碎錘姿態(tài)求得各關節(jié)角的目標值［9］，并將目標值轉化為液壓缸運動的長度。

4  閉環(huán)反饋調(diào)節(jié)模塊設計

閉環(huán)反饋調(diào)節(jié)模塊主要是用于對各液壓缸的位置運動誤差進行調(diào)節(jié)。使用PID控制方法進行閉環(huán)調(diào)節(jié)，PID控制參數(shù)通過經(jīng)驗法進行設計，獲取合適的各液壓缸的PID控制參數(shù)。通過PID閉環(huán)反饋調(diào)節(jié)后，每個液壓缸關節(jié)的位置運動誤差在0.01mm以內(nèi)。PID控制模型圖見圖3。

5  液壓裝置控制模塊設計

液壓裝置控制模塊通過CAN總線與各關節(jié)的電液比例控制系統(tǒng)進行通信，控制各個液壓缸的閥門開度、運動方向和運動速度。通過液壓裝置控制模塊設計，可以控制機械臂各關節(jié)的運動。

6  手眼標定模塊設計

控制系統(tǒng)軟件使用激光定位平臺獲取視覺的坐標系點，為將視覺坐標系和機械臂坐標系進行整合，需要進行手眼標定模塊設計，手眼標定模塊使用最小二乘算法來計算兩坐標系之間的齊次變換矩陣。通過齊次變換矩陣可以將視覺坐標系的目標坐標和目標姿態(tài)轉化為機械臂坐標系下的坐標，進一步通過運動學模塊求出各液壓缸的變化量，通過液壓裝置控制模塊和閉環(huán)反饋調(diào)節(jié)模塊運動到指定的目標位置［10］。

7  運動控制模塊設計

運動控制模塊主要完成機械臂的多軸運動控制，在軟件中使用了三次樣條曲線插值的方法，將逆解求出的各關節(jié)角目標值、各關節(jié)編碼器反饋的各關節(jié)角當前值和約定的整體運動時間作為插值輸入?yún)⒘看?，求得每個定時器周期下各關節(jié)角的位置、速度和加速度值。利用液壓缸的運動特性，進行機械臂多軸運動控制。

8  結論

使用本文設計的控制系統(tǒng)軟件，通過激光定位裝置獲取6組目標點，取末端破碎錘與地面垂直的姿態(tài)為最終姿態(tài)，通過實驗數(shù)據(jù)表1可發(fā)現(xiàn)定位誤差在5cm內(nèi)，定位時間為10s，且可調(diào)。通過實驗可證明該控制系統(tǒng)軟件滿足了破拆機器人機械臂運動自動化的目標。

參考文獻

［1］劉慶運.破拆救援機器人關鍵技術［J］.現(xiàn)代制造工程,2009,30(7):149-153.

［2］楊政,尚建忠，王彪，等.液壓驅動機械臂的軌跡規(guī)劃［J］.機械研究與應用,2009,25(4):47-51.

［3］周孟然,吳永祥.基于紅外激光定位技術的礦井提升機位置跟蹤系統(tǒng)的研究［J］.煤炭學報,2002,27(6):658-660.

［4］胡敏,梁聰慧，鄭慶華，等.多關節(jié)液壓臂架實時控制軟件的設計與實現(xiàn)［J］.中國工程機械學報,2013,11(3):243-247.

［5］蔡自興.機器人學［M］.北京：清華大學出版社,2000.

［6］CRAIGJ.Introductiontorobotics:mechanicsandcontrol（thirdedition）［M］.PearsonEducation,Inc.,2005.

［7］陸震.冗余自由度機器人原理及應用［M］.北京：機械工業(yè)出版社,2006.

［8］XIANGJ,ZHONGC,WEIW.Avariedweightsmethodforthekinematiccontrolofredundantmanipulatorswithmultipleconstraints［J］.IEEETransactionsonRobotics,2012,28(2):330-340.

［9］CARRILERWF,KHOSLAPK，KROGHBH.Pathplanningformobilemanipulatorsformultipletaskexceution［J］.IEEETransactionsonRoboticsandAutomation,1991,7(3):403-408.

［10］FINZELR,HELDUSERS,JANGDS.Electrohydrauliccontrolsystemsformobilemachinerywithlowenergyconsumption［C］.ProceeingsoftheSeventhInternationalConferenceonFluidPowerTransmissionandControl,2009:214-219.