何 鋒，章小建，趙江海

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

破拆機(jī)器人機(jī)械臂自主運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì)*

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

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

針對(duì)現(xiàn)有破拆機(jī)器人手工定位時(shí)間長(zhǎng)、定位不精確的問(wèn)題，研發(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)，提高工作效率。

破拆機(jī)器人；運(yùn)動(dòng)學(xué)；自主運(yùn)動(dòng)

0 引言

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

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

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

圖1 破拆機(jī)器人整機(jī)實(shí)物圖

本文實(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)，安裝了角度編碼器傳感器，可檢測(cè)當(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)、無(wú)線視頻采集與傳輸系統(tǒng)、激光定位平臺(tái)控制系統(tǒng)及機(jī)器人主控PC。電液比例控制系統(tǒng)主要是通過(guò)控制比例閥閥口開度大小來(lái)控制液壓缸體內(nèi)液體的流速，而流速與液壓缸體的運(yùn)動(dòng)速度及關(guān)節(jié)角的即時(shí)速度成近似線性關(guān)系。無(wú)線視頻采集與傳輸系統(tǒng)是用于操作員遠(yuǎn)離作業(yè)現(xiàn)場(chǎng)環(huán)境下，通過(guò)遠(yuǎn)程無(wú)線視頻來(lái)觀察定位作業(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 類庫(kù)開發(fā)。在控制系統(tǒng)中設(shè)計(jì)了運(yùn)動(dòng)學(xué)計(jì)算模塊、閉環(huán)反饋調(diào)節(jié)模塊、液壓裝置控制模塊、運(yùn)動(dòng)控制模塊、手眼標(biāo)定模塊、無(wú)線視頻采集與處理模塊等?？刂葡到y(tǒng)軟件界面如圖2。

圖2 破拆機(jī)器人控制系統(tǒng)軟件界面

破拆機(jī)器人激光定位下機(jī)械臂自主運(yùn)動(dòng)整體控制系統(tǒng)工作原理為：操作員通過(guò)遠(yuǎn)程視頻監(jiān)控系統(tǒng)觀察作業(yè)現(xiàn)場(chǎng)，并根據(jù)現(xiàn)場(chǎng)環(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)的角度和激光測(cè)距儀當(dāng)前的讀數(shù)，運(yùn)用前面給出的激光云臺(tái)坐標(biāo)系求解公式很容易計(jì)算出作業(yè)目標(biāo)點(diǎn)在激光定位平臺(tái)坐標(biāo)系下的空間坐標(biāo)，該坐標(biāo)通過(guò)剛性轉(zhuǎn)置可轉(zhuǎn)換為機(jī)械臂坐標(biāo)系下的作業(yè)目標(biāo)的空間坐標(biāo)值。通過(guò)運(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)定裝置測(cè)出的作業(yè)目標(biāo)距離，經(jīng)過(guò)旋轉(zhuǎn)及偏置后得到視覺(jué)云臺(tái)坐標(biāo)系下三維坐標(biāo)。激光測(cè)距傳感器安裝在云臺(tái)坐標(biāo)系下的Y軸原點(diǎn)，且激光器發(fā)射方向與Y軸正向一致，若當(dāng)前激光測(cè)距傳感器讀數(shù)是dm，則作業(yè)目標(biāo)在第五坐標(biāo)系下的坐標(biāo)表示為 [0,d,0]。(2)通過(guò)基坐標(biāo)系與末端執(zhí)行器坐標(biāo)系之間的齊次轉(zhuǎn)換矩陣計(jì)算獲取激光測(cè)距系統(tǒng)下的目標(biāo)物體坐標(biāo)。(3)通過(guò)運(yùn)動(dòng)學(xué)逆解將第一坐標(biāo)系下目標(biāo)點(diǎn)坐標(biāo)及設(shè)定的末端執(zhí)行器位姿轉(zhuǎn)換為液壓機(jī)械臂五個(gè)關(guān)節(jié)角的角度值。(4)通過(guò)關(guān)節(jié)角與液壓缸長(zhǎng)度的映射關(guān)系得到液壓缸運(yùn)動(dòng)的目標(biāo)長(zhǎng)度。(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)的對(duì)應(yīng)關(guān)系建立。逆運(yùn)動(dòng)學(xué)求解部分完成在指定目標(biāo)位置和指定末端破碎錘姿態(tài)的情況下求出滿足能量最優(yōu)條件下[8]的各關(guān)節(jié)角的目標(biāo)值。通過(guò)運(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)的長(zhǎng)度。

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

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

圖3 PID控制結(jié)構(gòu)圖

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

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

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

控制系統(tǒng)軟件使用激光定位平臺(tái)獲取視覺(jué)的坐標(biāo)系點(diǎn)，為將視覺(jué)坐標(biāo)系和機(jī)械臂坐標(biāo)系進(jìn)行整合，需要進(jìn)行手眼標(biāo)定模塊設(shè)計(jì)，手眼標(biāo)定模塊使用最小二乘算法來(lái)計(jì)算兩坐標(biāo)系之間的齊次變換矩陣。通過(guò)齊次變換矩陣可以將視覺(jué)坐標(biāo)系的目標(biāo)坐標(biāo)和目標(biāo)姿態(tài)轉(zhuǎn)化為機(jī)械臂坐標(biāo)系下的坐標(biāo)，進(jìn)一步通過(guò)運(yùn)動(dòng)學(xué)模塊求出各液壓缸的變化量，通過(guò)液壓裝置控制模塊和閉環(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)軟件，通過(guò)激光定位裝置獲取6組目標(biāo)點(diǎn)，取末端破碎錘與地面垂直的姿態(tài)為最終姿態(tài)，通過(guò)實(shí)驗(yàn)數(shù)據(jù)表1可發(fā)現(xiàn)定位誤差在5 cm內(nèi)，定位時(shí)間為10 s，且可調(diào)。通過(guò)實(shí)驗(yàn)可證明該控制系統(tǒng)軟件滿足了破拆機(jī)器人機(jī)械臂運(yùn)動(dòng)自動(dòng)化的目標(biāo)。

表1 控制系統(tǒng)軟件實(shí)驗(yàn)數(shù)據(jù)

[1] 劉慶運(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.

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

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

[5] 蔡自興. 機(jī)器人學(xué)[M]. 北京：清華大學(xué)出版社, 2000.

[6] CRAIG J. Introduction to robotics:mechanics and control (third edition)[M].Pearson Education,Inc.,2005.

[7] 陸震. 冗余自由度機(jī)器人原理及應(yīng)用[M]. 北京：機(jī)械工業(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.

[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.

[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.

Software design of demolish robot manipulator autonomous motion control system

He Feng1，2，Zhang Xiaojian1，2，Zhao Jianghai1，2

(1.Robotic Lab, Institute of Advanced Manufacturing Technology, Changzhou 213164, China;2.Robotic Lab, Institute of Advanced Manufacturing Technology, Hefei Institutes of Physical Science, Chinese Academy of Sciences, Changzhou 213164，China)

Aiming at the problems that the existing demolish robot has long manual positioning time and inaccurate positioning, the control system software of the demolish manipulator autonomous motion which based on laser-positioning is given. The paper has designed the kinematic model, close-loop model, hydraulic device controller model, hand-eye calibration model and motion controller model. The application results show that the software system can complete the design indexes of the control system and improve the work efficiency.

demolish robot; kinematic; autonomous motion

國(guó)家科技支撐計(jì)劃(2015BAK06B02)；江蘇省產(chǎn)學(xué)研合作計(jì)劃(BY2016040-02)

TP273

A

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.

2016-12-14)

何鋒(1977-)，男，碩士，工程師，主要研究方向：機(jī)器人。