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

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

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

　　關(guān)鍵詞：破拆機(jī)器人；運(yùn)動(dòng)學(xué)；自主運(yùn)動(dòng)

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

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

0引言

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

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

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

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

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

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

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

　　自主破拆的算法步驟為：（1）根據(jù)標(biāo)定裝置測出的作業(yè)目標(biāo)距離，經(jīng)過旋轉(zhuǎn)及偏置后得到視覺云臺(tái)坐標(biāo)系下三維坐標(biāo)。激光測距傳感器安裝在云臺(tái)坐標(biāo)系下的Y軸原點(diǎn)，且激光器發(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)換矩陣計(jì)算獲取激光測距系統(tǒng)下的目標(biāo)物體坐標(biāo)。(3)通過運(yùn)動(dòng)學(xué)逆解將第一坐標(biāo)系下目標(biāo)點(diǎn)坐標(biāo)及設(shè)定的末端執(zhí)行器位姿轉(zhuǎn)換為液壓機(jī)械臂五個(gè)關(guān)節(jié)角的角度值。(4)通過關(guān)節(jié)角與液壓缸長度的映射關(guān)系得到液壓缸運(yùn)動(dòng)的目標(biāo)長度。(5)使用 PID 算法控制液壓元件的速度信號(hào)，使其快速準(zhǔn)確地到達(dá)目標(biāo)點(diǎn)，完成破拆定位。

3運(yùn)動(dòng)學(xué)模塊設(shè)計(jì)

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

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

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

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

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

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

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

7運(yùn)動(dòng)控制模塊設(shè)計(jì)

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

8結(jié)論

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

參考文獻(xiàn)

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

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

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

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

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

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

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

　?。?］ 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.

　　［9］ 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.

　?。?0］ 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.