何鋒1，2，章小建1，2，趙江海1，2

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

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

　　關(guān)鍵詞：破拆機器人；運動學(xué)；自主運動

　　中圖分類號：TP273文獻(xiàn)標(biāo)識碼：ADOI： 10.19358/j.issn.1674-7720.2017.09.030

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

0引言

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

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

　　1破拆機器人總體硬件架構(gòu)

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

2破拆機器人軟件設(shè)計

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

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

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

3運動學(xué)模塊設(shè)計

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

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

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

5液壓裝置控制模塊設(shè)計

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

6手眼標(biāo)定模塊設(shè)計

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

7運動控制模塊設(shè)計

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

8結(jié)論

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

參考文獻(xiàn)

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

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

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

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

　?。?］ 蔡自興. 機器人學(xué)［M］. 北京：清華大學(xué)出版社, 2000.

　?。?］ CRAIG J. Introduction to robotics:mechanics and control （third edition）［M］.Pearson Education,Inc.,2005.

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

　　［8］ XIANG J, ZHONG C, WEI W. A varied weights method for the kinematic control of redundant manipulators with multiple constraints［J］. IEEE Transactions on Robotics, 2012,28(2):330-340.

　?。?］ CARRILER W F, KHOSLA P K， KROGH B H. Path planning for mobile manipulators for multiple task exceution［J］. IEEE Transactions on Robotics and Automation, 1991,7(3):403-408.

　　［10］ FINZEL R, HELDUSER S, JANG D S. Electrohydraulic control systems for mobile machinery with low energy consumption［C］.Proceeings of the Seventh International Conference on Fluid Power Transmission and Control, 2009:214-219.