破拆機器人機械臂自主運動控制系統(tǒng)軟件設計*

何 鋒，章小建，趙江海

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

破拆機器人機械臂自主運動控制系統(tǒng)軟件設計*

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

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

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

破拆機器人；運動學；自主運動

0 引言

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

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

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

圖1 破拆機器人整機實物圖

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

2 破拆機器人軟件設計

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

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

破拆機器人激光定位下機械臂自主運動整體控制系統(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è)目標距離，經過旋轉及偏置后得到視覺云臺坐標系下三維坐標。激光測距傳感器安裝在云臺坐標系下的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)反饋調節(jié)模塊設計

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

圖3 PID控制結構圖

5 液壓裝置控制模塊設計

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

6 手眼標定模塊設計

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

7 運動控制模塊設計

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

8 結論

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

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

[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] CRAIG J. Introduction to robotics:mechanics and control (third edition)[M].Pearson Education,Inc.,2005.

[7] 陸震. 冗余自由度機器人原理及應用[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.

[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

國家科技支撐計劃(2015BAK06B02)；江蘇省產學研合作計劃(BY2016040-02)

TP273

A

10.19358/j.issn.1674- 7720.2017.09.030

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

2016-12-14)

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